Mercurial Hosting > traffic-intelligence
annotate trafficintelligence/moving.py @ 1240:bb14f919d1cb
cleaned use of centile (np only) and added info in classify-objects
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 05 Feb 2024 14:14:14 -0500 |
parents | d5695e0b59d9 |
children | ab4c72b9475c |
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 |
1029
c6cf75a2ed08
reorganization of imports
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1028
diff
changeset
|
4 import copy |
c6cf75a2ed08
reorganization of imports
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1028
diff
changeset
|
5 from math import sqrt, atan2, cos, sin |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
6 |
1240
bb14f919d1cb
cleaned use of centile (np only) and added info in classify-objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1233
diff
changeset
|
7 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum, issubdtype, integer as npinteger, percentile |
1210
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
8 from matplotlib.pyplot import plot, text, arrow |
672
5473b7460375
moved and rationalized imports in modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
666
diff
changeset
|
9 from scipy.spatial.distance import cdist |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
10 from scipy.signal import savgol_filter |
1029
c6cf75a2ed08
reorganization of imports
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1028
diff
changeset
|
11 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
12 try: |
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
13 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
|
14 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
|
15 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
|
16 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
|
17 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
|
18 shapelyAvailable = False |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
19 |
1030
aafbc0bab925
moved method around to avoid cross-dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1029
diff
changeset
|
20 from trafficintelligence import utils, cvutils |
aafbc0bab925
moved method around to avoid cross-dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1029
diff
changeset
|
21 from trafficintelligence.base import VideoFilenameAddable |
aafbc0bab925
moved method around to avoid cross-dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1029
diff
changeset
|
22 |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
23 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
24 class Interval(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
25 '''Generic interval: a subset of real numbers (not iterable)''' |
26 | 26 def __init__(self, first=0, last=-1, revert = False): |
27 if revert and last<first: | |
28 self.first=last | |
29 self.last=first | |
30 else: | |
31 self.first=first | |
32 self.last=last | |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
33 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
34 def __str__(self): |
1064
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
35 return '{0}-{1}'.format(self.first, self.last) |
104
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
36 |
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
37 def __repr__(self): |
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
38 return self.__str__() |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
39 |
776
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
40 def __eq__(self, other): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
41 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
|
42 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
43 def empty(self): |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
44 return self.first > self.last |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
45 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
46 def center(self): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
47 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
|
48 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
49 def length(self): |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
50 '''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
|
51 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
|
52 |
96
9928c2fa72cc
added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
93
diff
changeset
|
53 def equal(self, i2): |
9928c2fa72cc
added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
93
diff
changeset
|
54 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
|
55 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
56 def getList(self): |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
57 return [self.first, self.last] |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
58 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
59 def contains(self, instant): |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
60 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
|
61 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
62 def inside(self, interval2): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
63 '''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
|
64 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
|
65 |
785
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
66 def shift(self, offset): |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
67 self.first += offset |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
68 self.last += offset |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
69 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
70 @classmethod |
1064
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
71 def parse(cls, s): |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
72 if '-' in s: |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
73 tmp = s.split('-') |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
74 if len(tmp) == 2: |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
75 return cls(int(tmp[0]), int(tmp[1])) # TODO with floats? |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
76 print(s+' is not a valid representation of an interval') |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
77 return None |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
78 |
cbc026dacf0b
changed interval string representation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1044
diff
changeset
|
79 @classmethod |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
80 def union(cls, interval1, interval2): |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
81 '''Smallest interval comprising self and interval2''' |
1076 | 82 return cls(min(interval1.first, interval2.first), max(interval1.last, interval2.last)) |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
83 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
84 @classmethod |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
85 def intersection(cls, interval1, interval2): |
104
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
86 '''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
|
87 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
|
88 |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
89 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
|
90 if not Interval.intersection(self, interval2).empty(): |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
91 return 0 |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
92 elif self.first > interval2.last: |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
93 return self.first - interval2.last |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
94 elif self.last < interval2.first: |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
95 return interval2.first - self.last |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
96 else: |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
97 return None |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
98 |
727
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
99 @classmethod |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
100 def unionIntervals(cls, intervals): |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
101 '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
|
102 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
|
103 for i in intervals[1:]: |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
104 inter = cls.union(inter, i) |
c6d4ea05a2d0
adding ability to deal with multivariate indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
726
diff
changeset
|
105 return inter |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
106 |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
107 |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
108 class TimeInterval(Interval): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
109 '''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
|
110 |
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
111 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
|
112 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
|
113 It also (mostly) works with datetime.datetime''' |
26 | 114 |
828
14e4ad7c7420
work on merging data for synchronized views
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
812
diff
changeset
|
115 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
|
116 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
|
117 |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
118 @staticmethod |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
119 def fromInterval(inter): |
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
120 return TimeInterval(inter.first, inter.last) |
26 | 121 |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
122 def __getitem__(self, i): |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
123 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
|
124 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
|
125 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
|
126 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
127 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
|
128 #elif isinstance( key, slice ): |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
129 |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
130 def __iter__(self): |
107
916678481896
corrected bug for TimeInterval interation and added corresponding test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
105
diff
changeset
|
131 self.iterInstantNum = -1 |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
132 return self |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
133 |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
134 def __next__(self): |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
135 if self.iterInstantNum >= self.length()-1: |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
136 raise StopIteration |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
137 else: |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
138 self.iterInstantNum += 1 |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
139 return self[self.iterInstantNum] |
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
140 |
26 | 141 def length(self): |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
142 '''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
|
143 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
|
144 |
688
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
145 def __len__(self): |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
146 return self.length() |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
147 |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
148 # class BoundingPolygon: |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
149 # '''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
|
150 # with methods to create intersection, unions... |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
151 # ''' |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
152 # We will use the polygon class of Shapely |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
153 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
154 class STObject(object): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
155 '''Class for spatio-temporal object, i.e. with temporal and spatial existence |
6 | 156 (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
|
157 |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
158 It may not mean that the object is defined |
6 | 159 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
|
160 |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
161 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
|
162 self.num = num |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
163 self.timeInterval = timeInterval |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
164 self.boundingPolygon = boundingPolygon |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
165 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
166 def empty(self): |
688
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
167 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
|
168 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
169 def getNum(self): |
211 | 170 return self.num |
171 | |
688
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
172 def __len__(self): |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
173 return self.timeInterval.length() |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
174 |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
175 def length(self): |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
176 return self.timeInterval.length() |
f2b52355a286
made length a STObject method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
685
diff
changeset
|
177 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
178 def getFirstInstant(self): |
40
9f16aee24b7e
corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents:
39
diff
changeset
|
179 return self.timeInterval.first |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
180 |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
181 def getLastInstant(self): |
40
9f16aee24b7e
corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents:
39
diff
changeset
|
182 return self.timeInterval.last |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
183 |
1001
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
184 def setFirstInstant(self, t): |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
185 if t <= self.timeInterval.last: |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
186 self.timeInterval.first = t |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
187 else: |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
188 print('new first instant is after last, not changing') |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
189 |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
190 def setLastInstant(self, t): |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
191 if t >= self.timeInterval.first: |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
192 self.timeInterval.last = t |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
193 else: |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
194 print('new last instant is before first, not changing') |
cc7c6b821ae6
added methods for setting instants
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
998
diff
changeset
|
195 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
196 def getTimeInterval(self): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
197 return self.timeInterval |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
198 |
108
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
199 def existsAtInstant(self, t): |
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
200 return self.timeInterval.contains(t) |
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
201 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
202 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
|
203 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval()) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
204 |
785
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
205 def shiftTimeInterval(self, offset): |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
206 self.timeInterval.shift(offset) |
3aa6102ccc12
addressed issues with ground truth annotations shifted in time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
781
diff
changeset
|
207 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
208 class Point(object): |
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
209 def __init__(self, x, y): |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
210 self.x = x |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
211 self.y = y |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
212 |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
213 def __str__(self): |
675
ab3fdff42624
corrected old format for Point.__str__
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
674
diff
changeset
|
214 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
|
215 |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
216 def __repr__(self): |
98
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
217 return self.__str__() |
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
218 |
776
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
219 def __eq__(self, other): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
220 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
|
221 |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
222 def __add__(self, other): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
223 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
|
224 |
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
225 def __sub__(self, other): |
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
226 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
|
227 |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
228 def __neg__(self): |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
229 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
|
230 |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
231 def __mul__(self, alpha): |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
232 'Warning, returns a new Point' |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
233 return Point(self.x*alpha, self.y*alpha) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
234 |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
235 def divide(self, alpha): |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
236 'Warning, returns a new Point' |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
237 return Point(self.x/alpha, self.y/alpha) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
238 |
575
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
239 def __getitem__(self, i): |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
240 if i == 0: |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
241 return self.x |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
242 elif i == 1: |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
243 return self.y |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
244 else: |
13df64a9ff9d
added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
574
diff
changeset
|
245 raise IndexError() |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
246 |
690 | 247 def orthogonal(self, clockwise = True): |
248 'Returns the orthogonal vector' | |
249 if clockwise: | |
250 return Point(self.y, -self.x) | |
251 else: | |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
252 return Point(-self.y, self.x) |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
253 |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
254 def normalize(self): |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
255 return self.divide(self.norm2()) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
256 |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
257 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
|
258 'Projects point projected on v, v.orthogonal()' |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
259 e1 = v.normalize() |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
260 e2 = e1.orthogonal(clockwise) |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
261 return Point(Point.dot(self, e1), Point.dot(self, e2)) |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
262 |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
263 def rotate(self, theta): |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
264 return Point(self.x*cos(theta)-self.y*sin(theta), self.x*sin(theta)+self.y*cos(theta)) |
1109
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
265 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
266 def plot(self, options = 'o', **kwargs): |
211 | 267 plot([self.x], [self.y], options, **kwargs) |
41 | 268 |
771 | 269 @staticmethod |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
270 def plotSegment(p1, p2, options = 'o', withOrigin = True, **kwargs): |
771 | 271 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs) |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
272 p1.plot('or', **kwargs) |
771 | 273 |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
274 def angle(self): |
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
275 return atan2(self.y, self.x) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
276 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
277 def norm2Squared(self): |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
278 '''2-norm distance (Euclidean distance)''' |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
279 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
|
280 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
281 def norm2(self): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
282 '''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
|
283 return sqrt(self.norm2Squared()) |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
284 |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
285 def norm1(self): |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
286 return abs(self.x)+abs(self.y) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
287 |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
288 def normMax(self): |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
289 return max(abs(self.x),abs(self.y)) |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
290 |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
291 def aslist(self): |
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
292 return [self.x, self.y] |
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
293 |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
294 def astuple(self): |
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
295 return (self.x, self.y) |
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
296 |
223
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
297 def asint(self): |
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
298 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
|
299 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
300 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
|
301 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
|
302 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
|
303 |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
304 def homographyProject(self, homography): |
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
305 projected = cvutils.homographyProject(array([[self.x], [self.y]]), homography) |
98
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
306 return Point(projected[0], projected[1]) |
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
307 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
308 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
|
309 '''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
|
310 (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
|
311 |
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
|
312 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
|
313 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
314 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
|
315 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
316 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
|
317 counter = 0; |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
318 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
319 p1 = polygon[0,:]; |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
320 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
|
321 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
|
322 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
|
323 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
|
324 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
|
325 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
|
326 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
|
327 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
|
328 counter+=1; |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
329 p1=p2 |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
330 return (counter%2 == 1); |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
331 |
49 | 332 @staticmethod |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
333 def fromList(p): |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
334 return Point(p[0], p[1]) |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
335 |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
336 @staticmethod |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
337 def dot(p1, p2): |
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
338 'Scalar product' |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
339 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
|
340 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
341 @staticmethod |
90
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
342 def cross(p1, p2): |
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
343 'Cross product' |
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
344 return p1.x*p2.y-p1.y*p2.x |
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
345 |
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
346 @staticmethod |
987
f026ce2af637
found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
982
diff
changeset
|
347 def parallel(p1, p2): |
f026ce2af637
found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
982
diff
changeset
|
348 return Point.cross(p1, p2) == 0. |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
349 |
987
f026ce2af637
found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
982
diff
changeset
|
350 @staticmethod |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
351 def cosine(p1, p2): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
352 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
|
353 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
354 @staticmethod |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
355 def distanceNorm2(p1, p2): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
356 return (p1-p2).norm2() |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
357 |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
358 @staticmethod |
1205
3905b393ade0
kitti loading code seems to be working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1203
diff
changeset
|
359 def plotAll(points, closePolygon = False, options = '', **kwargs): |
1203
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
360 xCoords = [p.x for p in points] |
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
361 yCoords = [p.y for p in points] |
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
362 if closePolygon: |
1205
3905b393ade0
kitti loading code seems to be working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1203
diff
changeset
|
363 xCoords.append(xCoords[0]) |
3905b393ade0
kitti loading code seems to be working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1203
diff
changeset
|
364 yCoords.append(yCoords[0]) |
1203
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
365 plot(xCoords, yCoords, options, **kwargs) |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
366 |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
367 def similarOrientation(self, refDirection, cosineThreshold): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
368 '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
|
369 return Point.cosine(self, refDirection) >= cosineThreshold |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
370 |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
371 @staticmethod |
484
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
372 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
|
373 '''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
|
374 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
|
375 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
|
376 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
|
377 dv = v1-v2 |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
378 dp = p1-p2 |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
379 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
|
380 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
|
381 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
|
382 |
504
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
383 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
|
384 if delta >= 0: |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
385 deltaRoot = sqrt(delta) |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
386 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
|
387 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
|
388 if ttc1 >= 0 and ttc2 >= 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
389 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
|
390 elif ttc1 >= 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
391 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
|
392 elif ttc2 >= 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
393 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
|
394 else: # ttc1 < 0 and ttc2 < 0: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
395 return None |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
396 else: |
881
8ba82b371eea
work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
880
diff
changeset
|
397 return None |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
398 |
1094
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
399 @staticmethod |
1214
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
400 def timeToCollisionPoly(corners1, v1, corners2, v2): |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
401 """ |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
402 :param bbox_1: list: [bbox_1 array 4*2 , v_1_x (float), v_1_y (float)] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
403 :param bbox_2: [bbox_2_x array 4*2, v_2_x (float), v_2_y (float)] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
404 :return: ttc |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
405 """ |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
406 from sympy import solve |
1214
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
407 from sympy.abc import t |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
408 def NewFourPoints(bbox, col_time, v_x, v_y): |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
409 return [i + col_time * v_x for i in bbox[0]], [j + col_time * v_y for j in bbox[1]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
410 |
1214
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
411 v_1_x, v_1_y = v1.x, v1.y |
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
412 v_2_x, v_2_y = v2.x, v2.y |
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
413 x_bbox_1 = [p.x for p in corners1] |
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
414 y_bbox_1 = [p.y for p in corners1] |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
415 bbox_1 = [x_bbox_1, y_bbox_1] |
1214
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
416 x_bbox_2 = [p.x for p in corners2] |
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
417 y_bbox_2 = [p.y for p in corners2] |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
418 bbox_2 = [x_bbox_2, y_bbox_2] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
419 t_total = [] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
420 line = [[0, 1], [1, 2], [2, 3], [3, 0]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
421 for i in range(4): |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
422 for j in range(4): |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
423 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
424 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
425 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] |
1217
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
426 p0_x_t = p0_x + v_2_x * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
427 p0_y_t = p0_y + v_2_y * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
428 p1_x_t = p1_x + v_1_x * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
429 p1_y_t = p1_y + v_1_y * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
430 p2_x_t = p2_x + v_1_x * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
431 p2_y_t = p2_y + v_1_y * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
432 f = (p2_x_t - p1_x_t) * (p0_y_t - p1_y_t) - (p0_x_t - p1_x_t) * (p2_y_t - p1_y_t) |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
433 collision_t = solve(f, t, set=True, dict=True) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
434 if collision_t and collision_t[0][t] >= 0: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
435 collision_t = collision_t[0][t] |
1217
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
436 #print(f'collision_t: {collision_t} >>>>>>>>>') |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
437 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t, v_1_x, v_1_y) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
438 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t, v_2_x, v_2_y) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
439 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
440 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
441 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
442 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
443 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
444 if min_x == max_x: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
445 if min_y <= p0_y <= max_y: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
446 t_total.append(collision_t) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
447 elif min_y == max_y: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
448 if min_x <= p0_x <= max_x: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
449 t_total.append(collision_t) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
450 else: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
451 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
452 t_total.append(collision_t) |
1217
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
453 |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
454 x_temps = x_bbox_2 |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
455 x_bbox_2 = x_bbox_1 |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
456 x_bbox_1 = x_temps |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
457 # print(f'Change bounding box1 and box2 >>>>>>>>>>>>>>>>>>>\n\n\n') |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
458 for i in range(4): |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
459 for j in range(4): |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
460 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
461 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
462 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] |
1217
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
463 p0_x_t = p0_x + v_2_x * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
464 p0_y_t = p0_y + v_2_y * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
465 p1_x_t = p1_x + v_1_x * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
466 p1_y_t = p1_y + v_1_y * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
467 p2_x_t = p2_x + v_1_x * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
468 p2_y_t = p2_y + v_1_y * t |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
469 f = (p2_x_t - p1_x_t) * (p0_y_t - p1_y_t) - (p0_x_t - p1_x_t) * (p2_y_t - p1_y_t) |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
470 collision_t = solve(f, t, set=True, dict=True) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
471 if collision_t and collision_t[0][t] >= 0: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
472 collision_t = collision_t[0][t] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
473 # print(f'collision_t: {collision_t} >>>>>>>>>') |
1217
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
474 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t, v_1_x, v_1_y) |
5038c357b57f
updating code for direct computation (very slow solver)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1214
diff
changeset
|
475 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t, v_2_x, v_2_y) |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
476 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
477 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
478 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
479 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
480 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
481 if min_x == max_x: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
482 if min_y <= p0_y <= max_y: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
483 t_total.append(collision_t) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
484 |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
485 elif min_y == max_y: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
486 if min_x <= p0_x <= max_x: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
487 t_total.append(collision_t) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
488 |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
489 else: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
490 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
491 t_total.append(collision_t) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
492 |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
493 if t_total: |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
494 collision_t_min = min(t_total) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
495 # print(f'collision_time:') |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
496 # print(collision_t_min) |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
497 else: |
1214
01c24c1cdb70
implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1213
diff
changeset
|
498 collision_t_min = None |
1213
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
499 # print(f'No collision') |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
500 return collision_t_min |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
501 |
3f2214125164
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1212
diff
changeset
|
502 @staticmethod |
583
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
503 def midPoint(p1, p2): |
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
504 '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
|
505 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
|
506 |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
507 @staticmethod |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
508 def agg(points, aggFunc = mean): |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
509 return Point(aggFunc([p.x for p in points]), aggFunc([p.y for p in points])) |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
510 |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
511 @staticmethod |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
512 def boundingRectangle(points, v): |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
513 '''Returns the bounding rectangle of the points, aligned on the vector v |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
514 A list of points is returned: front left, front right, rear right, rear left''' |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
515 e1 = v.normalize() |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
516 e2 = e1.orthogonal(False) |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
517 xCoords = [] |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
518 yCoords = [] |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
519 for p in points: |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
520 xCoords.append(Point.dot(e1, p)) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
521 yCoords.append(Point.dot(e2, p)) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
522 xmin = min(xCoords) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
523 xmax = max(xCoords) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
524 ymin = min(yCoords) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
525 ymax = max(yCoords) |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
526 frontLeft = Point(xmax, ymax) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
527 frontRight = Point(xmax, ymin) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
528 rearLeft = Point(xmin, ymax) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
529 rearRight = Point(xmin, ymin) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
530 originalE1 = Point(Point.dot(e1, Point(1,0)),Point.dot(e2, Point(1,0))) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
531 originalE2 = Point(Point.dot(e1, Point(0,1)),Point.dot(e2, Point(0,1))) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
532 return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]] |
1109
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
533 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
534 if shapelyAvailable: |
1210
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
535 def pointsToShapely(points): |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
536 'Returns shapely polygon' |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
537 return Polygon([p.astuple() for p in points]) |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
538 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
539 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
|
540 '''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
|
541 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
|
542 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
|
543 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
|
544 prepared_polygon = prep(polygon) |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
545 return list(filter(prepared_polygon.contains, points)) |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
546 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
547 # Functions for coordinate transformation |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
548 # From Paul St-Aubin's PVA tools |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
549 def prepareAlignments(alignments): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
550 '''Prepares alignments (list of splines, each typically represented as a Trajectory) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
551 - computes cumulative distances |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
552 - approximates slope singularity by giving some slope roundoff (account for roundoff error)''' |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
553 for alignment in alignments: |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
554 alignment.computeCumulativeDistances() |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
555 p1 = alignment[0] |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
556 for i in range(len(alignment)-1): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
557 p2 = alignment[i+1] |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
558 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
|
559 p2.x += 0.0000000001 |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
560 if(round(p1.y, 10) == round(p2.y, 10)): |
1094
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
561 p2.y += 0.0000000001 |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
562 p1 = p2 |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
563 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
564 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): |
1094
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
565 ''' Point-projection (Q) on line defined by 2 points (P0,P1). |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
566 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
|
567 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
568 if(p0x == p1x and p0y == p1y): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
569 return None |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
570 try: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
571 #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
|
572 # 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
|
573 # p1x += 0.0000000001 |
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
574 # if(round(p0y, 10) == round(p1y, 10)): |
1094
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
575 # p1y += 0.0000000001 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
576 #make the calculation |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
577 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
|
578 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
|
579 except ZeroDivisionError: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
580 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
|
581 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
582 import pdb; |
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
583 pdb.set_trace() |
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
584 return Point(X, Y) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
585 |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
586 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
587 ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments). |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
588 A alignment is a list of points (class Point), most likely a trajectory. |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
589 |
659
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
590 Output: |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
591 ======= |
1094
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
592 [alignment index, |
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
593 subsegment leading point index, |
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
594 snapped point, |
c96388c696ac
adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1086
diff
changeset
|
595 subsegment distance, |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
596 alignment distance, |
659
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
597 orthogonal point offset] |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
598 |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
599 or None |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
600 ''' |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
601 minOffsetY = float('inf') |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
602 #For each alignment |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
603 for alignmentIdx in range(len(alignments)): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
604 #For each alignment point index |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
605 for alignment_p in range(len(alignments[alignmentIdx])-1): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
606 #Get closest point on alignment |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
607 closestPoint = ppldb2p(p.x,p.y,alignments[alignmentIdx][alignment_p][0],alignments[alignmentIdx][alignment_p][1],alignments[alignmentIdx][alignment_p+1][0],alignments[alignmentIdx][alignment_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
|
608 if closestPoint is None: |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
609 print('Error: Alignment {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(alignmentIdx, alignment_p)) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
610 return None |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
611 # check if the projected point is in between the current segment of the alignment bounds |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
612 if utils.inBetween(alignments[alignmentIdx][alignment_p][0], alignments[alignmentIdx][alignment_p+1][0], closestPoint.x) and utils.inBetween(alignments[alignmentIdx][alignment_p][1], alignments[alignmentIdx][alignment_p+1][1], closestPoint.y): |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
613 offsetY = Point.distanceNorm2(closestPoint, p) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
614 if offsetY < minOffsetY: |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
615 minOffsetY = offsetY |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
616 snappedAlignmentIdx = alignmentIdx |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
617 snappedAlignmentLeadingPoint = alignment_p |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
618 snappedPoint = Point(closestPoint.x, closestPoint.y) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
619 #Jump loop if significantly close |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
620 if offsetY < goodEnoughAlignmentDistance: |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
621 break |
780
1b22d81ef5ff
cleaned and checked storage with functions for curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
776
diff
changeset
|
622 |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
623 #Get sub-segment distance |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
624 if minOffsetY != float('inf'): |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
625 subsegmentDistance = Point.distanceNorm2(snappedPoint, alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint]) |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
626 #Get cumulative alignment distance (total segment distance) |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
627 alignmentDistanceS = alignments[snappedAlignmentIdx].getCumulativeDistance(snappedAlignmentLeadingPoint) + subsegmentDistance |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
628 orthogonalAlignmentVector = (alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint+1]-alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint]).orthogonal() |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
629 offsetVector = p-snappedPoint |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
630 if Point.dot(orthogonalAlignmentVector, offsetVector) < 0: |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
631 minOffsetY = -minOffsetY |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
632 return [snappedAlignmentIdx, snappedAlignmentLeadingPoint, snappedPoint, subsegmentDistance, alignmentDistanceS, minOffsetY] |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
633 else: |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
634 print('Offset for point {} is infinite (check with prepareAlignments if some alignment 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
|
635 return None |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
636 |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
637 def getXYfromSY(s, y, alignmentNum, alignments): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
638 ''' Find X,Y coordinate from S,Y data. |
568
538fb47b3007
minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
639 if mode = 0 : return Snapped X,Y |
538fb47b3007
minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
640 if mode !=0 : return Real X,Y |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
641 ''' |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
642 alignment = alignments[alignmentNum] |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
643 i = 1 |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
644 while s > alignment.getCumulativeDistance(i) and i < len(alignment): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
645 i += 1 |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
646 if i < len(alignment): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
647 d = s - alignment.getCumulativeDistance(i-1) # distance on subsegment |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
648 #Get difference vector and then snap |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
649 dv = alignment[i] - alignment[i-1] |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
650 normalizedV = dv.normalize() |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
651 #snapped = alignment[i-1] + normalizedV*d # snapped point coordinate along alignment |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
652 # add offset finally |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
653 orthoNormalizedV = normalizedV.orthogonal() |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
654 return alignment[i-1] + normalizedV*d + orthoNormalizedV*y |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
655 else: |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
656 print('Curvilinear point {} is past the end of the alignement'.format((s, y, alignmentNum))) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
657 return None |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
658 |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
659 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
660 class NormAngle(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
661 '''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
|
662 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
663 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
|
664 self.norm = norm |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
665 self.angle = angle |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
666 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
667 @staticmethod |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
668 def fromPoint(p): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
669 norm = p.norm2() |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
670 if norm > 0: |
945
05d4302bf67e
working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
943
diff
changeset
|
671 angle = p.angle() |
339 | 672 else: |
673 angle = 0. | |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
674 return NormAngle(norm, angle) |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
675 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
676 def __add__(self, other): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
677 '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
|
678 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
|
679 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
680 def getPoint(self): |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
681 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
|
682 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
683 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
684 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
|
685 '''Predicts the position in nTimeSteps at constant speed/acceleration''' |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
686 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
|
687 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
688 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
|
689 '''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
|
690 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
|
691 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
|
692 predictedSpeedTheta = speedOrientation+control |
928
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
693 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
|
694 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
|
695 predictedPosition = position+predictedSpeedTheta.getPoint() |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
696 return predictedPosition, predictedSpeedTheta |
245
bd8ab323c198
corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
244
diff
changeset
|
697 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
698 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
699 class FlowVector(object): |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
700 '''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
|
701 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
|
702 def __init__(self, position, velocity): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
703 '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
|
704 self.position = position |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
705 self.velocity = velocity |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
706 |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
707 def __add__(self, other): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
708 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
|
709 |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
710 def __mul__(self, alpha): |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
711 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
|
712 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
713 def plot(self, options = '', **kwargs): |
211 | 714 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
|
715 self.position.plot(options+'x', **kwargs) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
716 |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
717 @staticmethod |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
718 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
|
719 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
|
720 |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
721 def intersection(p1, p2, p3, p4): |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
722 ''' Intersection point (x,y) of the segments [p1, p2] and [p3, p4] |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
723 Returns the intersection point and the ratio of its position along [p1, p2] from p1 |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
724 |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
725 Based on http://paulbourke.net/geometry/pointlineplane/''' |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
726 dp12 = p2-p1 |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
727 dp34 = p4-p3 |
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
728 #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
|
729 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
|
730 if det == 0.: |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
731 return None, None |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
732 else: |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
733 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
734 return p1+dp12.__mul__(ua), ua |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
735 |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
736 # def intersection(p1, p2, dp1, dp2): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
737 # '''Returns the intersection point between the two lines |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
738 # 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
|
739 # from numpy import matrix |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
740 # from numpy.linalg import linalg |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
741 # A = matrix([[dp1.y, -dp1.x], |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
742 # [dp2.y, -dp2.x]]) |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
743 # 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
|
744 # [dp2.y*p2.x-dp2.x*p2.y]]) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
745 |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
746 # if linalg.det(A) == 0: |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
747 # return None |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
748 # else: |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
749 # intersection = linalg.solve(A,B) |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
750 # 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
|
751 |
152
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
752 def segmentIntersection(p1, p2, p3, p4): |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
753 '''Returns the intersecting point (and ratio along [p1, p2]) of the segments [p1, p2] and [p3, p4], None otherwise''' |
152
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
754 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
755 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()): |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
756 return None, None |
152
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
757 else: |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
758 inter, ratio = intersection(p1, p2, p3, p4) |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
759 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
|
760 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
|
761 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
|
762 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
|
763 and utils.inBetween(p3.y, p4.y, inter.y)): |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
764 return inter, ratio |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
765 else: |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
766 return None, None |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
767 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
768 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
|
769 '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
770 inter, ratio = 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
|
771 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y): |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
772 return inter, ratio |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
773 else: |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
774 return None, None |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
775 |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
776 def segmentOrientationCrossing(p1, p2, p3, p4): |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
777 '''Returns the direction of the crossing, assuming there is a crossing: True means right (p3) to left (p4) (along the orthogonal vector to [p1, p2] (positive trigonometric orientation), False the other way''' |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
778 dp12 = p2-p1 |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
779 ortho = dp12.orthogonal(False) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
780 return Point.dot(ortho, p4-p3) > 0 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
781 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
782 class Trajectory(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
783 '''Class for trajectories: temporal sequence of positions |
22 | 784 |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
785 The class is iterable''' |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
786 |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
787 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
|
788 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
|
789 self.positions = positions |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
790 else: |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
791 self.positions = [[],[]] |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
792 |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
793 @staticmethod |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
794 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
|
795 t = Trajectory() |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
796 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
|
797 t.addPosition(p0) |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
798 for i in range(nPoints-1): |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
799 p0 += v |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
800 t.addPosition(p0) |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
801 return t |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
802 |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
803 @staticmethod |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
804 def load(line1, line2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
805 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
|
806 [float(n) for n in line2.split(' ')]]) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
807 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
808 @staticmethod |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
809 def fromPointList(points): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
810 t = Trajectory() |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
811 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
|
812 for p in points: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
813 t.addPositionXY(p[0],p[1]) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
814 else: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
815 for p in points: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
816 t.addPosition(p) |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
817 return t |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
818 |
1233
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
819 @staticmethod |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
820 def fromPointDict(points): |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
821 '''Points is a dict of points where keys are time instants |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
822 and there are (probably) missing positions''' |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
823 instants = sorted(list(points)) |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
824 # find all gaps |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
825 t1 = instants[0] |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
826 gap = False |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
827 gaps = [] |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
828 for t in range(instants[0], instants[-1]+1): |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
829 if t in instants: |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
830 if gap: |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
831 t2 = t |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
832 # store gap |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
833 gaps.append([t1, t2]) |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
834 gap = False |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
835 t1 = t |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
836 else: |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
837 t1 = t |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
838 else: |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
839 gap = True |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
840 # interpolate for gaps |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
841 for gap in gaps: |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
842 v = (points[gap[1]]-points[gap[0]]).divide(gap[1]-gap[0]) |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
843 for t in range(gap[0]+1, gap[1]): |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
844 points[t]=points[t-1]+v |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
845 return Trajectory.fromPointList([points[t] for t in range(instants[0], instants[-1]+1)]) |
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
846 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
847 def __len__(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
848 return len(self.positions[0]) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
849 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
850 def length(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
851 return self.__len__() |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
852 |
582
7e1ae4d97f1a
corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
581
diff
changeset
|
853 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
|
854 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
|
855 |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
856 def __getitem__(self, i): |
1209
2064e52019db
work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1207
diff
changeset
|
857 if isinstance(i, int):# or issubdtype(i, npinteger): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
858 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
|
859 elif isinstance(i, slice): |
3a06007a4bb7
modularized save trajectories, added slice to Trajectory, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
904
diff
changeset
|
860 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
|
861 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
862 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
|
863 |
16 | 864 def __str__(self): |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
865 return ' '.join([self.__getitem__(i).__str__() for i in range(self.length())]) |
16 | 866 |
69
cc192d0450b3
added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
68
diff
changeset
|
867 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
|
868 return self.__str__() |
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
869 |
22 | 870 def __iter__(self): |
871 self.iterInstantNum = 0 | |
872 return self | |
873 | |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
874 def __next__(self): |
22 | 875 if self.iterInstantNum >= self.length(): |
876 raise StopIteration | |
877 else: | |
878 self.iterInstantNum += 1 | |
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
879 return self[self.iterInstantNum-1] |
22 | 880 |
776
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
881 def __eq__(self, other): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
882 if self.length() == other.length(): |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
883 result = True |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
884 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
|
885 result = result and (p == po) |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
886 return result |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
887 else: |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
888 return False |
84420159c5f4
added __eq__ functions for Point and Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
771
diff
changeset
|
889 |
1122
58efbe18f102
slight modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1117
diff
changeset
|
890 def append(self,other): |
58efbe18f102
slight modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1117
diff
changeset
|
891 '''adds positions of other to the trajectory (in-place modification)''' |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
892 for i in range(2): |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
893 self.positions[i] += other.positions[i] |
1122
58efbe18f102
slight modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1117
diff
changeset
|
894 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
895 def setPositionXY(self, i, x, y): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
896 if i < self.__len__(): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
897 self.positions[0][i] = x |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
898 self.positions[1][i] = y |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
899 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
900 def setPosition(self, i, p): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
901 self.setPositionXY(i, p.x, p.y) |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
902 |
1222
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
903 def reset(self, x, y): |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
904 for i in range(self.__len__()): |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
905 self.positions[0][i] = x |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
906 self.positions[1][i] = y |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
907 |
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
908 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
|
909 self.positions[0].append(x) |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
910 self.positions[1].append(y) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
911 |
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
912 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
|
913 self.addPositionXY(p.x, p.y) |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
914 |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
915 def duplicateLastPosition(self): |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
916 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
|
917 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
|
918 |
1222
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
919 def agg(self, aggFunc = mean): |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
920 return Point.agg(self, aggFunc) |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
921 |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
922 @staticmethod |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
923 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
|
924 if lastCoordinate is None: |
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
925 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
|
926 elif 0 <= lastCoordinate <= len(positions[0]): |
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
927 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
|
928 if withOrigin: |
211 | 929 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
|
930 if objNum is not None: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
931 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
|
932 |
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
933 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
|
934 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
|
935 |
8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
929
diff
changeset
|
936 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
|
937 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
|
938 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
939 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
|
940 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
|
941 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
942 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
|
943 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
|
944 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
945 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
|
946 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
|
947 [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
|
948 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
|
949 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
950 def getXCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
951 return self.positions[0] |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
952 |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
953 def getYCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
954 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
|
955 |
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
956 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
|
957 return array(self.positions) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
958 |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
959 def xBounds(self): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
960 # 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
|
961 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates())) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
962 |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
963 def yBounds(self): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
964 # 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
|
965 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates())) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
966 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
967 def add(self, traj2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
968 '''Returns a new trajectory of the same length''' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
969 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
|
970 print('Trajectories of different lengths') |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
971 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
972 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
973 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
|
974 [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
|
975 |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
976 def subtract(self, traj2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
977 '''Returns a new trajectory of the same length''' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
978 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
|
979 print('Trajectories of different lengths') |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
980 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
981 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
982 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
|
983 [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
|
984 |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
985 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
|
986 '''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
|
987 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
|
988 [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
|
989 |
1222
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
990 def filterMovingWindow(self, halfWidth, mode = 'valid'): |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
991 '''Returns a new Trajectory obtained after the smoothing of the input by a moving average''' |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
992 return Trajectory([utils.filterMovingWindow(self.positions[0], halfWidth, mode), |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
993 utils.filterMovingWindow(self.positions[1], halfWidth, mode)]) |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
994 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
995 def differentiate(self, doubleLastPosition = False): |
339 | 996 diff = Trajectory() |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
997 for i in range(1, self.length()): |
339 | 998 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
|
999 if doubleLastPosition: |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1000 diff.addPosition(diff[-1]) |
339 | 1001 return diff |
1002 | |
1196
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1003 def filterSG(self, window_length, polyorder, deriv=0, delta=1.0, axis=-1, mode='nearest', cval=0.0, nInstantsIgnoredAtEnds = 2): |
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1004 '''Filters the trajectory using the Savitsky Golay filter |
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1005 if deriv = 1, the method differentiates |
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1006 Warning: high order polynomials yield artefacts |
761
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
1007 |
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
1008 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
|
1009 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
|
1010 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
|
1011 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
|
1012 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
|
1013 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
|
1014 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
|
1015 |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1016 https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.savgol_filter.html#scipy.signal.savgol_filter''' |
1193
d324305c1240
solving issues with extreme values when filtering using savitsky golay
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1190
diff
changeset
|
1017 filtered = savgol_filter(self.positions, window_length, polyorder, deriv, delta, axis, mode, cval) |
1196
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1018 length = self.length() |
1181 | 1019 if nInstantsIgnoredAtEnds >=1: |
1196
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1020 if nInstantsIgnoredAtEnds >= length/2: |
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1021 n = int(round(length/2))-1 |
1195
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1022 else: |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1023 n = nInstantsIgnoredAtEnds |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1024 filtered = filtered[:,n:-n] |
1193
d324305c1240
solving issues with extreme values when filtering using savitsky golay
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1190
diff
changeset
|
1025 return Trajectory(filtered.tolist()) |
761
15ddc8715236
added savitzky golay for differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
727
diff
changeset
|
1026 |
16 | 1027 def norm(self): |
1028 '''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
|
1029 return hypot(self.positions[0], self.positions[1]) |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
1030 |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1031 def computeCumulativeDistances(self): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1032 '''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
|
1033 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
|
1034 self.distances = [] |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
1035 self.cumulativeDistances = [0.] |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1036 p1 = self[0] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1037 cumulativeDistance = 0. |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1038 for i in range(self.length()-1): |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1039 p2 = self[i+1] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1040 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
|
1041 cumulativeDistance += self.distances[-1] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1042 self.cumulativeDistances.append(cumulativeDistance) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1043 p1 = p2 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1044 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1045 def getDistance(self,i): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1046 '''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
|
1047 if i < self.length()-1: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1048 return self.distances[i] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1049 else: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1050 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
|
1051 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1052 def getCumulativeDistance(self, i): |
1115
cef7aa2f9931
minor shorthand method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1114
diff
changeset
|
1053 '''Returns the cumulative distance between the beginning and point i''' |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
1054 if i < self.length(): |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1055 return self.cumulativeDistances[i] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
1056 else: |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
1057 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
|
1058 |
1115
cef7aa2f9931
minor shorthand method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1114
diff
changeset
|
1059 def getTotalDistance(self): |
cef7aa2f9931
minor shorthand method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1114
diff
changeset
|
1060 '''Returns the total distance (shorthand for cumulative distance [-1]''' |
cef7aa2f9931
minor shorthand method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1114
diff
changeset
|
1061 return self.getCumulativeDistance(-1) |
cef7aa2f9931
minor shorthand method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1114
diff
changeset
|
1062 |
762
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1063 def getMaxDistance(self, metric): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1064 'Returns the maximum distance between points in the trajectory' |
762
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1065 positions = self.getPositions().asArray().T |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1066 return cdist(positions, positions, metric = metric).max() |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1067 |
937
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1068 def getClosestPoint(self, p1, maxDist2 = None): |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1069 '''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
|
1070 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
|
1071 TODO: could use cdist for different metrics''' |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1072 distances2 = [] |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1073 minDist2 = float('inf') |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1074 i = -1 |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1075 for p2 in self: |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1076 distances2.append(Point.distanceNorm2(p1, p2)) |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1077 if distances2[-1] < minDist2: |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1078 minDist2 = distances2[-1] |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
1079 i = len(distances2)-1 |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
1080 if maxDist2 is not None and minDist2 < maxDist2: |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
1081 return None |
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
1082 else: |
937
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1083 return i |
b67a784beb69
work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
936
diff
changeset
|
1084 |
381
387cc0142211
script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
379
diff
changeset
|
1085 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1086 '''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
|
1087 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
|
1088 count = 0 |
383
0ce2210790b1
fixed stupid naming bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
381
diff
changeset
|
1089 lengthThreshold = float(self.length())*minProportion |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1090 for p in self: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1091 if p.similarOrientation(refDirection, cosineThreshold): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1092 count += 1 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1093 return count >= lengthThreshold |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1094 |
49 | 1095 def wiggliness(self): |
762
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1096 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
|
1097 if straightDistance > 0: |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1098 return self.getCumulativeDistance(self.length()-1)/float(straightDistance) |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1099 else: |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1100 return None |
49 | 1101 |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1102 def getIntersections(self, p1, p2, computeOrientations = False): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1103 '''Returns a list of the indices at which the trajectory |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1104 intersects with the segment of extremities p1 and p2 |
771 | 1105 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
|
1106 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
|
1107 intersections = [] |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1108 rightToLeftOrientations = [] |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1109 |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1110 for i in range(self.length()-1): |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1111 q1=self.__getitem__(i) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1112 q2=self.__getitem__(i+1) |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1113 p, ratio = segmentIntersection(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
|
1114 if p is not None: |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1115 # if q1.x != q2.x: |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1116 # ratio = (p.x-q1.x)/(q2.x-q1.x) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1117 # elif q1.y != q2.y: |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1118 # ratio = (p.y-q1.y)/(q2.y-q1.y) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1119 # else: |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1120 # ratio = 0 |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1121 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
|
1122 intersections.append(p) |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1123 if computeOrientations: |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1124 rightToLeftOrientations.append(segmentOrientationCrossing(p1, p2, q1, q2)) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1125 return indices, intersections, rightToLeftOrientations |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1126 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
1127 def getLineIntersections(self, p1, p2): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1128 '''Returns a list of the indices at which the trajectory |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1129 intersects with the line going through p1 and p2 |
771 | 1130 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
|
1131 indices = [] |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
1132 intersections = [] |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1133 |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1134 for i in range(self.length()-1): |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
1135 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
|
1136 q2=self.__getitem__(i+1) |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1137 p, ratio = segmentLineIntersection(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
|
1138 if p is not None: |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1139 # if q1.x != q2.x: |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1140 # ratio = (p.x-q1.x)/(q2.x-q1.x) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1141 # elif q1.y != q2.y: |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1142 # ratio = (p.y-q1.y)/(q2.y-q1.y) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1143 # else: |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1144 # ratio = 0 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
1145 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
|
1146 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
|
1147 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
|
1148 |
1109
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1149 def subTrajectoryInInterval(self, inter): |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
1150 '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
|
1151 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
|
1152 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
|
1153 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
|
1154 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1155 return None |
762
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1156 |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1157 def subSample(self, step): |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1158 'Returns the positions very step' |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1159 return Trajectory([self.positions[0][::step], |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1160 self.positions[1][::step]]) |
d6f0e0cab07d
added functionalities for Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
761
diff
changeset
|
1161 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
1162 if shapelyAvailable: |
863
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1163 def getInstantsInPolygon(self, polygon): |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1164 '''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
|
1165 instants = [] |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1166 n = self.length() |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1167 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
|
1168 if polygon.contains(shapelyPoint(x, y)): |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1169 instants.append(t) |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1170 return instants |
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1171 |
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
|
1172 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
|
1173 '''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
|
1174 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
|
1175 |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1176 t2 is another trajectory (could be velocities) |
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
|
1177 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
|
1178 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
|
1179 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
|
1180 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
|
1181 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
|
1182 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
|
1183 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
|
1184 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
|
1185 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
|
1186 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
|
1187 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
|
1188 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
|
1189 return traj, traj2 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1190 |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1191 def proportionInPolygon(self, polygon, minProportion = 0.5): |
863
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1192 instants = self.getInstantsInPolygon(polygon) |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1193 lengthThreshold = float(self.length())*minProportion |
863
a8ca72dc1564
work on user detectors
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
851
diff
changeset
|
1194 return len(instants) >= lengthThreshold |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1195 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
|
1196 def getTrajectoryInPolygon(self, polygon, t2 = None): |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1197 '''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
|
1198 (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
|
1199 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
|
1200 inPolygon = [] |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1201 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
|
1202 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
|
1203 if inPolygon[-1]: |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1204 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
|
1205 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
|
1206 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
|
1207 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
|
1208 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
|
1209 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
|
1210 return traj, traj2 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1211 |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1212 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
|
1213 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
|
1214 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
|
1215 return sum(inPolygon) >= lengthThreshold |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
1216 |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
1217 @staticmethod |
369
027e254f0b53
lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
368
diff
changeset
|
1218 def lcss(t1, t2, lcss): |
027e254f0b53
lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
368
diff
changeset
|
1219 return lcss.compute(t1, t2) |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
1220 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1221 class CurvilinearTrajectory(Trajectory): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1222 '''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
|
1223 longitudinal coordinate is stored as first coordinate (exterior name S) |
1085
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1224 lateral coordinate is stored as second coordinate |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1225 the third "lane" coordinate is for an alignment id, |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1226 whether explicit for a list/dict of alignments, |
1085
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1227 or implicit for a road with lane numbers''' |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1228 |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1229 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
|
1230 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
|
1231 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
|
1232 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
|
1233 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
|
1234 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1235 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
|
1236 if lanes is None or len(lanes) != self.length(): |
1085
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1237 self.lanes = [None]*int(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
|
1238 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
1239 self.lanes = lanes |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1240 |
1085
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1241 @staticmethod |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1242 def generate(s, v, nPoints, lane, y = 0): |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1243 '''s is initial position, v is velocity |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1244 0 in lateral coordinate by default |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1245 TODO 2D velocity for lane change?''' |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1246 S = [s] |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1247 for i in range(nPoints-1): |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1248 S.append(S[-1]+v) |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1249 Y = [y]*nPoints |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1250 lanes = [lane]*nPoints |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1251 return CurvilinearTrajectory(S, Y, lanes) |
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1252 |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1253 @staticmethod |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1254 def fromTrajectoryProjection(t, alignments, halfWidth = 3): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1255 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1256 (curvilinearPositions instance) which holds information about the |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1257 curvilinear coordinates using alignment metadata. |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1258 From Paul St-Aubin's PVA tools |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1259 ====== |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1260 |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1261 Input: |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1262 ====== |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1263 alignments = a list of alignments, where each alignment is a list of |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1264 points (class Point). |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1265 halfWidth = moving average window (in points) in which to smooth |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1266 lane changes. As per tools_math.cat_mvgavg(), this term |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1267 is a search *radius* around the center of the window. |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1268 |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1269 ''' |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1270 curvilinearPositions = CurvilinearTrajectory() |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1271 |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1272 #For each point |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1273 for i in range(int(t.length())): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1274 result = getSYfromXY(t[i], alignments) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1275 |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1276 # Error handling |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1277 if(result is None): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1278 print('Warning: trajectory at point {} {} has alignment errors (alignment snapping)\nCurvilinear trajectory could not be computed'.format(i, t[i])) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1279 else: |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1280 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1281 curvilinearPositions.addPositionSYL(S, Y, align) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1282 |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1283 ## Go back through points and correct lane |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1284 #Run through objects looking for outlier point |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1285 smoothed_lanes = utils.filterCategoricalMovingWindow(curvilinearPositions.getLanes(), halfWidth) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1286 ## Recalculate projected point to new lane |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1287 lanes = curvilinearPositions.getLanes() |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1288 if(lanes != smoothed_lanes): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1289 for i in range(len(lanes)): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1290 if(lanes[i] != smoothed_lanes[i]): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1291 result = getSYfromXY(t[i],[alignments[smoothed_lanes[i]]]) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1292 |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1293 # Error handling |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1294 if(result is None): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1295 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1296 print(' Warning: trajectory at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(i, t[i])) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1297 else: |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1298 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1299 curvilinearPositions.setPosition(i, S, Y, align) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1300 return curvilinearPositions |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1301 |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1302 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
|
1303 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
|
1304 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
|
1305 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
1306 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
|
1307 #elif isinstance( key, slice ): |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1308 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1309 def getSCoordinates(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1310 return self.getXCoordinates() |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1311 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1312 def getLanes(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1313 return self.lanes |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1314 |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1315 def getSCoordAt(self, i): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1316 return self.positions[0][i] |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1317 |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1318 def getYCoordAt(self, i): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1319 return self.positions[1][i] |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1320 |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1321 def getLaneAt(self, i): |
1098
469e36eea158
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1097
diff
changeset
|
1322 return self.lanes[i] |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1323 |
1109
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1324 def subTrajectoryInInterval(self, inter): |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1325 'Returns all curvilinear positions between index inter.first and index.last (included)' |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1326 if inter.first >=0 and inter.last<= self.length(): |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1327 return CurvilinearTrajectory(self.positions[0][inter.first:inter.last+1], |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1328 self.positions[1][inter.first:inter.last+1], |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1329 self.lanes[inter.first:inter.last+1]) |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1330 else: |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1331 return None |
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1332 |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1333 def append(self, other): |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1334 Trajectory.append(self, other) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1335 self.lanes.append(other.getLanes()) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1336 |
1085
7853106677b7
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1081
diff
changeset
|
1337 def addPositionSYL(self, s, y, lane = None): |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1338 self.addPositionXY(s,y) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1339 self.lanes.append(lane) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1340 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1341 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
|
1342 '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
|
1343 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
|
1344 |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1345 def duplicateLastPosition(self): |
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1346 super(CurvilinearTrajectory, self).duplicateLastPosition() |
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1347 self.lanes.append(self.lanes[-1]) |
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1348 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1349 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
|
1350 self.setPositionXY(i, s, y) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1351 if i < self.__len__(): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1352 self.lanes[i] = lane |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
1353 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1354 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
|
1355 diff = CurvilinearTrajectory() |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1356 p1 = self[0] |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1357 for i in range(1, self.length()): |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1358 p2 = self[i] |
1098
469e36eea158
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1097
diff
changeset
|
1359 if p2[2] == p1[2]: |
469e36eea158
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1097
diff
changeset
|
1360 laneChange = None |
469e36eea158
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1097
diff
changeset
|
1361 else: |
469e36eea158
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1097
diff
changeset
|
1362 laneChange = (p1[2], p2[2]) |
469e36eea158
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1097
diff
changeset
|
1363 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], laneChange) |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1364 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
|
1365 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
|
1366 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
|
1367 return diff |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
1368 |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1369 def getIntersections(self, S1, lane = None): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1370 '''Returns a list of the indices at which the trajectory |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1371 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
|
1372 (in provided lane if lane is not None) |
771 | 1373 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
|
1374 indices = [] |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1375 for i in range(self.length()-1): |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1376 q1=self.__getitem__(i) |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1377 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
|
1378 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
|
1379 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
|
1380 return indices |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
1381 |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1382 ################## |
1210
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1383 # Geometry |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1384 ################## |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1385 |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1386 class Geometry(object): |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1387 '''Generic class for outline and size of object ''' |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1388 |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1389 |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1390 class CarGeometry(Geometry): |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1391 '''Default car geometry as rectangle ''' |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1392 |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1393 ################## |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1394 # Moving Objects |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1395 ################## |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1396 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1397 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
|
1398 'car', |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1399 '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
|
1400 'motorcycle', |
1227
eb3936809ea5
reverting to using bicycle
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1222
diff
changeset
|
1401 'bicycle', |
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
1402 'bus', |
1125
b358bed29ab4
updates and bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1117
diff
changeset
|
1403 'truck', |
b358bed29ab4
updates and bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1117
diff
changeset
|
1404 'automated'] |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1405 |
1233
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
1406 coco2Types = {0: 2, 1: 4, 2: 1, 3: 3, 5: 5, 7: 6} |
1226
d478d3122804
change of bicycle to cyclist
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1222
diff
changeset
|
1407 cocoTypeNames = {0: 'person', |
1228 | 1408 1: 'bicycle', |
1409 2: 'car', | |
1410 3: 'motorcycle', | |
1411 5: 'bus', | |
1233
d5695e0b59d9
saving results from ultralytics works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1228
diff
changeset
|
1412 #6: 'train', |
1228 | 1413 7: 'truck'} |
1218
1f0b1fc172f8
adding COCO person and vehicle types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1217
diff
changeset
|
1414 |
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
1415 userType2Num = utils.inverseEnumeration(userTypeNames) |
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
1416 |
959
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1417 class CarClassifier: |
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1418 def predict(self, hog): |
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1419 return userType2Num['car'] |
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
1420 carClassifier = CarClassifier() |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1421 |
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
|
1422 class MovingObject(STObject, VideoFilenameAddable): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1423 '''Class for moving objects: a spatio-temporal object |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1424 with a trajectory and a geometry (constant volume over time) |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1425 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
|
1426 ''' |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
1427 |
1203
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
1428 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown'], nObjects = None, features = None, initCurvilinear = False): |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
1429 super(MovingObject, self).__init__(num, timeInterval) |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1430 if initCurvilinear: |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1431 self.curvilinearPositions = positions |
1098
469e36eea158
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1097
diff
changeset
|
1432 self.curvilinearVelocities = velocities # third component is (previousAlignmentIdx, newAlignmentIdx) or None if no change |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1433 else: |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1434 self.positions = positions |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1435 self.velocities = velocities |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
1436 self.geometry = geometry |
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1437 self.userType = userType |
1041
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1438 self.setNObjects(nObjects) # a feature has None for nObjects |
1203
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
1439 self.features = features |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
1440 # compute bounding polygon from trajectory |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1441 |
1075 | 1442 @staticmethod |
1079
845d694af7b8
reversed minor bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1077
diff
changeset
|
1443 def croppedTimeInterval(obj, value, after = True): |
1075 | 1444 newTimeInterval = TimeInterval(obj.getFirstInstant(), min(value, obj.getLastInstant())) if after else TimeInterval(max(obj.getFirstInstant(), value), obj.getLastInstant()) |
1445 if obj.positions is not None : | |
1136
30171d4fd3df
moving.py [Issue #22 from Etienne Beauchamp In annotationTool Correction]
Tertuis Ouédraogo <tertuis95@gmail.com>
parents:
1135
diff
changeset
|
1446 newPositions = obj.positions[slice(newTimeInterval.first - obj.getLastInstant(), newTimeInterval.last + 1 - obj.getLastInstant())] |
1075 | 1447 else: |
1448 newPositions = None | |
1449 if obj.velocities is not None : | |
1136
30171d4fd3df
moving.py [Issue #22 from Etienne Beauchamp In annotationTool Correction]
Tertuis Ouédraogo <tertuis95@gmail.com>
parents:
1135
diff
changeset
|
1450 newVelocities = obj.velocities[slice(newTimeInterval.first - obj.getLastInstant(), newTimeInterval.last + 1 - obj.getLastInstant())] |
1075 | 1451 else: |
1452 newVelocities = None | |
1079
845d694af7b8
reversed minor bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1077
diff
changeset
|
1453 if obj.hasFeatures(): |
845d694af7b8
reversed minor bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1077
diff
changeset
|
1454 newFeatures = [f.croppedTimeInterval(value, after) for f in obj.features] |
1075 | 1455 else: |
1456 newFeatures = None | |
1457 res = MovingObject(obj.getNum(), newTimeInterval, newPositions, newVelocities, obj.geometry, obj.userType, obj.nObjects) | |
1458 res.features = newFeatures | |
1459 res.featureNumbers = obj.featureNumbers | |
1079
845d694af7b8
reversed minor bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1077
diff
changeset
|
1460 #if hasattr(obj, 'projectedPositions'): |
845d694af7b8
reversed minor bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1077
diff
changeset
|
1461 # res.projectedPositions = obj.projectedPositions[slice(newTimeInterval.first, newTimeInterval.last+1)] |
1075 | 1462 return res |
1463 | |
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
|
1464 |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
1465 @staticmethod |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1466 def aggregateTrajectories(features, interval = None, aggFunc = mean): |
995
349cd5e73f79
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
994
diff
changeset
|
1467 'Computes the aggregate trajectory from list of MovingObject features' |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1468 positions = Trajectory() |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1469 velocities = Trajectory() |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1470 if interval is None: |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1471 inter = TimeInterval.unionIntervals([f.getTimeInterval() for f in features]) |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1472 else: |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1473 inter = interval |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1474 for t in inter: |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1475 points = [] |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1476 vels = [] |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1477 for f in features: |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1478 if f.existsAtInstant(t): |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1479 points.append(f.getPositionAtInstant(t)) |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1480 vels.append(f.getVelocityAtInstant(t)) |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1481 positions.addPosition(Point.agg(points, aggFunc)) |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1482 velocities.addPosition(Point.agg(vels, aggFunc)) |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1483 return inter, positions, velocities |
995
349cd5e73f79
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
994
diff
changeset
|
1484 |
349cd5e73f79
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
994
diff
changeset
|
1485 @staticmethod |
918
3a06007a4bb7
modularized save trajectories, added slice to Trajectory, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
904
diff
changeset
|
1486 def generate(num, p, v, timeInterval): |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1487 nPoints = int(timeInterval.length()) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1488 positions = Trajectory.generate(p, v, nPoints) |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1489 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints])) |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
1490 |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1491 def updatePositions(self): |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1492 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
1493 |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
1494 @staticmethod |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1495 def concatenate(obj1, obj2, num = None, newFeatureNum = None): |
1018
d7afc59f6966
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1016
diff
changeset
|
1496 '''Concatenates two objects, whether overlapping temporally or not |
d7afc59f6966
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1016
diff
changeset
|
1497 |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1498 Positions will be recomputed if features are merged |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1499 Otherwise, only featureNumbers and/or features will be merged''' |
997
4f3387a242a1
updated utils to python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
996
diff
changeset
|
1500 if num is None: |
4f3387a242a1
updated utils to python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
996
diff
changeset
|
1501 newNum = obj1.getNum() |
4f3387a242a1
updated utils to python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
996
diff
changeset
|
1502 else: |
4f3387a242a1
updated utils to python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
996
diff
changeset
|
1503 newNum = num |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1504 commonTimeInterval = obj1.commonTimeInterval(obj2) |
1180
dc28364f34b9
minor cleanup
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1179
diff
changeset
|
1505 if commonTimeInterval.empty(): # and emptyInterval.length() >= 2: not needed 2 to include situations successive positions, but issues with missing last velocity and reloading |
dc28364f34b9
minor cleanup
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1179
diff
changeset
|
1506 emptyInterval = TimeInterval(min(obj1.getLastInstant(),obj2.getLastInstant()), max(obj1.getFirstInstant(),obj2.getFirstInstant())) |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1507 if newFeatureNum is None: |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1508 print('Not merging objects {} and {}, missing new feature number'.format(obj1.getNum(),obj2.getNum())) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1509 return None, None |
1018
d7afc59f6966
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1016
diff
changeset
|
1510 else: |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1511 if obj1.existsAtInstant(emptyInterval.last): |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1512 firstObject = obj2 |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1513 secondObject = obj1 |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1514 else: |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1515 firstObject = obj1 |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1516 secondObject = obj2 |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1517 v = (secondObject.getPositionAtInstant(emptyInterval.last)-firstObject.getPositionAtInstant(emptyInterval.first)).divide(emptyInterval.length()-1) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1518 positions = copy.deepcopy(firstObject.getPositions()) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1519 velocities = copy.deepcopy(firstObject.getPositions()) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1520 featurePositions = Trajectory() |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1521 featureVelocities = Trajectory() |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1522 p = firstObject.getPositionAtInstant(emptyInterval.first) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1523 # init new feature with position at last instant of 1st obj |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1524 featurePositions.addPosition(p) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1525 featureVelocities.addPosition(v) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1526 for t in range(emptyInterval.first+1, emptyInterval.last): |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1527 p=p+v |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1528 positions.addPosition(p) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1529 velocities.addPosition(v) |
1134
4b2a55d570c1
resolve issue for short features when merging objects manually
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1133
diff
changeset
|
1530 featurePositions.addPosition(p) |
4b2a55d570c1
resolve issue for short features when merging objects manually
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1133
diff
changeset
|
1531 featureVelocities.addPosition(v) |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1532 # last position to feature |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1533 p = secondObject.getPositionAtInstant(emptyInterval.last) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1534 v = secondObject.getVelocityAtInstant(emptyInterval.last) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1535 featurePositions.addPosition(p) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1536 featureVelocities.addPosition(v) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1537 # copy second trajectory |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1538 positions.append(secondObject.getPositions()) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1539 velocities.append(secondObject.getVelocities()) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1540 newObject = MovingObject(newNum, TimeInterval(firstObject.getFirstInstant(), secondObject.getLastInstant()), positions, velocities, nObjects = 1) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1541 newFeature = MovingObject(newFeatureNum, emptyInterval, featurePositions, featureVelocities) |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1542 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'): |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1543 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers+[newFeatureNum] |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1544 if obj1.hasFeatures() and obj2.hasFeatures(): |
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1545 newObject.features = obj1.getFeatures()+obj2.getFeatures()+[newFeature] |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1546 else: # time intervals overlap |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1547 newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval()) |
1041
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1548 newObject = MovingObject(newNum, newTimeInterval, nObjects = 1) # hypothesis is that it's the same object being reunited |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1549 newFeature = None |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1550 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'): |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1551 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1552 if obj1.hasFeatures() and obj2.hasFeatures(): |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1553 newObject.features = obj1.getFeatures()+obj2.getFeatures() |
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1554 newObject.updatePositions() |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1555 else: |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1556 print('Cannot update object positions without features') |
1018
d7afc59f6966
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1016
diff
changeset
|
1557 # user type |
d7afc59f6966
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1016
diff
changeset
|
1558 if obj1.getUserType() != obj2.getUserType(): |
d7afc59f6966
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1016
diff
changeset
|
1559 print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()])) |
1019
5d2f6afae35b
work on object concatenation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1018
diff
changeset
|
1560 newObject.setUserType(obj1.getUserType()) |
1177
aa88acf06876
rewrote object concatenation method, cleaner
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1170
diff
changeset
|
1561 return newObject, newFeature |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
1562 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1563 def getObjectInTimeInterval(self, inter): |
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1564 '''Returns a new object extracted from self, |
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1565 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
|
1566 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
|
1567 if not intersection.empty(): |
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
1568 trajectoryInterval = TimeInterval(intersection.first-self.getFirstInstant(), intersection.last-self.getFirstInstant()) |
1146
b219d5a1bb55
added code to categorize interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1136
diff
changeset
|
1569 obj = MovingObject(self.num, intersection, self.positions.subTrajectoryInInterval(trajectoryInterval), self.geometry, self.userType, self.nObjects) |
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
|
1570 if self.velocities is not None: |
1146
b219d5a1bb55
added code to categorize interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1136
diff
changeset
|
1571 obj.velocities = self.velocities.subTrajectoryInInterval(trajectoryInterval) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1572 return obj |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1573 else: |
978
184f1dd307f9
corrected print and exception statements for Python 3
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
959
diff
changeset
|
1574 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
|
1575 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1576 |
726
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1577 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
|
1578 '''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
|
1579 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
|
1580 if homography is not None: |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
1581 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
|
1582 else: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1583 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
|
1584 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
|
1585 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
|
1586 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
|
1587 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1588 #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
|
1589 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
|
1590 # '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
|
1591 l = 0 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1592 intervalLabels = [] |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1593 prev = True |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1594 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
|
1595 if i: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1596 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
|
1597 l += 1 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1598 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
|
1599 else: |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1600 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
|
1601 prev = i |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1602 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
|
1603 subObjects = [] |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1604 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
|
1605 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
|
1606 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
|
1607 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
|
1608 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
|
1609 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
|
1610 |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1611 return subObjects |
43ae3a1af290
added functionality to display matchings between ground truth and tracked objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
723
diff
changeset
|
1612 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1613 def getPositions(self): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1614 return self.positions |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1615 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1616 def getVelocities(self): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1617 return self.velocities |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1618 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1619 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
|
1620 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
|
1621 |
943
b1e8453c207c
work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
939
diff
changeset
|
1622 def computeCumulativeDistances(self): |
b1e8453c207c
work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
939
diff
changeset
|
1623 self.positions.computeCumulativeDistances() |
b1e8453c207c
work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
939
diff
changeset
|
1624 |
335
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1625 def getCurvilinearPositions(self): |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1626 if hasattr(self, 'curvilinearPositions'): |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1627 return self.curvilinearPositions |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1628 else: |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1629 return None |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1630 |
1123
0548a78852b8
added accessor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1122
diff
changeset
|
1631 def getCurvilinearVelocities(self): |
0548a78852b8
added accessor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1122
diff
changeset
|
1632 if hasattr(self, 'curvilinearVelocities'): |
0548a78852b8
added accessor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1122
diff
changeset
|
1633 return self.curvilinearVelocities |
0548a78852b8
added accessor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1122
diff
changeset
|
1634 else: |
0548a78852b8
added accessor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1122
diff
changeset
|
1635 return None |
0548a78852b8
added accessor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1122
diff
changeset
|
1636 |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1637 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
|
1638 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
|
1639 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
|
1640 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
|
1641 if withOrigin: |
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1642 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
|
1643 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1644 instants = [] |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1645 coords = [] |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1646 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
|
1647 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
|
1648 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
|
1649 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
|
1650 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1651 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
|
1652 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
|
1653 plot(instants, coords, options, **kwargs) |
660
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1654 if withOrigin and len(instants)>0: |
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1655 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
|
1656 else: |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1657 print('Object {} has no curvilinear positions'.format(self.getNum())) |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1658 |
1116
a3982d591a61
placeholder implementation for interpolateCurvilinearPositions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1115
diff
changeset
|
1659 def interpolateCurvilinearPositions(self, t, alignments = None): |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1660 '''Linear interpolation of curvilinear positions, t being a float''' |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1661 if hasattr(self, 'curvilinearPositions'): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1662 if self.existsAtInstant(t): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1663 i = int(floor(t)) |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1664 p1 = self.getCurvilinearPositionAtInstant(i) |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1665 p2 = self.getCurvilinearPositionAtInstant(i+1) |
1114
7135b5eaa6b4
correcting poor requirement for interpolateCurvilinearPositions (when changing alignment)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1112
diff
changeset
|
1666 if p1[2] == p2[2]: |
7135b5eaa6b4
correcting poor requirement for interpolateCurvilinearPositions (when changing alignment)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1112
diff
changeset
|
1667 alpha = t-float(i) |
7135b5eaa6b4
correcting poor requirement for interpolateCurvilinearPositions (when changing alignment)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1112
diff
changeset
|
1668 return [(1-alpha)*p1[0]+alpha*p2[0], (1-alpha)*p1[1]+alpha*p2[1], p1[2]] |
1117 | 1669 elif alignments is not None: # can be done only if assuming there is no missing alignmentn where the object has no coordinate |
1116
a3982d591a61
placeholder implementation for interpolateCurvilinearPositions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1115
diff
changeset
|
1670 pass # TODO |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1671 else: |
1116
a3982d591a61
placeholder implementation for interpolateCurvilinearPositions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1115
diff
changeset
|
1672 print('Object {} changes lane at {} and alignments are not provided'.format(self.getNum(), t)) |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1673 else: |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1674 print('Object {} does not exist at {}'.format(self.getNum(), t)) |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1675 else: |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1676 print('Object {} has no curvilinear positions'.format(self.getNum())) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1677 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1678 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
|
1679 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
|
1680 |
1041
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1681 def getNObjects(self): |
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1682 return self.nObjects |
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1683 |
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1684 def setNObjects(self, nObjects): |
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1685 if nObjects is None or nObjects >= 1: |
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1686 self.nObjects = nObjects |
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1687 else: |
fc7c0f38e8a6
added nObjects to MovingObject, with loading/saving
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1037
diff
changeset
|
1688 print('Number of objects represented by object {} must be greater or equal to 1 ({})'.format(self.getNum(), nObjects)) |
1075 | 1689 self.nObjects = None |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1690 |
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
|
1691 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
|
1692 '''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
|
1693 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
|
1694 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
|
1695 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
|
1696 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
|
1697 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
|
1698 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
|
1699 |
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
|
1700 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
|
1701 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
|
1702 |
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
|
1703 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
|
1704 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
|
1705 |
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
|
1706 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
|
1707 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
|
1708 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
|
1709 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
|
1710 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
|
1711 |
869
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1712 def getNLongestFeatures(self, nFeatures = 1): |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1713 if self.features is None: |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1714 return [] |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1715 else: |
eb2f8ce2b39d
added method for longest feature
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
863
diff
changeset
|
1716 tmp = utils.sortByLength(self.getFeatures(), reverse = True) |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1717 return tmp[:min(len(tmp), nFeatures)] |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1718 |
1044
75a6ad604cc5
work on motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1041
diff
changeset
|
1719 def getFeatureNumbersOverTime(self): |
666
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1720 '''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
|
1721 dict instant -> number of features''' |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1722 if self.hasFeatures(): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1723 featureNumbers = {} |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1724 for t in self.getTimeInterval(): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1725 n = 0 |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1726 for f in self.getFeatures(): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1727 if f.existsAtInstant(t): |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1728 n += 1 |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1729 featureNumbers[t]=n |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1730 return featureNumbers |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1731 else: |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1732 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
|
1733 return None |
93633ce122c3
added function to count features in objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
665
diff
changeset
|
1734 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
1735 def getSpeeds(self, nInstantsIgnoredAtEnds = 0): |
1197
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1736 '''Returns the scalar speed''' |
682 | 1737 speeds = self.getVelocities().norm() |
1738 if nInstantsIgnoredAtEnds > 0: | |
1739 n = min(nInstantsIgnoredAtEnds, int(floor(self.length()/2.))) | |
1740 return speeds[n:-n] | |
1741 else: | |
1742 return speeds | |
49 | 1743 |
1197
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1744 def getAccelerations(self, fromSpeeds = True, nInstantsIgnoredAtEnds = 0): |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1745 '''Returns the scalar acceleration by differentiating the speeds (fromSpeeds = True) or by differentiating the velocity (vector) and taking the norm (frompSpeeds = False. In the second case, speed is always positive''' |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1746 if fromSpeeds: |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1747 speeds = array(self.getSpeeds()) |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1748 accelerations = speeds[1:]-speeds[:-1] |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1749 else: |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1750 accelerations = self.getVelocities().differentiate().norm() |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1751 if nInstantsIgnoredAtEnds > 0: |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1752 n = min(nInstantsIgnoredAtEnds, int(floor(len(accelerations)/2.))) |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1753 return accelerations[n:-n] |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1754 else: |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1755 return accelerations |
0475b4cd0cfb
adding simple direct acceleration calculation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1196
diff
changeset
|
1756 |
1196
d5566af60a69
correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1195
diff
changeset
|
1757 def getAccelerationsSG(self, window_length, polyorder, delta=1.0, axis=-1, mode='nearest', cval=0.0, nInstantsIgnoredAtEnds = 0): |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1758 '''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
|
1759 Caution about previously filtered data''' |
1193
d324305c1240
solving issues with extreme values when filtering using savitsky golay
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1190
diff
changeset
|
1760 speeds = self.getSpeeds() |
1195
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1761 wlength = min(window_length, len(speeds)) |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1762 if wlength % 2 == 0: |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1763 wlength -=1 |
1193
d324305c1240
solving issues with extreme values when filtering using savitsky golay
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1190
diff
changeset
|
1764 filtered = savgol_filter(speeds, wlength, min(wlength-1, polyorder), 1, delta, axis, mode, cval) |
d324305c1240
solving issues with extreme values when filtering using savitsky golay
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1190
diff
changeset
|
1765 if nInstantsIgnoredAtEnds >= 1: |
1195
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1766 if nInstantsIgnoredAtEnds >= len(speeds)/2: |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1767 n = int(round(len(speeds)/2))-1 |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1768 else: |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1769 n = nInstantsIgnoredAtEnds |
27a6a7f9b972
adjusting savgol use
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1193
diff
changeset
|
1770 return filtered[n:-n] |
1193
d324305c1240
solving issues with extreme values when filtering using savitsky golay
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1190
diff
changeset
|
1771 else: |
d324305c1240
solving issues with extreme values when filtering using savitsky golay
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1190
diff
changeset
|
1772 return filtered |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1773 |
633
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1774 def getSpeedIndicator(self): |
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1775 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
|
1776 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
|
1777 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1778 def getPositionAt(self, i): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1779 return self.positions[i] |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1780 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1781 def getVelocityAt(self, i): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1782 return self.velocities[i] |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1783 |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1784 def getPositionAtInstant(self, i): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1785 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
|
1786 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1787 def getVelocityAtInstant(self, i): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1788 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
|
1789 |
1097
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1790 def getCurvilinearPositionAt(self, i): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1791 return self.curvilinearPositions[i] |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1792 |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1793 def getCurvilinearVelocityAt(self, i): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1794 return self.curvilinearVelocities[i] |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1795 |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1796 def getCurvilinearPositionAtInstant(self, i): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1797 return self.curvilinearPositions[i-self.getFirstInstant()] |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1798 |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1799 def getCurvilinearVelocityAtInstant(self, i): |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1800 return self.curvilinearVelocities[i-self.getFirstInstant()] |
b3f8b26ee838
modification for simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1096
diff
changeset
|
1801 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1802 def getXCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1803 return self.positions.getXCoordinates() |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1804 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1805 def getYCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1806 return self.positions.getYCoordinates() |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1807 |
1222
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1808 def setStationary(self): |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1809 '''Resets the position to the mean of existing positions and sets speeds to 0 |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1810 And does the same to the features |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1811 TODO: other options (provide x, y, what to do with features?)''' |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1812 meanPosition = self.positions.agg(mean) |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1813 self.positions.reset(meanPosition.x, meanPosition.y) |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1814 self.velocities.reset(0,0) |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1815 if self.hasFeatures(): |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1816 for f in self.getFeatures(): |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1817 f.setStationary() |
69b531c7a061
added methods to reset trajectories and change object coordinates (including features)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1219
diff
changeset
|
1818 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1819 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
|
1820 if withIds: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1821 objNum = self.getNum() |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1822 else: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1823 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
|
1824 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
|
1825 for f in self.getFeatures(): |
517
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1826 f.positions.plot('r', True, timeStep, **kwargs) |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1827 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
|
1828 else: |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1829 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
|
1830 |
851
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1831 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
|
1832 if withIds: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1833 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
|
1834 else: |
07fb949ff98f
added display of object id
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
845
diff
changeset
|
1835 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs) |
22 | 1836 |
1210
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1837 def plotOutlineAtInstant(self, t, options = '', withVelocity = False, velocityMultiply = 5, arrowWidth=0.1, **kwargs): |
1203
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
1838 if self.hasFeatures(): |
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
1839 points = [f.getPositionAtInstant(t) for f in self.getFeatures()] |
1205
3905b393ade0
kitti loading code seems to be working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1203
diff
changeset
|
1840 Point.plotAll(points, True, options, **kwargs) |
1210
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1841 if withVelocity: |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1842 p = self.getPositionAtInstant(t) |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1843 v = self.getVelocityAtInstant(t)*velocityMultiply |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1844 arrow(p.x, p.y, v.x, v.y, width=arrowWidth) |
1203
7b3384a8e409
second version of code loading kitti data, to clean
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1197
diff
changeset
|
1845 |
626
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
1846 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
|
1847 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
|
1848 |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1849 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
|
1850 speeds = framerate*self.getSpeeds(nInstantsIgnoredAtEnds) |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1851 coef = utils.linearRegression(list(range(len(speeds))), speeds) |
1240
bb14f919d1cb
cleaned use of centile (np only) and added info in classify-objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1233
diff
changeset
|
1852 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), percentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0])) |
377 | 1853 if display: |
672
5473b7460375
moved and rationalized imports in modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
666
diff
changeset
|
1854 from matplotlib.pyplot import figure, axis |
377 | 1855 figure(1) |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
1856 self.plot() |
379 | 1857 axis('equal') |
377 | 1858 figure(2) |
1859 plot(list(self.getTimeInterval()), speeds) | |
877
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1860 figure(3) |
d1ff6917d082
added savitzky golay filter for accelerations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
869
diff
changeset
|
1861 plot(list(self.getTimeInterval()), self.getAccelerations(9, 3, speeds = speeds)) # arbitrary parameter |
377 | 1862 |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1863 @staticmethod |
653
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1864 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
|
1865 '''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
|
1866 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
|
1867 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
|
1868 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
|
1869 maxDistance = minDistance |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1870 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
|
1871 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
|
1872 if d<minDistance: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1873 minDistance = d |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1874 elif d>maxDistance: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1875 maxDistance = d |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1876 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
|
1877 else: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1878 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
|
1879 |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1880 @staticmethod |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1881 def distances(obj1, obj2, instant1, _instant2 = None): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1882 '''Returns the distances between all features of the 2 objects |
653
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1883 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
|
1884 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
|
1885 if _instant2 is None: |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1886 instant2 = instant1 |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1887 else: |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1888 instant2 = _instant2 |
546 | 1889 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)] |
1890 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)] | |
1891 return cdist(positions1, positions2, metric = 'euclidean') | |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1892 |
546 | 1893 @staticmethod |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1894 def minDistance(obj1, obj2, instant1, instant2 = None): |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1895 return MovingObject.distances(obj1, obj2, instant1, instant2).min() |
546 | 1896 |
1897 @staticmethod | |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1898 def maxDistance(obj1, obj2, instant, instant2 = None): |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1899 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
|
1900 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1901 def maxSize(self): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1902 '''Returns the max distance between features |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1903 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
|
1904 if hasattr(self, 'features'): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1905 nFeatures = -1 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1906 tMaxFeatures = 0 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1907 for t in self.getTimeInterval(): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1908 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
|
1909 if n > nFeatures: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1910 nFeatures = n |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1911 tMaxFeatures = t |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1912 return MovingObject.maxDistance(self, self, tMaxFeatures) |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1913 else: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1914 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
|
1915 return None |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1916 |
550 | 1917 def setRoutes(self, startRouteID, endRouteID): |
1918 self.startRouteID = startRouteID | |
1919 self.endRouteID = endRouteID | |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1920 |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1921 def getInstantsCrossingLine(self, p1, p2, computeOrientations = False): |
55
88d5ee5ac164
updated comments and added shell for interaction between road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
54
diff
changeset
|
1922 '''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
|
1923 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
|
1924 empty list if there is no crossing''' |
1170
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1925 indices, intersections, rightToLeftOrientations = self.positions.getIntersections(p1, p2, computeOrientations) |
b55adb13f262
added functions on line crossing orientation and important reorganization and cleaning of related code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1146
diff
changeset
|
1926 return [t+self.getFirstInstant() for t in indices], intersections, rightToLeftOrientations |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
1927 |
949
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1928 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
|
1929 '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
|
1930 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
|
1931 self.prototypeSimilarities = [] |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1932 for proto in prototypes: |
d6c1c05d11f5
modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
945
diff
changeset
|
1933 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
|
1934 similarities = lcss.similarityTable[-1, :-1].astype(float) |
1109
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
1935 self.prototypeSimilarities.append(similarities/minimum(arange(1., len(similarities)+1), proto.getMovingObject().length()*ones(len(similarities)))) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
1936 |
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
|
1937 @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
|
1938 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
|
1939 '''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
|
1940 |
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
|
1941 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
|
1942 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
|
1943 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
|
1944 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
|
1945 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
|
1946 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
|
1947 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
|
1948 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
|
1949 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
|
1950 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
|
1951 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
|
1952 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
|
1953 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
|
1954 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
|
1955 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
|
1956 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
|
1957 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
|
1958 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
|
1959 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
|
1960 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
1961 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1962 '''Predicts the position of object at instant+deltaT, |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
223
diff
changeset
|
1963 at constant speed''' |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
1964 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
|
1965 |
1086
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1966 def projectCurvilinear(self, alignments, halfWidth = 3): |
8734742c08c0
major refactoring of curvilinear trajectory projections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1085
diff
changeset
|
1967 self.curvilinearPositions = CurvilinearTrajectory.fromTrajectoryProjection(self.getPositions(), alignments, halfWidth) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1968 |
562
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1969 def computeSmoothTrajectory(self, minCommonIntervalLength): |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1970 '''Computes the trajectory as the mean of all features |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1971 if a feature exists, its position is |
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
1972 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1973 Warning work in progress |
562
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1974 TODO? not use the first/last 1-.. positions''' |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1975 nFeatures = len(self.features) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1976 if nFeatures == 0: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1977 print('Empty object features\nCannot compute smooth trajectory') |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1978 else: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1979 # compute the relative position vectors |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1980 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1981 for i in range(nFeatures): |
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
1982 for j in range(i): |
562
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1983 fi = self.features[i] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1984 fj = self.features[j] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1985 inter = fi.commonTimeInterval(fj) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1986 if inter.length() >= minCommonIntervalLength: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1987 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
|
1988 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
|
1989 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
|
1990 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
|
1991 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
|
1992 relativePositions[(j,i)] = -relativePositions[(i,j)] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1993 |
1212
af329f3330ba
work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1210
diff
changeset
|
1994 def getBoundingPolygon(self, t, shape = 'raw'): |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
1995 '''Returns a bounding box for the feature positions at instant |
1210
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1996 bounding box format is a list of points |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1997 shape is the type of output shape: |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1998 - raw means the list of feature positions |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
1999 - rect means the bounding rectangle aligned with velocity''' |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
2000 if self.hasFeatures(): |
1210
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2001 positions = [f.getPositionAtInstant(t) for f in self.getFeatures() if f.existsAtInstant(t)] |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2002 if shape == 'rect': |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2003 return Point.boundingRectangle(positions, self.getVelocityAtInstant(t)) |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2004 elif shape == 'raw': |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2005 return positions |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2006 else: |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2007 print('Unknown shape') |
1bad5f9b60de
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1209
diff
changeset
|
2008 return None |
1106
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
2009 else: |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
2010 print('Object {} has no features'.format(self.getNum())) |
799ef82caa1a
added code to compute bounding rectangle around each user
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1100
diff
changeset
|
2011 return None |
1109
6e8ab471ebd4
minor adjustment to Lionel s needs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1106
diff
changeset
|
2012 |
1132
09ef0dc994a0
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1126
diff
changeset
|
2013 def motDistanceAtInstant(self, obj, instant): |
09ef0dc994a0
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1126
diff
changeset
|
2014 '''Returns distance for computing CLEAR MOT metrics |
09ef0dc994a0
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1126
diff
changeset
|
2015 (returns an actual value, otherwise munkres does not terminate)''' |
09ef0dc994a0
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1126
diff
changeset
|
2016 return Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant)) |
09ef0dc994a0
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1126
diff
changeset
|
2017 |
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
|
2018 ### |
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
|
2019 # 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
|
2020 ### |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2021 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
|
2022 '''Classifies slow and fast road users |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
2023 slow: non-motorized -> pedestrians |
607 | 2024 fast: motorized -> cars |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
2025 |
608
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
2026 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
|
2027 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
|
2028 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
|
2029 if aggregationFunc(speeds) >= threshold: |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
2030 self.setUserType(userType2Num['car']) |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
2031 else: |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
2032 self.setUserType(userType2Num['pedestrian']) |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
2033 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2034 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
|
2035 '''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
|
2036 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
|
2037 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
|
2038 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
|
2039 |
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
|
2040 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
|
2041 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
|
2042 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
|
2043 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
|
2044 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
|
2045 return x''' |
626
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
2046 if not hasattr(self, 'aggregatedSpeed'): |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2047 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds)) |
581
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
2048 userTypeProbabilities = {} |
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
2049 for userTypename in speedProbabilities: |
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
2050 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
|
2051 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
|
2052 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
|
2053 |
928
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
2054 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
|
2055 '''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
|
2056 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2057 TODO? compute speed for longest feature?''' |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2058 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds)) |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2059 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM is None: |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2060 self.appearanceClassifier = pedBikeCarSVM |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2061 elif self.aggregatedSpeed < bikeCarSpeedThreshold: |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2062 self.appearanceClassifier = bikeCarSVM |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2063 else: |
959
4f32d82ca390
corrected error due to change in Hog (scikit image)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
956
diff
changeset
|
2064 self.appearanceClassifier = carClassifier |
928
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
2065 # project feature positions |
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
2066 if self.hasFeatures(): |
063d1267585d
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
921
diff
changeset
|
2067 for f in self.getFeatures(): |
935
0e63a918a1ca
updated classify-objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
933
diff
changeset
|
2068 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
|
2069 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
|
2070 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
|
2071 |
929
be28a3538dc9
work in progress on projection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
928
diff
changeset
|
2072 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
|
2073 '''Extracts the image box around the object |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2074 (of square size max(width, height) of the box around the features, |
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
|
2075 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
|
2076 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
|
2077 and applies the SVM model on it''' |
929
be28a3538dc9
work in progress on projection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
928
diff
changeset
|
2078 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
|
2079 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
|
2080 hog = cvutils.HOG(croppedImg, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm) |
993
e8eabef7857c
update to OpenCV3 for python
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
987
diff
changeset
|
2081 self.userTypes[instant] = self.appearanceClassifier.predict(hog.reshape(1,hog.size)) |
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
|
2082 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
|
2083 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
|
2084 |
898
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
2085 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
|
2086 '''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
|
2087 (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
|
2088 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2089 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
|
2090 With default parameters, the general (ped-bike-car) classifier will be used |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2091 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2092 Considered categories are the keys of speedProbabilities''' |
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2093 if not hasattr(self, 'aggregatedSpeed') or not hasattr(self, 'userTypes'): |
845 | 2094 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
|
2095 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
|
2096 |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2097 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
|
2098 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
|
2099 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
|
2100 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
|
2101 # compute P(Speed|Class) |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2102 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
|
2103 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
|
2104 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
|
2105 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
|
2106 # compute P(Class|Appearance) |
680
da1352b89d02
classification is working
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
675
diff
changeset
|
2107 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
|
2108 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
|
2109 for t in self.userTypes: |
898
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
2110 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
|
2111 # 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
|
2112 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
|
2113 for userTypeNum in userTypeProbabilities: |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
2114 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
|
2115 # 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
|
2116 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
|
2117 self.setUserType(userType2Num['unknown']) |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
2118 else: |
1fc901d983ed
better take into account unknown appearance classification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
893
diff
changeset
|
2119 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
|
2120 |
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
|
2121 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
|
2122 '''Classifies the object based on its location (projected to image space) |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2123 areas is a dictionary of matrix of the size of the image space |
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
|
2124 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
|
2125 |
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
|
2126 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
|
2127 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
|
2128 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
|
2129 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
|
2130 if homography is not None: |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
2131 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
|
2132 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
|
2133 self.projectedPositions = obj.positions |
523
ce4eaabacc26
modified internal implementation of user type for classifyUserTypeArea
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
522
diff
changeset
|
2134 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
|
2135 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
|
2136 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
|
2137 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
|
2138 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
|
2139 # 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
|
2140 return possibleUserTypes |
520
fd9641cbd24b
added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
517
diff
changeset
|
2141 |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
2142 @staticmethod |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
2143 def collisionCourseDotProduct(movingObject1, movingObject2, instant): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
2144 '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
|
2145 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
|
2146 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
|
2147 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
|
2148 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
2149 @staticmethod |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
2150 def collisionCourseCosine(movingObject1, movingObject2, instant): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
2151 '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
|
2152 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
|
2153 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
|
2154 |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
2155 |
921
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2156 class Prototype(object): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2157 'Class for a prototype' |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2158 |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2159 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
|
2160 self.filename = filename |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2161 self.num = num |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2162 self.trajectoryType = trajectoryType |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2163 self.nMatchings = nMatchings |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2164 self.movingObject = None |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2165 |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2166 def getFilename(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2167 return self.filename |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2168 def getNum(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2169 return self.num |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2170 def getTrajectoryType(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2171 return self.trajectoryType |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2172 def getNMatchings(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2173 return self.nMatchings |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2174 def getMovingObject(self): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2175 return self.movingObject |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2176 def setMovingObject(self, o): |
630934595871
work in progress with prototype class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
918
diff
changeset
|
2177 self.movingObject = o |
1035
933588568bec
major update to learn motion pattern, see program description
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1030
diff
changeset
|
2178 def __str__(self): |
933588568bec
major update to learn motion pattern, see program description
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1030
diff
changeset
|
2179 return '{} {} {}'.format(self.filename, self.num, self.trajectoryType) |
1037
6a6c37eb3a74
added function to load prototype assignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1035
diff
changeset
|
2180 def __eq__(self, p2): |
6a6c37eb3a74
added function to load prototype assignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1035
diff
changeset
|
2181 return self.filename == p2.filename and self.num == p2.num and self.trajectoryType == p2.trajectoryType |
6a6c37eb3a74
added function to load prototype assignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1035
diff
changeset
|
2182 def __hash__(self): |
6a6c37eb3a74
added function to load prototype assignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1035
diff
changeset
|
2183 return hash((self.filename, self.num, self.trajectoryType)) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
2184 |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
2185 ################## |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
2186 # Annotations |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
2187 ################## |
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
2188 |
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
|
2189 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
|
2190 '''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
|
2191 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
|
2192 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
|
2193 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
2194 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
|
2195 |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2196 Its center is the center of the box (generalize to other shapes?) |
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
|
2197 (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
|
2198 ''' |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
2199 |
1132
09ef0dc994a0
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1126
diff
changeset
|
2200 def __init__(self, topLeftPositions, bottomRightPositions, num = None, timeInterval = 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
|
2201 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
|
2202 self.topLeftPositions = topLeftPositions.getPositions() |
5800a87f11ae
corrected one bug and changed attribute names
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
588
diff
changeset
|
2203 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
|
2204 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
2205 def computeCentroidTrajectory(self, homography = None): |
939
a2f3f3ca241e
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
937
diff
changeset
|
2206 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
|
2207 if homography is not None: |
936
56cc8a1f7082
removed all old versions of projection methods
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
935
diff
changeset
|
2208 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
|
2209 |
1081
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2210 def matchObjects(obj1, obj2, instant, matchingDistance): |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2211 '''Indicates if obj matches obj2 with threshold matchingDistance |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2212 Returns distance if below matchingDistance, matchingDistance+1 otherwise |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2213 (returns an actual value, otherwise munkres does not terminate)''' |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2214 d = Point.distanceNorm2(obj1.getPositionAtInstant(instant), obj2.getPositionAtInstant(instant)) |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2215 if d < matchingDistance: |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2216 return d |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2217 else: |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2218 return matchingDistance + 1 |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2219 |
346b41cbc81a
added code idea from Paul St Aubin
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1079
diff
changeset
|
2220 # TODO class to have different matching methods, eg with something like matchObjects |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2221 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
|
2222 '''Computes the CLEAR MOT metrics |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2223 |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2224 Reference: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2225 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
|
2226 |
1190
d24d57e4de24
work on optimization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1185
diff
changeset
|
2227 objects and annotations are supposed to be 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
|
2228 current implementation is BBMovingObject (bounding boxes) |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
2229 mathingDistance is threshold on matching between annotation and object |
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
2230 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2231 TO: tracker output (objects) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2232 GT: ground truth (annotations) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2233 |
624
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
2234 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
|
2235 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
|
2236 mme number of mismatches |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
2237 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
|
2238 gt number of GT.frames |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
2239 |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2240 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
|
2241 matches is a dict |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2242 matches[i] is the list of matches for GT/TO i |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2243 the list of matches is a dict, indexed by time, for the TO/GT id matched at time t |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2244 (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
|
2245 |
624
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
2246 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
|
2247 (add argument useDistanceForWeights = False)''' |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2248 from munkres import Munkres |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2249 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2250 munk = Munkres() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2251 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
|
2252 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
|
2253 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
|
2254 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
|
2255 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
|
2256 mme = 0 # number of mismatches |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2257 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
|
2258 if returnMatches: |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2259 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
|
2260 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
|
2261 else: |
196a1fd498ba
removing unnecessary complexity of varying number of return elements
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
949
diff
changeset
|
2262 gtMatches = None |
196a1fd498ba
removing unnecessary complexity of varying number of return elements
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
949
diff
changeset
|
2263 toMatches = None |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
2264 for t in range(firstInstant, lastInstant+1): |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2265 previousMatches = matches.copy() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2266 # 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
|
2267 toDelete = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2268 for a in matches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2269 if a.existsAtInstant(t) and matches[a].existsAtInstant(t): |
1132
09ef0dc994a0
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1126
diff
changeset
|
2270 d = a.motDistanceAtInstant(matches[a], t) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2271 if d < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2272 dist += d |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
2273 else: |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2274 toDelete.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2275 else: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2276 toDelete.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2277 for a in toDelete: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2278 del matches[a] |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
2279 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2280 # match all unmatched GT-TO |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
2281 matchedGTs = list(matches.keys()) |
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
2282 matchedTOs = list(matches.values()) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2283 costs = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2284 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
|
2285 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
|
2286 nGTs = len(matchedGTs)+len(unmatchedGTs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2287 nTOs = len(matchedTOs)+len(unmatchedTOs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2288 if len(unmatchedTOs) > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2289 for a in unmatchedGTs: |
1133
c4d9c270f999
modification for performance computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1132
diff
changeset
|
2290 costs.append([a.motDistanceAtInstant(o, t) for o in unmatchedTOs]) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2291 if len(costs) > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2292 newMatches = munk.compute(costs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2293 for k,v in newMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2294 if costs[k][v] < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2295 matches[unmatchedGTs[k]]=unmatchedTOs[v] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2296 dist += costs[k][v] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2297 if debug: |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
2298 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.items()])) |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2299 if returnMatches: |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
2300 for a,o in matches.items(): |
723
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2301 gtMatches[a.getNum()][t] = o.getNum() |
e14e2101a5a9
returns detailed matching information for clear mot
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
690
diff
changeset
|
2302 toMatches[o.getNum()][t] = a.getNum() |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
2303 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2304 # compute metrics elements |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2305 ct += len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2306 mt += nGTs-len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2307 fpt += nTOs-len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2308 gt += nGTs |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2309 # compute mismatches |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2310 # 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
|
2311 mismatches = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2312 for a in matches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2313 if a in previousMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2314 if matches[a] != previousMatches[a]: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2315 mismatches.append(a) |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
2316 elif matches[a] in list(previousMatches.values()): |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2317 mismatches.append(matches[a]) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2318 for a in previousMatches: |
998
933670761a57
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
997
diff
changeset
|
2319 if a not in matches and previousMatches[a] in list(matches.values()): |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2320 mismatches.append(previousMatches[a]) |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2321 if debug: |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2322 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
|
2323 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
|
2324 # 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
|
2325 mme += len(set(mismatches)) |
1110
6bbcd9433732
formatting and addition of one method to CurvilinearTrajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1109
diff
changeset
|
2326 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2327 if ct > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2328 motp = dist/ct |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
2329 else: |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
2330 motp = None |
595
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
2331 if gt > 0: |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
2332 mota = 1.-float(mt+fpt+mme)/gt |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
2333 else: |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
2334 mota = None |
956
196a1fd498ba
removing unnecessary complexity of varying number of return elements
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
949
diff
changeset
|
2335 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
|
2336 |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
2337 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
|
2338 '''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
|
2339 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
|
2340 figure() |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
2341 for obj in objects: |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
2342 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
|
2343 axis('equal') |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
2344 |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
2345 |
1016 | 2346 if __name__ == "__main__": |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
2347 import doctest |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
2348 import unittest |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
2349 suite = doctest.DocFileSuite('tests/moving.txt') |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
2350 #suite = doctest.DocTestSuite() |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
2351 unittest.TextTestRunner().run(suite) |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
2352 #doctest.testmod() |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
2353 #doctest.testfile("example.txt") |
1096
9a32d63bae3f
update from Lionel Nebot Janvier
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
1094
diff
changeset
|
2354 if shapelyAvailable: |
635
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
2355 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
|
2356 unittest.TextTestRunner().run(suite) |