Mercurial Hosting > traffic-intelligence

annotate trafficintelligence/moving.py @ 1249:2aa56b101041

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