Mercurial Hosting > traffic-intelligence
changeset 474:59903d14d244
added functions to undistort trajectories from inverse mapping
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 13 Mar 2014 01:48:04 -0400 |
parents | 178c69ba2970 |
children | 819eef979d3f 6551a3cf1750 |
files | python/cvutils.py |
diffstat | 1 files changed, 36 insertions(+), 2 deletions(-) [+] |
line wrap: on
line diff
--- 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