Mercurial Hosting > traffic-intelligence

annotate scripts/test-compute-object-position-from-features.py @ 1228:5654c9173548

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merged (bicycle)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 12 Jul 2023 13:21:08 -0400
parents 933670761a57
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 936
diff changeset
1 #!/usr/bin/env python3
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
2
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
3 import sys
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
4
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
5 import matplotlib.mlab as pylab
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
6 import matplotlib.pyplot as plt
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
7 import numpy as np
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
8
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
9 import cv
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
10 import utils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
11 import cvutils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
12 import ubc_utils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
13 import moving
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
14
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
15 # use something like getopt to manage arguments if necessary
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
16
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
17 if len(sys.argv) < 3:
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
18 print('Usage: {0} <video-filename> <n-objects>'.format(sys.argv[0]))
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
19 sys.exit()
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
20
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
21 if sys.argv[1].endswith('.avi'):
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
22 videoFilenamePrefix = utils.removeExtension(sys.argv[1],'.')
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
23 else:
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
24 videoFilenamePrefix = sys.argv[1]
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
25
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
26 objectNum = int(sys.argv[2])
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
27
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
28 objects = ubc_utils.loadTrajectories(videoFilenamePrefix+'-objects.txt', objectNum+1)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
29 obj = objects[objectNum]
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
30 features = ubc_utils.loadTrajectories(videoFilenamePrefix+'-features.txt', max(obj.featureNumbers)+1)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
31 h = np.loadtxt(videoFilenamePrefix+'-homography.txt')
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
32
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
33 invh = cvutils.invertHomography(h)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
34
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
35 def computeGroundTrajectory(features, homography, timeInterval = None):
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
36 '''Computes a trajectory for the set of features as the closes point to the ground
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
37 using the homography in image space'''
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
38 if not timeInterval:
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
39 raise Exception('not implemented') # compute from the features
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
40
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
41 yCoordinates = -np.ones((len(features),int(timeInterval.length())))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
42 for i,f in enumerate(features):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
43 traj = f.getPositions().asArray()
936
56cc8a1f7082 removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 334
diff changeset
44 imgTraj = cvutils.homographyProject(traj, homography)
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
45 yCoordinates[i,f.getFirstInstant()-timeInterval.first:f.getLastInstant()+1-timeInterval.first] = imgTraj[1,:]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
46
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
47 indices = np.argmax(yCoordinates,0)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
48 newTraj = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
49 for j,idx in enumerate(indices):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
50 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
51 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant()))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
52
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
53 return newTraj
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
54
109
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
55 def computeMedianTrajectory(features, timeInterval = None):
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
56 if not timeInterval:
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
57 raise Exception('not implemented') # compute from the features
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
58
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
59 newTraj = moving.Trajectory()
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
60 for t in timeInterval:
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
61 points = []
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
62 for f in features:
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
63 if f.existsAtInstant(t):
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
64 points.append(f.getPositionAtInstant(t).aslist())
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
65 med = np.median(np.array(points), 0)
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
66 newTraj.addPositionXY(med[0], med[1])
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
67
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
68 return newTraj
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
69
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
70 # TODO version median: conversion to large matrix will not work, have to do it frame by frame
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
71
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
72 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 936
diff changeset
73 kalman=cv2.CreateKalman(6, 4)
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
74 kalman.transition_matrix[0,2]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
75 kalman.transition_matrix[0,4]=1./2
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
76 kalman.transition_matrix[1,3]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
77 kalman.transition_matrix[1,5]=1./2
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
78 kalman.transition_matrix[2,4]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
79 kalman.transition_matrix[3,5]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
80
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
81 cv.SetIdentity(kalman.measurement_matrix, 1.)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
82 cv.SetIdentity(kalman.process_noise_cov, processNoiseCov)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
83 cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
84 cv.SetIdentity(kalman.error_cov_post, 1.)
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
85
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
86 p = positions[0]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
87 v = velocities[0]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
88 v2 = velocities[2]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
89 a = (v2-v).multiply(0.5)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
90 kalman.state_post[0,0]=p.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
91 kalman.state_post[1,0]=p.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
92 kalman.state_post[2,0]=v.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
93 kalman.state_post[3,0]=v.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
94 kalman.state_post[4,0]=a.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
95 kalman.state_post[5,0]=a.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
96
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
97 filteredPositions = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
98 filteredVelocities = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
99 measurement = cv.CreateMat(4,1,cv.CV_32FC1)
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 936
diff changeset
100 for i in range(positions.length()):
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 936
diff changeset
101 kalman.predict() # no control
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
102 p = positions[i]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
103 v = velocities[i]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
104 measurement[0,0] = p.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
105 measurement[1,0] = p.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
106 measurement[2,0] = v.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
107 measurement[3,0] = v.y
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 936
diff changeset
108 kalman.correct(measurement)
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
109 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0])
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
110 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0])
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
111
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
112 return (filteredPositions, filteredVelocities)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
113
109
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
114 groundTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval())
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
115 (filteredPositions, filteredVelocities) = kalmanFilter(groundTrajectory, obj.getVelocities(), 0.1, 0.1)
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
116
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
117 #medianTrajectory = computeMedianTrajectory([features[i] for i in obj.featureNumbers], obj.getTimeInterval())
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
118
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
119 delta = []
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
120 for t in obj.getTimeInterval():
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
121 p1 = obj.getPositionAtInstant(t)
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
122 p2 = groundTrajectory[t-obj.getFirstInstant()]
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
123 delta.append((p1-p2).aslist())
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
124
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
125 delta = np.median(delta, 0)
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
126
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
127 translated = moving.Trajectory()
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
128 for t in obj.getTimeInterval():
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
129 p1 = obj.getPositionAtInstant(t)
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
130 p1.x -= delta[0]
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
131 p1.y -= delta[1]
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
132 translated.addPosition(p1)
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
133
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
134 plt.clf()
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
135 obj.draw('rx-')
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
136 for fnum in obj.featureNumbers: features[fnum].draw()
109
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
137 groundTrajectory.draw('bx-')
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
138 filteredPositions.draw('gx-')
109
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
139 translated.draw('kx-')
04a874e1f19f added median trajectory computation, other tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 106
diff changeset
140 #medianTrajectory.draw('kx-')
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
141 plt.axis('equal')