Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 288:e0d41c7f53d4

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