# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1394136079 18000
# Node ID 5304299e53a521781d018069cfe875559ec7a937
# Parent  e891a41c6c75fa468fa1a14313ac9234d78612d0
added coordinate undistortion function

diff -r e891a41c6c75 -r 5304299e53a5 python/cvutils.py
--- a/python/cvutils.py	Wed Mar 05 17:47:26 2014 -0500
+++ b/python/cvutils.py	Thu Mar 06 15:01:19 2014 -0500
@@ -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'''