Mercurial > hg > nsaunier > traffic-intelligence

comparison python/cvutils.py @ 474:59903d14d244

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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)