Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 516:bce1fe45d1b2

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
corrected bugs detected by tests (because of moving functions around modules
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 06 Jun 2014 14:14:01 -0400
parents 727e3c529519
children 47d9970ee954
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
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
4 import utils
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
5 import cvutils
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
6
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
7 from math import sqrt
345
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
8 from numpy import median
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
9
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
10 try:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
11 from shapely.geometry import Polygon, Point as shapelyPoint
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
12 from shapely.prepared import prep
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
13 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
14 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
15 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
16 shapelyAvailable = False
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
17
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
18 __metaclass__ = type
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
19
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
20 class Interval(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
21 '''Generic interval: a subset of real numbers (not iterable)'''
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
22 def __init__(self, first=0, last=-1, revert = False):
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
23 if revert and last<first:
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
24 self.first=last
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
25 self.last=first
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
26 else:
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
27 self.first=first
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
28 self.last=last
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
29
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
30 def __str__(self):
104
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
31 return '[{0}, {1}]'.format(self.first, self.last)
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
32
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
33 def __repr__(self):
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
34 return self.__str__()
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
35
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
36 def empty(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
37 return self.first > self.last
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
38
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
39 def center(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
40 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
41
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
42 def length(self):
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
43 '''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
44 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
45
96
9928c2fa72cc added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 93
diff changeset
46 def equal(self, i2):
9928c2fa72cc added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 93
diff changeset
47 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
48
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
49 def getList(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
50 return [self.first, self.last]
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
51
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
52 def contains(self, instant):
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
53 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
54
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
55 def inside(self, interval2):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
56 '''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
57 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
58
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
59 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
60 def union(cls, interval1, interval2):
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
61 '''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
62 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
63
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
64 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
65 def intersection(cls, interval1, interval2):
104
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
66 '''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
67 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
68
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
69 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
70 if not Interval.intersection(self, interval2).empty():
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
71 return 0
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
72 elif self.first > interval2.last:
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
73 return self.first - interval2.last
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
74 elif self.last < interval2.first:
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
75 return interval2.first - self.last
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
76 else:
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
77 return None
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
78
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
79
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
80 def unionIntervals(intervals):
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
81 'returns the smallest interval containing all intervals'
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
82 inter = intervals[0]
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
83 for i in intervals[1:]:
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
84 inter = Interval.union(inter, i)
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
85 return inter
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
86
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
87
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
88 class TimeInterval(Interval):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
89 '''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
90
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
91 For example: based on frame numbers (hence the modified length method)
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
92 It may be modified directly by setting first and last'''
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
93
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
94 def __init__(self, first=0, last=-1):
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
95 super(TimeInterval, self).__init__(first, last, False)
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
96
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
97 @staticmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
98 def fromInterval(inter):
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
99 return TimeInterval(inter.first, inter.last)
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
100
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
101 def __getitem__(self, i):
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
102 if not self.empty():
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
103 return self.first+i
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
104
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
105 def __iter__(self):
107
916678481896 corrected bug for TimeInterval interation and added corresponding test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 105
diff changeset
106 self.iterInstantNum = -1
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
107 return self
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
108
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
109 def next(self):
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
110 if self.iterInstantNum >= self.length()-1:
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
111 raise StopIteration
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
112 else:
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
113 self.iterInstantNum += 1
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
114 return self[self.iterInstantNum]
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
115
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
116 def length(self):
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
117 '''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
118 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
119
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
120 # class BoundingPolygon:
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
121 # '''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
122 # with methods to create intersection, unions...
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
123 # '''
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
124 # We will use the polygon class of Shapely
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
125
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
126 class STObject(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
127 '''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
128 (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
129
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
130 It may not mean that the object is defined
6
597d61c1eebe minor doc update
Nicolas Saunier <nico@confins.net>
parents: 2
diff changeset
131 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
132
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
133 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
134 self.num = num
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
135 self.timeInterval = timeInterval
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
136 self.boundingPolygon = boundingPolygon
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
137
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
138 def empty(self):
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
139 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
140
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
141 def getNum(self):
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
142 return self.num
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
143
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
144 def getFirstInstant(self):
40
9f16aee24b7e corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents: 39
diff changeset
145 return self.timeInterval.first
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
146
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
147 def getLastInstant(self):
40
9f16aee24b7e corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents: 39
diff changeset
148 return self.timeInterval.last
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
149
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
150 def getTimeInterval(self):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
151 return self.timeInterval
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
152
108
6efe470ea5e5 added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 107
diff changeset
153 def existsAtInstant(self, t):
6efe470ea5e5 added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 107
diff changeset
154 return self.timeInterval.contains(t)
6efe470ea5e5 added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 107
diff changeset
155
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
156 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
157 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval())
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
158
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
159 class Point(object):
25
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
160 def __init__(self, x, y):
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
161 self.x = x
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
162 self.y = y
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
163
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
164 def __str__(self):
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
165 return '(%f,%f)'%(self.x,self.y)
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
166
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
167 def __repr__(self):
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
168 return self.__str__()
25
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
169
184
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
170 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
171 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
172
25
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
173 def __sub__(self, other):
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
174 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
175
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
176 def __neg__(self):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
177 return Point(-self.x, -self.y)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
178
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 104
diff changeset
179 def multiply(self, alpha):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 104
diff changeset
180 return Point(self.x*alpha, self.y*alpha)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 104
diff changeset
181
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
182 def plot(self, options = 'o', **kwargs):
41
eb78c6edc0c8 added drawing for Point
Nicolas Saunier <nico@confins.net>
parents: 40
diff changeset
183 from matplotlib.pylab import plot
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
184 plot([self.x], [self.y], options, **kwargs)
41
eb78c6edc0c8 added drawing for Point
Nicolas Saunier <nico@confins.net>
parents: 40
diff changeset
185
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
186 def norm2Squared(self):
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
187 '''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
188 return self.x*self.x+self.y*self.y
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
189
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
190 def norm2(self):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
191 '''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
192 return sqrt(self.norm2Squared())
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
193
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
194 def norm1(self):
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
195 return abs(self.x)+abs(self.y)
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
196
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
197 def normMax(self):
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
198 return max(abs(self.x),abs(self.y))
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
199
64
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
200 def aslist(self):
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
201 return [self.x, self.y]
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
202
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
203 def astuple(self):
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
204 return (self.x, self.y)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
205
223
c31722fcc9de minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 214
diff changeset
206 def asint(self):
c31722fcc9de minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 214
diff changeset
207 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
208
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
209 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
210 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
211 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
212
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
213 def project(self, homography):
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
214 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
215 projected = cvutils.projectArray(homography, array([[self.x], [self.y]]))
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
216 return Point(projected[0], projected[1])
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
217
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
218 def inPolygonNoShapely(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
219 '''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
220 (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
221
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
222 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
223
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
224 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
225
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
226 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
227 counter = 0;
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
228
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
229 p1 = polygon[0,:];
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
230 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
231 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
232 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
233 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
234 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
235 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
236 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
237 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
238 counter+=1;
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
239 p1=p2
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
240 return (counter%2 == 1);
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
241
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
242 @staticmethod
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
243 def dot(p1, p2):
89
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
244 'Scalar product'
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
245 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
246
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
247 @staticmethod
90
f84293ad4611 renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 89
diff changeset
248 def cross(p1, p2):
f84293ad4611 renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 89
diff changeset
249 'Cross product'
89
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
250 return p1.x*p2.y-p1.y*p2.x
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
251
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
252 @staticmethod
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
253 def cosine(p1, p2):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
254 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
255
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
256 @staticmethod
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
257 def distanceNorm2(p1, p2):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
258 return (p1-p2).norm2()
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
259
64
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
260 @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
261 def plotAll(points, **kwargs):
64
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
262 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
263 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
264
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
265 def similarOrientation(self, refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
266 '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
267 return Point.cosine(self, refDirection) >= cosineThreshold
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
268
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
269 @staticmethod
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
270 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
271 '''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
272 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
273 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
274 and the circle of radius collisionThreshold around the other road user'''
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
275 from math import sqrt
504
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
276 dv = v1-v2
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
277 dp = p1-p2
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
278 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
279 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
280 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
281
504
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
282 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
283 if delta >= 0:
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
284 deltaRoot = sqrt(delta)
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
285 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
286 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
287 if ttc1 >= 0 and ttc2 >= 0:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
288 ttc = min(ttc1,ttc2)
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
289 else:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
290 if ttc1 < 0:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
291 ttc = ttc2
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
292 elif ttc2 < 0:
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
293 ttc = ttc1
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
294 else: # ttc1 < 0 and ttc2 < 0:
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
295 ttc = None
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
296 else:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
297 ttc = None
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
298 return ttc
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
299
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
300
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
301 if shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
302 def pointsInPolygon(points, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
303 '''Optimized tests of a series of points within (Shapely) polygon '''
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
304 prepared_polygon = prep(polygon)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
305 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
306
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
307
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
308 class NormAngle(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
309 '''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
310
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
311 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
312 self.norm = norm
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
313 self.angle = angle
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
314
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
315 @staticmethod
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
316 def fromPoint(p):
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
317 from math import atan2
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
318 norm = p.norm2()
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
319 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
320 angle = atan2(p.y, p.x)
339
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
321 else:
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
322 angle = 0.
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
323 return NormAngle(norm, angle)
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
324
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
325 def __add__(self, other):
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
326 '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
327 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
328
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
329 def getPoint(self):
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
330 from math import cos, sin
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
331 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
332
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
333
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
334 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
335 '''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
336 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
337
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
338 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
339 '''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
340 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
341 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
342 predictedSpeedTheta = speedOrientation+control
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
343 if maxSpeed:
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
344 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
345 predictedPosition = position+predictedSpeedTheta.getPoint()
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
346 return predictedPosition, predictedSpeedTheta
245
bd8ab323c198 corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 244
diff changeset
347
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
348
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
349 class FlowVector(object):
184
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
350 '''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
351 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
352 def __init__(self, position, velocity):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
353 '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
354 self.position = position
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
355 self.velocity = velocity
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
356
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
357 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
358 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
359
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
360 def multiply(self, alpha):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
361 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
362
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
363 def plot(self, options = '', **kwargs):
184
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
364 from matplotlib.pylab import plot
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
365 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
366 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
367
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
368 @staticmethod
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
369 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
370 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
371
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
372 def intersection(p1, p2, dp1, dp2):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
373 '''Returns the intersection point between the two lines
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
374 defined by the respective vectors (dp) and origin points (p)'''
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
375 from numpy import matrix
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
376 from numpy.linalg import linalg
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
377 A = matrix([[dp1.y, -dp1.x],
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
378 [dp2.y, -dp2.x]])
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
379 B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
380 [dp2.y*p2.x-dp2.x*p2.y]])
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
381
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
382 if linalg.det(A) == 0:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
383 return None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
384 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
385 intersection = linalg.solve(A,B)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
386 return Point(intersection[0,0], intersection[1,0])
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
387
152
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 148
diff changeset
388 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
389 '''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
390
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
391 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
392 return None
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 148
diff changeset
393 else:
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
394 dp1 = p2-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
395 dp3 = p4-p3
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
396 inter = intersection(p1, p3, dp1, dp3)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
397 if (inter != None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
398 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
399 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
400 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
401 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
402 return inter
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
403 else:
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
404 return None
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
405
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
406 # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection
152
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 148
diff changeset
407
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
408 class Trajectory(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
409 '''Class for trajectories: temporal sequence of positions
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
410
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
411 The class is iterable'''
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
412
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
413 def __init__(self, positions=None):
331
40790d93200e corrected a bug for Trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 329
diff changeset
414 if positions != None:
113
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
415 self.positions = positions
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
416 else:
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
417 self.positions = [[],[]]
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
418
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
419 @staticmethod
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
420 def load(line1, line2):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
421 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
422 [float(n) for n in line2.split(' ')]])
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
423
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
424 @staticmethod
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
425 def fromPointList(points):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
426 t = Trajectory()
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
427 for p in points:
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
428 t.addPosition(p)
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
429 return t
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
430
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
431 def __len__(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
432 return len(self.positions[0])
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
433
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
434 def length(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
435 return self.__len__()
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
436
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
437 def __str__(self):
39
e47168f6b694 corrected printing bug
Nicolas Saunier <nico@confins.net>
parents: 38
diff changeset
438 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
439
69
cc192d0450b3 added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 68
diff changeset
440 def __repr__(self):
cc192d0450b3 added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 68
diff changeset
441 return str(self)
cc192d0450b3 added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 68
diff changeset
442
23
5f2921ad4f7e made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents: 22
diff changeset
443 def __getitem__(self, i):
25
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
444 return Point(self.positions[0][i], self.positions[1][i])
23
5f2921ad4f7e made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents: 22
diff changeset
445
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
446 def __iter__(self):
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
447 self.iterInstantNum = 0
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
448 return self
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
449
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
450 def next(self):
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
451 if self.iterInstantNum >= self.length():
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
452 raise StopIteration
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
453 else:
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
454 self.iterInstantNum += 1
23
5f2921ad4f7e made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents: 22
diff changeset
455 return self[self.iterInstantNum-1]
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
456
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
457 def setPositionXY(self, i, x, y):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
458 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
459 self.positions[0][i] = x
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
460 self.positions[1][i] = y
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
461
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
462 def setPosition(self, i, p):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
463 self.setPositionXY(i, p.x, p.y)
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
464
71
45e958ccd9bd added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 69
diff changeset
465 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
466 self.positions[0].append(x)
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
467 self.positions[1].append(y)
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
468
71
45e958ccd9bd added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 69
diff changeset
469 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
470 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
471
343
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
472 def duplicateLastPosition(self):
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
473 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
474 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
475
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
476 @staticmethod
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
477 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, **kwargs):
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
478 from matplotlib.pylab import plot
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
479 if lastCoordinate == None:
270
05c9b0cb8202 updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
480 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
481 elif 0 <= lastCoordinate <= len(positions[0]):
270
05c9b0cb8202 updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
482 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
483 if withOrigin:
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
484 plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs)
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
485
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
486 def project(self, homography):
318
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
487 return Trajectory(cvutils.projectTrajectory(homography, self.positions))
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
488
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
489 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
490 Trajectory._plot(self.positions, options, withOrigin, None, timeStep, **kwargs)
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
491
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
492 def plotAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
493 Trajectory._plot(self.positions, options, withOrigin, lastCoordinate, timeStep, **kwargs)
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
494
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
495 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
496 from matplotlib.pylab import plot
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
497 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]],
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
498 [-x*nPixelsPerUnitDistance+imageHeight for x in self.positions[1]]]
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
499 Trajectory._plot(imgPositions, options, withOrigin, timeStep, **kwargs)
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
500
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
501 def getXCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
502 return self.positions[0]
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
503
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
504 def getYCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
505 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
506
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 91
diff changeset
507 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
508 from numpy.core.multiarray import array
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 91
diff changeset
509 return array(self.positions)
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
510
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
511 def xBounds(self):
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
512 # 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
513 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
514
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
515 def yBounds(self):
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
516 # 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
517 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
518
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
519 def add(self, traj2):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
520 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
521 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
522 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
523 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
524 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
525 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
526 [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
527
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
528 def subtract(self, traj2):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
529 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
530 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
531 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
532 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
533 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
534 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
535 [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
536
339
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
537 def differentiate(self):
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
538 diff = Trajectory()
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
539 for i in xrange(1, self.length()):
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
540 diff.addPosition(self[i]-self[i-1])
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
541 return diff
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
542
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
543 def norm(self):
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
544 '''Returns the list of the norms at each instant'''
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
545 # def add(x, y): return x+y
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
546 # sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]])
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
547 # return sqrt(sq)
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
548 from numpy import hypot
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
549 return hypot(self.positions[0], self.positions[1])
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
550
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
551 def cumulatedDisplacement(self):
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
552 'Returns the sum of the distances between each successive point'
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
553 displacement = 0
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
554 for i in xrange(self.length()-1):
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
555 displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1))
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
556 return displacement
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
557
381
387cc0142211 script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 379
diff changeset
558 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
387cc0142211 script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 379
diff changeset
559 '''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
560 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
561 count = 0
383
0ce2210790b1 fixed stupid naming bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 381
diff changeset
562 lengthThreshold = float(self.length())*minProportion
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
563 for p in self:
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
564 if p.similarOrientation(refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
565 count += 1
381
387cc0142211 script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 379
diff changeset
566 if count > lengthThreshold:
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
567 return True
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
568 return False
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
569
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
570
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
571 def wiggliness(self):
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
572 return self.cumulatedDisplacement()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)))
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
573
83
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
574 def getIntersections(self, p1, p2):
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
575 '''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
576 intersects with the segment of extremities p1 and p2
daa05fae1a70 modified the type of the result of interval lengths to float, added comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 90
diff changeset
577 the list is empty if there is no crossing'''
83
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
578 indices = []
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
579
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
580 for i in xrange(self.length()-1):
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
581 q1=self.__getitem__(i)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
582 q2=self.__getitem__(i+1)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
583 p = utils.segmentIntersection(q1, q2, p1, p2)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
584 if p:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
585 if q1.x != q2.x:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
586 ratio = (p.x-q1.x)/(q2.x-q1.x)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
587 elif q1.y != q2.y:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
588 ratio = (p.y-q1.y)/(q2.y-q1.y)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
589 else:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
590 ratio = 0
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
591 indices.append(i+ratio)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
592 return indices
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
593
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
594 def getTrajectoryInInterval(self, inter):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
595 if inter.first >=0 and inter.last<= self.length():
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
596 return Trajectory([self.positions[0][inter.first:inter.last],
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
597 self.positions[1][inter.first:inter.last]])
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
598 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
599 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
600
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
601 def getTrajectoryInPolygonNoShapely(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
602 '''Returns the trajectory built with the set of points inside the polygon
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
603 (array of Nx2 coordinates of the polygon vertices)'''
113
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
604 traj = 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
605 for p in self:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
606 if p.inPolygonNoShapely(polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
607 traj.addPosition(p)
113
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
608 return traj
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
609
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
610 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
611 def getTrajectoryInPolygon(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
612 '''Returns the trajectory built with the set of points inside the (shapely) polygon'''
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
613 traj = Trajectory()
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
614 points = [p.asShapely() for p in self]
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
615 for p in pointsInPolygon(points, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
616 traj.addPositionXY(p.x, p.y)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
617 return traj
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
618
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
619 @staticmethod
369
027e254f0b53 lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 368
diff changeset
620 def lcss(t1, t2, lcss):
027e254f0b53 lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 368
diff changeset
621 return lcss.compute(t1, t2)
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
622
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
623 class CurvilinearTrajectory(Trajectory):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
624 '''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
625 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
626 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
627
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
628 def __init__(self, S = [], Y = [], lanes = []):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
629 self.positions = [S,Y]
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
630 self.lanes = lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
631
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
632 def __getitem__(self,i):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
633 return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
634
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
635 def getSCoordinates(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
636 return self.getXCoordinates()
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
637
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
638 def getLanes(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
639 return self.lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
640
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
641 def addPosition(self, s, y, lane):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
642 self.addPositionXY(s,y)
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
643 self.lanes.append(lane)
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
644
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
645 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
646 self.setPositionXY(i, s, y)
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
647 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
648 self.lanes[i] = lane
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
649
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
650 def getIntersections(self, S1, lane = None):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
651 '''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
652 goes past the curvilinear coordinate S1
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
653 (in provided lane if lane != None)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
654 the list is empty if there is no crossing'''
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
655 indices = []
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
656 for i in xrange(self.length()-1):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
657 q1=self.__getitem__(i)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
658 q2=self.__getitem__(i+1)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
659 if q1[0] <= S1 <= q2[0] and lane == None or (self.lanes[i] == lane and self.lanes[i+1] == lane):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
660 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
661 return indices
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
662
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
663 ##################
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
664 # Moving Objects
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
665 ##################
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
666
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
667 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
668 'car',
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
669 '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
670 'motorcycle',
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
671 'bicycle',
185
c06379f25ab8 utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 184
diff changeset
672 'bus',
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
673 'truck']
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
674
185
c06379f25ab8 utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 184
diff changeset
675 userType2Num = utils.inverseEnumeration(userTypeNames)
c06379f25ab8 utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 184
diff changeset
676
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
677 class MovingObject(STObject):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
678 '''Class for moving objects: a spatio-temporal object
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
679 with a trajectory and a geometry (constant volume over time) and a usertype (e.g. road user) coded as a number (see
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
680 '''
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
681
411
31604ef1cad4 added hog functions and the display of road user types if known
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 383
diff changeset
682 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
683 super(MovingObject, self).__init__(num, timeInterval)
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
684 self.positions = positions
345
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
685 self.velocities = velocities
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
686 self.geometry = geometry
54
c354d41ef7cd corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 49
diff changeset
687 self.userType = userType
317
d280b881e860 added indicator min distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 290
diff changeset
688 self.features = None
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
689 # compute bounding polygon from trajectory
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
690
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
691 def getObjectInTimeInterval(self, inter):
54
c354d41ef7cd corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 49
diff changeset
692 '''Returns a new object extracted from self,
c354d41ef7cd corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 49
diff changeset
693 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
694 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
695 if not intersection.empty():
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 96
diff changeset
696 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
697 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
698 if self.velocities:
97
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 96
diff changeset
699 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval)
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
700 return obj
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
701 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
702 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
703 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
704
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
705 def length(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
706 return self.timeInterval.length()
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
707
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
708 def getPositions(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
709 return self.positions
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
710
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
711 def getVelocities(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
712 return self.velocities
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
713
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
714 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
715 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
716
335
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
717 def getCurvilinearPositions(self):
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
718 if hasattr(self, 'curvilinearPositions'):
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
719 return self.curvilinearPositions
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
720 else:
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
721 return None
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
722
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
723 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
724 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
725
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
726 def setFeatures(self, features):
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
727 self.features = [features[i] for i in self.featureNumbers]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
728
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
729 def getSpeeds(self):
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
730 return self.getVelocities().norm()
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
731
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
732 def getPositionAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
733 return self.positions[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
734
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
735 def getVelocityAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
736 return self.velocities[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
737
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
738 def getPositionAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
739 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
740
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
741 def getVelocityAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
742 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
743
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
744 def getXCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
745 return self.positions.getXCoordinates()
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
746
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
747 def getYCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
748 return self.positions.getYCoordinates()
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
749
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
750 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
751 self.positions.plot(options, withOrigin, timeStep, **kwargs)
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
752
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
753 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
754 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, imageHeight, options, withOrigin, timeStep, **kwargs)
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
755
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
756 def play(self, videoFilename, homography = None):
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
757 cvutils.displayTrajectories(videoFilename, [self], homography, self.getFirstInstant(), self.getLastInstant())
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
758
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
759 def speedDiagnostics(self, framerate = 1., display = False):
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
760 from numpy import std
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
761 from scipy.stats import scoreatpercentile
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
762 speeds = framerate*self.getSpeeds()
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
763 coef = utils.linearRegression(range(len(speeds)), speeds)
379
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 378
diff changeset
764 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
765 if display:
379
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 378
diff changeset
766 from matplotlib.pyplot import figure, plot, axis
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
767 figure(1)
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
768 self.plot()
379
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 378
diff changeset
769 axis('equal')
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
770 figure(2)
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
771 plot(list(self.getTimeInterval()), speeds)
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
772
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
773 @staticmethod
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
774 def distances(obj1, obj2, instant):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
775 from scipy.spatial.distance import cdist
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
776 positions1 = [f.getPositionAtInstant(instant).astuple() for f in obj1.features if f.existsAtInstant(instant)]
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
777 positions2 = [f.getPositionAtInstant(instant).astuple() for f in obj2.features if f.existsAtInstant(instant)]
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
778 return cdist(positions1, positions2, metric = 'euclidean')
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
779
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
780 @staticmethod
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
781 def minDistance(obj1, obj2, instant):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
782 return MovingObject.distances(obj1, obj2, instant).min()
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
783
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
784 @staticmethod
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
785 def maxDistance(obj1, obj2, instant):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
786 return MovingObject.distances(obj1, obj2, instant).max()
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
787
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
788 def maxSize(self):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
789 '''Returns the max distance between features
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
790 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
791 if hasattr(self, 'features'):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
792 nFeatures = -1
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
793 tMaxFeatures = 0
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
794 for t in self.getTimeInterval():
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
795 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
796 if n > nFeatures:
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
797 nFeatures = n
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
798 tMaxFeatures = t
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
799 return MovingObject.maxDistance(self, self, tMaxFeatures)
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
800 else:
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
801 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
802 return None
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
803
83
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
804 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
805 '''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
806 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
807 empty list if there is no crossing'''
83
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
808 indices = self.positions.getIntersections(p1, p2)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
809 return [t+self.getFirstInstant() for t in indices]
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
810
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
811 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
812 '''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
813 at constant speed'''
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
814 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
815
501
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
816 def classifyUserTypeSpeed(self, threshold, statisticsFunc = median, ignoreNInstantsAtEnds = 0):
345
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
817 '''Classifies slow and fast road users
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
818 slow: non-motorized -> pedestrians
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
819 fast: motorized -> cars'''
501
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
820 if ignoreNInstantsAtEnds > 0:
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
821 speeds = self.velocities.norm()[ignoreNInstantsAtEnds:-ignoreNInstantsAtEnds]
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
822 else:
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
823 speeds = self.velocities.norm()
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
824 if statisticsFunc(speeds) >= threshold:
345
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
825 self.setUserType(userType2Num['car'])
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
826 else:
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
827 self.setUserType(userType2Num['pedestrian'])
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
828
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
829 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
830 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
831 '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
832 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
833 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
834 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
835
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
836 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
837 def collisionCourseCosine(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
838 '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
839 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
840 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
841
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
842 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
843 '''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
844 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
845 figure()
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
846 for obj in objects:
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
847 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
848 axis('equal')
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
849
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
850
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
851 if __name__ == "__main__":
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
852 import doctest
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
853 import unittest
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
854 suite = doctest.DocFileSuite('tests/moving.txt')
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
855 #suite = doctest.DocTestSuite()
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
856 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
857 #doctest.testmod()
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
858 #doctest.testfile("example.txt")