Mercurial Hosting > traffic-intelligence
comparison python/compute-object-from-features.py @ 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 | ce4cb46b3603 |
children |
comparison
equal
deleted
inserted
replaced
108:6efe470ea5e5 | 109:04a874e1f19f |
---|---|
50 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first)) | 50 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first)) |
51 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant())) | 51 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant())) |
52 | 52 |
53 return newTraj | 53 return newTraj |
54 | 54 |
55 def computeMedianTrajectory(features, timeInterval = None): | |
56 if not timeInterval: | |
57 raise Exception('not implemented') # compute from the features | |
58 | |
59 newTraj = moving.Trajectory() | |
60 for t in timeInterval: | |
61 points = [] | |
62 for f in features: | |
63 if f.existsAtInstant(t): | |
64 points.append(f.getPositionAtInstant(t).aslist()) | |
65 med = np.median(np.array(points), 0) | |
66 newTraj.addPositionXY(med[0], med[1]) | |
67 | |
68 return newTraj | |
69 | |
55 # TODO version median: conversion to large matrix will not work, have to do it frame by frame | 70 # TODO version median: conversion to large matrix will not work, have to do it frame by frame |
56 | 71 |
57 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov): | 72 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov): |
58 kalman=cv.CreateKalman(6, 4) | 73 kalman=cv.CreateKalman(6, 4) |
59 kalman.transition_matrix[0,2]=1 | 74 kalman.transition_matrix[0,2]=1 |
94 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0]) | 109 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0]) |
95 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0]) | 110 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0]) |
96 | 111 |
97 return (filteredPositions, filteredVelocities) | 112 return (filteredPositions, filteredVelocities) |
98 | 113 |
99 lowTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval()) | 114 groundTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval()) |
100 (filteredPositions, filteredVelocities) = kalmanFilter(lowTrajectory, obj.getVelocities(), 0.1, 0.1) | 115 (filteredPositions, filteredVelocities) = kalmanFilter(groundTrajectory, obj.getVelocities(), 0.1, 0.1) |
116 | |
117 #medianTrajectory = computeMedianTrajectory([features[i] for i in obj.featureNumbers], obj.getTimeInterval()) | |
118 | |
119 delta = [] | |
120 for t in obj.getTimeInterval(): | |
121 p1 = obj.getPositionAtInstant(t) | |
122 p2 = groundTrajectory[t-obj.getFirstInstant()] | |
123 delta.append((p1-p2).aslist()) | |
124 | |
125 delta = np.median(delta, 0) | |
126 | |
127 translated = moving.Trajectory() | |
128 for t in obj.getTimeInterval(): | |
129 p1 = obj.getPositionAtInstant(t) | |
130 p1.x -= delta[0] | |
131 p1.y -= delta[1] | |
132 translated.addPosition(p1) | |
101 | 133 |
102 plt.clf() | 134 plt.clf() |
103 obj.draw('rx-') | 135 obj.draw('rx-') |
104 for fnum in obj.featureNumbers: features[fnum].draw() | 136 for fnum in obj.featureNumbers: features[fnum].draw() |
105 lowTrajectory.draw('bx-') | 137 groundTrajectory.draw('bx-') |
106 filteredPositions.draw('gx-') | 138 filteredPositions.draw('gx-') |
139 translated.draw('kx-') | |
140 #medianTrajectory.draw('kx-') | |
107 plt.axis('equal') | 141 plt.axis('equal') |