Mercurial Hosting > traffic-intelligence

annotate python/cvutils.py @ 203:e2f31813ade6

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added code to display trajectories on videa
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 06 Mar 2012 18:10:19 -0500
parents 8e7b354666ec
children 966c2cd2bd9f
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
1 #! /usr/bin/env python
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
2 '''Image/Video utilities'''
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
3
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
4 import Image, ImageDraw # PIL
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
5 try:
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
6 import cv2
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
7 opencvExists = True
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
8 except ImportError:
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
9 print('OpenCV library could not be loaded')
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
10 opencvExists = False
99
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
11 from sys import stdout
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
12
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
13 import utils
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
14
44
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
15 #import aggdraw # agg on top of PIL (antialiased drawing)
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
16 #import utils
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
17
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
18 __metaclass__ = type
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
19
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
20 cvRed = (0,0,255)
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
21 cvGreen = (0,255,0)
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
22 cvBlue = (255,0,0)
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
23 cvColors = utils.PlottingPropertyValues([cvRed,
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
24 cvGreen,
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
25 cvBlue])
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
26
156
2eef5620c0b3 added key values for opencv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 154
diff changeset
27 cvKeyNumbers = {'a':1048673,
2eef5620c0b3 added key values for opencv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 154
diff changeset
28 'n': 1048686,
2eef5620c0b3 added key values for opencv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 154
diff changeset
29 'y': 1048697}
2eef5620c0b3 added key values for opencv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 154
diff changeset
30
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
31 def drawLines(filename, origins, destinations, w = 1, resultFilename='image.png'):
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
32 '''Draws lines over the image '''
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
33
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
34 img = Image.open(filename)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
35
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
36 draw = ImageDraw.Draw(img)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
37 #draw = aggdraw.Draw(img)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
38 #pen = aggdraw.Pen("red", width)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
39 for p1, p2 in zip(origins, destinations):
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
40 draw.line([p1.x, p1.y, p2.x, p2.y], width = w, fill = (256,0,0))
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
41 #draw.line([p1.x, p1.y, p2.x, p2.y], pen)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
42 del draw
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
43
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
44 #out = utils.openCheck(resultFilename)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
45 img.save(resultFilename)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
46
159
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
47 def matlab2PointCorrespondences(filename):
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
48 '''Loads and converts the point correspondences saved
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
49 by the matlab camera calibration tool'''
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
50 from numpy.lib.io import loadtxt, savetxt
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
51 from numpy.lib.function_base import append
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
52 points = loadtxt(filename, delimiter=',')
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
53 savetxt(utils.removeExtension(filename)+'-point-correspondences.txt',append(points[:,:2].T, points[:,3:].T, axis=0))
115f7f90286d updated calibration-translation and added function to convert point correspondences
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 156
diff changeset
54
160
b0719b3ad3db created function to load point correspondences and updates scripts that use it
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 159
diff changeset
55 def loadPointCorrespondences(filename):
b0719b3ad3db created function to load point correspondences and updates scripts that use it
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 159
diff changeset
56 '''Loads and returns the corresponding points in world (first 2 lines) and image spaces (last 2 lines)'''
b0719b3ad3db created function to load point correspondences and updates scripts that use it
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 159
diff changeset
57 from numpy.lib.io import loadtxt
b0719b3ad3db created function to load point correspondences and updates scripts that use it
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 159
diff changeset
58 from numpy import float32
b0719b3ad3db created function to load point correspondences and updates scripts that use it
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 159
diff changeset
59 points = loadtxt(filename, dtype=float32)
b0719b3ad3db created function to load point correspondences and updates scripts that use it
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 159
diff changeset
60 return (points[:2,:].T, points[2:,:].T) # (world points, image points)
b0719b3ad3db created function to load point correspondences and updates scripts that use it
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 159
diff changeset
61
99
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
62 def computeHomography(srcPoints, dstPoints, method=0, ransacReprojThreshold=0.0):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
63 '''Returns the homography matrix mapping from srcPoints to dstPoints (dimension Nx2)'''
151
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
64 #cvSrcPoints = arrayToCvMat(srcPoints);
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
65 #cvDstPoints = arrayToCvMat(dstPoints);
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
66 #H = cv.CreateMat(3, 3, cv.CV_64FC1)
4af774bb186d wrote a simple script to compute homography from point correspondences and display the reprojection for visual verification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 150
diff changeset
67 H, mask = cv2.findHomography(srcPoints, dstPoints, method, ransacReprojThreshold)
99
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
68 return H
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
69
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
70 def cvMatToArray(cvmat):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
71 '''Converts an OpenCV CvMat to numpy array.'''
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
72 from numpy.core.multiarray import zeros
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
73 a = zeros((cvmat.rows, cvmat.cols))#array([[0.0]*cvmat.width]*cvmat.height)
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
74 for i in xrange(cvmat.rows):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
75 for j in xrange(cvmat.cols):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
76 a[i,j] = cvmat[i,j]
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
77 return a
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
78
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
79 if opencvExists:
152
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 151
diff changeset
80 def arrayToCvMat(a, t = cv2.cv.CV_64FC1):
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
81 '''Converts a numpy array to an OpenCV CvMat, with default type CV_64FC1.'''
152
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 151
diff changeset
82 cvmat = cv2.cv.CreateMat(a.shape[0], a.shape[1], t)
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
83 for i in range(cvmat.rows):
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
84 for j in range(cvmat.cols):
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
85 cvmat[i,j] = a[i,j]
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
86 return cvmat
99
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
87
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
88 def draw(img, positions, color, lastCoordinate = None):
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
89 last = lastCoordinate+1
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
90 if lastCoordinate != None and lastCoordinate >=0:
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
91 last = min(positions.length()-1, lastCoordinate)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
92 for i in range(0, last-1):
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
93 cv2.line(img, positions[i].astuple(), positions[i+1].astuple(), color)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
94
149
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
95 def playVideo(filename):
150
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
96 '''Plays the video'''
149
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
97 capture = cv2.VideoCapture(filename)
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
98 if capture.isOpened():
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
99 key = -1
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
100 while key!= 113: # 'q'
149
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
101 ret, img = capture.read()
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
102 if ret:
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
103 cv2.imshow('frame', img)
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
104 key = cv2.waitKey(5)
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
105
150
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
106 def getImagesFromVideo(filename, nImages = 1):
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
107 '''Returns nImages images from the video sequence'''
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
108 images = []
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
109 capture = cv2.VideoCapture(filename)
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
110 if capture.isOpened():
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
111 ret = False
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
112 while len(images)<nImages:
171
8e7b354666ec corrected bug in to get images from video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
113 ret, img = capture.read()
150
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
114 while not ret:
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
115 ret, img = capture.read()
171
8e7b354666ec corrected bug in to get images from video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
116 if img.size>0:
8e7b354666ec corrected bug in to get images from video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 160
diff changeset
117 images.append(img)
150
404f3cade05f added python function to get image frames from video filenames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 149
diff changeset
118 return images
149
0f552c8b1650 added python function to play video
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 116
diff changeset
119
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
120 def displayTrajectories(videoFilename, objects, homography = None):
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
121 '''Displays the objects overlaid frame by frame over the video '''
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
122 capture = cv2.VideoCapture(videoFilename)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
123 if capture.isOpened():
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
124 key = -1
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
125 frameNum = 1
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
126 while key!= 113: # 'q'
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
127 ret, img = capture.read()
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
128 if ret:
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
129 print(frameNum)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
130 for obj in objects:
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
131 if obj.existsAtInstant(frameNum):
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
132 #obj.getTimeInterval()
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
133 if homography != None and obj.getFirstInstant() == frameNum:
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
134 obj.projectedPositions = obj.positions.project(homography)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
135 draw(img, obj.projectedPositions, cvRed, frameNum-obj.getFirstInstant())
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
136 cv2.imshow('frame', img)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
137 key = cv2.waitKey(50)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
138 frameNum += 1
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 171
diff changeset
139
99
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
140 def printCvMat(cvmat, out = stdout):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
141 '''Prints the cvmat to out'''
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
142 for i in xrange(cvmat.rows):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
143 for j in xrange(cvmat.cols):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
144 out.write('{0} '.format(cvmat[i,j]))
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
145 out.write('\n')
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
146
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
147 def projectArray(homography, points):
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
148 '''Returns the coordinates of the projected points (format 2xN points)
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
149 through homography'''
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
150 from numpy.core._dotblas import dot
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
151 from numpy.core.multiarray import array
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
152 from numpy.lib.function_base import append
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
153
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
154 if points.shape[0] != 2:
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
155 raise Exception('points of dimension {0} {1}'.format(points.shape[0], points.shape[1]))
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
156
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
157 if (homography!=None) and homography.size>0:
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
158 augmentedPoints = append(points,[[1]*points.shape[1]], 0)
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
159 prod = dot(homography, augmentedPoints)
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
160 return prod[0:2]/prod[2]
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
161 else:
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
162 return p
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
163
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
164 def project(homography, p):
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
165 '''Returns the coordinates of the projection of the point p
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
166 through homography'''
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
167 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
168 return projectArray(homography, array([[p[0]],p[1]]))
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
169
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
170 def projectTrajectory(homography, trajectory):
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
171 '''Projects a series of points in the format
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
172 [[x1, x2, ...],
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
173 [y1, y2, ...]]'''
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
174 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
175 return projectArray(homography, array(trajectory))
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
176
48
8aed225f71d8 rearranged code for function readline and getlines (and characters), moved invertHomography to cvutils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 44
diff changeset
177 def invertHomography(homography):
8aed225f71d8 rearranged code for function readline and getlines (and characters), moved invertHomography to cvutils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 44
diff changeset
178 'Returns an inverted homography'
8aed225f71d8 rearranged code for function readline and getlines (and characters), moved invertHomography to cvutils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 44
diff changeset
179 from numpy.linalg.linalg import inv
8aed225f71d8 rearranged code for function readline and getlines (and characters), moved invertHomography to cvutils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 44
diff changeset
180 invH = inv(homography)
8aed225f71d8 rearranged code for function readline and getlines (and characters), moved invertHomography to cvutils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 44
diff changeset
181 invH /= invH[2,2]
8aed225f71d8 rearranged code for function readline and getlines (and characters), moved invertHomography to cvutils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 44
diff changeset
182 return invH
8aed225f71d8 rearranged code for function readline and getlines (and characters), moved invertHomography to cvutils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 44
diff changeset
183
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
184 if opencvExists:
154
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
185 def computeTranslation(img1, img2, img1Points, maxTranslation2, minNMatches, windowSize = (5,5), level = 5, criteria = (cv2.TERM_CRITERIA_EPS, 0, 0.01)):
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
186 '''Computes the translation of img2 with respect to img1
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
187 (loaded using OpenCV as numpy arrays)
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
188 img1Points are used to compute the translation
100
2a3cafcf5faf added function to compute the translation between two images
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 99
diff changeset
189
154
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
190 TODO add diagnostic if data is all over the place, and it most likely is not a translation (eg zoom, other non linear distortion)'''
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
191 from numpy.core.multiarray import array
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
192 from numpy.lib.function_base import median
154
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
193 from numpy.core.fromnumeric import sum
100
2a3cafcf5faf added function to compute the translation between two images
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 99
diff changeset
194
154
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
195 nextPoints = array([])
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
196 (img2Points, status, track_error) = cv2.calcOpticalFlowPyrLK(img1, img2, img1Points, nextPoints, winSize=windowSize, maxLevel=level, criteria=criteria)
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
197 # calcOpticalFlowPyrLK(prevImg, nextImg, prevPts[, nextPts[, status[, err[, winSize[, maxLevel[, criteria[, derivLambda[, flags]]]]]]]]) -> nextPts, status, err
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
198 delta = []
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
199 for (k, (p1,p2)) in enumerate(zip(img1Points, img2Points)):
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
200 if status[k] == 1:
154
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
201 dp = p2-p1
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
202 d = sum(dp**2)
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
203 if d < maxTranslation2:
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
204 delta.append(dp)
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
205 if len(delta) >= minNMatches:
668710d4c773 updated computeTranslation with cv2
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
206 return median(delta, axis=0)
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
207 else:
156
2eef5620c0b3 added key values for opencv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 154
diff changeset
208 print(dp)
112
67555e968b5e added tests if OpenCV python libraries are not available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 102
diff changeset
209 return None