Camera Projection Matrix Decomposition Issues

edit

I have been having serious issues getting a camera calibration algorithm to work. Currently, I am trying to take a series of images taken from an aerial camera and compute the intrinsic and extrinsic parameters. Eventually, I can get telemetry data to compute the extrinsics, however for now, I am stuck solving it manually. The primary data I have is a set of UTM Coordinates for multiple points in each image as well as the corresponding pixel location and elevation.

To simplify my problem, I am creating a test program which uses a drastically simplified camera model with very, very basic coordinates. I can take a set of camera parameters and build a matrix, however when I decompose the matrix and rebuild it, the reprojection error is completely off. In particular, the algorithm works up until I need to get the scale factor for the intrinsic parameters. (fx/fy) in my notation. My code has a couple links to resources which I am referring to.

I was wondering if anyone here knows of a resource for me to investigate why this is so difficult? I would be greatful for either a working demonstration to compare, a good research paper with equations, or any advice. I have my source code for my dummy test at https://github.com/marvins/temp_images/blob/master/camera_test.cpp . Please forgive any horrifying coding practices. I have started hacking at it.

Also, I have a simple image of my camera model to test on.

I would be more than happy to upload any successful code that I produce, as well as the material for a basic demonstration. I am completely stuck and a little desperate.

Thank you very much for your help.