Mercurial > hg > nsaunier > traffic-intelligence

comparison python/compute-object-from-features.py @ 109:04a874e1f19f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added median trajectory computation, other tests
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 15 Jul 2011 03:05:00 -0400
parents ce4cb46b3603
children
comparison
equal deleted inserted replaced
108:6efe470ea5e5 109:04a874e1f19f
50 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first)) 50 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first))
51 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant())) 51 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant()))
52 52
53 return newTraj 53 return newTraj
54 54
55 def computeMedianTrajectory(features, timeInterval = None):
56 if not timeInterval:
57 raise Exception('not implemented') # compute from the features
58
59 newTraj = moving.Trajectory()
60 for t in timeInterval:
61 points = []
62 for f in features:
63 if f.existsAtInstant(t):
64 points.append(f.getPositionAtInstant(t).aslist())
65 med = np.median(np.array(points), 0)
66 newTraj.addPositionXY(med[0], med[1])
67
68 return newTraj
69
55 # TODO version median: conversion to large matrix will not work, have to do it frame by frame 70 # TODO version median: conversion to large matrix will not work, have to do it frame by frame
56 71
57 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov): 72 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
58 kalman=cv.CreateKalman(6, 4) 73 kalman=cv.CreateKalman(6, 4)
59 kalman.transition_matrix[0,2]=1 74 kalman.transition_matrix[0,2]=1
94 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0]) 109 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0])
95 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0]) 110 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0])
96 111
97 return (filteredPositions, filteredVelocities) 112 return (filteredPositions, filteredVelocities)
98 113
99 lowTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval()) 114 groundTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval())
100 (filteredPositions, filteredVelocities) = kalmanFilter(lowTrajectory, obj.getVelocities(), 0.1, 0.1) 115 (filteredPositions, filteredVelocities) = kalmanFilter(groundTrajectory, obj.getVelocities(), 0.1, 0.1)
116
117 #medianTrajectory = computeMedianTrajectory([features[i] for i in obj.featureNumbers], obj.getTimeInterval())
118
119 delta = []
120 for t in obj.getTimeInterval():
121 p1 = obj.getPositionAtInstant(t)
122 p2 = groundTrajectory[t-obj.getFirstInstant()]
123 delta.append((p1-p2).aslist())
124
125 delta = np.median(delta, 0)
126
127 translated = moving.Trajectory()
128 for t in obj.getTimeInterval():
129 p1 = obj.getPositionAtInstant(t)
130 p1.x -= delta[0]
131 p1.y -= delta[1]
132 translated.addPosition(p1)
101 133
102 plt.clf() 134 plt.clf()
103 obj.draw('rx-') 135 obj.draw('rx-')
104 for fnum in obj.featureNumbers: features[fnum].draw() 136 for fnum in obj.featureNumbers: features[fnum].draw()
105 lowTrajectory.draw('bx-') 137 groundTrajectory.draw('bx-')
106 filteredPositions.draw('gx-') 138 filteredPositions.draw('gx-')
139 translated.draw('kx-')
140 #medianTrajectory.draw('kx-')
107 plt.axis('equal') 141 plt.axis('equal')