--- a/python/cvutils.py	Thu Jul 13 00:52:53 2017 -0400
+++ b/python/cvutils.py	Fri Jul 14 00:12:03 2017 -0400
@@ -519,11 +519,69 @@
             out.write('{0} '.format(cvmat[i,j]))
         out.write('\n')
 
+def homographyProject(points, homography, output3D = False):
+    '''Returns the coordinates of the points (2xN array) projected through homography'''
+    if points.shape[0] != 2:
+        raise Exception('points of dimension {}'.format(points.shape))
+
+    if homography is not None and homography.size>0:
+        if output3D:
+            outputDim = 3
+        else:
+            outputDim = 2
+        augmentedPoints = append(points,[[1]*points.shape[1]], 0) # 3xN
+        prod = dot(homography, augmentedPoints)
+        return prod[:outputDim,:]/prod[2]
+    elif output3D:
+        return append(points,[[1]*points.shape[1]], 0) # 3xN
+    else:
+        return points
+
+def imageToWorldProject(points, intrinsicCameraMatrix = None, distortionCoefficients = None, homography = None):
+    '''Projects points (2xN array) from image (video) space to world space
+    1. through undistorting if provided by intrinsic camera matrix and distortion coefficients
+    2. through homograph projection (from ideal point (no camera) to world)'''
+    if points.shape[0] != 2:
+        raise Exception('points of dimension {}'.format(points.shape))
+
+    if intrinsicCameraMatrix is not None and distortionCoefficients is not None:
+        undistortedPoints = cv2.undistortPoints(points.T.reshape(1,points.shape[1], 2), intrinsicCameraMatrix, distortionCoefficients).reshape(-1,2)
+        return homographyProject(undistortedPoints.T, homography)
+    else:
+        return homographyProject(points, homography)
+
+def worldToImageProject(points, intrinsicCameraMatrix = None, distortionCoefficients = None, homography = None):
+    '''Projects points (2xN array) from image (video) space to world space
+    1. through undistorting if provided by intrinsic camera matrix and distortion coefficients
+    2. through homograph projection (from ideal point (no camera) to world)'''
+    if points.shape[0] != 2:
+        raise Exception('points of dimension {}'.format(points.shape))
+
+    if intrinsicCameraMatrix is not None and distortionCoefficients is not None:
+        projected3D = homographyProject(points, homography, True)
+        projected, jacobian = cv2.projectPoints(projected3D.T, (0.,0.,0.), (0.,0.,0.), intrinsicCameraMatrix, distortionCoefficients) # in: 3xN, out: 2x1xN
+        return projected.reshape(-1,2).T
+    else:
+        return homographyProject(points, homography)
+    
+def newCameraProject(points, newCameraMatrix):
+    '''Projects points (2xN array) as if seen by camera
+    (or reverse by inverting the camera matrix)'''
+    if points.shape[0] != 2:
+        raise Exception('points of dimension {}'.format(points.shape))
+
+    if newCameraMatrix is not None:
+        augmentedPoints = append(points,[[1]*points.shape[1]], 0) # 3xN
+        projected = dot(newCameraMatrix, augmentedPoints)
+        return projected[:2,:]
+    else:
+        return points
+
 def projectArray(homography, points, intrinsicCameraMatrix = None, distortionCoefficients = None, newCameraMatrix = None):
     '''Returns the coordinates of the projected points through homography
     (format: array 2xN points)'''
     if points.shape[0] != 2:
-        raise Exception('points of dimension {0} {1}'.format(points.shape[0], points.shape[1]))
+        raise Exception('points of dimension {}'.format(points.shape))
 
     augmentedPoints = append(points,[[1]*points.shape[1]], 0) # 3xN
     if homography is not None and homography.size>0:

--- a/python/tests/cvutils.txt	Thu Jul 13 00:52:53 2017 -0400
+++ b/python/tests/cvutils.txt	Fri Jul 14 00:12:03 2017 -0400
@@ -12,7 +12,7 @@
 True
 >>> imgPoints = array([[[150.,170.],[220.,340.],[340.,440.],[401.,521.]]])
 >>> newCameraMatrix = cv2.getDefaultNewCameraMatrix(intrinsicCameraMatrix, (int(round(width*multiplicationFactor)), int(round(height*multiplicationFactor))), True)
->>> undistortedPoints = cv2.undistortPoints(imgPoints, intrinsicCameraMatrix, distortionCoefficients, P = newCameraMatrix).reshape(-1, 2)
+>>> undistortedPoints = cv2.undistortPoints(imgPoints, intrinsicCameraMatrix, distortionCoefficients, P = newCameraMatrix).reshape(-1, 2) # undistort and project as if seen by new camera
 >>> invNewCameraMatrix = linalg.inv(newCameraMatrix)
 >>> tmp = ones((imgPoints[0].shape[0], 3))
 >>> tmp[:,:2] = undistortedPoints
@@ -22,8 +22,20 @@
 True
 >>> (absolute(origPoints[0,:]-imgPoints[0][0,:])).max() < 6.
 True
->>> origPoints = cvutils.projectArray(None, undistortedPoints.T, intrinsicCameraMatrix, distortionCoefficients, newCameraMatrix).T
+>>> reducedPoints2 = cvutils.newCameraProject(undistortedPoints.T, invNewCameraMatrix)
+>>> (reducedPoints == reducedPoints).all()
+True
+
+>>> undistortedPoints = cv2.undistortPoints(imgPoints, intrinsicCameraMatrix, distortionCoefficients).reshape(-1, 2) # undistort to ideal points
+>>> origPoints = cvutils.worldToImageProject(undistortedPoints.T, intrinsicCameraMatrix, distortionCoefficients).T
 >>> (round(origPoints[1:,:]) == imgPoints[0][1:,:]).all()
 True
 >>> (absolute(origPoints[0,:]-imgPoints[0][0,:])).max() < 6.
 True
+
+>>> undistortedPoints = cvutils.imageToWorldProject(imgPoints[0].T, intrinsicCameraMatrix, distortionCoefficients)
+>>> origPoints = cvutils.worldToImageProject(undistortedPoints, intrinsicCameraMatrix, distortionCoefficients).T
+>>> (round(origPoints[1:,:]) == imgPoints[0][1:,:]).all()
+True
+>>> (absolute(origPoints[0,:]-imgPoints[0][0,:])).max() < 6.
+True