Mercurial Hosting > traffic-intelligence
changeset 109:04a874e1f19f
added median trajectory computation, other tests
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 15 Jul 2011 03:05:00 -0400 |
parents | 6efe470ea5e5 |
children | fae55a4c7a5a |
files | python/compute-object-from-features.py |
diffstat | 1 files changed, 37 insertions(+), 3 deletions(-) [+] |
line wrap: on
line diff
--- 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')