Mercurial > hg > nsaunier > traffic-intelligence

annotate python/cvutils.py @ 99:e7dc5a780f09

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added conversion functions and homography computation
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 12 Jul 2011 17:01:59 -0400
parents b85912ab4064
children 2a3cafcf5faf
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
99
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
5 import cv
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
6 from sys import stdout
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
7
44
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
8 #import aggdraw # agg on top of PIL (antialiased drawing)
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
9 from moving import Point
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
10 #import utils
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
11
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
12 __metaclass__ = type
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
13
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
14 def drawLines(filename, origins, destinations, w = 1, resultFilename='image.png'):
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
15 '''Draws lines over the image '''
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
16
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
17 img = Image.open(filename)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
18
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
19 draw = ImageDraw.Draw(img)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
20 #draw = aggdraw.Draw(img)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
21 #pen = aggdraw.Pen("red", width)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
22 for p1, p2 in zip(origins, destinations):
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
23 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
24 #draw.line([p1.x, p1.y, p2.x, p2.y], pen)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
25 del draw
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
26
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
27 #out = utils.openCheck(resultFilename)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
28 img.save(resultFilename)
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
29
99
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
30 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
31 '''Returns the homography matrix mapping from srcPoints to dstPoints (dimension Nx2)'''
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
32 cvSrcPoints = arrayToCvMat(srcPoints);
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
33 cvDstPoints = arrayToCvMat(dstPoints);
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
34 H = cv.CreateMat(3, 3, cv.CV_64FC1)
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
35 cv.FindHomography(cvSrcPoints, cvDstPoints, H, method, ransacReprojThreshold)
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
36 return H
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
37
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
38 def cvMatToArray(cvmat):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
39 '''Converts an OpenCV CvMat to numpy array.'''
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
40 from numpy.core.multiarray import zeros
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
41 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
42 for i in xrange(cvmat.rows):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
43 for j in xrange(cvmat.cols):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
44 a[i,j] = cvmat[i,j]
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
45 return a
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
46
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
47 def arrayToCvMat(a, t = cv.CV_64FC1):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
48 '''Converts a numpy array to an OpenCV CvMat, with default type CV_64FC1.'''
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
49 cvmat = cv.CreateMat(a.shape[0], a.shape[1], t)
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
50 for i in range(cvmat.rows):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
51 for j in range(cvmat.cols):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
52 cvmat[i,j] = a[i,j]
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
53 return cvmat
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
54
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
55 def printCvMat(cvmat, out = stdout):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
56 '''Prints the cvmat to out'''
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
57 for i in xrange(cvmat.rows):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
58 for j in xrange(cvmat.cols):
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
59 out.write('{0} '.format(cvmat[i,j]))
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
60 out.write('\n')
e7dc5a780f09 added conversion functions and homography computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 98
diff changeset
61
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
62 def projectArray(homography, points):
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
63 '''Returns the coordinates of the projected points (format 2xN points)
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
64 through homography'''
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
65 from numpy.core._dotblas import dot
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
66 from numpy.core.multiarray import array
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
67 from numpy.lib.function_base import append
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
68
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
69 if points.shape[0] != 2:
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
70 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
71
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
72 if (homography!=None) and homography.size>0:
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
73 augmentedPoints = append(points,[[1]*points.shape[1]], 0)
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
74 prod = dot(homography, augmentedPoints)
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
75 return prod[0:2]/prod[2]
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
76 else:
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
77 return p
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
78
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
79 def project(homography, p):
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
80 '''Returns the coordinates of the projection of the point p
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
81 through homography'''
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
82 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
83 return projectArray(homography, array([[p[0]],p[1]]))
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
84
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
85 def projectTrajectory(homography, trajectory):
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
86 '''Projects a series of points in the format
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
87 [[x1, x2, ...],
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
88 [y1, y2, ...]]'''
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
89 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 63
diff changeset
90 return projectArray(homography, array(trajectory))
28
9ae709a2e8d0 rearranged code
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
91
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
92 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
93 '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
94 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
95 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
96 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
97 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
98
60
1d36a676c745 minor class name change
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 48
diff changeset
99 class FourWayIntersection:
44
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
100 '''Simple class for simple intersection outline'''
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
101 def __init__(self, dimension, coordX, coordY):
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
102 self.dimension = dimension
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
103 self.coordX = coordX
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
104 self.coordY = coordY
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
105
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
106 def plot(self, options = 'k'):
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
107 from matplotlib.pyplot import plot, axis
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
108
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
109 minX = min(self.dimension[0])
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
110 maxX = max(self.dimension[0])
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
111 minY = min(self.dimension[1])
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
112 maxY = max(self.dimension[1])
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
113
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
114 plot([minX, self.coordX[0], self.coordX[0]], [self.coordY[0], self.coordY[0], minY],options)
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
115 plot([self.coordX[1], self.coordX[1], maxX], [minY, self.coordY[0], self.coordY[0]],options)
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
116 plot([minX, self.coordX[0], self.coordX[0]], [self.coordY[1], self.coordY[1], maxY],options)
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
117 plot([self.coordX[1], self.coordX[1], maxX], [maxY, self.coordY[1], self.coordY[1]],options)
be3ae926e4e8 added simple intersection description, function to load collision points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 28
diff changeset
118 axis('equal')