# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1310713500 14400
# Node ID 04a874e1f19f1d8d2b2c31b80538b1526cdb0b3b
# Parent  6efe470ea5e50af75a92ffeae07a55c89ae8d010
added median trajectory computation, other tests

diff -r 6efe470ea5e5 -r 04a874e1f19f python/compute-object-from-features.py
--- a/python/compute-object-from-features.py	Fri Jul 15 03:04:39 2011 -0400
+++ b/python/compute-object-from-features.py	Fri Jul 15 03:05:00 2011 -0400
@@ -52,6 +52,21 @@
 
     return newTraj
 
+def computeMedianTrajectory(features, timeInterval = None):
+    if not timeInterval:
+        raise Exception('not implemented') # compute from the features
+    
+    newTraj = moving.Trajectory()
+    for t in timeInterval:
+        points = []
+        for f in features:
+            if f.existsAtInstant(t):
+                points.append(f.getPositionAtInstant(t).aslist())
+        med = np.median(np.array(points), 0)
+        newTraj.addPositionXY(med[0], med[1])
+
+    return newTraj
+
 # TODO version median: conversion to large matrix will not work, have to do it frame by frame
 
 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
@@ -96,12 +111,31 @@
 
     return (filteredPositions, filteredVelocities)
 
-lowTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval())
-(filteredPositions, filteredVelocities) = kalmanFilter(lowTrajectory, obj.getVelocities(), 0.1, 0.1)
+groundTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval())
+(filteredPositions, filteredVelocities) = kalmanFilter(groundTrajectory, obj.getVelocities(), 0.1, 0.1)
+
+#medianTrajectory = computeMedianTrajectory([features[i] for i in obj.featureNumbers], obj.getTimeInterval())
+
+delta = []
+for t in obj.getTimeInterval():
+    p1 = obj.getPositionAtInstant(t)
+    p2 = groundTrajectory[t-obj.getFirstInstant()]
+    delta.append((p1-p2).aslist())
+
+delta = np.median(delta, 0)
+
+translated = moving.Trajectory()
+for t in obj.getTimeInterval():
+    p1 = obj.getPositionAtInstant(t)
+    p1.x -= delta[0]
+    p1.y -= delta[1]
+    translated.addPosition(p1)
 
 plt.clf()
 obj.draw('rx-')
 for fnum in obj.featureNumbers: features[fnum].draw()
-lowTrajectory.draw('bx-')
+groundTrajectory.draw('bx-')
 filteredPositions.draw('gx-')
+translated.draw('kx-')
+#medianTrajectory.draw('kx-')
 plt.axis('equal')