Mercurial Hosting > traffic-intelligence
annotate scripts/test-compute-object-position-from-features.py @ 1159:e1e7acef8eab
moved trajectory management library into Traffic Intelligence
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 22 Feb 2021 22:09:35 -0500 |
parents | 933670761a57 |
children |
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') |