Mercurial Hosting > traffic-intelligence
comparison python/compute-object-from-features.py @ 106:ce4cb46b3603
added kalman filtering and rearranged functions
author | Nicolas Saunier <nico@confins.net> |
---|---|
date | Thu, 14 Jul 2011 20:05:01 -0400 |
parents | 9844c69d8fa2 |
children | 04a874e1f19f |
comparison
equal
deleted
inserted
replaced
105:9844c69d8fa2 | 106:ce4cb46b3603 |
---|---|
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 def computeObject(features, homography, timeInterval = None): | 35 def computeGroundTrajectory(features, homography, timeInterval = None): |
36 '''Computes a trajectory for the set of features as the closes point to the ground | |
37 using the homography in image space''' | |
36 if not timeInterval: | 38 if not timeInterval: |
37 raise Exception('not implemented') # compute from the features | 39 raise Exception('not implemented') # compute from the features |
38 | 40 |
39 yCoordinates = -np.ones((len(features),int(timeInterval.length()))) | 41 yCoordinates = -np.ones((len(features),int(timeInterval.length()))) |
40 for i,f in enumerate(features): | 42 for i,f in enumerate(features): |
48 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first)) | 50 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first)) |
49 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant())) | 51 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant())) |
50 | 52 |
51 return newTraj | 53 return newTraj |
52 | 54 |
53 # TODO version median | 55 # TODO version median: conversion to large matrix will not work, have to do it frame by frame |
54 | 56 |
55 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov): | 57 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov): |
56 kalman=cv.CreateKalman(6, 4) | 58 kalman=cv.CreateKalman(6, 4) |
57 kalman.transition_matrix[0,2]=1 | 59 kalman.transition_matrix[0,2]=1 |
58 kalman.transition_matrix[0,4]=1./2 | 60 kalman.transition_matrix[0,4]=1./2 |
92 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0]) | 94 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]) | 95 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0]) |
94 | 96 |
95 return (filteredPositions, filteredVelocities) | 97 return (filteredPositions, filteredVelocities) |
96 | 98 |
97 (filteredPositions, filteredVelocities) = kalmanFilter(newTraj, obj.getVelocities(), 1e-5, 1e-1) | 99 lowTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval()) |
100 (filteredPositions, filteredVelocities) = kalmanFilter(lowTrajectory, obj.getVelocities(), 0.1, 0.1) | |
98 | 101 |
99 plt.clf() | 102 plt.clf() |
100 obj.draw('rx') | 103 obj.draw('rx-') |
101 for fnum in obj.featureNumbers: features[fnum].draw() | 104 for fnum in obj.featureNumbers: features[fnum].draw() |
102 newTraj.draw('bx') | 105 lowTrajectory.draw('bx-') |
106 filteredPositions.draw('gx-') | |
103 plt.axis('equal') | 107 plt.axis('equal') |