Mercurial Hosting > traffic-intelligence

annotate python/compute-object-from-features.py @ 105:9844c69d8fa2

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added multiply method to Point
author Nicolas Saunier <nico@confins.net>
date Thu, 14 Jul 2011 19:48:30 -0400
parents 1621b46a1523
children ce4cb46b3603
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
1 #!/usr/bin/env python
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
2
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
3 import sys
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
4
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
5 import matplotlib.mlab as pylab
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
6 import matplotlib.pyplot as plt
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
7 import numpy as np
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
8
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
9 import cv
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
10 import utils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
11 import cvutils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
12 import ubc_utils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
13 import moving
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
14
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
15 # use something like getopt to manage arguments if necessary
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
16
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
17 if len(sys.argv) < 3:
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
18 print('Usage: {0} <video-filename> <n-objects>'.format(sys.argv[0]))
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
19 sys.exit()
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
20
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
21 if sys.argv[1].endswith('.avi'):
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
22 videoFilenamePrefix = utils.removeExtension(sys.argv[1],'.')
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
23 else:
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
24 videoFilenamePrefix = sys.argv[1]
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
25
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
26 objectNum = int(sys.argv[2])
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
27
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
28 objects = ubc_utils.loadTrajectories(videoFilenamePrefix+'-objects.txt', objectNum+1)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
29 obj = objects[objectNum]
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
30 features = ubc_utils.loadTrajectories(videoFilenamePrefix+'-features.txt', max(obj.featureNumbers)+1)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
31 h = np.loadtxt(videoFilenamePrefix+'-homography.txt')
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
32
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
33 invh = cvutils.invertHomography(h)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
34
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
35 def computeObject(features, homography, timeInterval = None):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
36 if not timeInterval:
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
37 raise Exception('not implemented') # compute from the features
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
38
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
39 yCoordinates = -np.ones((len(features),int(timeInterval.length())))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
40 for i,f in enumerate(features):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
41 traj = f.getPositions().asArray()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
42 imgTraj = cvutils.projectArray(homography, traj)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
43 yCoordinates[i,f.getFirstInstant()-timeInterval.first:f.getLastInstant()+1-timeInterval.first] = imgTraj[1,:]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
44
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
45 indices = np.argmax(yCoordinates,0)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
46 newTraj = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
47 for j,idx in enumerate(indices):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
48 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
49 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant()))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
50
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
51 return newTraj
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
52
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
53 # TODO version median
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
54
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
55 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
56 kalman=cv.CreateKalman(6, 4)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
57 kalman.transition_matrix[0,2]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
58 kalman.transition_matrix[0,4]=1./2
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
59 kalman.transition_matrix[1,3]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
60 kalman.transition_matrix[1,5]=1./2
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
61 kalman.transition_matrix[2,4]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
62 kalman.transition_matrix[3,5]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
63
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
64 cv.SetIdentity(kalman.measurement_matrix, 1.)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
65 cv.SetIdentity(kalman.process_noise_cov, processNoiseCov)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
66 cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
67 cv.SetIdentity(kalman.error_cov_post, 1.)
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
68
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
69 p = positions[0]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
70 v = velocities[0]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
71 v2 = velocities[2]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
72 a = (v2-v).multiply(0.5)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
73 kalman.state_post[0,0]=p.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
74 kalman.state_post[1,0]=p.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
75 kalman.state_post[2,0]=v.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
76 kalman.state_post[3,0]=v.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
77 kalman.state_post[4,0]=a.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
78 kalman.state_post[5,0]=a.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
79
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
80 filteredPositions = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
81 filteredVelocities = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
82 measurement = cv.CreateMat(4,1,cv.CV_32FC1)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
83 for i in xrange(positions.length()):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
84 cv.KalmanPredict(kalman) # no control
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
85 p = positions[i]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
86 v = velocities[i]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
87 measurement[0,0] = p.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
88 measurement[1,0] = p.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
89 measurement[2,0] = v.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
90 measurement[3,0] = v.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
91 cv.KalmanCorrect(kalman, measurement)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
92 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0])
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
93 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0])
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
94
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
95 return (filteredPositions, filteredVelocities)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
96
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
97 (filteredPositions, filteredVelocities) = kalmanFilter(newTraj, obj.getVelocities(), 1e-5, 1e-1)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
98
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
99 plt.clf()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
100 obj.draw('rx')
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
101 for fnum in obj.featureNumbers: features[fnum].draw()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
102 newTraj.draw('bx')
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
103 plt.axis('equal')