# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1394660209 14400
# Node ID a50c026fdf1412380d90c4b0099f5d2b4b37d950
# Parent  a8f95bbd79bcf80c1a80f7dfa3d93e68440efb40
functions to compute inverse mapping

diff -r a8f95bbd79bc -r a50c026fdf14 python/cvutils.py
--- a/python/cvutils.py	Wed Mar 12 13:36:54 2014 -0400
+++ b/python/cvutils.py	Wed Mar 12 17:36:49 2014 -0400
@@ -265,7 +265,7 @@
         H, mask = cv2.findHomography(array(srcPoints), array(dstPoints), method, ransacReprojThreshold)
         return H
 
-    def correctedCoordinates(x,y, map1, map2):
+    def correctedCoordinates(x,y, map1, map2, maxDistance = 1.):
         '''Returns the coordinates of a point in undistorted image
         map1 and map2 are the mapping functions from undistorted image
         to distorted (original image)
@@ -274,13 +274,25 @@
         from matplotlib.mlab import find
         distx = abs(map1-x)
         disty = abs(map2-y)
-        indices = logical_and(distx<1, disty<1)
+        indices = logical_and(distx<maxDistance, disty<maxDistance)
         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 computeInverseMapping(originalImageSize, map1, map2):
+        'Computes inverse mapping from maps provided by cv2.initUndistortRectifyMap'
+        from numpy import ones, isnan
+        invMap1 = -ones(originalImageSize)
+        invMap2 = -ones(originalImageSize)
+        for x in range(0,originalImageSize[1]):
+            for y in range(0,originalImageSize[0]):
+                res = cvutils.correctedCoordinates(x,y, map1, map2)
+                if not isnan(res).any():
+                    invMap1[y,x] = res[0]
+                    invMap2[y,x] = res[1]
+        return invMap1, invMap2
+
 def printCvMat(cvmat, out = stdout):
     '''Prints the cvmat to out'''
     print('Deprecated, use new interface')