Mercurial Hosting > traffic-intelligence
changeset 105:9844c69d8fa2
added multiply method to Point
author | Nicolas Saunier <nico@confins.net> |
---|---|
date | Thu, 14 Jul 2011 19:48:30 -0400 |
parents | 13187af8622d |
children | ce4cb46b3603 |
files | python/compute-object-from-features.py python/moving.py |
diffstat | 2 files changed, 70 insertions(+), 12 deletions(-) [+] |
line wrap: on
line diff
--- a/python/compute-object-from-features.py Thu Jul 14 17:15:09 2011 -0400 +++ b/python/compute-object-from-features.py Thu Jul 14 19:48:30 2011 -0400 @@ -32,17 +32,72 @@ invh = cvutils.invertHomography(h) -yCoordinates = -np.ones((len(obj.featureNumbers),int(obj.length()))) -for i,fnum in enumerate(obj.featureNumbers): - traj = features[fnum].getPositions().asArray() - imgTraj = cvutils.projectArray(invh, traj) - yCoordinates[i,features[fnum].getFirstInstant()-obj.getFirstInstant():features[fnum].getLastInstant()+1-obj.getFirstInstant()] = imgTraj[1,:] +def computeObject(features, homography, timeInterval = None): + if not timeInterval: + raise Exception('not implemented') # compute from the features + + yCoordinates = -np.ones((len(features),int(timeInterval.length()))) + for i,f in enumerate(features): + traj = f.getPositions().asArray() + imgTraj = cvutils.projectArray(homography, traj) + yCoordinates[i,f.getFirstInstant()-timeInterval.first:f.getLastInstant()+1-timeInterval.first] = imgTraj[1,:] + + indices = np.argmax(yCoordinates,0) + newTraj = moving.Trajectory() + for j,idx in enumerate(indices): + newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first)) + #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant())) + + return newTraj + +# TODO version median + +def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov): + kalman=cv.CreateKalman(6, 4) + kalman.transition_matrix[0,2]=1 + kalman.transition_matrix[0,4]=1./2 + kalman.transition_matrix[1,3]=1 + kalman.transition_matrix[1,5]=1./2 + kalman.transition_matrix[2,4]=1 + kalman.transition_matrix[3,5]=1 + + cv.SetIdentity(kalman.measurement_matrix, 1.) + cv.SetIdentity(kalman.process_noise_cov, processNoiseCov) + cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov) + cv.SetIdentity(kalman.error_cov_post, 1.) -indices = argmax(yCoordinates,0) -newTraj = moving.Trajectory() -for j,idx in enumerate(indices): - newTraj.addPosition(features[obj.featureNumbers[idx]].getPositionAtInstant(j+obj.getFirstInstant())) + p = positions[0] + v = velocities[0] + v2 = velocities[2] + a = (v2-v).multiply(0.5) + kalman.state_post[0,0]=p.x + kalman.state_post[1,0]=p.y + kalman.state_post[2,0]=v.x + kalman.state_post[3,0]=v.y + kalman.state_post[4,0]=a.x + kalman.state_post[5,0]=a.y + + filteredPositions = moving.Trajectory() + filteredVelocities = moving.Trajectory() + measurement = cv.CreateMat(4,1,cv.CV_32FC1) + for i in xrange(positions.length()): + cv.KalmanPredict(kalman) # no control + p = positions[i] + v = velocities[i] + measurement[0,0] = p.x + measurement[1,0] = p.y + measurement[2,0] = v.x + measurement[3,0] = v.y + cv.KalmanCorrect(kalman, measurement) + filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0]) + filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0]) -# TODO -# use kalman filter over the resulting trajectory -# estimate the error terms using actual features + return (filteredPositions, filteredVelocities) + +(filteredPositions, filteredVelocities) = kalmanFilter(newTraj, obj.getVelocities(), 1e-5, 1e-1) + +plt.clf() +obj.draw('rx') +for fnum in obj.featureNumbers: features[fnum].draw() +newTraj.draw('bx') +plt.axis('equal')
--- a/python/moving.py Thu Jul 14 17:15:09 2011 -0400 +++ b/python/moving.py Thu Jul 14 19:48:30 2011 -0400 @@ -131,6 +131,9 @@ def __sub__(self, other): return Point(self.x-other.x, self.y-other.y) + def multiply(self, alpha): + return Point(self.x*alpha, self.y*alpha) + def draw(self, options = ''): from matplotlib.pylab import plot plot([self.x], [self.y], 'x'+options)