Mercurial > hg > nsaunier > traffic-intelligence

comparison python/compute-object-from-features.py @ 106:ce4cb46b3603

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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')