Mercurial > hg > nsaunier > traffic-intelligence

comparison 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
comparison
equal deleted inserted replaced
104:13187af8622d 105:9844c69d8fa2
30 features = ubc_utils.loadTrajectories(videoFilenamePrefix+'-features.txt', max(obj.featureNumbers)+1) 30 features = ubc_utils.loadTrajectories(videoFilenamePrefix+'-features.txt', max(obj.featureNumbers)+1)
31 h = np.loadtxt(videoFilenamePrefix+'-homography.txt') 31 h = np.loadtxt(videoFilenamePrefix+'-homography.txt')
32 32
33 invh = cvutils.invertHomography(h) 33 invh = cvutils.invertHomography(h)
34 34
35 yCoordinates = -np.ones((len(obj.featureNumbers),int(obj.length()))) 35 def computeObject(features, homography, timeInterval = None):
36 for i,fnum in enumerate(obj.featureNumbers): 36 if not timeInterval:
37 traj = features[fnum].getPositions().asArray() 37 raise Exception('not implemented') # compute from the features
38 imgTraj = cvutils.projectArray(invh, traj) 38
39 yCoordinates[i,features[fnum].getFirstInstant()-obj.getFirstInstant():features[fnum].getLastInstant()+1-obj.getFirstInstant()] = imgTraj[1,:] 39 yCoordinates = -np.ones((len(features),int(timeInterval.length())))
40 for i,f in enumerate(features):
41 traj = f.getPositions().asArray()
42 imgTraj = cvutils.projectArray(homography, traj)
43 yCoordinates[i,f.getFirstInstant()-timeInterval.first:f.getLastInstant()+1-timeInterval.first] = imgTraj[1,:]
40 44
41 indices = argmax(yCoordinates,0) 45 indices = np.argmax(yCoordinates,0)
42 newTraj = moving.Trajectory() 46 newTraj = moving.Trajectory()
43 for j,idx in enumerate(indices): 47 for j,idx in enumerate(indices):
44 newTraj.addPosition(features[obj.featureNumbers[idx]].getPositionAtInstant(j+obj.getFirstInstant())) 48 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first))
49 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant()))
45 50
46 # TODO 51 return newTraj
47 # use kalman filter over the resulting trajectory 52
48 # estimate the error terms using actual features 53 # TODO version median
54
55 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
56 kalman=cv.CreateKalman(6, 4)
57 kalman.transition_matrix[0,2]=1
58 kalman.transition_matrix[0,4]=1./2
59 kalman.transition_matrix[1,3]=1
60 kalman.transition_matrix[1,5]=1./2
61 kalman.transition_matrix[2,4]=1
62 kalman.transition_matrix[3,5]=1
63
64 cv.SetIdentity(kalman.measurement_matrix, 1.)
65 cv.SetIdentity(kalman.process_noise_cov, processNoiseCov)
66 cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov)
67 cv.SetIdentity(kalman.error_cov_post, 1.)
68
69 p = positions[0]
70 v = velocities[0]
71 v2 = velocities[2]
72 a = (v2-v).multiply(0.5)
73 kalman.state_post[0,0]=p.x
74 kalman.state_post[1,0]=p.y
75 kalman.state_post[2,0]=v.x
76 kalman.state_post[3,0]=v.y
77 kalman.state_post[4,0]=a.x
78 kalman.state_post[5,0]=a.y
79
80 filteredPositions = moving.Trajectory()
81 filteredVelocities = moving.Trajectory()
82 measurement = cv.CreateMat(4,1,cv.CV_32FC1)
83 for i in xrange(positions.length()):
84 cv.KalmanPredict(kalman) # no control
85 p = positions[i]
86 v = velocities[i]
87 measurement[0,0] = p.x
88 measurement[1,0] = p.y
89 measurement[2,0] = v.x
90 measurement[3,0] = v.y
91 cv.KalmanCorrect(kalman, measurement)
92 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0])
93 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0])
94
95 return (filteredPositions, filteredVelocities)
96
97 (filteredPositions, filteredVelocities) = kalmanFilter(newTraj, obj.getVelocities(), 1e-5, 1e-1)
98
99 plt.clf()
100 obj.draw('rx')
101 for fnum in obj.featureNumbers: features[fnum].draw()
102 newTraj.draw('bx')
103 plt.axis('equal')