--- a/python/cvutils.py	Wed Mar 12 17:47:49 2014 -0400
+++ b/python/cvutils.py	Thu Mar 13 01:48:04 2014 -0400
@@ -265,7 +265,7 @@
         H, mask = cv2.findHomography(array(srcPoints), array(dstPoints), method, ransacReprojThreshold)
         return H
 
-    def correctedCoordinates(x,y, map1, map2, maxDistance = 1.):
+    def undistortedCoordinates(map1, map2, x, y, 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)
@@ -280,6 +280,19 @@
         yWeights = 1-disty[indices]
         return dot(xWeights, closeCoordinates[1])/sum(xWeights), dot(yWeights, closeCoordinates[0])/sum(yWeights)
 
+    def undistortTrajectoryFromCVMapping(map1, map2, t):
+        '''test 'perfect' inversion'''
+        from moving import Trajectory
+        from numpy import isnan
+        undistortedTrajectory = Trajectory()
+        for i,p in enumerate(t):
+            res = undistortedCoordinates(map1, map2, p.x,p.y)
+            if not isnan(res).any():
+                undistortedTrajectory.addPositionXY(res[0], res[1])
+            else:
+                print i,p,res
+        return undistortedTrajectory
+
     def computeInverseMapping(originalImageSize, map1, map2):
         'Computes inverse mapping from maps provided by cv2.initUndistortRectifyMap'
         from numpy import ones, isnan
@@ -287,7 +300,7 @@
         invMap2 = -ones(originalImageSize)
         for x in range(0,originalImageSize[1]):
             for y in range(0,originalImageSize[0]):
-                res = correctedCoordinates(x,y, map1, map2)
+                res = undistortedCoordinates(x,y, map1, map2)
                 if not isnan(res).any():
                     invMap1[y,x] = res[0]
                     invMap2[y,x] = res[1]
@@ -339,6 +352,27 @@
     invH /= invH[2,2]
     return invH
 
+def undistortTrajectory(invMap1, invMap2, positions):
+    from numpy import floor, ceil
+    floorPositions = floor(positions)
+    #ceilPositions = ceil(positions)
+    undistortedTrajectory = [[],[]]
+    for i in xrange(len(positions[0])):
+        x,y = None, None
+        if positions[0][i]+1 < invMap1.shape[1] and positions[1][i]+1 < invMap1.shape[0]:
+            floorX = invMap1[floorPositions[1][i], floorPositions[0][i]]
+            floorY = invMap2[floorPositions[1][i], floorPositions[0][i]]
+            ceilX = invMap1[floorPositions[1][i]+1, floorPositions[0][i]+1]
+            ceilY = invMap2[floorPositions[1][i]+1, floorPositions[0][i]+1]
+            #ceilX = invMap1[ceilPositions[1][i], ceilPositions[0][i]]
+            #ceilY = invMap2[ceilPositions[1][i], ceilPositions[0][i]]
+            if floorX >=0 and floorY >=0 and ceilX >=0 and ceilY >=0:
+                x = floorX+(positions[0][i]-floorPositions[0][i])*(ceilX-floorX)
+                y = floorY+(positions[1][i]-floorPositions[1][i])*(ceilY-floorY)
+        undistortedTrajectory[0].append(x)
+        undistortedTrajectory[1].append(y)
+    return undistortedTrajectory
+
 if opencvAvailable:
     def computeTranslation(img1, img2, img1Points, maxTranslation2, minNMatches, windowSize = (5,5), level = 5, criteria = (cv2.TERM_CRITERIA_EPS, 0, 0.01)):
         '''Computes the translation of img2 with respect to img1