--- 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)