Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 935:0e63a918a1ca

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