--- 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: