Mercurial Hosting > traffic-intelligence
annotate python/moving.py @ 992:2cd1ce245024
update to python 3
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 13 Apr 2018 16:48:02 -0400 |
parents | f026ce2af637 |
children | e8eabef7857c |
rev | line source |
---|---|
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
1 #! /usr/bin/env python |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
2 '''Libraries for moving objects, trajectories...''' |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
3 |
664
455f9b93819c
added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
663
diff
changeset
|
4 import utils, cvutils |
665
15e244d2a1b5
corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
664
diff
changeset
|
5 from base import VideoFilenameAddable |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
6 |
672
5473b7460375
moved and rationalized imports in modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
666
diff
changeset
|
7 from math import sqrt, atan2, cos, sin |
949
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
8 from numpy import median, array, arange, zeros, ones, hypot, NaN, std, floor, float32, argwhere, minimum |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
9 from matplotlib.pyplot import plot, text |
672
5473b7460375
moved and rationalized imports in modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
666
diff
changeset
|
10 from scipy.stats import scoreatpercentile |
5473b7460375
moved and rationalized imports in modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
666
diff
changeset
|
11 from scipy.spatial.distance import cdist |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
12 from scipy.signal import savgol_filter |
16 | 13 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
14 try: |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
15 from shapely.geometry import Polygon, Point as shapelyPoint |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
16 from shapely.prepared import prep, PreparedGeometry |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
17 shapelyAvailable = True |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
18 except ImportError: |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
19 print('Shapely library could not be loaded') |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
20 shapelyAvailable = False |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
21 |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
22 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
23 class Interval(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
24 '''Generic interval: a subset of real numbers (not iterable)''' |
26 | 25 def __init__(self, first=0, last=-1, revert = False): |
26 if revert and last<first: | |
27 self.first=last | |
28 self.last=first | |
29 else: | |
30 self.first=first | |
31 self.last=last | |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
32 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
33 def __str__(self): |
104
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
34 return '[{0}, {1}]'.format(self.first, self.last) |
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
35 |
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
36 def __repr__(self): |
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
37 return self.__str__() |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
38 |
776
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
39 def __eq__(self, other): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
40 return ((self.first == other.first) and (self.last == other.last)) or ((self.first == other.last) and (self.last == other.first)) |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
41 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
42 def empty(self): |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
43 return self.first > self.last |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
44 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
45 def center(self): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
46 return (self.first+self.last)/2. |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
47 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
48 def length(self): |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
49 '''Returns the length of the interval''' |
91
daa05fae1a70
modified the type of the result of interval lengths to float, added comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
90
diff
changeset
|
50 return float(max(0,self.last-self.first)) |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
51 |
96
9928c2fa72cc
added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
93
diff
changeset
|
52 def equal(self, i2): |
9928c2fa72cc
added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
93
diff
changeset
|
53 return self.first==i2.first and self.last == i2.last |
9928c2fa72cc
added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
93
diff
changeset
|
54 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
55 def getList(self): |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
56 return [self.first, self.last] |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
57 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
58 def contains(self, instant): |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
59 return (self.first<=instant and self.last>=instant) |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
60 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
61 def inside(self, interval2): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
62 '''Indicates if the temporal interval of self is comprised in interval2''' |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
63 return (self.first >= interval2.first) and (self.last <= interval2.last) |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
64 |
785
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
65 def shift(self, offset): |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
66 self.first += offset |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
67 self.last += offset |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
68 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
69 @classmethod |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
70 def union(cls, interval1, interval2): |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
71 '''Smallest interval comprising self and interval2''' |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
72 return cls(min(interval1.first, interval2.first), max(interval2.last, interval2.last)) |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
73 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
74 @classmethod |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
75 def intersection(cls, interval1, interval2): |
104
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
76 '''Largest interval comprised in both self and interval2''' |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
77 return cls(max(interval1.first, interval2.first), min(interval1.last, interval2.last)) |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
78 |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
79 def distance(self, interval2): |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
80 if not Interval.intersection(self, interval2).empty(): |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
81 return 0 |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
82 elif self.first > interval2.last: |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
83 return self.first - interval2.last |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
84 elif self.last < interval2.first: |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
85 return interval2.first - self.last |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
86 else: |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
87 return None |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
88 |
727
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
89 @classmethod |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
90 def unionIntervals(cls, intervals): |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
91 'returns the smallest interval containing all intervals' |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
92 inter = cls(intervals[0].first, intervals[0].last) |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
93 for i in intervals[1:]: |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
94 inter = cls.union(inter, i) |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
95 return inter |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
96 |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
97 |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
98 class TimeInterval(Interval): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
99 '''Temporal interval: set of instants at fixed time step, between first and last, included |
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
100 |
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
101 For example: based on frame numbers (hence the modified length method) |
828
14e4ad7c7420
work on merging data for synchronized views
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
812
diff
changeset
|
102 It may be modified directly by setting first and last |
14e4ad7c7420
work on merging data for synchronized views
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
812
diff
changeset
|
103 It also (mostly) works with datetime.datetime''' |
26 | 104 |
828
14e4ad7c7420
work on merging data for synchronized views
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
812
diff
changeset
|
105 def __init__(self, first=0, last=-1, revert = False): |
14e4ad7c7420
work on merging data for synchronized views
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
812
diff
changeset
|
106 super(TimeInterval, self).__init__(first, last, revert) |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
107 |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
108 @staticmethod |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
109 def fromInterval(inter): |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
110 return TimeInterval(inter.first, inter.last) |
26 | 111 |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
112 def __getitem__(self, i): |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
113 if not self.empty(): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
114 if isinstance(i, int): |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
115 return self.first+i |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
116 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
117 raise TypeError("Invalid argument type.") |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
118 #elif isinstance( key, slice ): |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
119 |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
120 def __iter__(self): |
107
916678481896
corrected bug for TimeInterval interation and added corresponding test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
105
diff
changeset
|
121 self.iterInstantNum = -1 |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
122 return self |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
123 |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
124 def next(self): |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
125 if self.iterInstantNum >= self.length()-1: |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
126 raise StopIteration |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
127 else: |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
128 self.iterInstantNum += 1 |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
129 return self[self.iterInstantNum] |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
130 |
26 | 131 def length(self): |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
132 '''Returns the length of the interval''' |
91
daa05fae1a70
modified the type of the result of interval lengths to float, added comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
90
diff
changeset
|
133 return float(max(0,self.last-self.first+1)) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
134 |
688
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
135 def __len__(self): |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
136 return self.length() |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
137 |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
138 # class BoundingPolygon: |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
139 # '''Class for a polygon bounding a set of points |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
140 # with methods to create intersection, unions... |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
141 # ''' |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
142 # We will use the polygon class of Shapely |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
143 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
144 class STObject(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
145 '''Class for spatio-temporal object, i.e. with temporal and spatial existence |
6 | 146 (time interval and bounding polygon for positions (e.g. rectangle)). |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
147 |
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
148 It may not mean that the object is defined |
6 | 149 for all time instants within the time interval''' |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
150 |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
151 def __init__(self, num = None, timeInterval = None, boundingPolygon = None): |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
152 self.num = num |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
153 self.timeInterval = timeInterval |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
154 self.boundingPolygon = boundingPolygon |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
155 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
156 def empty(self): |
688
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
157 return self.timeInterval.empty()# or not self.boudingPolygon |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
158 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
159 def getNum(self): |
211 | 160 return self.num |
161 | |
688
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
162 def __len__(self): |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
163 return self.timeInterval.length() |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
164 |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
165 def length(self): |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
166 return self.timeInterval.length() |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
167 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
168 def getFirstInstant(self): |
40
9f16aee24b7e
corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents:
39
diff
changeset
|
169 return self.timeInterval.first |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
170 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
171 def getLastInstant(self): |
40
9f16aee24b7e
corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents:
39
diff
changeset
|
172 return self.timeInterval.last |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
173 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
174 def getTimeInterval(self): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
175 return self.timeInterval |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
176 |
108
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
177 def existsAtInstant(self, t): |
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
178 return self.timeInterval.contains(t) |
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
179 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
180 def commonTimeInterval(self, obj2): |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
181 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval()) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
182 |
785
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
183 def shiftTimeInterval(self, offset): |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
184 self.timeInterval.shift(offset) |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
185 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
186 class Point(object): |
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
187 def __init__(self, x, y): |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
188 self.x = x |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
189 self.y = y |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
190 |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
191 def __str__(self): |
675
ab3fdff42624
corrected old format for Point.__str__
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
674
diff
changeset
|
192 return '({:f},{:f})'.format(self.x,self.y) |
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
193 |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
194 def __repr__(self): |
98
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
195 return self.__str__() |
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
196 |
776
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
197 def __eq__(self, other): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
198 return (self.x == other.x) and (self.y == other.y) |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
199 |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
200 def __add__(self, other): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
201 return Point(self.x+other.x, self.y+other.y) |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
202 |
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
203 def __sub__(self, other): |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
204 return Point(self.x-other.x, self.y-other.y) |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
205 |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
206 def __neg__(self): |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
207 return Point(-self.x, -self.y) |
575
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
208 |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
209 def __getitem__(self, i): |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
210 if i == 0: |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
211 return self.x |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
212 elif i == 1: |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
213 return self.y |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
214 else: |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
215 raise IndexError() |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
216 |
690 | 217 def orthogonal(self, clockwise = True): |
218 'Returns the orthogonal vector' | |
219 if clockwise: | |
220 return Point(self.y, -self.x) | |
221 else: | |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
222 return Point(-self.y, self.x) |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
223 |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
224 def projectLocal(self, v, clockwise = True): |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
225 'Projects point projected on v, v.orthogonal()' |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
226 e1 = v/v.norm2() |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
227 e2 = e1.orthogonal(clockwise) |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
228 return Point(Point.dot(self, e1), Point.doc(self, e2)) |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
229 |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
230 def rotate(self, theta): |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
231 return Point(self.x*cos(theta)-self.y*sin(theta), self.x*sin(theta)+self.y*cos(theta)) |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
232 |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
233 def __mul__(self, alpha): |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
234 'Warning, returns a new Point' |
105
9844c69d8fa2
added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents:
104
diff
changeset
|
235 return Point(self.x*alpha, self.y*alpha) |
9844c69d8fa2
added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents:
104
diff
changeset
|
236 |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
237 def divide(self, alpha): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
238 'Warning, returns a new Point' |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
239 return Point(self.x/alpha, self.y/alpha) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
240 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
241 def plot(self, options = 'o', **kwargs): |
211 | 242 plot([self.x], [self.y], options, **kwargs) |
41 | 243 |
771 | 244 @staticmethod |
245 def plotSegment(p1, p2, options = 'o', **kwargs): | |
246 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs) | |
247 | |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
248 def angle(self): |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
249 return atan2(self.y, self.x) |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
250 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
251 def norm2Squared(self): |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
252 '''2-norm distance (Euclidean distance)''' |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
253 return self.x**2+self.y**2 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
254 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
255 def norm2(self): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
256 '''2-norm distance (Euclidean distance)''' |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
257 return sqrt(self.norm2Squared()) |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
258 |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
259 def norm1(self): |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
260 return abs(self.x)+abs(self.y) |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
261 |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
262 def normMax(self): |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
263 return max(abs(self.x),abs(self.y)) |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
264 |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
265 def aslist(self): |
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
266 return [self.x, self.y] |
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
267 |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
268 def astuple(self): |
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
269 return (self.x, self.y) |
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
270 |
223
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
271 def asint(self): |
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
272 return Point(int(self.x), int(self.y)) |
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
273 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
274 if shapelyAvailable: |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
275 def asShapely(self): |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
276 return shapelyPoint(self.x, self.y) |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
277 |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
278 def homographyProject(self, homography): |
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
279 projected = cvutils.homographyProject(array([[self.x], [self.y]]), homography) |
98
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
280 return Point(projected[0], projected[1]) |
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
281 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
282 def inPolygon(self, polygon): |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
283 '''Indicates if the point x, y is inside the polygon |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
284 (array of Nx2 coordinates of the polygon vertices) |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
285 |
92
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
286 taken from http://www.ariel.com.au/a/python-point-int-poly.html |
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
287 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
288 Use Polygon.contains if Shapely is installed''' |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
289 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
290 n = polygon.shape[0]; |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
291 counter = 0; |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
292 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
293 p1 = polygon[0,:]; |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
294 for i in range(n+1): |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
295 p2 = polygon[i % n,:]; |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
296 if self.y > min(p1[1],p2[1]): |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
297 if self.y <= max(p1[1],p2[1]): |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
298 if self.x <= max(p1[0],p2[0]): |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
299 if p1[1] != p2[1]: |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
300 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0]; |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
301 if p1[0] == p2[0] or self.x <= xinters: |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
302 counter+=1; |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
303 p1=p2 |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
304 return (counter%2 == 1); |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
305 |
49 | 306 @staticmethod |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
307 def fromList(p): |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
308 return Point(p[0], p[1]) |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
309 |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
310 @staticmethod |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
311 def dot(p1, p2): |
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
312 'Scalar product' |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
313 return p1.x*p2.x+p1.y*p2.y |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
314 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
315 @staticmethod |
90
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
316 def cross(p1, p2): |
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
317 'Cross product' |
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
318 return p1.x*p2.y-p1.y*p2.x |
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
319 |
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
320 @staticmethod |
987
f026ce2af637
found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
982
diff
changeset
|
321 def parallel(p1, p2): |
f026ce2af637
found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
982
diff
changeset
|
322 return Point.cross(p1, p2) == 0. |
f026ce2af637
found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
982
diff
changeset
|
323 |
f026ce2af637
found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
982
diff
changeset
|
324 @staticmethod |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
325 def cosine(p1, p2): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
326 return Point.dot(p1,p2)/(p1.norm2()*p2.norm2()) |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
327 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
328 @staticmethod |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
329 def distanceNorm2(p1, p2): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
330 return (p1-p2).norm2() |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
331 |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
332 @staticmethod |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
345
diff
changeset
|
333 def plotAll(points, **kwargs): |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
334 from matplotlib.pyplot import scatter |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
345
diff
changeset
|
335 scatter([p.x for p in points],[p.y for p in points], **kwargs) |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
336 |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
337 def similarOrientation(self, refDirection, cosineThreshold): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
338 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold' |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
339 return Point.cosine(self, refDirection) >= cosineThreshold |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
340 |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
341 @staticmethod |
484
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
342 def timeToCollision(p1, p2, v1, v2, collisionThreshold): |
504
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
343 '''Computes exact time to collision with a distance threshold |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
344 The unknown of the equation is the time to reach the intersection |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
345 between the relative trajectory of one road user |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
346 and the circle of radius collisionThreshold around the other road user''' |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
347 dv = v1-v2 |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
348 dp = p1-p2 |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
349 a = dv.norm2Squared()#(v1.x-v2.x)**2 + (v1.y-v2.y)**2 |
516
bce1fe45d1b2
corrected bugs detected by tests (because of moving functions around modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
350 b = 2*Point.dot(dv, dp)#2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) |
504
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
351 c = dp.norm2Squared() - collisionThreshold**2#(p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
352 |
504
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
353 delta = b**2 - 4*a*c |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
354 if delta >= 0: |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
355 deltaRoot = sqrt(delta) |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
356 ttc1 = (-b + deltaRoot)/(2*a) |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
357 ttc2 = (-b - deltaRoot)/(2*a) |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
358 if ttc1 >= 0 and ttc2 >= 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
359 return min(ttc1,ttc2) |
531
f012a8ad7a0e
corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
527
diff
changeset
|
360 elif ttc1 >= 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
361 return ttc1 |
531
f012a8ad7a0e
corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
527
diff
changeset
|
362 elif ttc2 >= 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
363 return ttc2 |
531
f012a8ad7a0e
corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
527
diff
changeset
|
364 else: # ttc1 < 0 and ttc2 < 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
365 return None |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
366 else: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
367 return None |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
368 |
583
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
369 @staticmethod |
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
370 def midPoint(p1, p2): |
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
371 'Returns the middle of the segment [p1, p2]' |
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
372 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
373 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
374 if shapelyAvailable: |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
375 def pointsInPolygon(points, polygon): |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
376 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)''' |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
377 if type(polygon) == PreparedGeometry: |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
378 prepared_polygon = polygon |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
379 else: |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
380 prepared_polygon = prep(polygon) |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
381 return filter(prepared_polygon.contains, points) |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
382 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
383 # Functions for coordinate transformation |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
384 # From Paul St-Aubin's PVA tools |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
385 def subsec_spline_dist(splines): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
386 ''' Prepare list of spline subsegments from a spline list. |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
387 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
388 Output: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
389 ======= |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
390 ss_spline_d[spline #][mode][station] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
391 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
392 where: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
393 mode=0: incremental distance |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
394 mode=1: cumulative distance |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
395 mode=2: cumulative distance with trailing distance |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
396 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
397 ss_spline_d = [] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
398 #Prepare subsegment distances |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
399 for spline in range(len(splines)): |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
400 ss_spline_d[spline]=[]#.append([[],[],[]]) |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
401 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Incremental distance |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
402 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Cumulative distance |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
403 ss_spline_d[spline].append(zeros(len(splines[spline]))) #Cumulative distance with trailing distance |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
404 for spline_p in range(len(splines[spline])): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
405 if spline_p > (len(splines[spline]) - 2): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
406 break |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
407 ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1]) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
408 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) |
568
538fb47b3007
minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
409 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p]) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
410 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
411 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
412 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
413 return ss_spline_d |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
414 |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
415 def prepareSplines(splines): |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
416 'Approximates slope singularity by giving some slope roundoff; account for roundoff error' |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
417 for spline in splines: |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
418 p1 = spline[0] |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
419 for i in xrange(len(spline)-1): |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
420 p2 = spline[i+1] |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
421 if(round(p1.x, 10) == round(p2.x, 10)): |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
422 p2.x += 0.0000000001 |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
423 if(round(p1.y, 10) == round(p2.y, 10)): |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
424 p2.y += 0.0000000001 |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
425 p1 = p2 |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
426 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
427 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
428 ''' Point-projection (Q) on line defined by 2 points (P0,P1). |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
429 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
430 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
431 if(p0x == p1x and p0y == p1y): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
432 return None |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
433 try: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
434 #Approximate slope singularity by giving some slope roundoff; account for roundoff error |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
435 # if(round(p0x, 10) == round(p1x, 10)): |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
436 # p1x += 0.0000000001 |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
437 # if(round(p0y, 10) == round(p1y, 10)): |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
438 # p1y += 0.0000000001 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
439 #make the calculation |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
440 Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x)) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
441 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
442 except ZeroDivisionError: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
443 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
444 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
445 import pdb; pdb.set_trace() |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
446 return Point(X,Y) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
447 |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
448 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
449 ''' Snap a point p to its nearest subsegment of it's nearest spline (from the list splines). |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
450 A spline is a list of points (class Point), most likely a trajectory. |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
451 |
659
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
452 Output: |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
453 ======= |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
454 [spline index, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
455 subsegment leading point index, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
456 snapped point, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
457 subsegment distance, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
458 spline distance, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
459 orthogonal point offset] |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
460 |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
461 or None |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
462 ''' |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
463 minOffsetY = float('inf') |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
464 #For each spline |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
465 for splineIdx in range(len(splines)): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
466 #For each spline point index |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
467 for spline_p in range(len(splines[splineIdx])-1): |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
468 #Get closest point on spline |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
469 closestPoint = ppldb2p(p.x,p.y,splines[splineIdx][spline_p][0],splines[splineIdx][spline_p][1],splines[splineIdx][spline_p+1][0],splines[splineIdx][spline_p+1][1]) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
470 if closestPoint is None: |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
471 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p)) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
472 return None |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
473 # check if the projected point is in between the current segment of the alignment bounds |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
474 if utils.inBetween(splines[splineIdx][spline_p][0], splines[splineIdx][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[splineIdx][spline_p][1], splines[splineIdx][spline_p+1][1], closestPoint.y): |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
475 offsetY = Point.distanceNorm2(closestPoint, p) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
476 if offsetY < minOffsetY: |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
477 minOffsetY = offsetY |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
478 snappedSplineIdx = splineIdx |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
479 snappedSplineLeadingPoint = spline_p |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
480 snappedPoint = Point(closestPoint.x, closestPoint.y) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
481 #Jump loop if significantly close |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
482 if offsetY < goodEnoughSplineDistance: |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
483 break |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
484 |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
485 #Get sub-segment distance |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
486 if minOffsetY != float('inf'): |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
487 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint]) |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
488 #Get cumulative alignment distance (total segment distance) |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
489 splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
490 orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal() |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
491 offsetVector = p-snappedPoint |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
492 if Point.dot(orthogonalSplineVector, offsetVector) < 0: |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
493 minOffsetY = -minOffsetY |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
494 return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
495 else: |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
496 print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p)) |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
497 return None |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
498 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
499 def getXYfromSY(s, y, splineNum, splines, mode = 0): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
500 ''' Find X,Y coordinate from S,Y data. |
568
538fb47b3007
minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
501 if mode = 0 : return Snapped X,Y |
538fb47b3007
minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
502 if mode !=0 : return Real X,Y |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
503 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
504 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
505 #(buckle in, it gets ugly from here on out) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
506 ss_spline_d = subsec_spline_dist(splines) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
507 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
508 #Find subsegment |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
509 snapped_x = None |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
510 snapped_y = None |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
511 for spline_ss_index in range(len(ss_spline_d[splineNum][1])): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
512 if(s < ss_spline_d[splineNum][1][spline_ss_index]): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
513 ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
514 #Get normal vector and then snap |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
515 vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0]) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
516 vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1]) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
517 magnitude = sqrt(vector_l_x**2 + vector_l_y**2) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
518 n_vector_x = vector_l_x/magnitude |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
519 n_vector_y = vector_l_y/magnitude |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
520 snapped_x = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
521 snapped_y = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
522 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
523 #Real values (including orthogonal projection of y)) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
524 real_x = snapped_x - y*n_vector_y |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
525 real_y = snapped_y + y*n_vector_x |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
526 break |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
527 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
528 if mode == 0 or (not snapped_x): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
529 if(not snapped_x): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
530 snapped_x = splines[splineNum][-1][0] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
531 snapped_y = splines[splineNum][-1][1] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
532 return [snapped_x,snapped_y] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
533 else: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
534 return [real_x,real_y] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
535 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
536 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
537 class NormAngle(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
538 '''Alternate encoding of a point, by its norm and orientation''' |
245
bd8ab323c198
corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
244
diff
changeset
|
539 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
540 def __init__(self, norm, angle): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
541 self.norm = norm |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
542 self.angle = angle |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
543 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
544 @staticmethod |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
545 def fromPoint(p): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
546 norm = p.norm2() |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
547 if norm > 0: |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
548 angle = p.angle() |
339 | 549 else: |
550 angle = 0. | |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
551 return NormAngle(norm, angle) |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
552 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
553 def __add__(self, other): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
554 'a norm cannot become negative' |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
555 return NormAngle(max(self.norm+other.norm, 0), self.angle+other.angle) |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
556 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
557 def getPoint(self): |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
558 return Point(self.norm*cos(self.angle), self.norm*sin(self.angle)) |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
559 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
560 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
561 def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): |
245
bd8ab323c198
corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
244
diff
changeset
|
562 '''Predicts the position in nTimeSteps at constant speed/acceleration''' |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
563 return initialVelocity + initialAcceleration.__mul__(nTimeSteps),initialPosition+initialVelocity.__mul__(nTimeSteps) + initialAcceleration.__mul__(nTimeSteps**2*0.5) |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
564 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
565 def predictPosition(position, speedOrientation, control, maxSpeed = None): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
566 '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta) |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
567 speedOrientation is the other encoding of velocity, (speed, orientation) |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
568 speedOrientation and control are NormAngle''' |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
569 predictedSpeedTheta = speedOrientation+control |
928
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
570 if maxSpeed is not None: |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
571 predictedSpeedTheta.norm = min(predictedSpeedTheta.norm, maxSpeed) |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
572 predictedPosition = position+predictedSpeedTheta.getPoint() |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
573 return predictedPosition, predictedSpeedTheta |
245
bd8ab323c198
corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
244
diff
changeset
|
574 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
575 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
576 class FlowVector(object): |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
577 '''Class to represent 4-D flow vectors, |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
578 ie a position and a velocity''' |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
579 def __init__(self, position, velocity): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
580 'position and velocity should be Point instances' |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
581 self.position = position |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
582 self.velocity = velocity |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
583 |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
584 def __add__(self, other): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
585 return FlowVector(self.position+other.position, self.velocity+other.velocity) |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
586 |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
587 def __mul__(self, alpha): |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
588 return FlowVector(self.position.__mul__(alpha), self.velocity.__mul__(alpha)) |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
589 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
590 def plot(self, options = '', **kwargs): |
211 | 591 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs) |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
592 self.position.plot(options+'x', **kwargs) |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
593 |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
594 @staticmethod |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
595 def similar(f1, f2, maxDistance2, maxDeltavelocity2): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
596 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
597 |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
598 def intersection(p1, p2, p3, p4): |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
599 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
600 http://paulbourke.net/geometry/pointlineplane/''' |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
601 dp12 = p2-p1 |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
602 dp34 = p4-p3 |
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
603 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y) |
674
01b89182891a
corrected bug for intersection of lines (thanks to Paul for finding)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
672
diff
changeset
|
604 det = float(dp34.y*dp12.x-dp34.x*dp12.y) |
01b89182891a
corrected bug for intersection of lines (thanks to Paul for finding)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
672
diff
changeset
|
605 if det == 0.: |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
606 return None |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
607 else: |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
608 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
609 return p1+dp12.__mul__(ua) |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
610 |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
611 # def intersection(p1, p2, dp1, dp2): |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
612 # '''Returns the intersection point between the two lines |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
613 # defined by the respective vectors (dp) and origin points (p)''' |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
614 # from numpy import matrix |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
615 # from numpy.linalg import linalg |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
616 # A = matrix([[dp1.y, -dp1.x], |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
617 # [dp2.y, -dp2.x]]) |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
618 # B = matrix([[dp1.y*p1.x-dp1.x*p1.y], |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
619 # [dp2.y*p2.x-dp2.x*p2.y]]) |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
620 |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
621 # if linalg.det(A) == 0: |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
622 # return None |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
623 # else: |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
624 # intersection = linalg.solve(A,B) |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
625 # return Point(intersection[0,0], intersection[1,0]) |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
626 |
152
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
627 def segmentIntersection(p1, p2, p3, p4): |
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
628 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' |
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
629 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
630 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()): |
152
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
631 return None |
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
632 else: |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
633 inter = intersection(p1, p2, p3, p4) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
634 if (inter is not None |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
635 and utils.inBetween(p1.x, p2.x, inter.x) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
636 and utils.inBetween(p3.x, p4.x, inter.x) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
637 and utils.inBetween(p1.y, p2.y, inter.y) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
638 and utils.inBetween(p3.y, p4.y, inter.y)): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
639 return inter |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
640 else: |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
641 return None |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
642 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
643 def segmentLineIntersection(p1, p2, p3, p4): |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
644 '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
645 inter = intersection(p1, p2, p3, p4) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
646 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y): |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
647 return inter |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
648 else: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
649 return None |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
650 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
651 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
652 class Trajectory(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
653 '''Class for trajectories: temporal sequence of positions |
22 | 654 |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
655 The class is iterable''' |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
656 |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
657 def __init__(self, positions=None): |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
658 if positions is not None: |
113
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
659 self.positions = positions |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
660 else: |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
661 self.positions = [[],[]] |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
662 |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
663 @staticmethod |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
664 def generate(p, v, nPoints): |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
665 t = Trajectory() |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
666 p0 = Point(p.x, p.y) |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
667 t.addPosition(p0) |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
668 for i in xrange(nPoints-1): |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
669 p0 += v |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
670 t.addPosition(p0) |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
671 return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints]) |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
672 |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
673 @staticmethod |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
674 def load(line1, line2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
675 return Trajectory([[float(n) for n in line1.split(' ')], |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
676 [float(n) for n in line2.split(' ')]]) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
677 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
678 @staticmethod |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
679 def fromPointList(points): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
680 t = Trajectory() |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
681 if isinstance(points[0], list) or isinstance(points[0], tuple): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
682 for p in points: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
683 t.addPositionXY(p[0],p[1]) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
684 else: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
685 for p in points: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
686 t.addPosition(p) |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
687 return t |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
688 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
689 def __len__(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
690 return len(self.positions[0]) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
691 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
692 def length(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
693 return self.__len__() |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
694 |
582
7e1ae4d97f1a
corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
581
diff
changeset
|
695 def empty(self): |
7e1ae4d97f1a
corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
581
diff
changeset
|
696 return self.__len__() == 0 |
7e1ae4d97f1a
corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
581
diff
changeset
|
697 |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
698 def __getitem__(self, i): |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
699 if isinstance(i, int): |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
700 return Point(self.positions[0][i], self.positions[1][i]) |
918
3a06007a4bb7
modularized save trajectories, added slice to Trajectory, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
904
diff
changeset
|
701 elif isinstance(i, slice): |
3a06007a4bb7
modularized save trajectories, added slice to Trajectory, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
904
diff
changeset
|
702 return Trajectory([self.positions[0][i],self.positions[1][i]]) |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
703 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
704 raise TypeError("Invalid argument type.") |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
705 |
16 | 706 def __str__(self): |
39 | 707 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())]) |
16 | 708 |
69
cc192d0450b3
added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
68
diff
changeset
|
709 def __repr__(self): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
710 return self.__str__() |
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
711 |
22 | 712 def __iter__(self): |
713 self.iterInstantNum = 0 | |
714 return self | |
715 | |
716 def next(self): | |
717 if self.iterInstantNum >= self.length(): | |
718 raise StopIteration | |
719 else: | |
720 self.iterInstantNum += 1 | |
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
721 return self[self.iterInstantNum-1] |
22 | 722 |
776
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
723 def __eq__(self, other): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
724 if self.length() == other.length(): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
725 result = True |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
726 for p, po in zip(self, other): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
727 result = result and (p == po) |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
728 return result |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
729 else: |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
730 return False |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
731 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
732 def setPositionXY(self, i, x, y): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
733 if i < self.__len__(): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
734 self.positions[0][i] = x |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
735 self.positions[1][i] = y |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
736 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
737 def setPosition(self, i, p): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
738 self.setPositionXY(i, p.x, p.y) |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
739 |
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
740 def addPositionXY(self, x, y): |
113
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
741 self.positions[0].append(x) |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
742 self.positions[1].append(y) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
743 |
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
744 def addPosition(self, p): |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
745 self.addPositionXY(p.x, p.y) |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
746 |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
747 def duplicateLastPosition(self): |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
748 self.positions[0].append(self.positions[0][-1]) |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
749 self.positions[1].append(self.positions[1][-1]) |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
750 |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
751 @staticmethod |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
752 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, objNum = None, **kwargs): |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
753 if lastCoordinate is None: |
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
754 plot(positions[0][::timeStep], positions[1][::timeStep], options, **kwargs) |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
755 elif 0 <= lastCoordinate <= len(positions[0]): |
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
756 plot(positions[0][:lastCoordinate:timeStep], positions[1][:lastCoordinate:timeStep], options, **kwargs) |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
757 if withOrigin: |
211 | 758 plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs) |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
759 if objNum is not None: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
760 text(positions[0][0], positions[1][0], '{}'.format(objNum)) |
933
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
761 |
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
762 def homographyProject(self, homography): |
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
763 return Trajectory(cvutils.homographyProject(array(self.positions), homography).tolist()) |
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
764 |
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
765 def newCameraProject(self, newCameraMatrix): |
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
766 return Trajectory(cvutils.newCameraProject(array(self.positions), newCameraMatrix).tolist()) |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
767 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
768 def plot(self, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs): |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
769 Trajectory._plot(self.positions, options, withOrigin, None, timeStep, objNum, **kwargs) |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
770 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
771 def plotAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs): |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
772 Trajectory._plot(self.positions, options, withOrigin, lastCoordinate, timeStep, objNum, **kwargs) |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
773 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
774 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs): |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
775 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]], |
620
aee4cbac9e0e
corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
595
diff
changeset
|
776 [x*nPixelsPerUnitDistance for x in self.positions[1]]] |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
777 Trajectory._plot(imgPositions, options, withOrigin, None, timeStep, objNum, **kwargs) |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
778 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
779 def getXCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
780 return self.positions[0] |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
781 |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
782 def getYCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
783 return self.positions[1] |
92
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
784 |
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
785 def asArray(self): |
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
786 return array(self.positions) |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
787 |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
788 def xBounds(self): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
789 # look for function that does min and max in one pass |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
790 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates())) |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
791 |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
792 def yBounds(self): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
793 # look for function that does min and max in one pass |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
794 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates())) |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
795 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
796 def add(self, traj2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
797 '''Returns a new trajectory of the same length''' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
798 if self.length() != traj2.length(): |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
799 print('Trajectories of different lengths') |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
800 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
801 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
802 return Trajectory([[a+b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
803 [a+b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
804 |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
805 def subtract(self, traj2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
806 '''Returns a new trajectory of the same length''' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
807 if self.length() != traj2.length(): |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
808 print('Trajectories of different lengths') |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
809 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
810 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
811 return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
812 [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
813 |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
814 def __mul__(self, alpha): |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
815 '''Returns a new trajectory of the same length''' |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
816 return Trajectory([[alpha*x for x in self.getXCoordinates()], |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
817 [alpha*y for y in self.getYCoordinates()]]) |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
818 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
819 def differentiate(self, doubleLastPosition = False): |
339 | 820 diff = Trajectory() |
821 for i in xrange(1, self.length()): | |
822 diff.addPosition(self[i]-self[i-1]) | |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
823 if doubleLastPosition: |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
824 diff.addPosition(diff[-1]) |
339 | 825 return diff |
826 | |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
827 def differentiateSG(self, window_length, polyorder, deriv=0, delta=1.0, axis=-1, mode='interp', cval=0.0, nInstantsIgnoredAtEnds = 2): |
761
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
828 '''Differentiates the trajectory using the Savitsky Golay filter |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
829 |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
830 window_length : The length of the filter window (i.e. the number of coefficients). window_length must be a positive odd integer. |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
831 polyorder : The order of the polynomial used to fit the samples. polyorder must be less than window_length. |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
832 deriv : The order of the derivative to compute. This must be a nonnegative integer. The default is 0, which means to filter the data without differentiating. |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
833 delta : The spacing of the samples to which the filter will be applied. This is only used if deriv > 0. Default is 1.0. |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
834 axis : The axis of the array x along which the filter is to be applied. Default is -1. |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
835 mode : Must be mirror, constant, nearest, wrap or interp. This determines the type of extension to use for the padded signal to which the filter is applied. When mode is constant, the padding value is given by cval. See the Notes for more details on mirror, constant, wrap, and nearest. When the interp mode is selected (the default), no extension is used. Instead, a degree polyorder polynomial is fit to the last window_length values of the edges, and this polynomial is used to evaluate the last window_length // 2 output values. |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
836 cval : Value to fill past the edges of the input if mode is constant. Default is 0.0. |
761
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
837 |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
838 https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.savgol_filter.html#scipy.signal.savgol_filter''' |
761
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
839 if removeBothEnds >=1: |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
840 pos = [self.positions[0][nInstantsIgnoredAtEnds:-nInstantsIgnoredAtEnds], |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
841 self.positions[1][nInstantsIgnoredAtEnds:-nInstantsIgnoredAtEnds]] |
761
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
842 else: |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
843 pos = self.positions |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
844 filtered = savgol_filter(pos, window_length, polyorder, deriv, delta, axis, mode, cval) |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
845 return Trajectory(filtered) |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
846 |
16 | 847 def norm(self): |
848 '''Returns the list of the norms at each instant''' | |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
849 return hypot(self.positions[0], self.positions[1]) |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
850 |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
851 def computeCumulativeDistances(self): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
852 '''Computes the distance from each point to the next and the cumulative distance up to the point |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
853 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)''' |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
854 self.distances = [] |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
855 self.cumulativeDistances = [0.] |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
856 p1 = self[0] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
857 cumulativeDistance = 0. |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
858 for i in xrange(self.length()-1): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
859 p2 = self[i+1] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
860 self.distances.append(Point.distanceNorm2(p1,p2)) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
861 cumulativeDistance += self.distances[-1] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
862 self.cumulativeDistances.append(cumulativeDistance) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
863 p1 = p2 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
864 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
865 def getDistance(self,i): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
866 '''Return the distance between points i and i+1''' |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
867 if i < self.length()-1: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
868 return self.distances[i] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
869 else: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
870 print('Index {} beyond trajectory length {}-1'.format(i, self.length())) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
871 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
872 def getCumulativeDistance(self, i): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
873 '''Return the cumulative distance between the beginning and point i''' |
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
874 if i < self.length(): |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
875 return self.cumulativeDistances[i] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
876 else: |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
877 print('Index {} beyond trajectory length {}'.format(i, self.length())) |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
878 |
762
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
879 def getMaxDistance(self, metric): |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
880 'Returns the maximum distance between points in the trajectory' |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
881 positions = self.getPositions().asArray().T |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
882 return cdist(positions, positions, metric = metric).max() |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
883 |
937
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
884 def getClosestPoint(self, p1, maxDist2 = None): |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
885 '''Returns the instant of the closest position in trajectory to p1 (and the point) |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
886 if maxDist is not None, will check the distance is smaller |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
887 TODO: could use cdist for different metrics''' |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
888 distances2 = [] |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
889 minDist2 = float('inf') |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
890 i = -1 |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
891 for p2 in self: |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
892 distances2.append(Point.distanceNorm2(p1, p2)) |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
893 if distances2[-1] < minDist2: |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
894 minDist2 = distances2[-1] |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
895 i = len(distances2)-1 |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
896 if maxDist2 is not None and minDist2 < maxDist2: |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
897 return None |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
898 else: |
937
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
899 return i |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
900 |
381
387cc0142211
script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
379
diff
changeset
|
901 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): |
387cc0142211
script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
379
diff
changeset
|
902 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
903 have a cosine with refDirection is smaller than cosineThreshold''' |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
904 count = 0 |
383
0ce2210790b1
fixed stupid naming bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
381
diff
changeset
|
905 lengthThreshold = float(self.length())*minProportion |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
906 for p in self: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
907 if p.similarOrientation(refDirection, cosineThreshold): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
908 count += 1 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
909 return count >= lengthThreshold |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
910 |
49 | 911 def wiggliness(self): |
762
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
912 straightDistance = Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)) |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
913 if straightDistance > 0: |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
914 return self.getCumulativeDistance(self.length()-1)/float(straightDistance) |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
915 else: |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
916 return None |
49 | 917 |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
918 def getIntersections(self, p1, p2): |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
919 '''Returns a list of the indices at which the trajectory |
91
daa05fae1a70
modified the type of the result of interval lengths to float, added comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
90
diff
changeset
|
920 intersects with the segment of extremities p1 and p2 |
771 | 921 Returns an empty list if there is no crossing''' |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
922 indices = [] |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
923 intersections = [] |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
924 |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
925 for i in xrange(self.length()-1): |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
926 q1=self.__getitem__(i) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
927 q2=self.__getitem__(i+1) |
771 | 928 p = segmentIntersection(q1, q2, p1, p2) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
929 if p is not None: |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
930 if q1.x != q2.x: |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
931 ratio = (p.x-q1.x)/(q2.x-q1.x) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
932 elif q1.y != q2.y: |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
933 ratio = (p.y-q1.y)/(q2.y-q1.y) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
934 else: |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
935 ratio = 0 |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
936 indices.append(i+ratio) |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
937 intersections.append(p) |
771 | 938 return indices, intersections |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
939 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
940 def getLineIntersections(self, p1, p2): |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
941 '''Returns a list of the indices at which the trajectory |
771 | 942 intersects with the line going through p1 and p2 |
943 Returns an empty list if there is no crossing''' | |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
944 indices = [] |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
945 intersections = [] |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
946 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
947 for i in xrange(self.length()-1): |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
948 q1=self.__getitem__(i) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
949 q2=self.__getitem__(i+1) |
771 | 950 p = segmentLineIntersection(p1, p2, q1, q2) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
951 if p is not None: |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
952 if q1.x != q2.x: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
953 ratio = (p.x-q1.x)/(q2.x-q1.x) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
954 elif q1.y != q2.y: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
955 ratio = (p.y-q1.y)/(q2.y-q1.y) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
956 else: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
957 ratio = 0 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
958 indices.append(i+ratio) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
959 intersections.append(p) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
960 return indices, intersections |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
961 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
962 def getTrajectoryInInterval(self, inter): |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
963 'Returns all position between index inter.first and index.last (included)' |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
964 if inter.first >=0 and inter.last<= self.length(): |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
965 return Trajectory([self.positions[0][inter.first:inter.last+1], |
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
966 self.positions[1][inter.first:inter.last+1]]) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
967 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
968 return None |
762
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
969 |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
970 def subSample(self, step): |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
971 'Returns the positions very step' |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
972 return Trajectory([self.positions[0][::step], |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
973 self.positions[1][::step]]) |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
974 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
975 if shapelyAvailable: |
863
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
976 def getInstantsInPolygon(self, polygon): |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
977 '''Returns the list of instants at which the trajectory is in the polygon''' |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
978 instants = [] |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
979 n = self.length() |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
980 for t, x, y in zip(range(n), self.positions[0], self.positions[1]): |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
981 if polygon.contains(shapelyPoint(x, y)): |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
982 instants.append(t) |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
983 return instants |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
984 |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
985 def getTrajectoryInPolygon(self, polygon, t2 = None): |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
986 '''Returns the trajectory built with the set of points inside the (shapely) polygon |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
987 The polygon could be a prepared polygon (faster) from prepared.prep |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
988 |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
989 t2 is another trajectory (could be velocities) |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
990 which is filtered based on the first (self) trajectory''' |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
991 traj = Trajectory() |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
992 inPolygon = [] |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
993 for x, y in zip(self.positions[0], self.positions[1]): |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
994 inPolygon.append(polygon.contains(shapelyPoint(x, y))) |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
995 if inPolygon[-1]: |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
996 traj.addPositionXY(x, y) |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
997 traj2 = Trajectory() |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
998 if t2 is not None: |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
999 for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]): |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1000 if inp: |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1001 traj2.addPositionXY(x, y) |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1002 return traj, traj2 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1003 |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1004 def proportionInPolygon(self, polygon, minProportion = 0.5): |
863
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1005 instants = self.getInstantsInPolygon(polygon) |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1006 lengthThreshold = float(self.length())*minProportion |
863
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1007 return len(instants) >= lengthThreshold |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1008 else: |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1009 def getTrajectoryInPolygon(self, polygon, t2 = None): |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1010 '''Returns the trajectory built with the set of points inside the polygon |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1011 (array of Nx2 coordinates of the polygon vertices)''' |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1012 traj = Trajectory() |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1013 inPolygon = [] |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1014 for p in self: |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1015 inPolygon.append(p.inPolygon(polygon)) |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1016 if inPolygon[-1]: |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1017 traj.addPosition(p) |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1018 traj2 = Trajectory() |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1019 if t2 is not None: |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1020 for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]): |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1021 if inp: |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1022 traj2.addPositionXY(p.x, p.y) |
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1023 return traj, traj2 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1024 |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1025 def proportionInPolygon(self, polygon, minProportion = 0.5): |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1026 inPolygon = [p.inPolygon(polygon) for p in self] |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1027 lengthThreshold = float(self.length())*minProportion |
781
7c38250ddfc7
updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
780
diff
changeset
|
1028 return sum(inPolygon) >= lengthThreshold |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1029 |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
1030 @staticmethod |
369
027e254f0b53
lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
368
diff
changeset
|
1031 def lcss(t1, t2, lcss): |
027e254f0b53
lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
368
diff
changeset
|
1032 return lcss.compute(t1, t2) |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
1033 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1034 class CurvilinearTrajectory(Trajectory): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1035 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1036 longitudinal coordinate is stored as first coordinate (exterior name S) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1037 lateral coordiante is stored as second coordinate''' |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1038 |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1039 def __init__(self, S = None, Y = None, lanes = None): |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1040 if S is None or Y is None or len(S) != len(Y): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1041 self.positions = [[],[]] |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1042 if S is not None and Y is not None and len(S) != len(Y): |
543
cb213269d330
defensive code for curvilinear trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
542
diff
changeset
|
1043 print("S and Y coordinates of different lengths\nInitializing to empty lists") |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1044 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1045 self.positions = [S,Y] |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1046 if lanes is None or len(lanes) != self.length(): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1047 self.lanes = [] |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1048 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1049 self.lanes = lanes |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1050 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1051 def __getitem__(self,i): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1052 if isinstance(i, int): |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1053 return [self.positions[0][i], self.positions[1][i], self.lanes[i]] |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1054 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
1055 raise TypeError("Invalid argument type.") |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1056 #elif isinstance( key, slice ): |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1057 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1058 def getSCoordinates(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1059 return self.getXCoordinates() |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1060 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1061 def getLanes(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1062 return self.lanes |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1063 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1064 def addPositionSYL(self, s, y, lane): |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1065 self.addPositionXY(s,y) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1066 self.lanes.append(lane) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1067 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1068 def addPosition(self, p): |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1069 'Adds position in the point format for curvilinear of list with 3 values' |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1070 self.addPositionSYL(p[0], p[1], p[2]) |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1071 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1072 def setPosition(self, i, s, y, lane): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1073 self.setPositionXY(i, s, y) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1074 if i < self.__len__(): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1075 self.lanes[i] = lane |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1076 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1077 def differentiate(self, doubleLastPosition = False): |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1078 diff = CurvilinearTrajectory() |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1079 p1 = self[0] |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1080 for i in xrange(1, self.length()): |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1081 p2 = self[i] |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1082 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2]) |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1083 p1=p2 |
582
7e1ae4d97f1a
corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
581
diff
changeset
|
1084 if doubleLastPosition and self.length() > 1: |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1085 diff.addPosition(diff[-1]) |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1086 return diff |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1087 |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1088 def getIntersections(self, S1, lane = None): |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1089 '''Returns a list of the indices at which the trajectory |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1090 goes past the curvilinear coordinate S1 |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1091 (in provided lane if lane is not None) |
771 | 1092 Returns an empty list if there is no crossing''' |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1093 indices = [] |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1094 for i in xrange(self.length()-1): |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1095 q1=self.__getitem__(i) |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1096 q2=self.__getitem__(i+1) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1097 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)): |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1098 indices.append(i+(S1-q1[0])/(q2[0]-q1[0])) |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1099 return indices |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1100 |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1101 ################## |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1102 # Moving Objects |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1103 ################## |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1104 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1105 userTypeNames = ['unknown', |
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1106 'car', |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1107 'pedestrian', |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1108 'motorcycle', |
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1109 'bicycle', |
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
1110 'bus', |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1111 'truck'] |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1112 |
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
1113 userType2Num = utils.inverseEnumeration(userTypeNames) |
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
1114 |
959
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1115 class CarClassifier: |
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1116 def predict(self, hog): |
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1117 return userType2Num['car'] |
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1118 carClassifier = CarClassifier() |
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1119 |
664
455f9b93819c
added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
663
diff
changeset
|
1120 class MovingObject(STObject, VideoFilenameAddable): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
1121 '''Class for moving objects: a spatio-temporal object |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1122 with a trajectory and a geometry (constant volume over time) |
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1123 and a usertype (e.g. road user) coded as a number (see userTypeNames) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
1124 ''' |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
1125 |
411
31604ef1cad4
added hog functions and the display of road user types if known
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
383
diff
changeset
|
1126 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown']): |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
1127 super(MovingObject, self).__init__(num, timeInterval) |
16 | 1128 self.positions = positions |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1129 self.velocities = velocities |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
1130 self.geometry = geometry |
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1131 self.userType = userType |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1132 self.features = None |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
1133 # compute bounding polygon from trajectory |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
1134 |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
1135 @staticmethod |
918
3a06007a4bb7
modularized save trajectories, added slice to Trajectory, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
904
diff
changeset
|
1136 def generate(num, p, v, timeInterval): |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
1137 positions, velocities = Trajectory.generate(p, v, int(timeInterval.length())) |
918
3a06007a4bb7
modularized save trajectories, added slice to Trajectory, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
904
diff
changeset
|
1138 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = velocities) |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
1139 |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
1140 @staticmethod |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1141 def concatenate(obj1, obj2, num = None): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1142 '''Concatenates two objects supposed to overlap temporally ''' |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1143 commonTimeInterval = obj1.commonTimeInterval(obj2) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1144 if commonTimeInterval.empty(): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1145 print('The two objects\' time intervals do not overlap: obj1 {} and obj2 {}'.format(obj1.getTimeInterval(), obj2.getTimeInterval())) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1146 return None |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1147 else: |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1148 if num is None: |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1149 newNum = obj1.getNum() |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1150 else: |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1151 newNum = num |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1152 newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval()) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1153 # positions |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1154 positions = Trajectory() |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1155 for t in newTimeInterval: |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1156 nTotal = 0. |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1157 p = Point(0.,0.) |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1158 for obj in [obj1, obj2]: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1159 if obj.existsAtInstant(t): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1160 if obj.hasFeatures(): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1161 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1162 else: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1163 n = 1. |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
1164 p += obj.getPositionAtInstant(t).__mul__(n) |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1165 nTotal += n |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1166 assert nTotal>0, 'there should be at least one point for each instant' |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1167 positions.addPosition(p.divide(nTotal)) |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1168 # velocities: if any |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1169 if hasattr(obj1, 'velocities') and hasattr(obj2, 'velocities'): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1170 velocities = Trajectory() |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1171 for t in newTimeInterval: |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1172 nTotal = 0. |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1173 p = Point(0.,0.) |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1174 for obj in [obj1, obj2]: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1175 if obj.existsAtInstant(t): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1176 if obj.hasFeatures(): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1177 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1178 else: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1179 n = 1. |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
1180 p += obj.getVelocityAtInstant(t).__mul__(n) |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1181 nTotal += n |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1182 assert n>0, 'there should be at least one point for each instant' |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1183 velocities.addPosition(p.divide(nTotal)) |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1184 else: |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1185 velocities = None |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1186 # TODO object envelop (polygon) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1187 # user type |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1188 if obj1.getUserType() != obj2.getUserType(): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1189 print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()])) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1190 |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1191 newObject = MovingObject(newNum, newTimeInterval, positions, velocities, userType = obj1.getUserType()) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1192 if obj1.hasFeatures() and obj2.hasFeatures(): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1193 newObject.features = obj1.getFeatures()+obj2.getFeatures() |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1194 return newObject |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
1195 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1196 def getObjectInTimeInterval(self, inter): |
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1197 '''Returns a new object extracted from self, |
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1198 restricted to time interval inter''' |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
1199 intersection = TimeInterval.intersection(inter, self.getTimeInterval()) |
97
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
1200 if not intersection.empty(): |
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
1201 trajectoryInterval = TimeInterval(intersection.first-self.getFirstInstant(), intersection.last-self.getFirstInstant()) |
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
1202 obj = MovingObject(self.num, intersection, self.positions.getTrajectoryInInterval(trajectoryInterval), self.geometry, self.userType) |
982
51d8406b2489
corrected bug when not using a homography and using a mask for CLEAR MOT metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
978
diff
changeset
|
1203 if self.velocities is not None: |
97
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
1204 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1205 return obj |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1206 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
1207 print('The object does not exist at {}'.format(inter)) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1208 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1209 |
726
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1210 def getObjectsInMask(self, mask, homography = None, minLength = 1): |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1211 '''Returns new objects made of the positions in the mask |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1212 mask is in the destination of the homography space''' |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1213 if homography is not None: |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
1214 self.projectedPositions = self.positions.homographyProject(homography) |
726
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1215 else: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1216 self.projectedPositions = self.positions |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1217 def inMask(positions, i, mask): |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1218 p = positions[i] |
982
51d8406b2489
corrected bug when not using a homography and using a mask for CLEAR MOT metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
978
diff
changeset
|
1219 return mask[int(p.y), int(p.x)] != 0. |
726
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1220 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1221 #subTimeIntervals self.getFirstInstant()+i |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1222 filteredIndices = [inMask(self.projectedPositions, i, mask) for i in range(int(self.length()))] |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1223 # 'connected components' in subTimeIntervals |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1224 l = 0 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1225 intervalLabels = [] |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1226 prev = True |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1227 for i in filteredIndices: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1228 if i: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1229 if not prev: # new interval |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1230 l += 1 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1231 intervalLabels.append(l) |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1232 else: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1233 intervalLabels.append(-1) |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1234 prev = i |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1235 intervalLabels = array(intervalLabels) |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1236 subObjects = [] |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1237 for l in set(intervalLabels): |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1238 if l >= 0: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1239 if sum(intervalLabels == l) >= minLength: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1240 times = [self.getFirstInstant()+i for i in range(len(intervalLabels)) if intervalLabels[i] == l] |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1241 subTimeInterval = TimeInterval(min(times), max(times)) |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1242 subObjects.append(self.getObjectInTimeInterval(subTimeInterval)) |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1243 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1244 return subObjects |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1245 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1246 def getPositions(self): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1247 return self.positions |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1248 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1249 def getVelocities(self): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1250 return self.velocities |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1251 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1252 def getUserType(self): |
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1253 return self.userType |
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1254 |
943
b1e8453c207c
work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
939
diff
changeset
|
1255 def computeCumulativeDistances(self): |
b1e8453c207c
work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
939
diff
changeset
|
1256 self.positions.computeCumulativeDistances() |
b1e8453c207c
work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
939
diff
changeset
|
1257 |
335
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1258 def getCurvilinearPositions(self): |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1259 if hasattr(self, 'curvilinearPositions'): |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1260 return self.curvilinearPositions |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1261 else: |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1262 return None |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1263 |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1264 def plotCurvilinearPositions(self, lane = None, options = '', withOrigin = False, **kwargs): |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1265 if hasattr(self, 'curvilinearPositions'): |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1266 if lane is None: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1267 plot(list(self.getTimeInterval()), self.curvilinearPositions.positions[0], options, **kwargs) |
660
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1268 if withOrigin: |
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1269 plot([self.getFirstInstant()], [self.curvilinearPositions.positions[0][0]], 'ro', **kwargs) |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1270 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1271 instants = [] |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1272 coords = [] |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1273 for t, p in zip(self.getTimeInterval(), self.curvilinearPositions): |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1274 if p[2] == lane: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1275 instants.append(t) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1276 coords.append(p[0]) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1277 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1278 instants.append(NaN) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1279 coords.append(NaN) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1280 plot(instants, coords, options, **kwargs) |
660
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1281 if withOrigin and len(instants)>0: |
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1282 plot([instants[0]], [coords[0]], 'ro', **kwargs) |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1283 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1284 print('Object {} has no curvilinear positions'.format(self.getNum())) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1285 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1286 def setUserType(self, userType): |
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1287 self.userType = userType |
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1288 |
880
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1289 def setFeatures(self, features, featuresOrdered = False): |
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1290 '''Sets the features in the features field based on featureNumbers |
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1291 if not all features are loaded from 0, one needs to renumber in a dict''' |
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1292 if featuresOrdered: |
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1293 tmp = features |
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1294 else: |
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1295 tmp = {f.getNum():f for f in features} |
000555430b28
adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
877
diff
changeset
|
1296 self.features = [tmp[i] for i in self.featureNumbers] |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
1297 |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1298 def getFeatures(self): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1299 return self.features |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1300 |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1301 def hasFeatures(self): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1302 return (self.features is not None) |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1303 |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1304 def getFeature(self, i): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1305 if self.hasFeatures() and i<len(self.features): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1306 return self.features[i] |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1307 else: |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1308 return None |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1309 |
869
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1310 def getNLongestFeatures(self, nFeatures = 1): |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1311 if self.features is None: |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1312 return [] |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1313 else: |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1314 tmp = utils.sortByLength(self.getFeatures(), reverse = True) |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1315 return tmp[:min(len(tmp), nFeatures)] |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1316 |
666
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1317 def getFeatureNumbers(self): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1318 '''Returns the number of features at each instant |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1319 dict instant -> number of features''' |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1320 if self.hasFeatures(): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1321 featureNumbers = {} |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1322 for t in self.getTimeInterval(): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1323 n = 0 |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1324 for f in self.getFeatures(): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1325 if f.existsAtInstant(t): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1326 n += 1 |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1327 featureNumbers[t]=n |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1328 return featureNumbers |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1329 else: |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1330 print('Object {} has no features loaded.'.format(self.getNum())) |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1331 return None |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1332 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1333 def getSpeeds(self, nInstantsIgnoredAtEnds = 0): |
682 | 1334 speeds = self.getVelocities().norm() |
1335 if nInstantsIgnoredAtEnds > 0: | |
1336 n = min(nInstantsIgnoredAtEnds, int(floor(self.length()/2.))) | |
1337 return speeds[n:-n] | |
1338 else: | |
1339 return speeds | |
49 | 1340 |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1341 def getAccelerations(self, window_length, polyorder, delta=1.0, axis=-1, mode='interp', cval=0.0, speeds = None, nInstantsIgnoredAtEnds = 0): |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1342 '''Returns the 1-D acceleration from the 1-D speeds |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1343 Caution about previously filtered data''' |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1344 if speeds is None: |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1345 speeds = self.getSpeeds(nInstantsIgnoredAtEnds) |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1346 return savgol_filter(speeds, window_length, polyorder, 1, delta, axis, mode, cval) |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1347 |
633
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1348 def getSpeedIndicator(self): |
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1349 from indicators import SeverityIndicator |
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1350 return SeverityIndicator('Speed', {t:self.getVelocityAtInstant(t).norm2() for t in self.getTimeInterval()}) |
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1351 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1352 def getPositionAt(self, i): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1353 return self.positions[i] |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1354 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1355 def getVelocityAt(self, i): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1356 return self.velocities[i] |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1357 |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1358 def getPositionAtInstant(self, i): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1359 return self.positions[i-self.getFirstInstant()] |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1360 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1361 def getVelocityAtInstant(self, i): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1362 return self.velocities[i-self.getFirstInstant()] |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1363 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1364 def getXCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1365 return self.positions.getXCoordinates() |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1366 |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1367 def getYCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1368 return self.positions.getYCoordinates() |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1369 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1370 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, withIds = False, **kwargs): |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1371 if withIds: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1372 objNum = self.getNum() |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1373 else: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1374 objNum = None |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1375 if withFeatures and self.hasFeatures(): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1376 for f in self.getFeatures(): |
517
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1377 f.positions.plot('r', True, timeStep, **kwargs) |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1378 self.positions.plot('bx-', True, timeStep, objNum, **kwargs) |
517
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1379 else: |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1380 self.positions.plot(options, withOrigin, timeStep, objNum, **kwargs) |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
1381 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1382 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, withIds = False, **kwargs): |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1383 if withIds: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1384 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, self.getNum(), **kwargs) |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1385 else: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1386 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs) |
22 | 1387 |
626
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
1388 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.): |
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
1389 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication) |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
1390 |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1391 def speedDiagnostics(self, framerate = 1., display = False, nInstantsIgnoredAtEnds=0): |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1392 speeds = framerate*self.getSpeeds(nInstantsIgnoredAtEnds) |
377 | 1393 coef = utils.linearRegression(range(len(speeds)), speeds) |
379 | 1394 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), scoreatpercentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0])) |
377 | 1395 if display: |
672
5473b7460375
moved and rationalized imports in modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
666
diff
changeset
|
1396 from matplotlib.pyplot import figure, axis |
377 | 1397 figure(1) |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
1398 self.plot() |
379 | 1399 axis('equal') |
377 | 1400 figure(2) |
1401 plot(list(self.getTimeInterval()), speeds) | |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1402 figure(3) |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1403 plot(list(self.getTimeInterval()), self.getAccelerations(9, 3, speeds = speeds)) # arbitrary parameter |
377 | 1404 |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1405 @staticmethod |
653
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1406 def minMaxDistance(obj1, obj2): |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1407 '''Computes the min max distance used for feature grouping''' |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1408 commonTimeInterval = obj1.commonTimeInterval(obj2) |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1409 if not commonTimeInterval.empty(): |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1410 minDistance = (obj1.getPositionAtInstant(commonTimeInterval.first)-obj2.getPositionAtInstant(commonTimeInterval.first)).norm2() |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1411 maxDistance = minDistance |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1412 for t in list(commonTimeInterval)[1:]: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1413 d = (obj1.getPositionAtInstant(t)-obj2.getPositionAtInstant(t)).norm2() |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1414 if d<minDistance: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1415 minDistance = d |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1416 elif d>maxDistance: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1417 maxDistance = d |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1418 return int(commonTimeInterval.length()), minDistance, maxDistance |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1419 else: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1420 return int(commonTimeInterval.length()), None, None |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1421 |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1422 @staticmethod |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1423 def distances(obj1, obj2, instant1, _instant2 = None): |
653
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1424 '''Returns the distances between all features of the 2 objects |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1425 at the same instant instant1 |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1426 or at instant1 and instant2''' |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1427 if _instant2 is None: |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1428 instant2 = instant1 |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1429 else: |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1430 instant2 = _instant2 |
546 | 1431 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)] |
1432 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)] | |
1433 return cdist(positions1, positions2, metric = 'euclidean') | |
1434 | |
1435 @staticmethod | |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1436 def minDistance(obj1, obj2, instant1, instant2 = None): |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1437 return MovingObject.distances(obj1, obj2, instant1, instant2).min() |
546 | 1438 |
1439 @staticmethod | |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1440 def maxDistance(obj1, obj2, instant, instant2 = None): |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1441 return MovingObject.distances(obj1, obj2, instant1, instant2).max() |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1442 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1443 def maxSize(self): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1444 '''Returns the max distance between features |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1445 at instant there are the most features''' |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1446 if hasattr(self, 'features'): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1447 nFeatures = -1 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1448 tMaxFeatures = 0 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1449 for t in self.getTimeInterval(): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1450 n = len([f for f in self.features if f.existsAtInstant(t)]) |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1451 if n > nFeatures: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1452 nFeatures = n |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1453 tMaxFeatures = t |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1454 return MovingObject.maxDistance(self, self, tMaxFeatures) |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1455 else: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1456 print('Load features to compute a maximum size') |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1457 return None |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
diff
changeset
|
1458 |
550 | 1459 def setRoutes(self, startRouteID, endRouteID): |
1460 self.startRouteID = startRouteID | |
1461 self.endRouteID = endRouteID | |
546 | 1462 |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1463 def getInstantsCrossingLane(self, p1, p2): |
55
88d5ee5ac164
updated comments and added shell for interaction between road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
54
diff
changeset
|
1464 '''Returns the instant(s) |
88d5ee5ac164
updated comments and added shell for interaction between road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
54
diff
changeset
|
1465 at which the object passes from one side of the segment to the other |
88d5ee5ac164
updated comments and added shell for interaction between road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
54
diff
changeset
|
1466 empty list if there is no crossing''' |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1467 indices, intersections = self.positions.getIntersections(p1, p2) |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1468 return [t+self.getFirstInstant() for t in indices] |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
1469 |
949
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1470 def computeTrajectorySimilarities(self, prototypes, lcss): |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1471 'Computes the similarities to the prototypes using the LCSS' |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1472 if not hasattr(self, 'prototypeSimilarities'): |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1473 self.prototypeSimilarities = [] |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1474 for proto in prototypes: |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1475 lcss.similarities(proto.getMovingObject().getPositions().asArray().T, self.getPositions().asArray().T) |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1476 similarities = lcss.similarityTable[-1, :-1].astype(float) |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1477 self.prototypeSimilarities.append(similarities/minimum(arange(1.,len(similarities)+1), proto.getMovingObject().length()*ones(len(similarities)))) |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1478 |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1479 @staticmethod |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1480 def computePET(obj1, obj2, collisionDistanceThreshold): |
635
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1481 '''Post-encroachment time based on distance threshold |
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1482 |
887
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1483 Returns the smallest time difference when the object positions are within collisionDistanceThreshold |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1484 and the instants at which each object is passing through its corresponding position''' |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1485 positions1 = [p.astuple() for p in obj1.getPositions()] |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1486 positions2 = [p.astuple() for p in obj2.getPositions()] |
887
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1487 n1 = len(positions1) |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1488 n2 = len(positions2) |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1489 pets = zeros((n1, n2)) |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1490 for i,t1 in enumerate(obj1.getTimeInterval()): |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1491 for j,t2 in enumerate(obj2.getTimeInterval()): |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1492 pets[i,j] = abs(t1-t2) |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1493 distances = cdist(positions1, positions2, metric = 'euclidean') |
887
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1494 smallDistances = (distances <= collisionDistanceThreshold) |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1495 if smallDistances.any(): |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1496 smallPets = pets[smallDistances] |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1497 petIdx = smallPets.argmin() |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1498 distanceIndices = argwhere(smallDistances)[petIdx] |
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1499 return smallPets[petIdx], obj1.getFirstInstant()+distanceIndices[0], obj2.getFirstInstant()+distanceIndices[1] |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1500 else: |
887
e2452abba0e7
added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
881
diff
changeset
|
1501 return None, None, None |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1502 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
1503 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
223
diff
changeset
|
1504 '''Predicts the position of object at instant+deltaT, |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
223
diff
changeset
|
1505 at constant speed''' |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
1506 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
223
diff
changeset
|
1507 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1508 def projectCurvilinear(self, alignments, ln_mv_av_win=3): |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1509 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1510 (curvilinearPositions instance) which holds information about the |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1511 curvilinear coordinates using alignment metadata. |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1512 From Paul St-Aubin's PVA tools |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1513 ====== |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1514 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1515 Input: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1516 ====== |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1517 alignments = a list of alignments, where each alignment is a list of |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1518 points (class Point). |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1519 ln_mv_av_win = moving average window (in points) in which to smooth |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1520 lane changes. As per tools_math.cat_mvgavg(), this term |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1521 is a search *radius* around the center of the window. |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1522 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1523 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1524 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1525 self.curvilinearPositions = CurvilinearTrajectory() |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1526 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1527 #For each point |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1528 for i in xrange(int(self.length())): |
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1529 result = getSYfromXY(self.getPositionAt(i), alignments) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1530 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1531 # Error handling |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1532 if(result is None): |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1533 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i))) |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1534 else: |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1535 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1536 self.curvilinearPositions.addPositionSYL(S, Y, align) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1537 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1538 ## Go back through points and correct lane |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1539 #Run through objects looking for outlier point |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1540 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1541 ## Recalculate projected point to new lane |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1542 lanes = self.curvilinearPositions.getLanes() |
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1543 if(lanes != smoothed_lanes): |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1544 for i in xrange(len(lanes)): |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1545 if(lanes[i] != smoothed_lanes[i]): |
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1546 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]]) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1547 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1548 # Error handling |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1549 if(result is None): |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1550 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1551 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i))) |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1552 else: |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1553 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1554 self.curvilinearPositions.setPosition(i, S, Y, align) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1555 |
562
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1556 def computeSmoothTrajectory(self, minCommonIntervalLength): |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1557 '''Computes the trajectory as the mean of all features |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1558 if a feature exists, its position is |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1559 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1560 Warning work in progress |
562
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1561 TODO? not use the first/last 1-.. positions''' |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1562 nFeatures = len(self.features) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1563 if nFeatures == 0: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1564 print('Empty object features\nCannot compute smooth trajectory') |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1565 else: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1566 # compute the relative position vectors |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1567 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1568 for i in xrange(nFeatures): |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1569 for j in xrange(i): |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1570 fi = self.features[i] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1571 fj = self.features[j] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1572 inter = fi.commonTimeInterval(fj) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1573 if inter.length() >= minCommonIntervalLength: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1574 xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)]) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1575 yi = array(fi.getYCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)]) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1576 xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1577 yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1578 relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi)) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1579 relativePositions[(j,i)] = -relativePositions[(i,j)] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1580 |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1581 ### |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1582 # User Type Classification |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1583 ### |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1584 def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, nInstantsIgnoredAtEnds = 0): |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1585 '''Classifies slow and fast road users |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1586 slow: non-motorized -> pedestrians |
607 | 1587 fast: motorized -> cars |
608
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
1588 |
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
1589 aggregationFunc can be any function that can be applied to a vector of speeds, including percentile: |
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
1590 aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile''' |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1591 speeds = self.getSpeeds(nInstantsIgnoredAtEnds) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1592 if aggregationFunc(speeds) >= threshold: |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1593 self.setUserType(userType2Num['car']) |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1594 else: |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1595 self.setUserType(userType2Num['pedestrian']) |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1596 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1597 def classifyUserTypeSpeed(self, speedProbabilities, aggregationFunc = median, nInstantsIgnoredAtEnds = 0): |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1598 '''Classifies road user per road user type |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1599 speedProbabilities are functions return P(speed|class) |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1600 in a dictionary indexed by user type names |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1601 Returns probabilities for each class |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1602 |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1603 for simple threshold classification, simply pass non-overlapping indicator functions (membership) |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1604 e.g. def indic(x): |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1605 if abs(x-mu) < sigma: |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1606 return 1 |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1607 else: |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1608 return x''' |
626
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
1609 if not hasattr(self, 'aggregatedSpeed'): |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1610 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds)) |
581
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
1611 userTypeProbabilities = {} |
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
1612 for userTypename in speedProbabilities: |
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
1613 userTypeProbabilities[userType2Num[userTypename]] = speedProbabilities[userTypename](self.aggregatedSpeed) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1614 self.setUserType(utils.argmaxDict(userTypeProbabilities)) |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1615 return userTypeProbabilities |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1616 |
928
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
1617 def initClassifyUserTypeHoGSVM(self, aggregationFunc, pedBikeCarSVM, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), nInstantsIgnoredAtEnds = 0, homography = None, intrinsicCameraMatrix = None, distortionCoefficients = None): |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1618 '''Initializes the data structures for classification |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1619 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1620 TODO? compute speed for longest feature?''' |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1621 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds)) |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1622 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM is None: |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1623 self.appearanceClassifier = pedBikeCarSVM |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1624 elif self.aggregatedSpeed < bikeCarSpeedThreshold: |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1625 self.appearanceClassifier = bikeCarSVM |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1626 else: |
959
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1627 self.appearanceClassifier = carClassifier |
928
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
1628 # project feature positions |
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
1629 if self.hasFeatures(): |
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
1630 for f in self.getFeatures(): |
935
0e63a918a1ca
updated classify-objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
933
diff
changeset
|
1631 pp = cvutils.worldToImageProject(f.getPositions().asArray(), intrinsicCameraMatrix, distortionCoefficients, homography).tolist() |
929
be28a3538dc9
work in progress on projection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
928
diff
changeset
|
1632 f.positions = Trajectory(pp) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1633 self.userTypes = {} |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1634 |
929
be28a3538dc9
work in progress on projection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
928
diff
changeset
|
1635 def classifyUserTypeHoGSVMAtInstant(self, img, instant, width, height, px, py, minNPixels, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm): |
812
21f10332c72b
moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
785
diff
changeset
|
1636 '''Extracts the image box around the object |
21f10332c72b
moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
785
diff
changeset
|
1637 (of square size max(width, height) of the box around the features, |
21f10332c72b
moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
785
diff
changeset
|
1638 with an added px or py for width and height (around the box)) |
21f10332c72b
moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
785
diff
changeset
|
1639 computes HOG on this cropped image (with parameters rescaleSize, orientations, pixelsPerCell, cellsPerBlock) |
21f10332c72b
moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
785
diff
changeset
|
1640 and applies the SVM model on it''' |
929
be28a3538dc9
work in progress on projection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
928
diff
changeset
|
1641 croppedImg = cvutils.imageBox(img, self, instant, width, height, px, py, minNPixels) |
685
94b291a5f933
several updates for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
682
diff
changeset
|
1642 if croppedImg is not None and len(croppedImg) > 0: |
959
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1643 hog = cvutils.HOG(croppedImg, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm) |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1644 self.userTypes[instant] = int(self.appearanceClassifier.predict(hog)) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1645 else: |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1646 self.userTypes[instant] = userType2Num['unknown'] |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1647 |
898
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1648 def classifyUserTypeHoGSVM(self, pedBikeCarSVM = None, width = 0, height = 0, homography = None, images = None, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), minSpeedEquiprobable = -1, speedProbabilities = None, aggregationFunc = median, maxPercentUnknown = 0.5, nInstantsIgnoredAtEnds = 0, px = 0.2, py = 0.2, minNPixels = 800, rescaleSize = (64, 64), orientations = 9, pixelsPerCell = (8,8), cellsPerBlock = (2,2)): |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1649 '''Agregates SVM detections in each image and returns probability |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1650 (proportion of instants with classification in each category) |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1651 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1652 images is a dictionary of images indexed by instant |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1653 With default parameters, the general (ped-bike-car) classifier will be used |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1654 |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1655 Considered categories are the keys of speedProbabilities''' |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1656 if not hasattr(self, 'aggregatedSpeed') or not hasattr(self, 'userTypes'): |
845 | 1657 print('Initializing the data structures for classification by HoG-SVM') |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1658 self.initClassifyUserTypeHoGSVM(aggregationFunc, pedBikeCarSVM, bikeCarSVM, pedBikeSpeedTreshold, bikeCarSpeedThreshold, nInstantsIgnoredAtEnds) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1659 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1660 if len(self.userTypes) != self.length() and images is not None: # if classification has not been done previously |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1661 for t in self.getTimeInterval(): |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1662 if t not in self.userTypes: |
812
21f10332c72b
moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
785
diff
changeset
|
1663 self.classifyUserTypeHoGSVMAtInstant(images[t], t, homography, width, height, px, py, minNPixels, rescaleSize, orientations, pixelsPerCell, cellsPerBlock) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1664 # compute P(Speed|Class) |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1665 if speedProbabilities is None or self.aggregatedSpeed < minSpeedEquiprobable: # equiprobable information from speed |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1666 userTypeProbabilities = {userType2Num['car']: 1., userType2Num['pedestrian']: 1., userType2Num['bicycle']: 1.} |
520
fd9641cbd24b
added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
517
diff
changeset
|
1667 else: |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1668 userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities} |
898
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1669 # compute P(Class|Appearance) |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1670 nInstantsUserType = {userTypeNum: 0 for userTypeNum in userTypeProbabilities}# number of instants the object is classified as userTypename |
898
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1671 nInstantsUserType[userType2Num['unknown']] = 0 |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1672 for t in self.userTypes: |
898
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1673 nInstantsUserType[self.userTypes[t]] += 1 #nInstantsUserType.get(self.userTypes[t], 0) + 1 |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1674 # result is P(Class|Appearance) x P(Speed|Class) |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1675 if nInstantsUserType[userType2Num['unknown']] < maxPercentUnknown*self.length(): # if not too many unknowns |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1676 for userTypeNum in userTypeProbabilities: |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1677 userTypeProbabilities[userTypeNum] *= nInstantsUserType[userTypeNum] |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1678 # class is the user type that maximizes usertype probabilities |
898
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1679 if nInstantsUserType[userType2Num['unknown']] >= maxPercentUnknown*self.length() and (speedProbabilities is None or self.aggregatedSpeed < minSpeedEquiprobable): # if no speed information and too many unknowns |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1680 self.setUserType(userType2Num['unknown']) |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1681 else: |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
1682 self.setUserType(utils.argmaxDict(userTypeProbabilities)) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1683 |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1684 def classifyUserTypeArea(self, areas, homography): |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1685 '''Classifies the object based on its location (projected to image space) |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1686 areas is a dictionary of matrix of the size of the image space |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1687 for different road users possible locations, indexed by road user type names |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1688 |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1689 TODO: areas could be a wrapper object with a contains method that would work for polygons and images (with wrapper class) |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1690 skip frames at beginning/end?''' |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1691 print('not implemented/tested yet') |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1692 if not hasattr(self, projectedPositions): |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1693 if homography is not None: |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
1694 self.projectedPositions = obj.positions.homographyProject(homography) |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1695 else: |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1696 self.projectedPositions = obj.positions |
523
ce4eaabacc26
modified internal implementation of user type for classifyUserTypeArea
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
522
diff
changeset
|
1697 possibleUserTypes = {userType: 0 for userType in range(len(userTypenames))} |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1698 for p in self.projectedPositions: |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1699 for userTypename in areas: |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1700 if areas[userTypename][p.x, p.y] != 0: |
523
ce4eaabacc26
modified internal implementation of user type for classifyUserTypeArea
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
522
diff
changeset
|
1701 possibleUserTypes[userType2Enum[userTypename]] += 1 |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1702 # what to do: threshold for most common type? self.setUserType() |
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1703 return possibleUserTypes |
520
fd9641cbd24b
added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
517
diff
changeset
|
1704 |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1705 @staticmethod |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1706 def collisionCourseDotProduct(movingObject1, movingObject2, instant): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1707 'A positive result indicates that the road users are getting closer' |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1708 deltap = movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant) |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1709 deltav = movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant) |
148
ad21db62b785
bug correction for cosine functions in python
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
115
diff
changeset
|
1710 return Point.dot(deltap, deltav) |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1711 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1712 @staticmethod |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1713 def collisionCourseCosine(movingObject1, movingObject2, instant): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1714 'A positive result indicates that the road users are getting closer' |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1715 return Point.cosine(movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant), #deltap |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1716 movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)) #deltav |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1717 |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1718 |
921
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1719 class Prototype(object): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1720 'Class for a prototype' |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1721 |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1722 def __init__(self, filename, num, trajectoryType, nMatchings = None): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1723 self.filename = filename |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1724 self.num = num |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1725 self.trajectoryType = trajectoryType |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1726 self.nMatchings = nMatchings |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1727 self.movingObject = None |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1728 |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1729 def getFilename(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1730 return self.filename |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1731 def getNum(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1732 return self.num |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1733 def getTrajectoryType(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1734 return self.trajectoryType |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1735 def getNMatchings(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1736 return self.nMatchings |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1737 def getMovingObject(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1738 return self.movingObject |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1739 def setMovingObject(self, o): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1740 self.movingObject = o |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1741 |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
1742 |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1743 ################## |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1744 # Annotations |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1745 ################## |
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1746 |
768
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1747 class BBMovingObject(MovingObject): |
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1748 '''Class for a moving object represented as a bounding box |
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1749 used for series of ground truth annotations using bounding boxes |
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1750 and for the output of Urban Tracker http://www.jpjodoin.com/urbantracker/ |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1751 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1752 By default in image space |
768
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1753 |
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1754 Its center is the center of the box (generalize to other shapes?) |
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1755 (computed after projecting if homography available) |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1756 ''' |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1757 |
589
5800a87f11ae
corrected one bug and changed attribute names
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
588
diff
changeset
|
1758 def __init__(self, num = None, timeInterval = None, topLeftPositions = None, bottomRightPositions = None, userType = userType2Num['unknown']): |
768
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1759 super(BBMovingObject, self).__init__(num, timeInterval, userType = userType) |
589
5800a87f11ae
corrected one bug and changed attribute names
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
588
diff
changeset
|
1760 self.topLeftPositions = topLeftPositions.getPositions() |
5800a87f11ae
corrected one bug and changed attribute names
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
588
diff
changeset
|
1761 self.bottomRightPositions = bottomRightPositions.getPositions() |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1762 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1763 def computeCentroidTrajectory(self, homography = None): |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
1764 self.positions = self.topLeftPositions.add(self.bottomRightPositions).__mul__(0.5) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1765 if homography is not None: |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
1766 self.positions = self.positions.homographyProject(homography) |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1767 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1768 def matches(self, obj, instant, matchingDistance): |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1769 '''Indicates if the annotation matches obj (MovingObject) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1770 with threshold matchingDistance |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1771 Returns distance if below matchingDistance, matchingDistance+1 otherwise |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1772 (returns an actual value, otherwise munkres does not terminate)''' |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1773 d = Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant)) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1774 if d < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1775 return d |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1776 else: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1777 return matchingDistance + 1 |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1778 |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1779 def computeClearMOT(annotations, objects, matchingDistance, firstInstant, lastInstant, returnMatches = False, debug = False): |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1780 '''Computes the CLEAR MOT metrics |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1781 |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1782 Reference: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1783 Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008) |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1784 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1785 objects and annotations are supposed to in the same space |
768
f8e0a8ea8402
updated the bounding box code (the name so that it is general for ground truth and UT)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
762
diff
changeset
|
1786 current implementation is BBMovingObject (bounding boxes) |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1787 mathingDistance is threshold on matching between annotation and object |
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1788 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1789 TO: tracker output (objects) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1790 GT: ground truth (annotations) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1791 |
624
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1792 Output: returns motp, mota, mt, mme, fpt, gt |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1793 mt number of missed GT.frames (sum of the number of GT not detected in each frame) |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1794 mme number of mismatches |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1795 fpt number of false alarm.frames (tracker objects without match in each frame) |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1796 gt number of GT.frames |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1797 |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1798 if returnMatches is True, return as 2 new arguments the GT and TO matches |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1799 matches is a dict |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1800 matches[i] is the list of matches for GT/TO i |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1801 the list of matches is a dict, indexed by time, for the TO/GT id matched at time t |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1802 (an instant t not present in matches[i] at which GT/TO exists means a missed detection or false alarm) |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1803 |
624
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1804 TODO: Should we use the distance as weights or just 1/0 if distance below matchingDistance? |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1805 (add argument useDistanceForWeights = False)''' |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1806 from munkres import Munkres |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1807 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1808 munk = Munkres() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1809 dist = 0. # total distance between GT and TO |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1810 ct = 0 # number of associations between GT and tracker output in each frame |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1811 gt = 0 # number of GT.frames |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1812 mt = 0 # number of missed GT.frames (sum of the number of GT not detected in each frame) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1813 fpt = 0 # number of false alarm.frames (tracker objects without match in each frame) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1814 mme = 0 # number of mismatches |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1815 matches = {} # match[i] is the tracker track associated with GT i (using object references) |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1816 if returnMatches: |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1817 gtMatches = {a.getNum():{} for a in annotations} |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1818 toMatches = {o.getNum():{} for o in objects} |
956
196a1fd498ba
removing unnecessary complexity of varying number of return elements
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
949
diff
changeset
|
1819 else: |
196a1fd498ba
removing unnecessary complexity of varying number of return elements
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
949
diff
changeset
|
1820 gtMatches = None |
196a1fd498ba
removing unnecessary complexity of varying number of return elements
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
949
diff
changeset
|
1821 toMatches = None |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1822 for t in xrange(firstInstant, lastInstant+1): |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1823 previousMatches = matches.copy() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1824 # go through currently matched GT-TO and check if they are still matched withing matchingDistance |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1825 toDelete = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1826 for a in matches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1827 if a.existsAtInstant(t) and matches[a].existsAtInstant(t): |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1828 d = a.matches(matches[a], t, matchingDistance) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1829 if d < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1830 dist += d |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1831 else: |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1832 toDelete.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1833 else: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1834 toDelete.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1835 for a in toDelete: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1836 del matches[a] |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
1837 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1838 # match all unmatched GT-TO |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1839 matchedGTs = matches.keys() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1840 matchedTOs = matches.values() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1841 costs = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1842 unmatchedGTs = [a for a in annotations if a.existsAtInstant(t) and a not in matchedGTs] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1843 unmatchedTOs = [o for o in objects if o.existsAtInstant(t) and o not in matchedTOs] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1844 nGTs = len(matchedGTs)+len(unmatchedGTs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1845 nTOs = len(matchedTOs)+len(unmatchedTOs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1846 if len(unmatchedTOs) > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1847 for a in unmatchedGTs: |
595
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1848 costs.append([a.matches(o, t, matchingDistance) for o in unmatchedTOs]) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1849 if len(costs) > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1850 newMatches = munk.compute(costs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1851 for k,v in newMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1852 if costs[k][v] < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1853 matches[unmatchedGTs[k]]=unmatchedTOs[v] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1854 dist += costs[k][v] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1855 if debug: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1856 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.iteritems()])) |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1857 if returnMatches: |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1858 for a,o in matches.iteritems(): |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1859 gtMatches[a.getNum()][t] = o.getNum() |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
1860 toMatches[o.getNum()][t] = a.getNum() |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1861 |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1862 # compute metrics elements |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1863 ct += len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1864 mt += nGTs-len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1865 fpt += nTOs-len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1866 gt += nGTs |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1867 # compute mismatches |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1868 # for gt that do not appear in both frames, check if the corresponding to was matched to another gt in previous/next frame |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1869 mismatches = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1870 for a in matches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1871 if a in previousMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1872 if matches[a] != previousMatches[a]: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1873 mismatches.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1874 elif matches[a] in previousMatches.values(): |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1875 mismatches.append(matches[a]) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1876 for a in previousMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1877 if a not in matches and previousMatches[a] in matches.values(): |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1878 mismatches.append(previousMatches[a]) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1879 if debug: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1880 for mm in set(mismatches): |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
1881 print('{} {}'.format(type(mm), mm.getNum())) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1882 # some object mismatches may appear twice |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1883 mme += len(set(mismatches)) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1884 |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1885 if ct > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1886 motp = dist/ct |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
1887 else: |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1888 motp = None |
595
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1889 if gt > 0: |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1890 mota = 1.-float(mt+fpt+mme)/gt |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1891 else: |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1892 mota = None |
956
196a1fd498ba
removing unnecessary complexity of varying number of return elements
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
949
diff
changeset
|
1893 return motp, mota, mt, mme, fpt, gt, gtMatches, toMatches |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
1894 |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1895 def plotRoadUsers(objects, colors): |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1896 '''Colors is a PlottingPropertyValues instance''' |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1897 from matplotlib.pyplot import figure, axis |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1898 figure() |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1899 for obj in objects: |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
1900 obj.plot(colors.get(obj.userType)) |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1901 axis('equal') |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1902 |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1903 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1904 if __name__ == "__main__": |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1905 import doctest |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1906 import unittest |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1907 suite = doctest.DocFileSuite('tests/moving.txt') |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1908 #suite = doctest.DocTestSuite() |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1909 unittest.TextTestRunner().run(suite) |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1910 #doctest.testmod() |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1911 #doctest.testfile("example.txt") |
635
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1912 if shapelyAvailable: |
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1913 suite = doctest.DocFileSuite('tests/moving_shapely.txt') |
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1914 unittest.TextTestRunner().run(suite) |