--- a/python/cvutils.py	Tue Mar 11 23:50:15 2014 -0400
+++ b/python/cvutils.py	Wed Mar 12 13:36:54 2014 -0400
@@ -265,6 +265,21 @@
         H, mask = cv2.findHomography(array(srcPoints), array(dstPoints), method, ransacReprojThreshold)
         return H
 
+    def correctedCoordinates(x,y, map1, map2):
+        '''Returns the coordinates of a point in undistorted image
+        map1 and map2 are the mapping functions from undistorted image
+        to distorted (original image)
+        map1(x,y) = originalx, originaly'''
+        from numpy import abs, logical_and, unravel_index, dot, sum
+        from matplotlib.mlab import find
+        distx = abs(map1-x)
+        disty = abs(map2-y)
+        indices = logical_and(distx<1, disty<1)
+        closeCoordinates = unravel_index(find(indices), distx.shape) # returns i,j, ie y,x
+        xWeights = 1-distx[indices]
+        yWeights = 1-disty[indices]
+        return dot(xWeights, closeCoordinates[1])/sum(xWeights), dot(yWeights, closeCoordinates[0])/sum(yWeights)
+
     
 def printCvMat(cvmat, out = stdout):
     '''Prints the cvmat to out'''