Undistort incorrectly modifying cameras and images.

[dbs4261]

I was having some issues with a data set and not having the undistortion come out how I expected. I tried processing the standard temple dataset only adjusting for the focal length. In this case the undistorted reconstruction and the undistorted images should be the same as the input reconstruction and the input images. This is not the case. The input principal point is the center of the image is (320, 240) on a 640x480 image and the output principal point is (277.525, 198.475) on a 638x438 image. I've been trying to make a fix on my own but I've found several inconsistencies and I cant tell what the correct procedure is. There is a primary bug caused by the shared intrinsics model, and undistort reconstruction repeatedly modifying it. There seems to be a secondary bug in the calculation of the scale caused by the shift of 0.5. This results in a scale that is not 1.0 cameras without distortion.