Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/cvutils.py @ 1028:cc5cb04b04b0
major update using the trafficintelligence package name and install through pip
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 15 Jun 2018 11:19:10 -0400 |
parents | python/cvutils.py@16932cefabc1 |
children | c6cf75a2ed08 |
comparison
equal
deleted
inserted
replaced
1027:6129296848d3 | 1028:cc5cb04b04b0 |
---|---|
1 #! /usr/bin/env python | |
2 '''Image/Video utilities''' | |
3 | |
4 from trafficintelligence import utils, moving | |
5 | |
6 try: | |
7 import cv2 | |
8 opencvAvailable = True | |
9 except ImportError: | |
10 print('OpenCV library could not be loaded (video replay functions will not be available)') # TODO change to logging module | |
11 opencvAvailable = False | |
12 try: | |
13 import skimage | |
14 skimageAvailable = True | |
15 except ImportError: | |
16 print('Scikit-image library could not be loaded (HoG-based classification methods will not be available)') | |
17 skimageAvailable = False | |
18 | |
19 from sys import stdout | |
20 from os import listdir | |
21 from subprocess import run | |
22 from math import floor, log10, ceil | |
23 | |
24 from numpy import dot, array, append, float32, loadtxt, savetxt, append, zeros, ones, identity, abs as npabs, logical_and, unravel_index, sum as npsum, isnan, mgrid, median, floor as npfloor, ceil as npceil | |
25 from numpy.linalg import inv | |
26 from matplotlib.mlab import find | |
27 from matplotlib.pyplot import imread, imsave | |
28 | |
29 videoFilenameExtensions = ['mov', 'avi', 'mp4', 'MOV', 'AVI', 'MP4'] | |
30 trackerExe = 'feature-based-tracking' | |
31 #importaggdraw # agg on top of PIL (antialiased drawing) | |
32 | |
33 cvRed = {'default': (0,0,255), | |
34 'colorblind': (0,114,178)} | |
35 cvGreen = {'default': (0,255,0), | |
36 'colorblind': (0,158,115)} | |
37 cvBlue = {'default': (255,0,0), | |
38 'colorblind': (213,94,0)} | |
39 cvCyan = {'default': (255, 255, 0), | |
40 'colorblind': (240,228,66)} | |
41 cvYellow = {'default': (0, 255, 255), | |
42 'colorblind': (86,180,233)} | |
43 cvMagenta = {'default': (255, 0, 255), | |
44 'colorblind': (204,121,167)} | |
45 cvWhite = {k: (255, 255, 255) for k in ['default', 'colorblind']} | |
46 cvBlack = {k: (0,0,0) for k in ['default', 'colorblind']} | |
47 | |
48 cvColors3 = {k: utils.PlottingPropertyValues([cvRed[k], cvGreen[k], cvBlue[k]]) for k in ['default', 'colorblind']} | |
49 cvColors = {k: utils.PlottingPropertyValues([cvRed[k], cvGreen[k], cvBlue[k], cvCyan[k], cvYellow[k], cvMagenta[k], cvWhite[k], cvBlack[k]]) for k in ['default', 'colorblind']} | |
50 | |
51 def quitKey(key): | |
52 return chr(key&255)== 'q' or chr(key&255) == 'Q' | |
53 | |
54 def saveKey(key): | |
55 return chr(key&255) == 's' | |
56 | |
57 def int2FOURCC(x): | |
58 fourcc = '' | |
59 for i in range(4): | |
60 fourcc += chr((x >> 8*i)&255) | |
61 return fourcc | |
62 | |
63 def rgb2gray(rgb): | |
64 return dot(rgb[...,:3], [0.299, 0.587, 0.144]) | |
65 | |
66 def matlab2PointCorrespondences(filename): | |
67 '''Loads and converts the point correspondences saved | |
68 by the matlab camera calibration tool''' | |
69 points = loadtxt(filename, delimiter=',') | |
70 savetxt(utils.removeExtension(filename)+'-point-correspondences.txt',append(points[:,:2].T, points[:,3:].T, axis=0)) | |
71 | |
72 def loadPointCorrespondences(filename): | |
73 '''Loads and returns the corresponding points in world (first 2 lines) and image spaces (last 2 lines)''' | |
74 points = loadtxt(filename, dtype=float32) | |
75 return (points[:2,:].T, points[2:,:].T) # (world points, image points) | |
76 | |
77 def cvMatToArray(cvmat): | |
78 '''Converts an OpenCV CvMat to numpy array.''' | |
79 print('Deprecated, use new interface') | |
80 a = zeros((cvmat.rows, cvmat.cols))#array([[0.0]*cvmat.width]*cvmat.height) | |
81 for i in range(cvmat.rows): | |
82 for j in range(cvmat.cols): | |
83 a[i,j] = cvmat[i,j] | |
84 return a | |
85 | |
86 def createWhiteImage(height, width, filename): | |
87 img = ones((height, width, 3), uint8)*255 | |
88 imsave(filename, img) | |
89 | |
90 if opencvAvailable: | |
91 def computeHomography(srcPoints, dstPoints, method=0, ransacReprojThreshold=3.0): | |
92 '''Returns the homography matrix mapping from srcPoints to dstPoints (dimension Nx2)''' | |
93 H, mask = cv2.findHomography(srcPoints, dstPoints, method, ransacReprojThreshold) | |
94 return H | |
95 | |
96 def cvPlot(img, positions, color, lastCoordinate = None, **kwargs): | |
97 if lastCoordinate is None: | |
98 last = positions.length()-1 | |
99 elif lastCoordinate >=0: | |
100 last = min(positions.length()-1, lastCoordinate) | |
101 for i in range(0, last): | |
102 cv2.line(img, positions[i].asint().astuple(), positions[i+1].asint().astuple(), color, **kwargs) | |
103 | |
104 def cvImshow(windowName, img, rescale = 1.0): | |
105 'Rescales the image (in particular if too large)' | |
106 from cv2 import resize | |
107 if rescale != 1.: | |
108 size = (int(round(img.shape[1]*rescale)), int(round(img.shape[0]*rescale))) | |
109 resizedImg = resize(img, size) | |
110 cv2.imshow(windowName, resizedImg) | |
111 else: | |
112 cv2.imshow(windowName, img) | |
113 | |
114 def computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients): | |
115 newImgSize = (int(round(width*undistortedImageMultiplication)), int(round(height*undistortedImageMultiplication))) | |
116 newCameraMatrix = cv2.getDefaultNewCameraMatrix(intrinsicCameraMatrix, newImgSize, True) | |
117 return cv2.initUndistortRectifyMap(intrinsicCameraMatrix, array(distortionCoefficients), None, newCameraMatrix, newImgSize, cv2.CV_32FC1), newCameraMatrix | |
118 | |
119 def playVideo(filenames, windowNames = None, firstFrameNums = None, frameRate = -1, interactive = False, printFrames = True, text = None, rescale = 1., step = 1, colorBlind = False): | |
120 '''Plays the video(s)''' | |
121 if colorBlind: | |
122 colorType = 'colorblind' | |
123 else: | |
124 colorType = 'default' | |
125 if len(filenames) == 0: | |
126 print('Empty filename list') | |
127 return | |
128 if windowNames is None: | |
129 windowNames = ['frame{}'.format(i) for i in range(len(filenames))] | |
130 wait = 5 | |
131 if rescale == 1.: | |
132 for windowName in windowNames: | |
133 cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) | |
134 if frameRate > 0: | |
135 wait = int(round(1000./frameRate)) | |
136 if interactive: | |
137 wait = 0 | |
138 captures = [cv2.VideoCapture(fn) for fn in filenames] | |
139 if array([cap.isOpened() for cap in captures]).all(): | |
140 key = -1 | |
141 ret = True | |
142 nFramesShown = 0 | |
143 if firstFrameNums is not None: | |
144 for i in range(len(captures)): | |
145 captures[i].set(cv2.CAP_PROP_POS_FRAMES, firstFrameNums[i]) | |
146 while ret and not quitKey(key): | |
147 rets = [] | |
148 images = [] | |
149 for cap in captures: | |
150 ret, img = cap.read() | |
151 rets.append(ret) | |
152 images.append(img) | |
153 ret = array(rets).all() | |
154 if ret: | |
155 if printFrames: | |
156 print('frame shown {0}'.format(nFramesShown)) | |
157 for i in range(len(filenames)): | |
158 if text is not None: | |
159 cv2.putText(images[i], text, (10,50), cv2.FONT_HERSHEY_PLAIN, 1, cvRed[colorType]) | |
160 cvImshow(windowNames[i], images[i], rescale) # cv2.imshow('frame', img) | |
161 key = cv2.waitKey(wait) | |
162 if saveKey(key): | |
163 cv2.imwrite('image-{}.png'.format(frameNum), img) | |
164 nFramesShown += step | |
165 if step > 1: | |
166 for i in range(len(captures)): | |
167 captures[i].set(cv2.CAP_PROP_POS_FRAMES, firstFrameNums[i]+nFramesShown) | |
168 cv2.destroyAllWindows() | |
169 else: | |
170 print('Video captures for {} failed'.format(filenames)) | |
171 | |
172 def infoVideo(filename): | |
173 '''Provides all available info on video ''' | |
174 cvPropertyNames = {cv2.CAP_PROP_FORMAT: "format", | |
175 cv2.CAP_PROP_FOURCC: "codec (fourcc)", | |
176 cv2.CAP_PROP_FPS: "fps", | |
177 cv2.CAP_PROP_FRAME_COUNT: "number of frames", | |
178 cv2.CAP_PROP_FRAME_HEIGHT: "heigh", | |
179 cv2.CAP_PROP_FRAME_WIDTH: "width", | |
180 cv2.CAP_PROP_RECTIFICATION: "rectification", | |
181 cv2.CAP_PROP_SATURATION: "saturation"} | |
182 capture = cv2.VideoCapture(filename) | |
183 videoProperties = {} | |
184 if capture.isOpened(): | |
185 for cvprop in [#cv2.CAP_PROP_BRIGHTNESS | |
186 #cv2.CAP_PROP_CONTRAST | |
187 #cv2.CAP_PROP_CONVERT_RGB | |
188 #cv2.CAP_PROP_EXPOSURE | |
189 cv2.CAP_PROP_FORMAT, | |
190 cv2.CAP_PROP_FOURCC, | |
191 cv2.CAP_PROP_FPS, | |
192 cv2.CAP_PROP_FRAME_COUNT, | |
193 cv2.CAP_PROP_FRAME_HEIGHT, | |
194 cv2.CAP_PROP_FRAME_WIDTH, | |
195 #cv2.CAP_PROP_GAIN, | |
196 #cv2.CAP_PROP_HUE | |
197 #cv2.CAP_PROP_MODE | |
198 #cv2.CAP_PROP_POS_AVI_RATIO | |
199 #cv2.CAP_PROP_POS_FRAMES | |
200 #cv2.CAP_PROP_POS_MSEC | |
201 #cv2.CAP_PROP_RECTIFICATION, | |
202 #cv2.CAP_PROP_SATURATION | |
203 ]: | |
204 prop = capture.get(cvprop) | |
205 if cvprop == cv2.CAP_PROP_FOURCC and prop > 0: | |
206 prop = int2FOURCC(int(prop)) | |
207 videoProperties[cvPropertyNames[cvprop]] = prop | |
208 else: | |
209 print('Video capture for {} failed'.format(filename)) | |
210 return videoProperties | |
211 | |
212 def getImagesFromVideo(videoFilename, firstFrameNum = 0, lastFrameNum = 1, step = 1, saveImage = False, outputPrefix = 'image'): | |
213 '''Returns nFrames images from the video sequence''' | |
214 images = [] | |
215 capture = cv2.VideoCapture(videoFilename) | |
216 if capture.isOpened(): | |
217 rawCount = capture.get(cv2.CAP_PROP_FRAME_COUNT) | |
218 if rawCount < 0: | |
219 rawCount = lastFrameNum+1 | |
220 nDigits = int(floor(log10(rawCount)))+1 | |
221 ret = False | |
222 capture.set(cv2.CAP_PROP_POS_FRAMES, firstFrameNum) | |
223 frameNum = firstFrameNum | |
224 while frameNum<lastFrameNum and frameNum<rawCount: | |
225 ret, img = capture.read() | |
226 i = 0 | |
227 while not ret and i<10: | |
228 ret, img = capture.read() | |
229 i += 1 | |
230 if img is not None and img.size>0: | |
231 if saveImage: | |
232 frameNumStr = format(frameNum, '0{}d'.format(nDigits)) | |
233 cv2.imwrite(outputPrefix+frameNumStr+'.png', img) | |
234 else: | |
235 images.append(img) | |
236 frameNum +=step | |
237 if step > 1: | |
238 capture.set(cv2.CAP_PROP_POS_FRAMES, frameNum) | |
239 capture.release() | |
240 else: | |
241 print('Video capture for {} failed'.format(videoFilename)) | |
242 return images | |
243 | |
244 def getFPS(videoFilename): | |
245 capture = cv2.VideoCapture(videoFilename) | |
246 if capture.isOpened(): | |
247 fps = capture.get(cv2.CAP_PROP_FPS) | |
248 capture.release() | |
249 return fps | |
250 else: | |
251 print('Video capture for {} failed'.format(videoFilename)) | |
252 return None | |
253 | |
254 def imageBoxSize(obj, frameNum, width, height, px = 0.2, py = 0.2): | |
255 'Computes the bounding box size of object at frameNum' | |
256 x = [] | |
257 y = [] | |
258 if obj.hasFeatures(): | |
259 for f in obj.getFeatures(): | |
260 if f.existsAtInstant(frameNum): | |
261 p = f.getPositionAtInstant(frameNum) | |
262 x.append(p.x) | |
263 y.append(p.y) | |
264 xmin = min(x) | |
265 xmax = max(x) | |
266 ymin = min(y) | |
267 ymax = max(y) | |
268 xMm = px * (xmax - xmin) | |
269 yMm = py * (ymax - ymin) | |
270 a = max(ymax - ymin + (2 * yMm), xmax - (xmin + 2 * xMm)) | |
271 yCropMin = int(max(0, .5 * (ymin + ymax - a))) | |
272 yCropMax = int(min(height - 1, .5 * (ymin + ymax + a))) | |
273 xCropMin = int(max(0, .5 * (xmin + xmax - a))) | |
274 xCropMax = int(min(width - 1, .5 * (xmin + xmax + a))) | |
275 return yCropMin, yCropMax, xCropMin, xCropMax | |
276 | |
277 def imageBox(img, obj, frameNum, width, height, px = 0.2, py = 0.2, minNPixels = 800): | |
278 'Computes the bounding box of object at frameNum' | |
279 yCropMin, yCropMax, xCropMin, xCropMax = imageBoxSize(obj, frameNum, width, height, px, py) | |
280 if yCropMax != yCropMin and xCropMax != xCropMin and (yCropMax - yCropMin) * (xCropMax - xCropMin) > minNPixels: | |
281 return img[yCropMin : yCropMax, xCropMin : xCropMax] | |
282 else: | |
283 return None | |
284 | |
285 def tracking(configFilename, grouping, videoFilename = None, dbFilename = None, homographyFilename = None, maskFilename = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, dryRun = False): | |
286 '''Runs the tracker in a subprocess | |
287 if grouping is True, it is feature grouping | |
288 otherwise it is feature tracking''' | |
289 if grouping: | |
290 trackingMode = '--gf' | |
291 else: | |
292 trackingMode = '--tf' | |
293 cmd = [trackerExe, configFilename, trackingMode, '--quiet'] | |
294 | |
295 if videoFilename is not None: | |
296 cmd += ['--video-filename', videoFilename] | |
297 if dbFilename is not None: | |
298 cmd += ['--database-filename', dbFilename] | |
299 if homographyFilename is not None: | |
300 cmd += ['--homography-filename', homographyFilename] | |
301 if maskFilename is not None: | |
302 cmd += ['--mask-filename', maskFilename] | |
303 if undistort: | |
304 cmd += ['--undistort', 'true'] | |
305 if intrinsicCameraMatrix is not None: # we currently have to save a file | |
306 from time import time | |
307 intrinsicCameraFilename = '/tmp/intrinsic-{}.txt'.format(time()) | |
308 savetxt(intrinsicCameraFilename, intrinsicCameraMatrix) | |
309 cmd += ['--intrinsic-camera-filename', intrinsicCameraFilename] | |
310 if distortionCoefficients is not None: | |
311 cmd += ['--distortion-coefficients '+' '.join([str(x) for x in distortionCoefficients])] | |
312 if dryRun: | |
313 print(cmd) | |
314 else: | |
315 run(cmd) | |
316 | |
317 def displayTrajectories(videoFilename, objects, boundingBoxes = {}, homography = None, firstFrameNum = 0, lastFrameNumArg = None, printFrames = True, rescale = 1., nFramesStep = 1, saveAllImages = False, nZerosFilenameArg = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1., annotations = [], gtMatches = {}, toMatches = {}, colorBlind = False): | |
318 '''Displays the objects overlaid frame by frame over the video ''' | |
319 if colorBlind: | |
320 colorType = 'colorblind' | |
321 else: | |
322 colorType = 'default' | |
323 | |
324 capture = cv2.VideoCapture(videoFilename) | |
325 width = int(capture.get(cv2.CAP_PROP_FRAME_WIDTH)) | |
326 height = int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT)) | |
327 | |
328 windowName = 'frame' | |
329 if rescale == 1.: | |
330 cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) | |
331 | |
332 if undistort: # setup undistortion | |
333 [map1, map2], newCameraMatrix = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients) | |
334 if capture.isOpened(): | |
335 key = -1 | |
336 ret = True | |
337 frameNum = firstFrameNum | |
338 capture.set(cv2.CAP_PROP_POS_FRAMES, firstFrameNum) | |
339 if lastFrameNumArg is None: | |
340 lastFrameNum = float("inf") | |
341 else: | |
342 lastFrameNum = lastFrameNumArg | |
343 if nZerosFilenameArg is None: | |
344 if lastFrameNumArg is None: | |
345 nZerosFilename = int(ceil(log10(objects[-1].getLastInstant()))) | |
346 else: | |
347 nZerosFilename = int(ceil(log10(lastFrameNum))) | |
348 else: | |
349 nZerosFilename = nZerosFilenameArg | |
350 while ret and not quitKey(key) and frameNum <= lastFrameNum: | |
351 ret, img = capture.read() | |
352 if ret: | |
353 if undistort: | |
354 img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR) | |
355 if printFrames: | |
356 print('frame {0}'.format(frameNum)) | |
357 # plot objects | |
358 for obj in objects[:]: | |
359 if obj.existsAtInstant(frameNum): | |
360 if not hasattr(obj, 'projectedPositions'): | |
361 obj.projectedPositions = obj.getPositions().homographyProject(homography) | |
362 if undistort: | |
363 obj.projectedPositions = obj.projectedPositions.newCameraProject(newCameraMatrix) | |
364 cvPlot(img, obj.projectedPositions, cvColors[colorType][obj.getNum()], frameNum-obj.getFirstInstant()) | |
365 if frameNum not in boundingBoxes and obj.hasFeatures(): | |
366 yCropMin, yCropMax, xCropMin, xCropMax = imageBoxSize(obj, frameNum, homography, width, height) | |
367 cv2.rectangle(img, (xCropMin, yCropMin), (xCropMax, yCropMax), cvBlue[colorType], 1) | |
368 objDescription = '{} '.format(obj.num) | |
369 if moving.userTypeNames[obj.userType] != 'unknown': | |
370 objDescription += moving.userTypeNames[obj.userType][0].upper() | |
371 if len(annotations) > 0: # if we loaded annotations, but there is no match | |
372 if frameNum not in toMatches[obj.getNum()]: | |
373 objDescription += " FA" | |
374 cv2.putText(img, objDescription, obj.projectedPositions[frameNum-obj.getFirstInstant()].asint().astuple(), cv2.FONT_HERSHEY_PLAIN, 1, cvColors[colorType][obj.getNum()]) | |
375 if obj.getLastInstant() == frameNum: | |
376 objects.remove(obj) | |
377 # plot object bounding boxes | |
378 if frameNum in boundingBoxes: | |
379 for rect in boundingBoxes[frameNum]: | |
380 cv2.rectangle(img, rect[0].asint().astuple(), rect[1].asint().astuple(), cvColors[colorType][obj.getNum()]) | |
381 # plot ground truth | |
382 if len(annotations) > 0: | |
383 for gt in annotations: | |
384 if gt.existsAtInstant(frameNum): | |
385 if frameNum in gtMatches[gt.getNum()]: | |
386 color = cvColors[colorType][gtMatches[gt.getNum()][frameNum]] # same color as object | |
387 else: | |
388 color = cvRed[colorType] | |
389 cv2.putText(img, 'Miss', gt.topLeftPositions[frameNum-gt.getFirstInstant()].asint().astuple(), cv2.FONT_HERSHEY_PLAIN, 1, color) | |
390 cv2.rectangle(img, gt.topLeftPositions[frameNum-gt.getFirstInstant()].asint().astuple(), gt.bottomRightPositions[frameNum-gt.getFirstInstant()].asint().astuple(), color) | |
391 # saving images and going to next | |
392 if not saveAllImages: | |
393 cvImshow(windowName, img, rescale) | |
394 key = cv2.waitKey() | |
395 if saveAllImages or saveKey(key): | |
396 cv2.imwrite('image-{{:0{}}}.png'.format(nZerosFilename).format(frameNum), img) | |
397 frameNum += nFramesStep | |
398 if nFramesStep > 1: | |
399 capture.set(cv2.CAP_PROP_POS_FRAMES, frameNum) | |
400 cv2.destroyAllWindows() | |
401 else: | |
402 print('Cannot load file ' + videoFilename) | |
403 | |
404 def computeHomographyFromPDTV(camera): | |
405 '''Returns the homography matrix at ground level from PDTV camera | |
406 https://bitbucket.org/hakanardo/pdtv''' | |
407 # camera = pdtv.load(cameraFilename) | |
408 srcPoints = [[x,y] for x, y in zip([1.,2.,2.,1.],[1.,1.,2.,2.])] # need floats!! | |
409 dstPoints = [] | |
410 for srcPoint in srcPoints: | |
411 projected = camera.image_to_world(tuple(srcPoint)) | |
412 dstPoints.append([projected[0], projected[1]]) | |
413 H, mask = cv2.findHomography(array(srcPoints), array(dstPoints), method = 0) # No need for different methods for finding homography | |
414 return H | |
415 | |
416 def getIntrinsicCameraMatrix(cameraData): | |
417 return array([[cameraData['f']*cameraData['Sx']/cameraData['dx'], 0, cameraData['Cx']], | |
418 [0, cameraData['f']/cameraData['dy'], cameraData['Cy']], | |
419 [0, 0, 1.]]) | |
420 | |
421 def getDistortionCoefficients(cameraData): | |
422 return array([cameraData['k']]+4*[0]) | |
423 | |
424 def undistortedCoordinates(map1, map2, x, y, maxDistance = 1.): | |
425 '''Returns the coordinates of a point in undistorted image | |
426 map1 and map2 are the mapping functions from undistorted image | |
427 to distorted (original image) | |
428 map1(x,y) = originalx, originaly''' | |
429 distx = npabs(map1-x) | |
430 disty = npabs(map2-y) | |
431 indices = logical_and(distx<maxDistance, disty<maxDistance) | |
432 closeCoordinates = unravel_index(find(indices), distx.shape) # returns i,j, ie y,x | |
433 xWeights = 1-distx[indices] | |
434 yWeights = 1-disty[indices] | |
435 return dot(xWeights, closeCoordinates[1])/npsum(xWeights), dot(yWeights, closeCoordinates[0])/npsum(yWeights) | |
436 | |
437 def undistortTrajectoryFromCVMapping(map1, map2, t): | |
438 '''test 'perfect' inversion''' | |
439 undistortedTrajectory = moving.Trajectory() | |
440 for i,p in enumerate(t): | |
441 res = undistortedCoordinates(map1, map2, p.x,p.y) | |
442 if not isnan(res).any(): | |
443 undistortedTrajectory.addPositionXY(res[0], res[1]) | |
444 else: | |
445 print('{} {} {}'.format(i,p,res)) | |
446 return undistortedTrajectory | |
447 | |
448 def computeInverseMapping(originalImageSize, map1, map2): | |
449 'Computes inverse mapping from maps provided by cv2.initUndistortRectifyMap' | |
450 invMap1 = -ones(originalImageSize) | |
451 invMap2 = -ones(originalImageSize) | |
452 for x in range(0,originalImageSize[1]): | |
453 for y in range(0,originalImageSize[0]): | |
454 res = undistortedCoordinates(x,y, map1, map2) | |
455 if not isnan(res).any(): | |
456 invMap1[y,x] = res[0] | |
457 invMap2[y,x] = res[1] | |
458 return invMap1, invMap2 | |
459 | |
460 def intrinsicCameraCalibration(path, checkerBoardSize=[6,7], secondPassSearch=False, display=False, fixK2 = True, fixK3 = True, zeroTangent = True): | |
461 ''' Camera calibration searches through all the images (jpg or png) located | |
462 in _path_ for matches to a checkerboard pattern of size checkboardSize. | |
463 These images should all be of the same camera with the same resolution. | |
464 | |
465 For best results, use an asymetric board and ensure that the image has | |
466 very high contrast, including the background. | |
467 | |
468 cherckerBoardSize is the number of internal corners (7x10 squares have 6x9 internal corners) | |
469 | |
470 The code below is based off of: | |
471 https://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html | |
472 Modified by Paul St-Aubin | |
473 ''' | |
474 import glob, os | |
475 | |
476 # termination criteria | |
477 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) | |
478 | |
479 # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) | |
480 objp = zeros((checkerBoardSize[0]*checkerBoardSize[1],3), float32) | |
481 objp[:,:2] = mgrid[0:checkerBoardSize[1],0:checkerBoardSize[0]].T.reshape(-1,2) | |
482 | |
483 # Arrays to store object points and image points from all the images. | |
484 objpoints = [] # 3d point in real world space | |
485 imgpoints = [] # 2d points in image plane. | |
486 | |
487 ## Loop throuhg all images in _path_ | |
488 images = glob.glob(os.path.join(path,'*.[jJ][pP][gG]'))+glob.glob(os.path.join(path,'*.[jJ][pP][eE][gG]'))+glob.glob(os.path.join(path,'*.[pP][nN][gG]')) | |
489 for fname in images: | |
490 img = cv2.imread(fname) | |
491 gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) | |
492 | |
493 # Find the chess board corners | |
494 ret, corners = cv2.findChessboardCorners(gray, (checkerBoardSize[1],checkerBoardSize[0]), None) | |
495 | |
496 # If found, add object points, image points (after refining them) | |
497 if ret: | |
498 print('Found pattern in '+fname) | |
499 | |
500 if secondPassSearch: | |
501 corners = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) | |
502 | |
503 objpoints.append(objp) | |
504 imgpoints.append(corners) | |
505 | |
506 # Draw and display the corners | |
507 if display: | |
508 cv2.drawChessboardCorners(img, (checkerBoardSize[1],checkerBoardSize[0]), corners, ret) | |
509 if img is not None: | |
510 cv2.imshow('img',img) | |
511 cv2.waitKey(0) | |
512 else: | |
513 print('Pattern not found in '+fname) | |
514 ## Close up image loading and calibrate | |
515 cv2.destroyAllWindows() | |
516 if len(objpoints) == 0 or len(imgpoints) == 0: | |
517 return None | |
518 try: | |
519 flags = 0 | |
520 if fixK2: | |
521 flags += cv2.CALIB_FIX_K2 | |
522 if fixK3: | |
523 flags += cv2.CALIB_FIX_K3 | |
524 if zeroTangent: | |
525 flags += cv2.CALIB_ZERO_TANGENT_DIST | |
526 ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None, flags = flags) | |
527 except NameError: | |
528 return None | |
529 savetxt('intrinsic-camera.txt', camera_matrix) | |
530 print('error: {}'.format(ret)) | |
531 return camera_matrix, dist_coeffs | |
532 | |
533 def undistortImage(img, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1., interpolation=cv2.INTER_LINEAR): | |
534 '''Undistorts the image passed in argument''' | |
535 width = img.shape[1] | |
536 height = img.shape[0] | |
537 [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients) | |
538 return cv2.remap(img, map1, map2, interpolation=interpolation) | |
539 | |
540 def homographyProject(points, homography, output3D = False): | |
541 '''Returns the coordinates of the points (2xN array) projected through homography''' | |
542 if points.shape[0] != 2: | |
543 raise Exception('points of dimension {}'.format(points.shape)) | |
544 | |
545 if homography is not None and homography.size>0: | |
546 if output3D: | |
547 outputDim = 3 | |
548 else: | |
549 outputDim = 2 | |
550 augmentedPoints = append(points,[[1]*points.shape[1]], 0) # 3xN | |
551 prod = dot(homography, augmentedPoints) | |
552 return prod[:outputDim,:]/prod[2] | |
553 elif output3D: | |
554 return append(points,[[1]*points.shape[1]], 0) # 3xN | |
555 else: | |
556 return points | |
557 | |
558 def imageToWorldProject(points, intrinsicCameraMatrix = None, distortionCoefficients = None, homography = None): | |
559 '''Projects points (2xN array) from image (video) space to world space | |
560 1. through undistorting if provided by intrinsic camera matrix and distortion coefficients | |
561 2. through homograph projection (from ideal point (no camera) to world)''' | |
562 if points.shape[0] != 2: | |
563 raise Exception('points of dimension {}'.format(points.shape)) | |
564 | |
565 if intrinsicCameraMatrix is not None and distortionCoefficients is not None: | |
566 undistortedPoints = cv2.undistortPoints(points.T.reshape(1,points.shape[1], 2), intrinsicCameraMatrix, distortionCoefficients).reshape(-1,2) | |
567 return homographyProject(undistortedPoints.T, homography) | |
568 else: | |
569 return homographyProject(points, homography) | |
570 | |
571 def worldToImageProject(points, intrinsicCameraMatrix = None, distortionCoefficients = None, homography = None): | |
572 '''Projects points (2xN array) from image (video) space to world space | |
573 1. through undistorting if provided by intrinsic camera matrix and distortion coefficients | |
574 2. through homograph projection (from ideal point (no camera) to world)''' | |
575 if points.shape[0] != 2: | |
576 raise Exception('points of dimension {}'.format(points.shape)) | |
577 | |
578 if intrinsicCameraMatrix is not None and distortionCoefficients is not None: | |
579 projected3D = homographyProject(points, homography, True) | |
580 projected, jacobian = cv2.projectPoints(projected3D.T, (0.,0.,0.), (0.,0.,0.), intrinsicCameraMatrix, distortionCoefficients) # in: 3xN, out: 2x1xN | |
581 return projected.reshape(-1,2).T | |
582 else: | |
583 return homographyProject(points, homography) | |
584 | |
585 def newCameraProject(points, newCameraMatrix): | |
586 '''Projects points (2xN array) as if seen by camera | |
587 (or reverse by inverting the camera matrix)''' | |
588 if points.shape[0] != 2: | |
589 raise Exception('points of dimension {}'.format(points.shape)) | |
590 | |
591 if newCameraMatrix is not None: | |
592 augmentedPoints = append(points,[[1]*points.shape[1]], 0) # 3xN | |
593 projected = dot(newCameraMatrix, augmentedPoints) | |
594 return projected[:2,:] | |
595 else: | |
596 return points | |
597 | |
598 if opencvAvailable: | |
599 def computeTranslation(img1, img2, img1Points, maxTranslation2, minNMatches, windowSize = (5,5), level = 5, criteria = (cv2.TERM_CRITERIA_EPS, 0, 0.01)): | |
600 '''Computes the translation of img2 with respect to img1 | |
601 (loaded using OpenCV as numpy arrays) | |
602 img1Points are used to compute the translation | |
603 | |
604 TODO add diagnostic if data is all over the place, and it most likely is not a translation (eg zoom, other non linear distortion)''' | |
605 | |
606 nextPoints = array([]) | |
607 (img2Points, status, track_error) = cv2.calcOpticalFlowPyrLK(img1, img2, img1Points, nextPoints, winSize=windowSize, maxLevel=level, criteria=criteria) | |
608 # calcOpticalFlowPyrLK(prevImg, nextImg, prevPts[, nextPts[, status[, err[, winSize[, maxLevel[, criteria[, derivLambda[, flags]]]]]]]]) -> nextPts, status, err | |
609 delta = [] | |
610 for (k, (p1,p2)) in enumerate(zip(img1Points, img2Points)): | |
611 if status[k] == 1: | |
612 dp = p2-p1 | |
613 d = npsum(dp**2) | |
614 if d < maxTranslation2: | |
615 delta.append(dp) | |
616 if len(delta) >= minNMatches: | |
617 return median(delta, axis=0) | |
618 else: | |
619 print(dp) | |
620 return None | |
621 | |
622 if skimageAvailable: | |
623 from skimage.feature import hog | |
624 from skimage import color, transform | |
625 | |
626 def HOG(image, rescaleSize = (64, 64), orientations = 9, pixelsPerCell = (8,8), cellsPerBlock = (2,2), blockNorm = 'L1', visualize = False, transformSqrt = False): | |
627 bwImg = color.rgb2gray(image) | |
628 inputImg = transform.resize(bwImg, rescaleSize) | |
629 features = hog(inputImg, orientations, pixelsPerCell, cellsPerBlock, blockNorm, visualize, transformSqrt, True) | |
630 if visualize: | |
631 from matplotlib.pyplot import imshow, figure, subplot | |
632 hogViz = features[1] | |
633 features = features[0] | |
634 figure() | |
635 subplot(1,2,1) | |
636 imshow(inputImg) | |
637 subplot(1,2,2) | |
638 imshow(hogViz) | |
639 return float32(features) | |
640 | |
641 def createHOGTrainingSet(imageDirectory, classLabel, rescaleSize = (64,64), orientations = 9, pixelsPerCell = (8,8), blockNorm = 'L1', cellsPerBlock = (2, 2), visualize = False, transformSqrt = False): | |
642 inputData = [] | |
643 for filename in listdir(imageDirectory): | |
644 img = imread(imageDirectory+filename) | |
645 features = HOG(img, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm, visualize, transformSqrt) | |
646 inputData.append(features) | |
647 | |
648 nImages = len(inputData) | |
649 return array(inputData, dtype = float32), array([classLabel]*nImages) | |
650 | |
651 | |
652 ######################### | |
653 # running tests | |
654 ######################### | |
655 | |
656 if __name__ == "__main__": | |
657 import doctest | |
658 import unittest | |
659 suite = doctest.DocFileSuite('tests/cvutils.txt') | |
660 #suite = doctest.DocTestSuite() | |
661 unittest.TextTestRunner().run(suite) | |
662 #doctest.testmod() | |
663 #doctest.testfile("example.txt") |