Mercurial Hosting > traffic-intelligence
comparison python/cvutils.py @ 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 | d337bffd7283 |
comparison
equal
deleted
inserted
replaced
473:178c69ba2970 | 474:59903d14d244 |
---|---|
263 projected = camera.image_to_world(tuple(srcPoint)) | 263 projected = camera.image_to_world(tuple(srcPoint)) |
264 dstPoints.append([projected[0], projected[1]]) | 264 dstPoints.append([projected[0], projected[1]]) |
265 H, mask = cv2.findHomography(array(srcPoints), array(dstPoints), method, ransacReprojThreshold) | 265 H, mask = cv2.findHomography(array(srcPoints), array(dstPoints), method, ransacReprojThreshold) |
266 return H | 266 return H |
267 | 267 |
268 def correctedCoordinates(x,y, map1, map2, maxDistance = 1.): | 268 def undistortedCoordinates(map1, map2, x, y, maxDistance = 1.): |
269 '''Returns the coordinates of a point in undistorted image | 269 '''Returns the coordinates of a point in undistorted image |
270 map1 and map2 are the mapping functions from undistorted image | 270 map1 and map2 are the mapping functions from undistorted image |
271 to distorted (original image) | 271 to distorted (original image) |
272 map1(x,y) = originalx, originaly''' | 272 map1(x,y) = originalx, originaly''' |
273 from numpy import abs, logical_and, unravel_index, dot, sum | 273 from numpy import abs, logical_and, unravel_index, dot, sum |
278 closeCoordinates = unravel_index(find(indices), distx.shape) # returns i,j, ie y,x | 278 closeCoordinates = unravel_index(find(indices), distx.shape) # returns i,j, ie y,x |
279 xWeights = 1-distx[indices] | 279 xWeights = 1-distx[indices] |
280 yWeights = 1-disty[indices] | 280 yWeights = 1-disty[indices] |
281 return dot(xWeights, closeCoordinates[1])/sum(xWeights), dot(yWeights, closeCoordinates[0])/sum(yWeights) | 281 return dot(xWeights, closeCoordinates[1])/sum(xWeights), dot(yWeights, closeCoordinates[0])/sum(yWeights) |
282 | 282 |
283 def undistortTrajectoryFromCVMapping(map1, map2, t): | |
284 '''test 'perfect' inversion''' | |
285 from moving import Trajectory | |
286 from numpy import isnan | |
287 undistortedTrajectory = Trajectory() | |
288 for i,p in enumerate(t): | |
289 res = undistortedCoordinates(map1, map2, p.x,p.y) | |
290 if not isnan(res).any(): | |
291 undistortedTrajectory.addPositionXY(res[0], res[1]) | |
292 else: | |
293 print i,p,res | |
294 return undistortedTrajectory | |
295 | |
283 def computeInverseMapping(originalImageSize, map1, map2): | 296 def computeInverseMapping(originalImageSize, map1, map2): |
284 'Computes inverse mapping from maps provided by cv2.initUndistortRectifyMap' | 297 'Computes inverse mapping from maps provided by cv2.initUndistortRectifyMap' |
285 from numpy import ones, isnan | 298 from numpy import ones, isnan |
286 invMap1 = -ones(originalImageSize) | 299 invMap1 = -ones(originalImageSize) |
287 invMap2 = -ones(originalImageSize) | 300 invMap2 = -ones(originalImageSize) |
288 for x in range(0,originalImageSize[1]): | 301 for x in range(0,originalImageSize[1]): |
289 for y in range(0,originalImageSize[0]): | 302 for y in range(0,originalImageSize[0]): |
290 res = correctedCoordinates(x,y, map1, map2) | 303 res = undistortedCoordinates(x,y, map1, map2) |
291 if not isnan(res).any(): | 304 if not isnan(res).any(): |
292 invMap1[y,x] = res[0] | 305 invMap1[y,x] = res[0] |
293 invMap2[y,x] = res[1] | 306 invMap2[y,x] = res[1] |
294 return invMap1, invMap2 | 307 return invMap1, invMap2 |
295 | 308 |
336 Unnecessary for reprojection over camera image''' | 349 Unnecessary for reprojection over camera image''' |
337 from numpy.linalg.linalg import inv | 350 from numpy.linalg.linalg import inv |
338 invH = inv(homography) | 351 invH = inv(homography) |
339 invH /= invH[2,2] | 352 invH /= invH[2,2] |
340 return invH | 353 return invH |
354 | |
355 def undistortTrajectory(invMap1, invMap2, positions): | |
356 from numpy import floor, ceil | |
357 floorPositions = floor(positions) | |
358 #ceilPositions = ceil(positions) | |
359 undistortedTrajectory = [[],[]] | |
360 for i in xrange(len(positions[0])): | |
361 x,y = None, None | |
362 if positions[0][i]+1 < invMap1.shape[1] and positions[1][i]+1 < invMap1.shape[0]: | |
363 floorX = invMap1[floorPositions[1][i], floorPositions[0][i]] | |
364 floorY = invMap2[floorPositions[1][i], floorPositions[0][i]] | |
365 ceilX = invMap1[floorPositions[1][i]+1, floorPositions[0][i]+1] | |
366 ceilY = invMap2[floorPositions[1][i]+1, floorPositions[0][i]+1] | |
367 #ceilX = invMap1[ceilPositions[1][i], ceilPositions[0][i]] | |
368 #ceilY = invMap2[ceilPositions[1][i], ceilPositions[0][i]] | |
369 if floorX >=0 and floorY >=0 and ceilX >=0 and ceilY >=0: | |
370 x = floorX+(positions[0][i]-floorPositions[0][i])*(ceilX-floorX) | |
371 y = floorY+(positions[1][i]-floorPositions[1][i])*(ceilY-floorY) | |
372 undistortedTrajectory[0].append(x) | |
373 undistortedTrajectory[1].append(y) | |
374 return undistortedTrajectory | |
341 | 375 |
342 if opencvAvailable: | 376 if opencvAvailable: |
343 def computeTranslation(img1, img2, img1Points, maxTranslation2, minNMatches, windowSize = (5,5), level = 5, criteria = (cv2.TERM_CRITERIA_EPS, 0, 0.01)): | 377 def computeTranslation(img1, img2, img1Points, maxTranslation2, minNMatches, windowSize = (5,5), level = 5, criteria = (cv2.TERM_CRITERIA_EPS, 0, 0.01)): |
344 '''Computes the translation of img2 with respect to img1 | 378 '''Computes the translation of img2 with respect to img1 |
345 (loaded using OpenCV as numpy arrays) | 379 (loaded using OpenCV as numpy arrays) |