Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 607:84690dfe5560

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
add some functions for behaviour analysis
author MohamedGomaa
date Tue, 25 Nov 2014 22:49:47 -0500
parents 6ebfb43e938e
children 078adacd72a4 0954aaf28231
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
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 583
diff changeset
8 from numpy import median,percentile
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
9
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
10 try:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
11 from shapely.geometry import Polygon, Point as shapelyPoint
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
12 from shapely.prepared import prep
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
13 shapelyAvailable = True
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
14 except ImportError:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
15 print('Shapely library could not be loaded')
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
16 shapelyAvailable = False
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
17
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
18 __metaclass__ = type
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
19
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
20 class Interval(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
21 '''Generic interval: a subset of real numbers (not iterable)'''
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
22 def __init__(self, first=0, last=-1, revert = False):
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
23 if revert and last<first:
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
24 self.first=last
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
25 self.last=first
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
26 else:
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
27 self.first=first
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
28 self.last=last
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
29
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
30 def __str__(self):
104
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
31 return '[{0}, {1}]'.format(self.first, self.last)
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
32
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
33 def __repr__(self):
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
34 return self.__str__()
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
35
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
36 def empty(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
37 return self.first > self.last
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
38
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
39 def center(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
40 return (self.first+self.last)/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
41
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
42 def length(self):
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
43 '''Returns the length of the interval'''
91
daa05fae1a70 modified the type of the result of interval lengths to float, added comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 90
diff changeset
44 return float(max(0,self.last-self.first))
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
45
96
9928c2fa72cc added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 93
diff changeset
46 def equal(self, i2):
9928c2fa72cc added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 93
diff changeset
47 return self.first==i2.first and self.last == i2.last
9928c2fa72cc added equal method to intervals
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 93
diff changeset
48
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
49 def getList(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
50 return [self.first, self.last]
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
51
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
52 def contains(self, instant):
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
53 return (self.first<=instant and self.last>=instant)
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
54
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
55 def inside(self, interval2):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
56 '''Indicates if the temporal interval of self is comprised in interval2'''
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
57 return (self.first >= interval2.first) and (self.last <= interval2.last)
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
58
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
59 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
60 def union(cls, interval1, interval2):
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
61 '''Smallest interval comprising self and interval2'''
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
62 return cls(min(interval1.first, interval2.first), max(interval2.last, interval2.last))
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
63
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
64 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
65 def intersection(cls, interval1, interval2):
104
13187af8622d finally added the representation of intervals
Nicolas Saunier <nico@confins.net>
parents: 98
diff changeset
66 '''Largest interval comprised in both self and interval2'''
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
67 return cls(max(interval1.first, interval2.first), min(interval1.last, interval2.last))
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
68
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
69 def distance(self, interval2):
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
70 if not Interval.intersection(self, interval2).empty():
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
71 return 0
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
72 elif self.first > interval2.last:
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
73 return self.first - interval2.last
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
74 elif self.last < interval2.first:
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
75 return interval2.first - self.last
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
76 else:
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
77 return None
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
78
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
79
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
80 def unionIntervals(intervals):
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
81 'returns the smallest interval containing all intervals'
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
82 inter = intervals[0]
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
83 for i in intervals[1:]:
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
84 inter = Interval.union(inter, i)
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
85 return inter
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
86
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
87
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
88 class TimeInterval(Interval):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
89 '''Temporal interval: set of instants at fixed time step, between first and last, included
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
90
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
91 For example: based on frame numbers (hence the modified length method)
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
92 It may be modified directly by setting first and last'''
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
93
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
94 def __init__(self, first=0, last=-1):
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
95 super(TimeInterval, self).__init__(first, last, False)
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
96
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
97 @staticmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
98 def fromInterval(inter):
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
99 return TimeInterval(inter.first, inter.last)
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
100
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
101 def __getitem__(self, i):
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
102 if not self.empty():
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
103 if isinstance(i, int):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
104 return self.first+i
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
105 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
106 raise TypeError, "Invalid argument type."
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
107 #elif isinstance( key, slice ):
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
108
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
109 def __iter__(self):
107
916678481896 corrected bug for TimeInterval interation and added corresponding test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 105
diff changeset
110 self.iterInstantNum = -1
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
111 return self
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
112
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
113 def next(self):
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
114 if self.iterInstantNum >= self.length()-1:
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
115 raise StopIteration
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
116 else:
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
117 self.iterInstantNum += 1
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
118 return self[self.iterInstantNum]
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
119
26
54d9cb0c902b generalized intervals
Nicolas Saunier <nico@confins.net>
parents: 25
diff changeset
120 def length(self):
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
121 '''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
122 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
123
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
124 # class BoundingPolygon:
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
125 # '''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
126 # with methods to create intersection, unions...
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
127 # '''
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
128 # We will use the polygon class of Shapely
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
129
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
130 class STObject(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
131 '''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
132 (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
133
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
134 It may not mean that the object is defined
6
597d61c1eebe minor doc update
Nicolas Saunier <nico@confins.net>
parents: 2
diff changeset
135 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
136
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
137 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
138 self.num = num
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
139 self.timeInterval = timeInterval
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
140 self.boundingPolygon = boundingPolygon
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
141
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
142 def empty(self):
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
143 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
144
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
145 def getNum(self):
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
146 return self.num
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
147
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
148 def getFirstInstant(self):
40
9f16aee24b7e corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents: 39
diff changeset
149 return self.timeInterval.first
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
150
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
151 def getLastInstant(self):
40
9f16aee24b7e corrected bug for first and last of TimeInterval
Nicolas Saunier <nico@confins.net>
parents: 39
diff changeset
152 return self.timeInterval.last
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
153
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
154 def getTimeInterval(self):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
155 return self.timeInterval
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
156
108
6efe470ea5e5 added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 107
diff changeset
157 def existsAtInstant(self, t):
6efe470ea5e5 added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 107
diff changeset
158 return self.timeInterval.contains(t)
6efe470ea5e5 added test existsAtInstant to STObject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 107
diff changeset
159
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
160 def commonTimeInterval(self, obj2):
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
161 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval())
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
162
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
163 class Point(object):
25
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
164 def __init__(self, x, y):
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
165 self.x = x
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
166 self.y = y
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
167
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
168 def __str__(self):
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
169 return '(%f,%f)'%(self.x,self.y)
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
170
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
171 def __repr__(self):
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
172 return self.__str__()
25
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
173
184
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
174 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
175 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
176
25
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
177 def __sub__(self, other):
28e546861263 added Point class and modified trajectory accordingly
Nicolas Saunier <nico@confins.net>
parents: 23
diff changeset
178 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
179
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
180 def __neg__(self):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
181 return Point(-self.x, -self.y)
575
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
182
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
183 def __getitem__(self, i):
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
184 if i == 0:
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
185 return self.x
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
186 elif i == 1:
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
187 return self.y
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
188 else:
13df64a9ff9d added function to access point class as 2D list
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 574
diff changeset
189 raise IndexError()
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
190
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
191 def orthogonal(self):
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
192 return Point(self.y, -self.x)
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
193
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 104
diff changeset
194 def multiply(self, alpha):
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
195 'Warning, returns a new Point'
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 104
diff changeset
196 return Point(self.x*alpha, self.y*alpha)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 104
diff changeset
197
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
198 def plot(self, options = 'o', **kwargs):
41
eb78c6edc0c8 added drawing for Point
Nicolas Saunier <nico@confins.net>
parents: 40
diff changeset
199 from matplotlib.pylab import plot
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
200 plot([self.x], [self.y], options, **kwargs)
41
eb78c6edc0c8 added drawing for Point
Nicolas Saunier <nico@confins.net>
parents: 40
diff changeset
201
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
202 def norm2Squared(self):
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
203 '''2-norm distance (Euclidean distance)'''
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
204 return self.x**2+self.y**2
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
205
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
206 def norm2(self):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
207 '''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
208 return sqrt(self.norm2Squared())
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
209
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
210 def norm1(self):
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
211 return abs(self.x)+abs(self.y)
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
212
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
213 def normMax(self):
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
214 return max(abs(self.x),abs(self.y))
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
215
64
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
216 def aslist(self):
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
217 return [self.x, self.y]
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
218
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
219 def astuple(self):
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
220 return (self.x, self.y)
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
221
223
c31722fcc9de minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 214
diff changeset
222 def asint(self):
c31722fcc9de minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 214
diff changeset
223 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
224
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
225 if shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
226 def asShapely(self):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
227 return shapelyPoint(self.x, self.y)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
228
98
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
229 def project(self, homography):
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
230 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
231 projected = cvutils.projectArray(homography, array([[self.x], [self.y]]))
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
232 return Point(projected[0], projected[1])
b85912ab4064 refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 97
diff changeset
233
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
234 def inPolygonNoShapely(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
235 '''Indicates if the point x, y is inside the polygon
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
236 (array of Nx2 coordinates of the polygon vertices)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
237
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
238 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
239
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
240 Use Polygon.contains if Shapely is installed'''
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
241
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
242 n = polygon.shape[0];
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
243 counter = 0;
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
244
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
245 p1 = polygon[0,:];
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
246 for i in range(n+1):
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
247 p2 = polygon[i % n,:];
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
248 if self.y > min(p1[1],p2[1]):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
249 if self.y <= max(p1[1],p2[1]):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
250 if self.x <= max(p1[0],p2[0]):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
251 if p1[1] != p2[1]:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
252 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0];
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
253 if p1[0] == p2[0] or self.x <= xinters:
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
254 counter+=1;
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
255 p1=p2
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
256 return (counter%2 == 1);
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
257
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
258 @staticmethod
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
259 def fromList(p):
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
260 return Point(p[0], p[1])
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
261
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
262 @staticmethod
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
263 def dot(p1, p2):
89
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
264 'Scalar product'
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
265 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
266
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
267 @staticmethod
90
f84293ad4611 renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 89
diff changeset
268 def cross(p1, p2):
f84293ad4611 renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 89
diff changeset
269 'Cross product'
89
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
270 return p1.x*p2.y-p1.y*p2.x
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
271
f88a19695bba added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 83
diff changeset
272 @staticmethod
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
273 def cosine(p1, p2):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
274 return Point.dot(p1,p2)/(p1.norm2()*p2.norm2())
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
275
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
276 @staticmethod
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
277 def distanceNorm2(p1, p2):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
278 return (p1-p2).norm2()
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
279
64
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
280 @staticmethod
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 345
diff changeset
281 def plotAll(points, **kwargs):
64
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
282 from matplotlib.pyplot import scatter
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 345
diff changeset
283 scatter([p.x for p in points],[p.y for p in points], **kwargs)
64
c75bcdaed00f added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 62
diff changeset
284
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
285 def similarOrientation(self, refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
286 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold'
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
287 return Point.cosine(self, refDirection) >= cosineThreshold
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
288
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
289 @staticmethod
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
290 def timeToCollision(p1, p2, v1, v2, collisionThreshold):
504
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
291 '''Computes exact time to collision with a distance threshold
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
292 The unknown of the equation is the time to reach the intersection
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
293 between the relative trajectory of one road user
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
294 and the circle of radius collisionThreshold around the other road user'''
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
295 from math import sqrt
504
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
296 dv = v1-v2
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
297 dp = p1-p2
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
298 a = dv.norm2Squared()#(v1.x-v2.x)**2 + (v1.y-v2.y)**2
516
bce1fe45d1b2 corrected bugs detected by tests (because of moving functions around modules
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
299 b = 2*Point.dot(dv, dp)#2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y))
504
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
300 c = dp.norm2Squared() - collisionThreshold**2#(p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
301
504
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
302 delta = b**2 - 4*a*c
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
303 if delta >= 0:
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
304 deltaRoot = sqrt(delta)
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
305 ttc1 = (-b + deltaRoot)/(2*a)
a40c75f04903 optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 501
diff changeset
306 ttc2 = (-b - deltaRoot)/(2*a)
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
307 if ttc1 >= 0 and ttc2 >= 0:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
308 ttc = min(ttc1,ttc2)
531
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 527
diff changeset
309 elif ttc1 >= 0:
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 527
diff changeset
310 ttc = ttc1
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 527
diff changeset
311 elif ttc2 >= 0:
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 527
diff changeset
312 ttc = ttc2
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 527
diff changeset
313 else: # ttc1 < 0 and ttc2 < 0:
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 527
diff changeset
314 ttc = None
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
315 else:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
316 ttc = None
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
317 return ttc
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
318
583
6ebfb43e938e added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 582
diff changeset
319 @staticmethod
6ebfb43e938e added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 582
diff changeset
320 def midPoint(p1, p2):
6ebfb43e938e added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 582
diff changeset
321 'Returns the middle of the segment [p1, p2]'
6ebfb43e938e added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 582
diff changeset
322 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y)
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
323
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
324 if shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
325 def pointsInPolygon(points, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
326 '''Optimized tests of a series of points within (Shapely) polygon '''
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
327 prepared_polygon = prep(polygon)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
328 return filter(prepared_polygon.contains, points)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
329
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
330 # Functions for coordinate transformation
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
331 # From Paul St-Aubin's PVA tools
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
332 def subsec_spline_dist(splines):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
333 ''' Prepare list of spline subsegments from a spline list.
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
334
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
335 Output:
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
336 =======
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
337 ss_spline_d[spline #][mode][station]
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
338
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
339 where:
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
340 mode=0: incremental distance
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
341 mode=1: cumulative distance
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
342 mode=2: cumulative distance with trailing distance
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
343 '''
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
344
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
345 from numpy import zeros
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
346 ss_spline_d = []
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
347 #Prepare subsegment distances
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
348 for spline in range(len(splines)):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
349 ss_spline_d.append([[],[],[]])
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
350 ss_spline_d[spline][0] = zeros(len(splines[spline])-1) #Incremental distance
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
351 ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
352 ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
353 for spline_p in range(len(splines[spline])):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
354 if spline_p > (len(splines[spline]) - 2):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
355 break
569
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
356 ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1])
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
357 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p])
568
538fb47b3007 minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
358 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p])
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
359
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
360 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1]
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
361
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
362 return ss_spline_d
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
363
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
364 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
365 ''' Point-projection (Q) on line defined by 2 points (P0,P1).
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
366 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
367 '''
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
368 if(p0x == p1x and p0y == p1y):
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
369 return None
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
370 try:
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
371 #Approximate slope singularity by giving some slope roundoff; account for roundoff error
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
372 if(round(p0x, 10) == round(p1x, 10)):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
373 p1x += 0.0000000001
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
374 if(round(p0y, 10) == round(p1y, 10)):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
375 p1y += 0.0000000001
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
376 #make the calculation
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
377 Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x))
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
378 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
379 except ZeroDivisionError:
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
380 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
381 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
382 import pdb; pdb.set_trace()
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
383 return Point(X,Y)
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
384
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
385 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
579
05c927c6d3cf curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 578
diff changeset
386 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). A spline is a list of points (class Point), most likely a trajectory.
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
387
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
388 Output:
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
389 =======
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
390 [spline index,
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
391 subsegment leading point index,
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
392 snapped point,
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
393 subsegment distance,
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
394 spline distance,
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
395 orthogonal point offset]
569
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
396 '''
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
397 minOffsetY = float('inf')
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
398 #For each spline
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
399 for spline in range(len(splines)):
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
400 #For each spline point index
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
401 for spline_p in range(len(splines[spline])-1):
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
402 #Get closest point on spline
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
403 closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1])
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
404 if closestPoint == None:
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
405 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
406 return None
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
407 # check if the
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
408 if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], closestPoint.y):
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
409 offsetY = Point.distanceNorm2(closestPoint, p)
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
410 if offsetY < minOffsetY:
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
411 minOffsetY = offsetY
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
412 snappedSpline = spline
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
413 snappedSplineLeadingPoint = spline_p
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
414 snappedPoint = Point(closestPoint.x, closestPoint.y)
573
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
415 #Jump loop if significantly close
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
416 if offsetY < goodEnoughSplineDistance:
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
417 break
cae4e5f3fe9f fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 571
diff changeset
418 #Get sub-segment distance
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
419 if minOffsetY != float('inf'):
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
420 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
421 #Get cumulative alignment distance (total segment distance)
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
422 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
423 orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal()
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
424 offsetVector = p-snappedPoint
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
425 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
426 minOffsetY = -minOffsetY
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
427 return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
428 else:
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
429 return None
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
430
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
431 def getXYfromSY(s, y, splineNum, splines, mode = 0):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
432 ''' Find X,Y coordinate from S,Y data.
568
538fb47b3007 minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
433 if mode = 0 : return Snapped X,Y
538fb47b3007 minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
434 if mode !=0 : return Real X,Y
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
435 '''
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
436
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
437 #(buckle in, it gets ugly from here on out)
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
438 ss_spline_d = subsec_spline_dist(splines)
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
439
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
440 #Find subsegment
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
441 snapped_x = None
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
442 snapped_y = None
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
443 for spline_ss_index in range(len(ss_spline_d[splineNum][1])):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
444 if(s < ss_spline_d[splineNum][1][spline_ss_index]):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
445 ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1]
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
446 #Get normal vector and then snap
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
447 vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0])
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
448 vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1])
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
449 magnitude = sqrt(vector_l_x**2 + vector_l_y**2)
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
450 n_vector_x = vector_l_x/magnitude
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
451 n_vector_y = vector_l_y/magnitude
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
452 snapped_x = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
453 snapped_y = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
454
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
455 #Real values (including orthogonal projection of y))
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
456 real_x = snapped_x - y*n_vector_y
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
457 real_y = snapped_y + y*n_vector_x
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
458 break
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
459
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
460 if mode == 0 or (not snapped_x):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
461 if(not snapped_x):
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
462 snapped_x = splines[splineNum][-1][0]
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
463 snapped_y = splines[splineNum][-1][1]
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
464 return [snapped_x,snapped_y]
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
465 else:
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
466 return [real_x,real_y]
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
467
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
468
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
469 class NormAngle(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
470 '''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
471
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
472 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
473 self.norm = norm
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
474 self.angle = angle
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
475
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
476 @staticmethod
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
477 def fromPoint(p):
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
478 from math import atan2
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
479 norm = p.norm2()
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
480 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
481 angle = atan2(p.y, p.x)
339
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
482 else:
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
483 angle = 0.
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
484 return NormAngle(norm, angle)
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
485
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
486 def __add__(self, other):
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
487 '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
488 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
489
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
490 def getPoint(self):
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
491 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
492 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
493
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
494
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
495 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
496 '''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
497 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
498
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
499 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
500 '''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
501 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
502 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
503 predictedSpeedTheta = speedOrientation+control
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
504 if maxSpeed:
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
505 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
506 predictedPosition = position+predictedSpeedTheta.getPoint()
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 249
diff changeset
507 return predictedPosition, predictedSpeedTheta
245
bd8ab323c198 corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 244
diff changeset
508
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
509
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
510 class FlowVector(object):
184
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
511 '''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
512 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
513 def __init__(self, position, velocity):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
514 '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
515 self.position = position
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
516 self.velocity = velocity
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
517
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
518 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
519 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
520
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
521 def multiply(self, alpha):
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
522 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
523
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
524 def plot(self, options = '', **kwargs):
184
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
525 from matplotlib.pylab import plot
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
526 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs)
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
527 self.position.plot(options+'x', **kwargs)
184
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
528
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
529 @staticmethod
d70e9b36889c initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 182
diff changeset
530 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
531 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
532
569
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
533 def intersection(p1, p2, p3, p4):
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
534 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4
571
a9c1d61a89b4 corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 570
diff changeset
535 http://paulbourke.net/geometry/pointlineplane/'''
569
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
536 dp12 = p2-p1
571
a9c1d61a89b4 corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 570
diff changeset
537 dp34 = p4-p3
a9c1d61a89b4 corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 570
diff changeset
538 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y)
a9c1d61a89b4 corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 570
diff changeset
539 det = dp34.y*dp12.x-dp34.x*dp12.y
569
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
540 if det == 0:
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
541 return None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
542 else:
571
a9c1d61a89b4 corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 570
diff changeset
543 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det
a9c1d61a89b4 corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 570
diff changeset
544 return p1+dp12.multiply(ua)
569
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
545
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
546 # def intersection(p1, p2, dp1, dp2):
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
547 # '''Returns the intersection point between the two lines
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
548 # defined by the respective vectors (dp) and origin points (p)'''
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
549 # from numpy import matrix
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
550 # from numpy.linalg import linalg
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
551 # A = matrix([[dp1.y, -dp1.x],
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
552 # [dp2.y, -dp2.x]])
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
553 # B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
554 # [dp2.y*p2.x-dp2.x*p2.y]])
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
555
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
556 # if linalg.det(A) == 0:
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
557 # return None
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
558 # else:
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
559 # intersection = linalg.solve(A,B)
0057c04f94d5 work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 567
diff changeset
560 # return Point(intersection[0,0], intersection[1,0])
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
561
152
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 148
diff changeset
562 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
563 '''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
564
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
565 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()):
152
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 148
diff changeset
566 return None
74b1fc68d4df re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 148
diff changeset
567 else:
571
a9c1d61a89b4 corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 570
diff changeset
568 inter = intersection(p1, p2, p3, p4)
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
569 if (inter != None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
570 and utils.inBetween(p1.x, p2.x, inter.x)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
571 and utils.inBetween(p3.x, p4.x, inter.x)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
572 and utils.inBetween(p1.y, p2.y, inter.y)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
573 and utils.inBetween(p3.y, p4.y, inter.y)):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
574 return inter
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
575 else:
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
576 return None
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
577
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
578 class Trajectory(object):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
579 '''Class for trajectories: temporal sequence of positions
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
580
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
581 The class is iterable'''
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
582
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
583 def __init__(self, positions=None):
331
40790d93200e corrected a bug for Trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 329
diff changeset
584 if positions != None:
113
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
585 self.positions = positions
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
586 else:
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
587 self.positions = [[],[]]
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
588
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
589 @staticmethod
555
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
590 def generate(p, v, nPoints):
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
591 t = Trajectory()
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
592 p0 = Point(p.x, p.y)
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
593 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
594 for i in xrange(nPoints-1):
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
595 p0 += v
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
596 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
597 return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints])
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
598
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
599 @staticmethod
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
600 def load(line1, line2):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
601 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
602 [float(n) for n in line2.split(' ')]])
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
603
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
604 @staticmethod
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
605 def fromPointList(points):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
606 t = Trajectory()
576
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
607 if isinstance(points[0], list) or isinstance(points[0], tuple):
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
608 for p in points:
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
609 t.addPositionXY(p[0],p[1])
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
610 else:
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
611 for p in points:
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
612 t.addPosition(p)
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
613 return t
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
614
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
615 def __len__(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
616 return len(self.positions[0])
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
617
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
618 def length(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
619 return self.__len__()
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
620
582
7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 581
diff changeset
621 def empty(self):
7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 581
diff changeset
622 return self.__len__() == 0
7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 581
diff changeset
623
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
624 def __getitem__(self, i):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
625 if isinstance(i, int):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
626 return Point(self.positions[0][i], self.positions[1][i])
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
627 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
628 raise TypeError, "Invalid argument type."
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
629 #elif isinstance( key, slice ):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
630
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
631 def __str__(self):
39
e47168f6b694 corrected printing bug
Nicolas Saunier <nico@confins.net>
parents: 38
diff changeset
632 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
633
69
cc192d0450b3 added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 68
diff changeset
634 def __repr__(self):
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
635 return self.__str__()
23
5f2921ad4f7e made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents: 22
diff changeset
636
546
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
637
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
638 def __iter__(self):
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
639 self.iterInstantNum = 0
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
640 return self
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
641
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
642 def next(self):
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
643 if self.iterInstantNum >= self.length():
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
644 raise StopIteration
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
645 else:
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
646 self.iterInstantNum += 1
23
5f2921ad4f7e made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents: 22
diff changeset
647 return self[self.iterInstantNum-1]
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
648
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
649 def setPositionXY(self, i, x, y):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
650 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
651 self.positions[0][i] = x
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
652 self.positions[1][i] = y
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
653
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
654 def setPosition(self, i, p):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
655 self.setPositionXY(i, p.x, p.y)
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
656
71
45e958ccd9bd added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 69
diff changeset
657 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
658 self.positions[0].append(x)
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
659 self.positions[1].append(y)
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
660
71
45e958ccd9bd added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 69
diff changeset
661 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
662 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
663
343
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
664 def duplicateLastPosition(self):
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
665 self.positions[0].append(self.positions[0][-1])
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
666 self.positions[1].append(self.positions[1][-1])
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
667
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
668 @staticmethod
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
669 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, **kwargs):
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
670 from matplotlib.pylab import plot
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
671 if lastCoordinate == None:
270
05c9b0cb8202 updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
672 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
673 elif 0 <= lastCoordinate <= len(positions[0]):
270
05c9b0cb8202 updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
674 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
675 if withOrigin:
211
ada6e8fbe4c6 2 Changes :
Francois Belisle <belisle.francois@gmail.com>
parents: 203
diff changeset
676 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
677
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
678 def project(self, homography):
318
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
679 return Trajectory(cvutils.projectTrajectory(homography, self.positions))
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
680
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
681 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
682 Trajectory._plot(self.positions, options, withOrigin, None, timeStep, **kwargs)
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
683
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
684 def plotAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
685 Trajectory._plot(self.positions, options, withOrigin, lastCoordinate, timeStep, **kwargs)
203
e2f31813ade6 added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 185
diff changeset
686
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
687 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
688 from matplotlib.pylab import plot
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
689 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
690 [-x*nPixelsPerUnitDistance+imageHeight for x in self.positions[1]]]
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
691 Trajectory._plot(imgPositions, options, withOrigin, timeStep, **kwargs)
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
692
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
693 def getXCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
694 return self.positions[0]
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
695
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
696 def getYCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
697 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
698
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 91
diff changeset
699 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
700 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
701 return array(self.positions)
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
702
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
703 def xBounds(self):
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
704 # 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
705 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
706
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
707 def yBounds(self):
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
708 # 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
709 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
710
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
711 def add(self, traj2):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
712 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
713 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
714 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
715 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
716 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
717 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
718 [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
719
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
720 def subtract(self, traj2):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
721 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
722 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
723 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
724 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
725 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
726 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
727 [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
728
542
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
729 def differentiate(self, doubleLastPosition = False):
339
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
730 diff = Trajectory()
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
731 for i in xrange(1, self.length()):
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
732 diff.addPosition(self[i]-self[i-1])
542
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
733 if doubleLastPosition:
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
734 diff.addPosition(diff[-1])
339
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
735 return diff
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
736
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
737 def norm(self):
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
738 '''Returns the list of the norms at each instant'''
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
739 # def add(x, y): return x+y
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
740 # 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
741 # return sqrt(sq)
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
742 from numpy import hypot
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
743 return hypot(self.positions[0], self.positions[1])
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 13
diff changeset
744
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
745 # def cumulatedDisplacement(self):
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
746 # 'Returns the sum of the distances between each successive point'
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
747 # displacement = 0
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
748 # for i in xrange(self.length()-1):
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
749 # displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1))
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
750 # return displacement
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
751
576
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
752 def computeCumulativeDistances(self):
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
753 '''Computes the distance from each point to the next and the cumulative distance up to the point
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
754 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)'''
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
755 self.distances = []
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
756 self.cumulativeDistances = [0.]
576
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
757 p1 = self[0]
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
758 cumulativeDistance = 0.
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
759 for i in xrange(self.length()-1):
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
760 p2 = self[i+1]
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
761 self.distances.append(Point.distanceNorm2(p1,p2))
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
762 cumulativeDistance += self.distances[-1]
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
763 self.cumulativeDistances.append(cumulativeDistance)
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
764 p1 = p2
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
765
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
766 def getDistance(self,i):
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
767 '''Return the distance between points i and i+1'''
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
768 if i < self.length()-1:
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
769 return self.distances[i]
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
770 else:
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
771 print('Index {} beyond trajectory length {}-1'.format(i, self.length()))
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
772
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
773 def getCumulativeDistance(self, i):
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
774 '''Return the cumulative distance between the beginning and point i'''
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
775 if i < self.length():
576
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
776 return self.cumulativeDistances[i]
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
777 else:
577
d0abd2ee17b9 changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 576
diff changeset
778 print('Index {} beyond trajectory length {}'.format(i, self.length()))
576
0eff0471f9cb added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 575
diff changeset
779
381
387cc0142211 script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 379
diff changeset
780 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
387cc0142211 script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 379
diff changeset
781 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity)
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
782 have a cosine with refDirection is smaller than cosineThreshold'''
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
783 count = 0
383
0ce2210790b1 fixed stupid naming bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 381
diff changeset
784 lengthThreshold = float(self.length())*minProportion
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
785 for p in self:
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
786 if p.similarOrientation(refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
787 count += 1
381
387cc0142211 script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 379
diff changeset
788 if count > lengthThreshold:
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
789 return True
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
790 return False
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
791
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
792 def wiggliness(self):
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 583
diff changeset
793 return self.computeCumulativeDistances()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)))
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
794
83
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
795 def getIntersections(self, p1, p2):
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
796 '''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
797 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
798 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
799 indices = []
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
800
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
801 for i in xrange(self.length()-1):
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
802 q1=self.__getitem__(i)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
803 q2=self.__getitem__(i+1)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
804 p = utils.segmentIntersection(q1, q2, p1, p2)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
805 if p:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
806 if q1.x != q2.x:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
807 ratio = (p.x-q1.x)/(q2.x-q1.x)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
808 elif q1.y != q2.y:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
809 ratio = (p.y-q1.y)/(q2.y-q1.y)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
810 else:
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
811 ratio = 0
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
812 indices.append(i+ratio)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
813 return indices
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
814
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
815 def getTrajectoryInInterval(self, inter):
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
816 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
817 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
818 self.positions[1][inter.first:inter.last]])
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
819 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
820 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
821
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
822 def getTrajectoryInPolygonNoShapely(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
823 '''Returns the trajectory built with the set of points inside the polygon
92
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 91
diff changeset
824 (array of Nx2 coordinates of the polygon vertices)'''
113
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
825 traj = Trajectory()
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
826 for p in self:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
827 if p.inPolygonNoShapely(polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
828 traj.addPosition(p)
113
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 108
diff changeset
829 return traj
79
5d487f183fe2 added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 74
diff changeset
830
372
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
831 if shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
832 def getTrajectoryInPolygon(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
833 '''Returns the trajectory built with the set of points inside the (shapely) polygon'''
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
834 traj = Trajectory()
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
835 points = [p.asShapely() for p in self]
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
836 for p in pointsInPolygon(points, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
837 traj.addPositionXY(p.x, p.y)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
838 return traj
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 369
diff changeset
839
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
840 @staticmethod
369
027e254f0b53 lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 368
diff changeset
841 def lcss(t1, t2, lcss):
027e254f0b53 lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 368
diff changeset
842 return lcss.compute(t1, t2)
284
f2cf16ad798f added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 280
diff changeset
843
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
844 class CurvilinearTrajectory(Trajectory):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
845 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
846 longitudinal coordinate is stored as first coordinate (exterior name S)
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
847 lateral coordiante is stored as second coordinate'''
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
848
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
849 def __init__(self, S = None, Y = None, lanes = None):
543
cb213269d330 defensive code for curvilinear trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 542
diff changeset
850 if S == None or Y == None or len(S) != len(Y):
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
851 self.positions = [[],[]]
552
ca6bded754ac corrected more bugs from merging (caught in regression tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 551
diff changeset
852 if S != None and Y != None and len(S) != len(Y):
543
cb213269d330 defensive code for curvilinear trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 542
diff changeset
853 print("S and Y coordinates of different lengths\nInitializing to empty lists")
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
854 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
855 self.positions = [S,Y]
543
cb213269d330 defensive code for curvilinear trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 542
diff changeset
856 if lanes == None or len(lanes) != self.length():
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
857 self.lanes = []
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
858 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
859 self.lanes = lanes
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
860
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
861 def __getitem__(self,i):
526
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
862 if isinstance(i, int):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
863 return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
864 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
865 raise TypeError, "Invalid argument type."
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 524
diff changeset
866 #elif isinstance( key, slice ):
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
867
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
868 def getSCoordinates(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
869 return self.getXCoordinates()
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
870
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
871 def getLanes(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
872 return self.lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
873
542
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
874 def addPositionSYL(self, s, y, lane):
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
875 self.addPositionXY(s,y)
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
876 self.lanes.append(lane)
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
877
542
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
878 def addPosition(self, p):
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
879 'Adds position in the point format for curvilinear of list with 3 values'
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
880 self.addPositionSYL(p[0], p[1], p[2])
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
881
327
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
882 def setPosition(self, i, s, y, lane):
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
883 self.setPositionXY(i, s, y)
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
884 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
885 self.lanes[i] = lane
42f2b46ec210 added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 318
diff changeset
886
542
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
887 def differentiate(self, doubleLastPosition = False):
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
888 diff = CurvilinearTrajectory()
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
889 p1 = self[0]
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
890 for i in xrange(1, self.length()):
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
891 p2 = self[i]
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
892 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2])
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
893 p1=p2
582
7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 581
diff changeset
894 if doubleLastPosition and self.length() > 1:
542
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
895 diff.addPosition(diff[-1])
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
896 return diff
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 531
diff changeset
897
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
898 def getIntersections(self, S1, lane = None):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
899 '''Returns a list of the indices at which the trajectory
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
900 goes past the curvilinear coordinate S1
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
901 (in provided lane if lane != None)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
902 the list is empty if there is no crossing'''
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
903 indices = []
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
904 for i in xrange(self.length()-1):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
905 q1=self.__getitem__(i)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
906 q2=self.__getitem__(i+1)
527
37830a831818 loading from VISSIM trajectory data works
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 526
diff changeset
907 if q1[0] <= S1 < q2[0] and (lane == None or (self.lanes[i] == lane and self.lanes[i+1] == lane)):
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
908 indices.append(i+(S1-q1[0])/(q2[0]-q1[0]))
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
909 return indices
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 411
diff changeset
910
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
911 ##################
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
912 # Moving Objects
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
913 ##################
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
914
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
915 userTypeNames = ['unknown',
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
916 'car',
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
917 'pedestrian',
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
918 'motorcycle',
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
919 'bicycle',
185
c06379f25ab8 utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 184
diff changeset
920 'bus',
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
921 'truck']
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
922
185
c06379f25ab8 utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 184
diff changeset
923 userType2Num = utils.inverseEnumeration(userTypeNames)
c06379f25ab8 utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 184
diff changeset
924
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
925 class MovingObject(STObject):
288
e0d41c7f53d4 updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 284
diff changeset
926 '''Class for moving objects: a spatio-temporal object
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
927 with a trajectory and a geometry (constant volume over time) and a usertype (e.g. road user) coded as a number (see
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
928 '''
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
929
411
31604ef1cad4 added hog functions and the display of road user types if known
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 383
diff changeset
930 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown']):
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
931 super(MovingObject, self).__init__(num, timeInterval)
16
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
932 self.positions = positions
345
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
933 self.velocities = velocities
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
934 self.geometry = geometry
54
c354d41ef7cd corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 49
diff changeset
935 self.userType = userType
517
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
936 self.features = []
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 6
diff changeset
937 # compute bounding polygon from trajectory
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
938
555
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
939 @staticmethod
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
940 def generate(p, v, timeInterval):
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
941 positions, velocities = Trajectory.generate(p, v, int(timeInterval.length()))
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
942 return MovingObject(timeInterval = timeInterval, positions = positions, velocities = velocities)
f13220f765e0 added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 552
diff changeset
943
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
944 def getObjectInTimeInterval(self, inter):
54
c354d41ef7cd corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 49
diff changeset
945 '''Returns a new object extracted from self,
c354d41ef7cd corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 49
diff changeset
946 restricted to time interval inter'''
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 288
diff changeset
947 intersection = TimeInterval.intersection(inter, self.getTimeInterval())
97
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 96
diff changeset
948 if not intersection.empty():
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 96
diff changeset
949 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
950 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
951 if self.velocities:
97
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 96
diff changeset
952 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval)
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
953 return obj
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
954 else:
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
955 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
956 return None
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
957
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
958 def length(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
959 return self.timeInterval.length()
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
960
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
961 def getPositions(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
962 return self.positions
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
963
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
964 def getVelocities(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
965 return self.velocities
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
966
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
967 def getUserType(self):
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
968 return self.userType
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
969
335
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
970 def getCurvilinearPositions(self):
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
971 if hasattr(self, 'curvilinearPositions'):
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
972 return self.curvilinearPositions
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
973 else:
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
974 return None
3950bfe22768 added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 331
diff changeset
975
329
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
976 def setUserType(self, userType):
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
977 self.userType = userType
a70c205ebdd9 added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 327
diff changeset
978
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
979 def setFeatures(self, features):
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
980 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
981
49
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
982 def getSpeeds(self):
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
983 return self.getVelocities().norm()
1fb5606506ae re-arrangement
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 43
diff changeset
984
38
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
985 def getPositionAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
986 return self.positions[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
987
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
988 def getVelocityAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
989 return self.velocities[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents: 30
diff changeset
990
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
991 def getPositionAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
992 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
993
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
994 def getVelocityAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
995 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
996
21
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
997 def getXCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
998 return self.positions.getXCoordinates()
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
999
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
1000 def getYCoordinates(self):
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
1001 return self.positions.getYCoordinates()
3c4629550f5f added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
1002
517
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
1003 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, **kwargs):
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
1004 if withFeatures:
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
1005 for f in self.features:
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
1006 f.positions.plot('r', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
1007 self.positions.plot('bx-', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
1008 else:
47d9970ee954 added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 516
diff changeset
1009 self.positions.plot(options, withOrigin, timeStep, **kwargs)
182
d3f6de6c3918 added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 152
diff changeset
1010
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
1011 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
1012 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, imageHeight, options, withOrigin, timeStep, **kwargs)
22
169cd4679366 made Trajectory iterable
Nicolas Saunier <nico@confins.net>
parents: 21
diff changeset
1013
248
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
1014 def play(self, videoFilename, homography = None):
571ba5ed22e2 added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
1015 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
1016
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1017 def speedDiagnostics(self, framerate = 1., display = False):
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
1018 from numpy import std
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1019 from scipy.stats import scoreatpercentile
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1020 speeds = framerate*self.getSpeeds()
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
1021 coef = utils.linearRegression(range(len(speeds)), speeds)
379
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 378
diff changeset
1022 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), scoreatpercentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0]))
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
1023 if display:
379
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 378
diff changeset
1024 from matplotlib.pyplot import figure, plot, axis
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
1025 figure(1)
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
1026 self.plot()
379
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 378
diff changeset
1027 axis('equal')
377
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
1028 figure(2)
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
1029 plot(list(self.getTimeInterval()), speeds)
2aed569f39e7 added utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 372
diff changeset
1030
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1031 @staticmethod
551
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1032 def distances(obj1, obj2, instant1, _instant2 = None):
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1033 from scipy.spatial.distance import cdist
551
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1034 if _instant2 == None:
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1035 instant2 = instant1
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1036 else:
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1037 instant2 = _instant2
546
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1038 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)]
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1039 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)]
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1040 return cdist(positions1, positions2, metric = 'euclidean')
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1041
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1042 @staticmethod
551
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1043 def minDistance(obj1, obj2, instant1, instant2 = None):
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1044 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
546
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1045
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1046 @staticmethod
551
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1047 def maxDistance(obj1, obj2, instant, instant2 = None):
dc3739ac2371 rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 550
diff changeset
1048 return MovingObject.distances(obj1, obj2, instant1, instant2).max()
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1049
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1050 def maxSize(self):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1051 '''Returns the max distance between features
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1052 at instant there are the most features'''
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1053 if hasattr(self, 'features'):
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1054 nFeatures = -1
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1055 tMaxFeatures = 0
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1056 for t in self.getTimeInterval():
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1057 n = len([f for f in self.features if f.existsAtInstant(t)])
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1058 if n > nFeatures:
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1059 nFeatures = n
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1060 tMaxFeatures = t
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1061 return MovingObject.maxDistance(self, self, tMaxFeatures)
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1062 else:
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1063 print('Load features to compute a maximum size')
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1064 return None
546
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1065
550
5668af2ff515 minor naming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 549
diff changeset
1066 def setRoutes(self, startRouteID, endRouteID):
5668af2ff515 minor naming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 549
diff changeset
1067 self.startRouteID = startRouteID
5668af2ff515 minor naming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 549
diff changeset
1068 self.endRouteID = endRouteID
546
6c0923f1ce68 add some functions for behaviour analysis
MohamedGomaa
parents: 531
diff changeset
1069
83
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
1070 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
1071 '''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
1072 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
1073 empty list if there is no crossing'''
83
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
1074 indices = self.positions.getIntersections(p1, p2)
41da2cdcd91c re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 79
diff changeset
1075 return [t+self.getFirstInstant() for t in indices]
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 26
diff changeset
1076
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
1077 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
1078 '''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
1079 at constant speed'''
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
1080 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
1081
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1082 def projectCurvilinear(self, alignments, ln_mv_av_win=3):
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1083 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1084 (curvilinearPositions instance) which holds information about the
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1085 curvilinear coordinates using alignment metadata.
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1086 From Paul St-Aubin's PVA tools
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1087 ======
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1088
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1089 Input:
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1090 ======
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1091 alignments = a list of alignments, where each alignment is a list of
579
05c927c6d3cf curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 578
diff changeset
1092 points (class Point).
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1093 ln_mv_av_win = moving average window (in points) in which to smooth
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1094 lane changes. As per tools_math.cat_mvgavg(), this term
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1095 is a search *radius* around the center of the window.
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1096
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1097 '''
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1098
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1099 self.curvilinearPositions = CurvilinearTrajectory()
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1100
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1101 #For each point
579
05c927c6d3cf curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 578
diff changeset
1102 for i in xrange(int(self.length())):
05c927c6d3cf curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 578
diff changeset
1103 result = getSYfromXY(self.getPositionAt(i), alignments)
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1104
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1105 # Error handling
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1106 if(result == None):
579
05c927c6d3cf curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 578
diff changeset
1107 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i)))
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1108 else:
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
1109 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1110 self.curvilinearPositions.addPositionSYL(S, Y, align)
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1111
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1112 ## Go back through points and correct lane
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1113 #Run through objects looking for outlier point
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1114 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win)
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1115 ## Recalculate projected point to new lane
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
1116 lanes = self.curvilinearPositions.getLanes()
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
1117 if(lanes != smoothed_lanes):
579
05c927c6d3cf curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 578
diff changeset
1118 for i in xrange(len(lanes)):
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
1119 if(lanes[i] != smoothed_lanes[i]):
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
1120 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1121
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1122 # Error handling
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1123 if(result == None):
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1124 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
1125 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i)))
574
e24eeb244698 first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 573
diff changeset
1126 else:
579
05c927c6d3cf curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 578
diff changeset
1127 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
578
fe4e9d2b807d finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 577
diff changeset
1128 self.curvilinearPositions.setPosition(i, S, Y, align)
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1129
562
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1130 def computeSmoothTrajectory(self, minCommonIntervalLength):
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1131 '''Computes the trajectory as the mean of all features
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1132 if a feature exists, its position is
567
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1133
072cedc3f33d first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 562
diff changeset
1134 Warning work in progress
562
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1135 TODO? not use the first/last 1-.. positions'''
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1136 from numpy import array, median
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1137 nFeatures = len(self.features)
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1138 if nFeatures == 0:
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1139 print('Empty object features\nCannot compute smooth trajectory')
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1140 else:
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1141 # compute the relative position vectors
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1142 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1143 for i in xrange(nFeatures):
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1144 for j in xrange(i):
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1145 fi = self.features[i]
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1146 fj = self.features[j]
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1147 inter = fi.commonTimeInterval(fj)
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1148 if inter.length() >= minCommonIntervalLength:
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1149 xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1150 yi = array(fi.getYCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1151 xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1152 yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1153 relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi))
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1154 relativePositions[(j,i)] = -relativePositions[(i,j)]
259ccb3dd962 work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 555
diff changeset
1155
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1156 ###
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1157 # User Type Classification
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1158 ###
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 583
diff changeset
1159 def classifyUserTypeSpeedMotorized(self, threshold, percentileFactor=95, ignoreNInstantsAtEnds = 0):
345
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
1160 '''Classifies slow and fast road users
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
1161 slow: non-motorized -> pedestrians
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 583
diff changeset
1162 fast: motorized -> cars
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 583
diff changeset
1163 The percentile function is the same as the median if percentileFactor=50, the same as the minimum if percentileFactor=0 and the same as the maximum if percentileFactor=100.'''
501
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
1164 if ignoreNInstantsAtEnds > 0:
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1165 speeds = self.getSpeeds()[ignoreNInstantsAtEnds:-ignoreNInstantsAtEnds]
501
c81cbd6953fb update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
1166 else:
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1167 speeds = self.getSpeeds()
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 583
diff changeset
1168 if percentile(speeds,percentileFactor) >= threshold:
345
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
1169 self.setUserType(userType2Num['car'])
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
1170 else:
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
1171 self.setUserType(userType2Num['pedestrian'])
fa64b2e3a64f added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 343
diff changeset
1172
524
1dced8932b08 corrected bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 523
diff changeset
1173 def classifyUserTypeSpeed(self, speedProbabilities, aggregationFunc = median):
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1174 '''Classifies road user per road user type
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1175 speedProbabilities are functions return P(speed|class)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1176 in a dictionary indexed by user type names
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1177 Returns probabilities for each class
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1178
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1179 for simple threshold classification, simply pass non-overlapping indicator functions (membership)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1180 e.g. def indic(x):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1181 if abs(x-mu) < sigma:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1182 return 1
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1183 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1184 return x'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1185 if not hasattr(self, aggregatedSpeed):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1186 self.aggregatedSpeed = aggregationFunc(self.getSpeeds())
581
10e8a9f2bd9f change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 579
diff changeset
1187 userTypeProbabilities = {}
10e8a9f2bd9f change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 579
diff changeset
1188 for userTypename in speedProbabilities:
10e8a9f2bd9f change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 579
diff changeset
1189 userTypeProbabilities[userType2Num[userTypename]] = speedProbabilities[userTypename](self.aggregatedSpeed)
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1190 self.setUserType(utils.argmaxDict(userTypeProbabilities))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1191 return userTypeProbabilities
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1192
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1193 def initClassifyUserTypeHoGSVM(self, aggregationFunc = median):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1194 '''Initializes the data structures for classification
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1195
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1196 TODO? compute speed for longest feature?
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1197 Skip beginning and end of feature for speed? Offer options instead of median'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1198 self.aggregatedSpeed = aggregationFunc(self.getSpeeds())
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1199 self.userTypes = {}
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1200
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1201 def classifyUserTypeHoGSVMAtInstant(self, img, pedBikeCarSVM, instant, homography, width, height, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), px = 0.2, py = 0.2, pixelThreshold = 800):
520
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1202 '''Extract the image box around the object and
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1203 applies the SVM model on it'''
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1204 from numpy import array
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1205 croppedImg, yCropMin, yCropMax, xCropMin, xCropMax = imageBox(img, self, instant, homography, width, height, px, py, pixelThreshold)
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1206 if len(croppedImg) > 0: # != []
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1207 hog = array([cvutils.HOG(croppedImg)], dtype = np.float32)
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1208 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM == None:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1209 self.userTypes[instant] = int(pedBikeCarSVM.predict(hog))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1210 elif self.aggregatedSpeed < bikeCarSpeedTreshold:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1211 self.userTypes[instant] = int(bikeCarSVM.predict(hog))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1212 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1213 self.userTypes[instant] = userType2Num['car']
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1214 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1215 self.userTypes[instant] = userType2Num['unknown']
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1216
524
1dced8932b08 corrected bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 523
diff changeset
1217 def classifyUserTypeHoGSVM(self, images, pedBikeCarSVM, homography, width, height, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), speedProbabilities = None, aggregationFunc = median, px = 0.2, py = 0.2, pixelThreshold = 800):
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1218 '''Agregates SVM detections in each image and returns probability
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1219 (proportion of instants with classification in each category)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1220
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1221 iamges is a dictionary of images indexed by instant
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1222 With default parameters, the general (ped-bike-car) classifier will be used
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1223 TODO? consider all categories?'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1224 if not hasattr(self, aggregatedSpeed) or not hasattr(self, userTypes):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1225 print('Initilize the data structures for classification by HoG-SVM')
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1226 self.initClassifyUserTypeHoGSVM(aggregationFunc)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1227
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1228 if len(self.userTypes) != self.length(): # if classification has not been done previously
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1229 for t in self.getTimeInterval():
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1230 if t not in self.userTypes:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1231 self.classifyUserTypeHoGSVMAtInstant(images[t], pedBikeCarSVM, t, homography, width, height, bikeCarSVM, pedBikeSpeedTreshold, bikeCarSpeedThreshold, px, py, pixelThreshold)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1232 # compute P(Speed|Class)
524
1dced8932b08 corrected bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 523
diff changeset
1233 if speedProbabilities == None: # equiprobable information from speed
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1234 userTypeProbabilities = {userType2Num['car']: 1., userType2Num['pedestrian']: 1., userType2Num['bicycle']: 1.}
520
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1235 else:
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1236 userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities}
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1237 # result is P(Class|Appearance) x P(Speed|Class)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1238 nInstantsUserType = {userType2Num[userTypename]: 0 for userTypename in userTypeProbabilities}# number of instants the object is classified as userTypename
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1239 for t in self.userTypes:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1240 nInstantsUserType[self.userTypes[t]] += 1
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1241 for userTypename in userTypeProbabilities:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1242 userTypeProbabilities[userTypename] *= nInstantsUserType[userTypename]
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1243 # class is the user type that maximizes usertype probabilities
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1244 self.setUserType(utils.argmaxDict(userTypeProbabilities))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1245
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1246 def classifyUserTypeArea(self, areas, homography):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1247 '''Classifies the object based on its location (projected to image space)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1248 areas is a dictionary of matrix of the size of the image space
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1249 for different road users possible locations, indexed by road user type names
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1250
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1251 TODO: areas could be a wrapper object with a contains method that would work for polygons and images (with wrapper class)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1252 skip frames at beginning/end?'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1253 print('not implemented/tested yet')
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1254 if not hasattr(self, projectedPositions):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1255 if homography != None:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1256 self.projectedPositions = obj.positions.project(homography)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1257 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1258 self.projectedPositions = obj.positions
523
ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 522
diff changeset
1259 possibleUserTypes = {userType: 0 for userType in range(len(userTypenames))}
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1260 for p in self.projectedPositions:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1261 for userTypename in areas:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1262 if areas[userTypename][p.x, p.y] != 0:
523
ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 522
diff changeset
1263 possibleUserTypes[userType2Enum[userTypename]] += 1
522
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1264 # what to do: threshold for most common type? self.setUserType()
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 520
diff changeset
1265 return possibleUserTypes
520
fd9641cbd24b added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 517
diff changeset
1266
67
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
1267 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
1268 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
1269 '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
1270 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
1271 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
1272 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
1273
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
1274 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
1275 def collisionCourseCosine(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 66
diff changeset
1276 'A positive result indicates that the road users are getting closer'
378
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1277 return Point.cosine(movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant), #deltap
3805b9639647 added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 377
diff changeset
1278 movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)) #deltav
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
1279
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
1280 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
1281 '''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
1282 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
1283 figure()
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
1284 for obj in objects:
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 504
diff changeset
1285 obj.plot(colors.get(obj.userType))
62
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
1286 axis('equal')
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
1287
290fceb125d2 moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 58
diff changeset
1288
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
1289 if __name__ == "__main__":
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
1290 import doctest
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
1291 import unittest
43
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
1292 suite = doctest.DocFileSuite('tests/moving.txt')
6d11d9e7ad4e methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 41
diff changeset
1293 #suite = doctest.DocTestSuite()
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
1294 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
1295 #doctest.testmod()
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
1296 #doctest.testfile("example.txt")