--- a/scripts/compute-homography.py	Fri Jul 14 00:12:03 2017 -0400
+++ b/scripts/compute-homography.py	Fri Jul 14 02:11:21 2017 -0400
@@ -36,47 +36,6 @@
 
 args = parser.parse_args()
 
-# TODO process camera intrinsic and extrinsic parameters to obtain image to world homography, taking example from Work/src/python/generate-homography.py script
-# cameraMat = load(videoFilenamePrefix+'-camera.txt');
-# T1 = cameraMat[3:6,:].copy();
-# A = cameraMat[0:3,0:3].copy();
-
-# # pay attention, rotation may be the transpose
-# # R = T1[:,0:3].T;
-# R = T1[:,0:3];
-# rT = dot(R, T1[:,3]/1000);
-# T = zeros((3,4),'f');
-# T[:,0:3] = R[:];
-# T[:,3] = rT;
-
-# AT = dot(A,T);
-
-# nPoints = 4;
-# worldPoints = cvCreateMat(nPoints, 3, CV_64FC1);
-# imagePoints = cvCreateMat(nPoints, 3, CV_64FC1);
-
-# # extract homography from the camera calibration
-# worldPoints = cvCreateMat(4, 3, CV_64FC1);
-# imagePoints = cvCreateMat(4, 3, CV_64FC1);
-
-# worldPoints[0,:] = [[1, 1, 0]];
-# worldPoints[1,:] = [[1, 2, 0]];
-# worldPoints[2,:] = [[2, 1, 0]];
-# worldPoints[3,:] = [[2, 2, 0]];
-
-# wPoints = [[1,1,2,2],
-#            [1,2,1,2],
-#            [0,0,0,0]];
-# iPoints = utils.worldToImage(AT, wPoints);
-
-# for i in range(nPoints):
-#     imagePoints[i,:] = [iPoints[:,i].tolist()];
-
-# H = cvCreateMat(3, 3, CV_64FC1);
-
-# cvFindHomography(imagePoints, worldPoints, H);
-
-
 homography = np.array([])
 if args.pointCorrespondencesFilename is not None:
     worldPts, videoPts = cvutils.loadPointCorrespondences(args.pointCorrespondencesFilename)
@@ -90,13 +49,15 @@
     worldImg = plt.imread(args.worldFilename)
     videoImg = plt.imread(args.videoFrameFilename)
     if args.undistort:        
-        [map1, map2] = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients)
+        [map1, map2], newCameraMatrix = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients)
         videoImg = cv2.remap(videoImg, map1, map2, interpolation=cv2.INTER_LINEAR)
     print('Click on {} points in the video frame'.format(args.nPoints))
     plt.figure()
     plt.imshow(videoImg)
     plt.tight_layout()
     videoPts = np.array(plt.ginput(args.nPoints, timeout=3000))
+    if args.undistort:
+        videoPts = cvutils.newCameraProject(videoPts, np.linalg.inv(newCameraMatrix))
     print('Click on {} points in the world image'.format(args.nPoints))
     plt.figure()
     plt.imshow(worldImg)
@@ -117,13 +78,16 @@
     worldImg = cv2.imread(args.worldFilename)
     videoImg = cv2.imread(args.videoFrameFilename)
     if args.undistort:
-        [map1, map2] = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients)
+        [map1, map2], newCameraMatrix = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients)
         videoImg = cv2.remap(videoImg, map1, map2, interpolation=cv2.INTER_LINEAR)
         if args.saveImages:
             cv2.imwrite(utils.removeExtension(args.videoFrameFilename)+'-undistorted.png', videoImg)
     invHomography = np.linalg.inv(homography)
-    projectedWorldPts = cvutils.projectArray(invHomography, worldPts.T).T
-    projectedVideoPts = cvutils.projectArray(homography, videoPts.T).T
+    projectedWorldPts = cvutils.homographyProject(worldPts.T, invHomography).T
+    projectedVideoPts = cvutils.homographyProject(videoPts.T, homography).T
+    if args.undistort:
+        projectedWorldPts = cvutils.newCameraProject(projectedWorldPts.T, newCameraMatrix).T
+        videoPts = cvutils.newCameraProject(videoPts.T, newCameraMatrix).T
     for i in range(worldPts.shape[0]):
         # world image
         cv2.circle(worldImg,tuple(np.int32(np.round(worldPts[i]/args.unitsPerPixel))),2,cvutils.cvBlue['default'])