I stumbled upon this old question looking for the same thing, but I did manage to find a program called "Autodesk 123D Catch" here: http://www.123dapp.com/catch that seems to do what you need.
If you had to write something yourself (the horror), I'd start with the OpenCV library; they have some strong 2D code, and some loosely related 3d code that may give you a foothold: http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html