Mercurial Hosting > traffic-intelligence
annotate python/moving.py @ 368:2db4e76599a1
implemented subsequence extraction and rearranged arguments
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Mon, 15 Jul 2013 15:08:53 -0400 |
| parents | 619ae9a9a788 |
| children | 027e254f0b53 |
| 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 | 9 |
|
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
10 #from shapely.geometry import Polygon |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
11 |
|
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
12 __metaclass__ = type |
|
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
13 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
14 class Interval(object): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
15 '''Generic interval: a subset of real numbers (not iterable)''' |
| 26 | 16 def __init__(self, first=0, last=-1, revert = False): |
| 17 if revert and last<first: | |
| 18 self.first=last | |
| 19 self.last=first | |
| 20 else: | |
| 21 self.first=first | |
| 22 self.last=last | |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
23 |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
24 def __str__(self): |
|
104
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
25 return '[{0}, {1}]'.format(self.first, self.last) |
|
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
26 |
|
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
27 def __repr__(self): |
|
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
28 return self.__str__() |
|
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 empty(self): |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
31 return self.first > self.last |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
32 |
|
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
33 def center(self): |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
34 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
|
35 |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
36 def length(self): |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
37 '''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
|
38 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
|
39 |
|
96
9928c2fa72cc
added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
93
diff
changeset
|
40 def equal(self, i2): |
|
9928c2fa72cc
added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
93
diff
changeset
|
41 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
|
42 |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
43 def getList(self): |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
44 return [self.first, self.last] |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
45 |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
46 def contains(self, instant): |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
47 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
|
48 |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
49 def inside(self, interval2): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
50 '''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
|
51 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
|
52 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
53 @classmethod |
|
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
54 def union(cls, interval1, interval2): |
|
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
55 '''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
|
56 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
|
57 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
58 @classmethod |
|
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
59 def intersection(cls, interval1, interval2): |
|
104
13187af8622d
finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents:
98
diff
changeset
|
60 '''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
|
61 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
|
62 |
|
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
63 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
|
64 if not Interval.intersection(self, interval2).empty(): |
|
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
65 return 0 |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
66 elif self.first > interval2.last: |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
67 return self.first - interval2.last |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
68 elif self.last < interval2.first: |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
69 return interval2.first - self.last |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
70 else: |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
71 return None |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
72 |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
73 |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
74 def unionIntervals(intervals): |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
75 'returns the smallest interval containing all intervals' |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
76 inter = intervals[0] |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
77 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
|
78 inter = Interval.union(inter, i) |
|
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
79 return inter |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
80 |
|
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
81 |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
82 class TimeInterval(Interval): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
83 '''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
|
84 |
|
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
85 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
|
86 It may be modified directly by setting first and last''' |
| 26 | 87 |
| 88 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
|
89 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
|
90 |
|
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
91 @staticmethod |
|
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
92 def fromInterval(inter): |
|
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
93 return TimeInterval(inter.first, inter.last) |
| 26 | 94 |
|
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
95 def __getitem__(self, i): |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
96 if not self.empty(): |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
97 return self.first+i |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
98 |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
99 def __iter__(self): |
|
107
916678481896
corrected bug for TimeInterval interation and added corresponding test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
105
diff
changeset
|
100 self.iterInstantNum = -1 |
|
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
101 return self |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
102 |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
103 def next(self): |
|
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
104 if self.iterInstantNum >= self.length()-1: |
|
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
105 raise StopIteration |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
106 else: |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
107 self.iterInstantNum += 1 |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
108 return self[self.iterInstantNum] |
|
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
109 |
| 26 | 110 def length(self): |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
111 '''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
|
112 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
|
113 |
|
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
114 # class BoundingPolygon: |
|
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
115 # '''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
|
116 # with methods to create intersection, unions... |
|
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
117 # ''' |
|
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
118 # We will use the polygon class of Shapely |
|
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
119 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
120 class STObject(object): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
121 '''Class for spatio-temporal object, i.e. with temporal and spatial existence |
| 6 | 122 (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
|
123 |
|
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
124 It may not mean that the object is defined |
| 6 | 125 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
|
126 |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
127 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
|
128 self.num = num |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
129 self.timeInterval = timeInterval |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
130 self.boundingPolygon = boundingPolygon |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
131 |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
132 def empty(self): |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
133 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
|
134 |
|
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
135 def getNum(self): |
| 211 | 136 return self.num |
| 137 | |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
138 def getFirstInstant(self): |
|
40
9f16aee24b7e
corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents:
39
diff
changeset
|
139 return self.timeInterval.first |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
140 |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
141 def getLastInstant(self): |
|
40
9f16aee24b7e
corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents:
39
diff
changeset
|
142 return self.timeInterval.last |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
143 |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
144 def getTimeInterval(self): |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
145 return self.timeInterval |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
146 |
|
108
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
147 def existsAtInstant(self, t): |
|
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
148 return self.timeInterval.contains(t) |
|
6efe470ea5e5
added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
107
diff
changeset
|
149 |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
150 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
|
151 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval()) |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
152 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
153 class Point(object): |
|
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
154 def __init__(self, x, y): |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
155 self.x = x |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
156 self.y = y |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
157 |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
158 def __str__(self): |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
159 return '(%f,%f)'%(self.x,self.y) |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
160 |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
161 def __repr__(self): |
|
98
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
162 return self.__str__() |
|
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
163 |
|
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
164 def __add__(self, other): |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
165 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
|
166 |
|
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
167 def __sub__(self, other): |
|
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
168 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
|
169 |
|
105
9844c69d8fa2
added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents:
104
diff
changeset
|
170 def multiply(self, alpha): |
|
9844c69d8fa2
added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents:
104
diff
changeset
|
171 return Point(self.x*alpha, self.y*alpha) |
|
9844c69d8fa2
added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents:
104
diff
changeset
|
172 |
| 211 | 173 def draw(self, options = 'o', **kwargs): |
| 41 | 174 from matplotlib.pylab import plot |
| 211 | 175 plot([self.x], [self.y], options, **kwargs) |
| 41 | 176 |
|
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
177 def norm2Squared(self): |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
178 '''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
|
179 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
|
180 |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
181 def norm2(self): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
182 '''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
|
183 return sqrt(self.norm2Squared()) |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
184 |
|
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
185 def norm1(self): |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
186 return abs(self.x)+abs(self.y) |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
187 |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
188 def normMax(self): |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
189 return max(abs(self.x),abs(self.y)) |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
190 |
|
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
191 def aslist(self): |
|
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
192 return [self.x, self.y] |
|
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
193 |
|
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
194 def astuple(self): |
|
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
195 return (self.x, self.y) |
|
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
196 |
|
223
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
197 def asint(self): |
|
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
198 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
|
199 |
|
98
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
200 def project(self, homography): |
|
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
201 from numpy.core.multiarray import array |
|
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
202 projected = cvutils.projectArray(homography, array([[self.x], [self.y]])) |
|
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
203 return Point(projected[0], projected[1]) |
|
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
204 |
|
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
205 def inPolygon(self, poly): |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
206 '''Returns if the point x, y is inside the polygon. |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
207 The polygon is defined by the ordered list of points in poly |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
208 |
|
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
|
209 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
|
210 |
|
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
211 Use points_inside_poly from matplotlib.nxutils''' |
|
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
212 |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
213 n = len(poly); |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
214 counter = 0; |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
215 |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
216 p1 = poly[0]; |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
217 for i in range(n+1): |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
218 p2 = poly[i % n]; |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
219 if self.y > min(p1.y,p2.y): |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
220 if self.y <= max(p1.y,p2.y): |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
221 if self.x <= max(p1.x,p2.x): |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
222 if p1.y != p2.y: |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
223 xinters = (self.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x; |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
224 if p1.x == p2.x or self.x <= xinters: |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
225 counter+=1; |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
226 p1=p2 |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
227 return (counter%2 == 1); |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
228 |
| 49 | 229 @staticmethod |
|
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
230 def dot(p1, p2): |
|
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
231 'Scalar product' |
|
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
232 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
|
233 |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
234 @staticmethod |
|
90
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
235 def cross(p1, p2): |
|
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
236 'Cross product' |
|
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
237 return p1.x*p2.y-p1.y*p2.x |
|
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
238 |
|
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
239 @staticmethod |
|
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
240 def distanceNorm2(p1, p2): |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
241 return (p1-p2).norm2() |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
242 |
|
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
243 @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
|
244 def plotAll(points, **kwargs): |
|
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
245 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
|
246 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
|
247 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
248 class NormAngle(object): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
249 '''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
|
250 |
|
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
251 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
|
252 self.norm = norm |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
253 self.angle = angle |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
254 |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
255 @staticmethod |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
256 def fromPoint(p): |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
257 from math import atan2 |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
258 norm = p.norm2() |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
259 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
|
260 angle = atan2(p.y, p.x) |
| 339 | 261 else: |
| 262 angle = 0. | |
|
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
263 return NormAngle(norm, angle) |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
264 |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
265 def __add__(self, other): |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
266 '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
|
267 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
|
268 |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
269 def getPoint(self): |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
270 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
|
271 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
|
272 |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
273 |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
274 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
|
275 '''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
|
276 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
|
277 |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
278 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
|
279 '''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
|
280 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
|
281 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
|
282 predictedSpeedTheta = speedOrientation+control |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
283 if maxSpeed: |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
284 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
|
285 predictedPosition = position+predictedSpeedTheta.getPoint() |
|
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
286 return predictedPosition, predictedSpeedTheta |
|
245
bd8ab323c198
corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
244
diff
changeset
|
287 |
|
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
288 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
289 class FlowVector(object): |
|
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
290 '''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
|
291 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
|
292 def __init__(self, position, velocity): |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
293 '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
|
294 self.position = position |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
295 self.velocity = velocity |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
296 |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
297 def __add__(self, other): |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
298 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
|
299 |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
300 def multiply(self, alpha): |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
301 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
|
302 |
| 211 | 303 def draw(self, options = '', **kwargs): |
|
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
304 from matplotlib.pylab import plot |
| 211 | 305 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs) |
| 306 self.position.draw(options+'x', **kwargs) | |
|
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
307 |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
308 @staticmethod |
|
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
309 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
|
310 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
|
311 |
|
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
312 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
|
313 '''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
|
314 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
|
315 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
|
316 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
|
317 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
|
318 [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
|
319 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
|
320 [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
|
321 |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
322 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
|
323 return None |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
324 else: |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
325 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
|
326 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
|
327 |
|
152
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
328 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
|
329 '''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
|
330 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
331 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
|
332 return None |
|
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
333 else: |
|
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
334 dp1 = p2-p1 |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
335 dp3 = p4-p3 |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
336 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
|
337 if (inter != None |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
338 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
|
339 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
|
340 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
|
341 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
|
342 return inter |
|
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
343 else: |
|
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
344 return None |
|
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
345 |
|
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
346 # 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
|
347 |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
348 class Trajectory(object): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
349 '''Class for trajectories: temporal sequence of positions |
| 22 | 350 |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
351 The class is iterable''' |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
352 |
|
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
353 def __init__(self, positions=None): |
|
331
40790d93200e
corrected a bug for Trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
329
diff
changeset
|
354 if positions != None: |
|
113
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
355 self.positions = positions |
|
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
356 else: |
|
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
357 self.positions = [[],[]] |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
358 |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
359 @staticmethod |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
360 def load(line1, line2): |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
361 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
|
362 [float(n) for n in line2.split(' ')]]) |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
363 |
|
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
364 @staticmethod |
|
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
365 def fromPointList(points): |
|
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
366 t = Trajectory() |
|
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
367 for p in points: |
|
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
368 t.addPosition(p) |
|
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
369 return t |
|
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
370 |
|
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
371 def __len__(self): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
372 return len(self.positions[0]) |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
373 |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
374 def length(self): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
375 return self.__len__() |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
376 |
| 16 | 377 def __str__(self): |
| 39 | 378 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())]) |
| 16 | 379 |
|
69
cc192d0450b3
added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
68
diff
changeset
|
380 def __repr__(self): |
|
cc192d0450b3
added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
68
diff
changeset
|
381 return str(self) |
|
cc192d0450b3
added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
68
diff
changeset
|
382 |
|
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
383 def __getitem__(self, i): |
|
25
28e546861263
added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents:
23
diff
changeset
|
384 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
|
385 |
| 22 | 386 def __iter__(self): |
| 387 self.iterInstantNum = 0 | |
| 388 return self | |
| 389 | |
| 390 def next(self): | |
| 391 if self.iterInstantNum >= self.length(): | |
| 392 raise StopIteration | |
| 393 else: | |
| 394 self.iterInstantNum += 1 | |
|
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
395 return self[self.iterInstantNum-1] |
| 22 | 396 |
|
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
397 def setPositionXY(self, i, x, y): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
398 if i < self.__len__(): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
399 self.positions[0][i] = x |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
400 self.positions[1][i] = y |
|
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
401 |
|
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
402 def setPosition(self, i, p): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
403 self.setPositionXY(i, p.x, p.y) |
|
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
404 |
|
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
405 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
|
406 self.positions[0].append(x) |
|
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
407 self.positions[1].append(y) |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
408 |
|
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
409 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
|
410 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
|
411 |
|
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
412 def duplicateLastPosition(self): |
|
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
413 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
|
414 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
|
415 |
|
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
416 @staticmethod |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
417 def _draw(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
|
418 from matplotlib.pylab import plot |
|
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
419 if lastCoordinate == None: |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
420 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
|
421 elif 0 <= lastCoordinate <= len(positions[0]): |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
422 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
|
423 if withOrigin: |
| 211 | 424 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
|
425 |
|
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
426 def project(self, homography): |
| 318 | 427 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
|
428 |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
429 def draw(self, options = '', withOrigin = False, timeStep = 1, **kwargs): |
|
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
430 Trajectory._draw(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
|
431 |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
432 def drawAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, **kwargs): |
|
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
433 Trajectory._draw(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
|
434 |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
435 def drawOnWorldImage(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
|
436 from matplotlib.pylab import plot |
|
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
437 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
|
438 [-x*nPixelsPerUnitDistance+imageHeight for x in self.positions[1]]] |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
439 Trajectory._draw(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
|
440 |
|
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
441 def getXCoordinates(self): |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
442 return self.positions[0] |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
443 |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
444 def getYCoordinates(self): |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
445 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
|
446 |
|
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
447 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
|
448 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
|
449 return array(self.positions) |
|
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
450 |
|
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
451 def xBounds(self): |
|
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
452 # 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
|
453 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates())) |
|
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
454 |
|
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
455 def yBounds(self): |
|
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
456 # 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
|
457 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates())) |
|
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
458 |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
459 def add(self, traj2): |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
460 '''Returns a new trajectory of the same length''' |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
461 if self.length() != traj2.length(): |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
462 print 'Trajectories of different lengths' |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
463 return None |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
464 else: |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
465 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
|
466 [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
|
467 |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
468 def subtract(self, traj2): |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
469 '''Returns a new trajectory of the same length''' |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
470 if self.length() != traj2.length(): |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
471 print 'Trajectories of different lengths' |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
472 return None |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
473 else: |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
474 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
|
475 [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
|
476 |
| 339 | 477 def differentiate(self): |
| 478 diff = Trajectory() | |
| 479 for i in xrange(1, self.length()): | |
| 480 diff.addPosition(self[i]-self[i-1]) | |
| 481 return diff | |
| 482 | |
| 16 | 483 def norm(self): |
| 484 '''Returns the list of the norms at each instant''' | |
| 485 # def add(x, y): return x+y | |
| 486 # sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]]) | |
| 487 # return sqrt(sq) | |
|
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
488 from numpy import hypot |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
489 return hypot(self.positions[0], self.positions[1]) |
|
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
490 |
| 49 | 491 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
|
492 'Returns the sum of the distances between each successive point' |
| 49 | 493 displacement = 0 |
| 494 for i in xrange(self.length()-1): | |
| 495 displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1)) | |
| 496 return displacement | |
| 497 | |
| 498 def wiggliness(self): | |
| 499 return self.cumulatedDisplacement()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))) | |
| 500 | |
|
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
501 def getIntersections(self, p1, p2): |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
502 '''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
|
503 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
|
504 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
|
505 indices = [] |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
506 |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
507 for i in xrange(self.length()-1): |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
508 q1=self.__getitem__(i) |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
509 q2=self.__getitem__(i+1) |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
510 p = utils.segmentIntersection(q1, q2, p1, p2) |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
511 if p: |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
512 if q1.x != q2.x: |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
513 ratio = (p.x-q1.x)/(q2.x-q1.x) |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
514 elif q1.y != q2.y: |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
515 ratio = (p.y-q1.y)/(q2.y-q1.y) |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
516 else: |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
517 ratio = 0 |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
518 indices.append(i+ratio) |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
519 return indices |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
520 |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
521 def getTrajectoryInInterval(self, inter): |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
522 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
|
523 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
|
524 self.positions[1][inter.first:inter.last]]) |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
525 else: |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
526 return None |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
527 |
|
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
|
528 def getTrajectoryInPolygon(self, polygon): |
|
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
529 '''Returns the set of points inside the polygon |
|
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
530 (array of Nx2 coordinates of the polygon vertices)''' |
|
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
531 import matplotlib.nxutils as nx |
|
113
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
532 traj = Trajectory() |
|
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
|
533 result = nx.points_inside_poly(self.asArray().T, polygon) |
|
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
534 for i in xrange(self.length()): |
|
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
|
535 if result[i]: |
|
113
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
536 traj.addPositionXY(self.positions[0][i], self.positions[1][i]) |
|
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
537 return traj |
|
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
538 |
|
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
539 # version 2: use shapely polygon contains |
| 19 | 540 |
|
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
541 @staticmethod |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
542 def norm2LCSS(t1, t2, threshold): |
|
368
2db4e76599a1
implemented subsequence extraction and rearranged arguments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
543 return utils.LCSS(t1, t2, lambda x,y: Point.distanceNorm2(x,y) <= threshold) |
|
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
544 |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
545 @staticmethod |
|
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
546 def normMaxLCSS(t1, t2, threshold): |
|
368
2db4e76599a1
implemented subsequence extraction and rearranged arguments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
547 return utils.LCSS(t1, t2, lambda p1, p2: (p1-p2).normMax() <= threshold) |
|
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
548 |
|
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
549 class CurvilinearTrajectory(Trajectory): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
550 '''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
|
551 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
|
552 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
|
553 |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
554 def __init__(self, S = [], Y = [], lanes = []): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
555 self.positions = [S,Y] |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
556 self.lanes = lanes |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
557 |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
558 def __getitem__(self,i): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
559 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
|
560 |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
561 def getSCoordinates(self): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
562 return self.getXCoordinates() |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
563 |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
564 def getLanes(self): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
565 return self.lanes |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
566 |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
567 def addPosition(self, s, y, lane): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
568 self.addPositionXY(s,y) |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
569 self.lanes.append(lane) |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
570 |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
571 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
|
572 self.setPositionXY(i, s, y) |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
573 if i < self.__len__(): |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
574 self.lanes[i] = lane |
|
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
575 |
|
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
576 ################## |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
577 # Moving Objects |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
578 ################## |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
579 |
|
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
580 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
|
581 'car', |
|
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
582 '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
|
583 'motorcycle', |
|
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
584 'bicycle', |
|
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
585 'bus', |
|
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
586 'truck'] |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
587 |
|
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
588 userType2Num = utils.inverseEnumeration(userTypeNames) |
|
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
589 |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
590 class MovingObject(STObject): |
|
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
591 '''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
|
592 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
|
593 ''' |
|
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
594 |
|
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
595 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = None): |
|
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
596 super(MovingObject, self).__init__(num, timeInterval) |
| 16 | 597 self.positions = positions |
|
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
598 self.velocities = velocities |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
599 self.geometry = geometry |
|
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
600 self.userType = userType |
|
317
d280b881e860
added indicator min distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
290
diff
changeset
|
601 self.features = None |
|
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
602 # 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
|
603 |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
604 def getObjectInTimeInterval(self, inter): |
|
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
605 '''Returns a new object extracted from self, |
|
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
606 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
|
607 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
|
608 if not intersection.empty(): |
|
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
609 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
|
610 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
|
611 if self.velocities: |
|
97
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
612 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval) |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
613 return obj |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
614 else: |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
615 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
|
616 return None |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
617 |
|
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
618 def length(self): |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
619 return self.timeInterval.length() |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
620 |
|
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
621 def getPositions(self): |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
622 return self.positions |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
623 |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
624 def getVelocities(self): |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
625 return self.velocities |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
626 |
|
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
627 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
|
628 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
|
629 |
|
335
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
630 def getCurvilinearPositions(self): |
|
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
631 if hasattr(self, 'curvilinearPositions'): |
|
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
632 return self.curvilinearPositions |
|
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
633 else: |
|
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
634 return None |
|
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
635 |
|
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
636 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
|
637 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
|
638 |
|
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
639 def setFeatures(self, features): |
|
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
640 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
|
641 |
| 49 | 642 def getSpeeds(self): |
| 643 return self.getVelocities().norm() | |
| 644 | |
|
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
645 def getPositionAt(self, i): |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
646 return self.positions[i] |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
647 |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
648 def getVelocityAt(self, i): |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
649 return self.velocities[i] |
|
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
650 |
|
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
651 def getPositionAtInstant(self, i): |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
652 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
|
653 |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
654 def getVelocityAtInstant(self, i): |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
655 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
|
656 |
|
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
657 def getXCoordinates(self): |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
658 return self.positions.getXCoordinates() |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
659 |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
660 def getYCoordinates(self): |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
661 return self.positions.getYCoordinates() |
|
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
662 |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
663 def draw(self, options = '', withOrigin = False, timeStep = 1, **kwargs): |
|
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
664 self.positions.draw(options, withOrigin, timeStep, **kwargs) |
|
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
665 |
|
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
666 def drawOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs): |
|
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
667 self.positions.drawOnWorldImage(nPixelsPerUnitDistance, imageHeight, options, withOrigin, timeStep, **kwargs) |
| 22 | 668 |
|
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
669 def play(self, videoFilename, homography = None): |
|
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
670 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
|
671 |
|
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
672 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
|
673 '''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
|
674 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
|
675 empty list if there is no crossing''' |
|
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
676 indices = self.positions.getIntersections(p1, p2) |
|
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
677 return [t+self.getFirstInstant() for t in indices] |
|
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
678 |
|
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
679 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
|
680 '''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
|
681 at constant speed''' |
|
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
682 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
|
683 |
|
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
684 def classifyUserTypeSpeed(self, threshold, statisticsFunc = median): |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
685 '''Classifies slow and fast road users |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
686 slow: non-motorized -> pedestrians |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
687 fast: motorized -> cars''' |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
688 if statisticsFunc(self.velocities.norm()) >= threshold: |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
689 self.setUserType(userType2Num['car']) |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
690 else: |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
691 self.setUserType(userType2Num['pedestrian']) |
|
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
692 |
|
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
693 @staticmethod |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
694 def collisionCourseDotProduct(movingObject1, movingObject2, instant): |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
695 '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
|
696 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
|
697 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
|
698 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
|
699 |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
700 @staticmethod |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
701 def collisionCourseCosine(movingObject1, movingObject2, instant): |
|
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
702 '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
|
703 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
|
704 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
|
705 return Point.dot(deltap, deltav)/(deltap.norm2()*deltav.norm2()) |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
706 |
|
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
707 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
|
708 '''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
|
709 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
|
710 figure() |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
711 for obj in objects: |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
712 obj.draw(colors.get(obj.userType)) |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
713 axis('equal') |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
714 |
|
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
715 |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
716 if __name__ == "__main__": |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
717 import doctest |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
718 import unittest |
|
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
719 suite = doctest.DocFileSuite('tests/moving.txt') |
|
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
720 #suite = doctest.DocTestSuite() |
|
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
721 unittest.TextTestRunner().run(suite) |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
722 #doctest.testmod() |
|
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
723 #doctest.testfile("example.txt") |