Mercurial Hosting > traffic-intelligence
annotate python/moving.py @ 664:455f9b93819c
added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 20 May 2015 12:04:22 +0200 |
parents | 65a867942eee |
children | 15e244d2a1b5 |
rev | line source |
---|---|
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
1 #! /usr/bin/env python |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
2 '''Libraries for moving objects, trajectories...''' |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
3 |
664
455f9b93819c
added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
663
diff
changeset
|
4 import utils, cvutils |
455f9b93819c
added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
663
diff
changeset
|
5 from storage import VideoFilenameAddable |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
6 |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
7 from math import sqrt |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
8 from numpy import median |
16 | 9 |
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 | 22 def __init__(self, first=0, last=-1, revert = False): |
23 if revert and last<first: | |
24 self.first=last | |
25 self.last=first | |
26 else: | |
27 self.first=first | |
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 | 93 |
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 | 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 | 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 | 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 | 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 | 146 return self.num |
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 |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
198 def divide(self, alpha): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
199 'Warning, returns a new Point' |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
200 return Point(self.x/alpha, self.y/alpha) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
201 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
202 def plot(self, options = 'o', **kwargs): |
41 | 203 from matplotlib.pylab import plot |
211 | 204 plot([self.x], [self.y], options, **kwargs) |
41 | 205 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
206 def norm2Squared(self): |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
207 '''2-norm distance (Euclidean distance)''' |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
208 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
|
209 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
210 def norm2(self): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
211 '''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
|
212 return sqrt(self.norm2Squared()) |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
213 |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
214 def norm1(self): |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
215 return abs(self.x)+abs(self.y) |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
216 |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
217 def normMax(self): |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
218 return max(abs(self.x),abs(self.y)) |
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
219 |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
220 def aslist(self): |
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
221 return [self.x, self.y] |
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
222 |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
223 def astuple(self): |
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
224 return (self.x, self.y) |
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
225 |
223
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
226 def asint(self): |
c31722fcc9de
minor modifications for integer drawing in OpenCV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
214
diff
changeset
|
227 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
|
228 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
229 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
|
230 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
|
231 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
|
232 |
98
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
233 def project(self, homography): |
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
234 from numpy.core.multiarray import array |
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
235 projected = cvutils.projectArray(homography, array([[self.x], [self.y]])) |
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
236 return Point(projected[0], projected[1]) |
b85912ab4064
refactored projection functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
97
diff
changeset
|
237 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
238 def inPolygon(self, polygon): |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
239 '''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
|
240 (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
|
241 |
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
|
242 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
|
243 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
244 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
|
245 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
246 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
|
247 counter = 0; |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
248 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
249 p1 = polygon[0,:]; |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
250 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
|
251 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
|
252 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
|
253 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
|
254 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
|
255 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
|
256 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
|
257 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
|
258 counter+=1; |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
259 p1=p2 |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
260 return (counter%2 == 1); |
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
261 |
49 | 262 @staticmethod |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
263 def fromList(p): |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
264 return Point(p[0], p[1]) |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
265 |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
266 @staticmethod |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
267 def dot(p1, p2): |
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
268 'Scalar product' |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
269 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
|
270 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
271 @staticmethod |
90
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
272 def cross(p1, p2): |
f84293ad4611
renamed inner to cross product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
89
diff
changeset
|
273 'Cross product' |
89
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
274 return p1.x*p2.y-p1.y*p2.x |
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
275 |
f88a19695bba
added inner product
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
83
diff
changeset
|
276 @staticmethod |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
277 def cosine(p1, p2): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
278 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
|
279 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
280 @staticmethod |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
281 def distanceNorm2(p1, p2): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
282 return (p1-p2).norm2() |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
283 |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
284 @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
|
285 def plotAll(points, **kwargs): |
64
c75bcdaed00f
added functions for plotting points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
62
diff
changeset
|
286 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
|
287 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
|
288 |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
289 def similarOrientation(self, refDirection, cosineThreshold): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
290 '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
|
291 return Point.cosine(self, refDirection) >= cosineThreshold |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
292 |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
293 @staticmethod |
484
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
294 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
|
295 '''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
|
296 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
|
297 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
|
298 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
|
299 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
|
300 dv = v1-v2 |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
301 dp = p1-p2 |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
302 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
|
303 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
|
304 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
|
305 |
504
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
306 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
|
307 if delta >= 0: |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
308 deltaRoot = sqrt(delta) |
a40c75f04903
optimized direct time to collision computation and added tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
501
diff
changeset
|
309 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
|
310 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
|
311 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
|
312 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
|
313 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
|
314 ttc = ttc1 |
f012a8ad7a0e
corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
527
diff
changeset
|
315 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
|
316 ttc = ttc2 |
f012a8ad7a0e
corrected bug in Point.timeToCollision that might result in negative TTCs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
527
diff
changeset
|
317 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
|
318 ttc = None |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
319 else: |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
320 ttc = None |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
321 return ttc |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
451
diff
changeset
|
322 |
583
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
323 @staticmethod |
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
324 def midPoint(p1, p2): |
6ebfb43e938e
added midpoint function (from laurent gauthier)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
582
diff
changeset
|
325 '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
|
326 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
|
327 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
328 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
|
329 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
|
330 '''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
|
331 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
|
332 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
|
333 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
334 # Functions for coordinate transformation |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
335 # 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
|
336 def subsec_spline_dist(splines): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
337 ''' 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
|
338 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
339 Output: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
340 ======= |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
341 ss_spline_d[spline #][mode][station] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
342 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
343 where: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
344 mode=0: incremental distance |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
345 mode=1: cumulative distance |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
346 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
|
347 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
348 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
349 from numpy import zeros |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
350 ss_spline_d = [] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
351 #Prepare subsegment distances |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
352 for spline in range(len(splines)): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
353 ss_spline_d.append([[],[],[]]) |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
354 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
|
355 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
|
356 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
|
357 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
|
358 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
|
359 break |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
360 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
|
361 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
|
362 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
|
363 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
364 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
|
365 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
366 return ss_spline_d |
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 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
|
369 ''' 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
|
370 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
|
371 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
372 if(p0x == p1x and p0y == p1y): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
373 return None |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
374 try: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
375 #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
|
376 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
|
377 p1x += 0.0000000001 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
378 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
|
379 p1y += 0.0000000001 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
380 #make the calculation |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
381 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
|
382 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
|
383 except ZeroDivisionError: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
384 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
|
385 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
|
386 import pdb; pdb.set_trace() |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
387 return Point(X,Y) |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
388 |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
389 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
390 ''' 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
|
391 |
659
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
392 Output: |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
393 ======= |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
394 [spline index, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
395 subsegment leading point index, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
396 snapped point, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
397 subsegment distance, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
398 spline distance, |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
399 orthogonal point offset] |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
400 |
784298512b60
minor modifications
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
653
diff
changeset
|
401 or None |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
402 ''' |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
403 minOffsetY = float('inf') |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
404 #For each spline |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
405 for spline in range(len(splines)): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
406 #For each spline point index |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
407 for spline_p in range(len(splines[spline])-1): |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
408 #Get closest point on spline |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
409 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]) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
410 if closestPoint is None: |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
411 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
|
412 return None |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
413 # check if the |
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
414 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
|
415 offsetY = Point.distanceNorm2(closestPoint, p) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
416 if offsetY < minOffsetY: |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
417 minOffsetY = offsetY |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
418 snappedSpline = spline |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
419 snappedSplineLeadingPoint = spline_p |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
420 snappedPoint = Point(closestPoint.x, closestPoint.y) |
573
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
421 #Jump loop if significantly close |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
422 if offsetY < goodEnoughSplineDistance: |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
423 break |
cae4e5f3fe9f
fixed and simplified getSYfromXY
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
571
diff
changeset
|
424 #Get sub-segment distance |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
425 if minOffsetY != float('inf'): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
426 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint]) |
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
427 #Get cumulative alignment distance (total segment distance) |
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
428 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance |
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
429 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
|
430 offsetVector = p-snappedPoint |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
431 if Point.dot(orthogonalSplineVector, offsetVector) < 0: |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
432 minOffsetY = -minOffsetY |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
433 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
|
434 else: |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
435 return None |
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
436 |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
437 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
|
438 ''' Find X,Y coordinate from S,Y data. |
568
538fb47b3007
minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
439 if mode = 0 : return Snapped X,Y |
538fb47b3007
minor modification
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
440 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
|
441 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
442 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
443 #(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
|
444 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
|
445 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
446 #Find subsegment |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
447 snapped_x = None |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
448 snapped_y = None |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
449 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
|
450 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
|
451 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
|
452 #Get normal vector and then snap |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
453 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
|
454 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
|
455 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
|
456 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
|
457 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
|
458 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
|
459 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
|
460 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
461 #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
|
462 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
|
463 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
|
464 break |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
465 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
466 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
|
467 if(not snapped_x): |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
468 snapped_x = splines[splineNum][-1][0] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
469 snapped_y = splines[splineNum][-1][1] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
470 return [snapped_x,snapped_y] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
471 else: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
472 return [real_x,real_y] |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
473 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
474 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
475 class NormAngle(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
476 '''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
|
477 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
478 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
|
479 self.norm = norm |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
480 self.angle = angle |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
481 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
482 @staticmethod |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
483 def fromPoint(p): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
484 from math import atan2 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
485 norm = p.norm2() |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
486 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
|
487 angle = atan2(p.y, p.x) |
339 | 488 else: |
489 angle = 0. | |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
490 return NormAngle(norm, angle) |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
491 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
492 def __add__(self, other): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
493 '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
|
494 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
|
495 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
496 def getPoint(self): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
497 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
|
498 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
|
499 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
500 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
501 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
|
502 '''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
|
503 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
|
504 |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
505 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
|
506 '''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
|
507 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
|
508 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
|
509 predictedSpeedTheta = speedOrientation+control |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
510 if maxSpeed: |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
511 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
|
512 predictedPosition = position+predictedSpeedTheta.getPoint() |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
249
diff
changeset
|
513 return predictedPosition, predictedSpeedTheta |
245
bd8ab323c198
corrected issue with predictPosiont static method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
244
diff
changeset
|
514 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
515 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
516 class FlowVector(object): |
184
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
517 '''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
|
518 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
|
519 def __init__(self, position, velocity): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
520 '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
|
521 self.position = position |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
522 self.velocity = velocity |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
523 |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
524 def __add__(self, other): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
525 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
|
526 |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
527 def multiply(self, alpha): |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
528 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
|
529 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
530 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
|
531 from matplotlib.pylab import plot |
211 | 532 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
|
533 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
|
534 |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
535 @staticmethod |
d70e9b36889c
initial work on flow vectors and clustering algorithms
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
182
diff
changeset
|
536 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
|
537 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
|
538 |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
539 def intersection(p1, p2, p3, p4): |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
540 ''' 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
|
541 http://paulbourke.net/geometry/pointlineplane/''' |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
542 dp12 = p2-p1 |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
543 dp34 = p4-p3 |
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
544 #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
|
545 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
|
546 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
|
547 return None |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
548 else: |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
549 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
|
550 return p1+dp12.multiply(ua) |
569
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
551 |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
552 # def intersection(p1, p2, dp1, dp2): |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
553 # '''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
|
554 # 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
|
555 # from numpy import matrix |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
556 # from numpy.linalg import linalg |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
557 # A = matrix([[dp1.y, -dp1.x], |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
558 # [dp2.y, -dp2.x]]) |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
559 # 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
|
560 # [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
|
561 |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
562 # if linalg.det(A) == 0: |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
563 # return None |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
564 # else: |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
565 # intersection = linalg.solve(A,B) |
0057c04f94d5
work in progress on intersections (for PET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
567
diff
changeset
|
566 # 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
|
567 |
152
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
568 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
|
569 '''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
|
570 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
571 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
|
572 return None |
74b1fc68d4df
re-organized code to avoid cyclic python module dependencies
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
148
diff
changeset
|
573 else: |
571
a9c1d61a89b4
corrected bug for segment intersection
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
570
diff
changeset
|
574 inter = intersection(p1, p2, p3, p4) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
575 if (inter is not None |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
576 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
|
577 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
|
578 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
|
579 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
|
580 return inter |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
581 else: |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
582 return None |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
583 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
584 def segmentLineIntersection(p1, p2, p3, p4): |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
585 '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
586 inter = intersection(p1, p2, p3, p4) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
587 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y): |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
588 return inter |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
589 else: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
590 return None |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
591 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
592 |
290
df58d361f19e
refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
288
diff
changeset
|
593 class Trajectory(object): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
594 '''Class for trajectories: temporal sequence of positions |
22 | 595 |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
596 The class is iterable''' |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
597 |
79
5d487f183fe2
added method to test points in polygons and tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
74
diff
changeset
|
598 def __init__(self, positions=None): |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
599 if positions is not None: |
113
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
600 self.positions = positions |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
601 else: |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
602 self.positions = [[],[]] |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
603 |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
604 @staticmethod |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
605 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
|
606 t = Trajectory() |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
607 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
|
608 t.addPosition(p0) |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
609 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
|
610 p0 += v |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
611 t.addPosition(p0) |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
612 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
|
613 |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
614 @staticmethod |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
615 def load(line1, line2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
616 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
|
617 [float(n) for n in line2.split(' ')]]) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
618 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
619 @staticmethod |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
620 def fromPointList(points): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
621 t = Trajectory() |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
622 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
|
623 for p in points: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
624 t.addPositionXY(p[0],p[1]) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
625 else: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
626 for p in points: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
627 t.addPosition(p) |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
628 return t |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
629 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
630 def __len__(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
631 return len(self.positions[0]) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
632 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
633 def length(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
634 return self.__len__() |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
635 |
582
7e1ae4d97f1a
corrected bug for curvilinear trajectory with only one position and differentiation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
581
diff
changeset
|
636 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
|
637 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
|
638 |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
639 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
|
640 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
|
641 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
|
642 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
643 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
|
644 #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
|
645 |
16 | 646 def __str__(self): |
39 | 647 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())]) |
16 | 648 |
69
cc192d0450b3
added full support for two implementations of indicators, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
68
diff
changeset
|
649 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
|
650 return self.__str__() |
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
651 |
546 | 652 |
22 | 653 def __iter__(self): |
654 self.iterInstantNum = 0 | |
655 return self | |
656 | |
657 def next(self): | |
658 if self.iterInstantNum >= self.length(): | |
659 raise StopIteration | |
660 else: | |
661 self.iterInstantNum += 1 | |
23
5f2921ad4f7e
made Trajectory indexable and timeinterval iterable
Nicolas Saunier <nico@confins.net>
parents:
22
diff
changeset
|
662 return self[self.iterInstantNum-1] |
22 | 663 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
664 def setPositionXY(self, i, x, y): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
665 if i < self.__len__(): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
666 self.positions[0][i] = x |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
667 self.positions[1][i] = y |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
668 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
669 def setPosition(self, i, p): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
670 self.setPositionXY(i, p.x, p.y) |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
671 |
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
672 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
|
673 self.positions[0].append(x) |
606010d1d9a4
corrected errors in trajectories (if empty) and getTrajectoryInPolygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
108
diff
changeset
|
674 self.positions[1].append(y) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
675 |
71
45e958ccd9bd
added new addPosition method to Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
69
diff
changeset
|
676 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
|
677 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
|
678 |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
679 def duplicateLastPosition(self): |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
339
diff
changeset
|
680 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
|
681 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
|
682 |
182
d3f6de6c3918
added drawing functialities, in particular on aerial image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
152
diff
changeset
|
683 @staticmethod |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
684 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
|
685 from matplotlib.pylab import plot |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
686 if lastCoordinate is None: |
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
687 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
|
688 elif 0 <= lastCoordinate <= len(positions[0]): |
270
05c9b0cb8202
updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
689 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
|
690 if withOrigin: |
211 | 691 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
|
692 |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
693 def project(self, homography): |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
694 return Trajectory(cvutils.projectTrajectory(homography, self.positions).tolist()) |
203
e2f31813ade6
added code to display trajectories on videa
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
185
diff
changeset
|
695 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
696 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
|
697 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
|
698 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
699 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
|
700 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
|
701 |
620
aee4cbac9e0e
corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
595
diff
changeset
|
702 def plotOnWorldImage(self, nPixelsPerUnitDistance, 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
|
703 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]], |
620
aee4cbac9e0e
corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
595
diff
changeset
|
704 [x*nPixelsPerUnitDistance for x in self.positions[1]]] |
aee4cbac9e0e
corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
595
diff
changeset
|
705 Trajectory._plot(imgPositions, options, withOrigin, None, timeStep, **kwargs) |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
706 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
707 def getXCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
708 return self.positions[0] |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
709 |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
710 def getYCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
711 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
|
712 |
a5ef9e40688e
makes use of matplotlib function to test if point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
91
diff
changeset
|
713 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
|
714 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
|
715 return array(self.positions) |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
716 |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
717 def xBounds(self): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
718 # 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
|
719 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates())) |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
720 |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
721 def yBounds(self): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
722 # 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
|
723 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates())) |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
724 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
725 def add(self, traj2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
726 '''Returns a new trajectory of the same length''' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
727 if self.length() != traj2.length(): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
728 print 'Trajectories of different lengths' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
729 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
730 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
731 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
|
732 [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
|
733 |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
734 def subtract(self, traj2): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
735 '''Returns a new trajectory of the same length''' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
736 if self.length() != traj2.length(): |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
737 print 'Trajectories of different lengths' |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
738 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
739 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
740 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
|
741 [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
|
742 |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
743 def multiply(self, alpha): |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
744 '''Returns a new trajectory of the same length''' |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
745 return Trajectory([[alpha*x for x in self.getXCoordinates()], |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
746 [alpha*y for y in self.getYCoordinates()]]) |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
747 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
748 def differentiate(self, doubleLastPosition = False): |
339 | 749 diff = Trajectory() |
750 for i in xrange(1, self.length()): | |
751 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
|
752 if doubleLastPosition: |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
753 diff.addPosition(diff[-1]) |
339 | 754 return diff |
755 | |
16 | 756 def norm(self): |
757 '''Returns the list of the norms at each instant''' | |
758 # def add(x, y): return x+y | |
759 # sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]]) | |
760 # return sqrt(sq) | |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
761 from numpy import hypot |
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
762 return hypot(self.positions[0], self.positions[1]) |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
13
diff
changeset
|
763 |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
764 # def cumulatedDisplacement(self): |
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
765 # '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
|
766 # displacement = 0 |
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
767 # for i in xrange(self.length()-1): |
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
768 # 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
|
769 # return displacement |
49 | 770 |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
771 def computeCumulativeDistances(self): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
772 '''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
|
773 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
|
774 self.distances = [] |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
775 self.cumulativeDistances = [0.] |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
776 p1 = self[0] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
777 cumulativeDistance = 0. |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
778 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
|
779 p2 = self[i+1] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
780 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
|
781 cumulativeDistance += self.distances[-1] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
782 self.cumulativeDistances.append(cumulativeDistance) |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
783 p1 = p2 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
784 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
785 def getDistance(self,i): |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
786 '''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
|
787 if i < self.length()-1: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
788 return self.distances[i] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
789 else: |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
790 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
|
791 |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
792 def getCumulativeDistance(self, i): |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
793 '''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
|
794 if i < self.length(): |
576
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
795 return self.cumulativeDistances[i] |
0eff0471f9cb
added functions to use trajectories as alignments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
575
diff
changeset
|
796 else: |
577
d0abd2ee17b9
changed arguments to type Point
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
576
diff
changeset
|
797 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
|
798 |
381
387cc0142211
script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
379
diff
changeset
|
799 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): |
387cc0142211
script to replay event annotations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
379
diff
changeset
|
800 '''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
|
801 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
|
802 count = 0 |
383
0ce2210790b1
fixed stupid naming bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
381
diff
changeset
|
803 lengthThreshold = float(self.length())*minProportion |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
804 for p in self: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
805 if p.similarOrientation(refDirection, cosineThreshold): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
806 count += 1 |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
807 return count >= lengthThreshold |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
808 |
49 | 809 def wiggliness(self): |
608
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
810 return self.getCumulativeDistance(self.length()-1)/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))) |
49 | 811 |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
812 def getIntersections(self, p1, p2): |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
813 '''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
|
814 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
|
815 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
|
816 indices = [] |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
817 intersections = [] |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
818 |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
819 for i in xrange(self.length()-1): |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
820 q1=self.__getitem__(i) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
821 q2=self.__getitem__(i+1) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
822 p = utils.segmentIntersection(q1, q2, p1, p2) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
823 if p is not None: |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
824 if q1.x != q2.x: |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
825 ratio = (p.x-q1.x)/(q2.x-q1.x) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
826 elif q1.y != q2.y: |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
827 ratio = (p.y-q1.y)/(q2.y-q1.y) |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
828 else: |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
829 ratio = 0 |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
830 indices.append(i+ratio) |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
831 intersections.append(p) |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
832 return indices |
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
833 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
834 def getLineIntersections(self, p1, p2): |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
835 '''Returns a list of the indices at which the trajectory |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
836 intersects with the segment of extremities p1 and p2 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
837 the list is empty if there is no crossing''' |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
838 indices = [] |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
839 intersections = [] |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
840 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
841 for i in xrange(self.length()-1): |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
842 q1=self.__getitem__(i) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
843 q2=self.__getitem__(i+1) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
844 p = utils.segmentLineIntersection(p1, p2, q1, q2) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
845 if p is not None: |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
846 if q1.x != q2.x: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
847 ratio = (p.x-q1.x)/(q2.x-q1.x) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
848 elif q1.y != q2.y: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
849 ratio = (p.y-q1.y)/(q2.y-q1.y) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
850 else: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
851 ratio = 0 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
852 indices.append(i+ratio) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
853 intersections.append(p) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
854 return indices, intersections |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
627
diff
changeset
|
855 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
856 def getTrajectoryInInterval(self, inter): |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
857 'Returns all position between index inter.first and index.last (included)' |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
858 if inter.first >=0 and inter.last<= self.length(): |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
859 return Trajectory([self.positions[0][inter.first:inter.last+1], |
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
860 self.positions[1][inter.first:inter.last+1]]) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
861 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
862 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
863 |
372
349eb1e09f45
Cleaned the methods/functions indicating if a point is in a polygon
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
369
diff
changeset
|
864 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
|
865 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
|
866 '''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
|
867 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
|
868 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
|
869 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
|
870 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
|
871 return traj |
627
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
872 |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
873 def proportionInPolygon(self, polygon, minProportion = 0.5): |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
874 pointsIn = pointsInPolygon([p.asShapely() for p in self], polygon) |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
875 lengthThreshold = float(self.length())*minProportion |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
876 return len(pointsIn) >= lengthThreshold |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
877 else: |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
878 def getTrajectoryInPolygon(self, polygon): |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
879 '''Returns the trajectory built with the set of points inside the polygon |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
880 (array of Nx2 coordinates of the polygon vertices)''' |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
881 traj = Trajectory() |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
882 for p in self: |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
883 if p.inPolygon(polygon): |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
884 traj.addPosition(p) |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
885 return traj |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
886 |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
887 def proportionInPolygon(self, polygon, minProportion = 0.5): |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
888 pointsInPolygon = [p.inPolygon(polygon) for p in self] |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
889 lengthThreshold = float(self.length())*minProportion |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
890 return len(pointsInPolygon) >= lengthThreshold |
82e9f78a4714
added test for location for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
626
diff
changeset
|
891 |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
892 @staticmethod |
369
027e254f0b53
lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
368
diff
changeset
|
893 def lcss(t1, t2, lcss): |
027e254f0b53
lcss subclass for indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
368
diff
changeset
|
894 return lcss.compute(t1, t2) |
284
f2cf16ad798f
added LCSS for trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
280
diff
changeset
|
895 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
896 class CurvilinearTrajectory(Trajectory): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
897 '''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
|
898 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
|
899 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
|
900 |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
901 def __init__(self, S = None, Y = None, lanes = None): |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
902 if S is None or Y is None or len(S) != len(Y): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
903 self.positions = [[],[]] |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
904 if S is not None and Y is not None and len(S) != len(Y): |
543
cb213269d330
defensive code for curvilinear trajectory initialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
542
diff
changeset
|
905 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
|
906 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
907 self.positions = [S,Y] |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
908 if lanes is None or len(lanes) != self.length(): |
526
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
909 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
|
910 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
911 self.lanes = lanes |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
912 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
913 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
|
914 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
|
915 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
|
916 else: |
21bdeb29f855
corrected bug in initialization of lists and loading trajectories from vissim files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
524
diff
changeset
|
917 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
|
918 #elif isinstance( key, slice ): |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
919 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
920 def getSCoordinates(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
921 return self.getXCoordinates() |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
922 |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
923 def getLanes(self): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
924 return self.lanes |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
925 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
926 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
|
927 self.addPositionXY(s,y) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
928 self.lanes.append(lane) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
929 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
930 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
|
931 '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
|
932 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
|
933 |
327
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
934 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
|
935 self.setPositionXY(i, s, y) |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
936 if i < self.__len__(): |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
937 self.lanes[i] = lane |
42f2b46ec210
added class for trajectories in curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
318
diff
changeset
|
938 |
542
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
939 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
|
940 diff = CurvilinearTrajectory() |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
941 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
|
942 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
|
943 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
|
944 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
|
945 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
|
946 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
|
947 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
|
948 return diff |
a3add9f751ef
added differentiate function for curvilinear trajectories and modified the addPosition functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
531
diff
changeset
|
949 |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
950 def getIntersections(self, S1, lane = None): |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
951 '''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
|
952 goes past the curvilinear coordinate S1 |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
953 (in provided lane if lane is not None) |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
954 the list is empty if there is no crossing''' |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
955 indices = [] |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
956 for i in xrange(self.length()-1): |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
957 q1=self.__getitem__(i) |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
958 q2=self.__getitem__(i+1) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
959 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)): |
451
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
960 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
|
961 return indices |
cd342a774806
Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
411
diff
changeset
|
962 |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
963 ################## |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
964 # Moving Objects |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
965 ################## |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
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 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
|
968 'car', |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
969 '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
|
970 'motorcycle', |
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
971 'bicycle', |
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
972 'bus', |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
973 'truck'] |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
974 |
185
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
975 userType2Num = utils.inverseEnumeration(userTypeNames) |
c06379f25ab8
utilities for user types
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
184
diff
changeset
|
976 |
664
455f9b93819c
added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
663
diff
changeset
|
977 class MovingObject(STObject, VideoFilenameAddable): |
288
e0d41c7f53d4
updated class/method descriptions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
284
diff
changeset
|
978 '''Class for moving objects: a spatio-temporal object |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
979 with a trajectory and a geometry (constant volume over time) |
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
980 and a usertype (e.g. road user) coded as a number (see userTypeNames) |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
981 ''' |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
982 |
411
31604ef1cad4
added hog functions and the display of road user types if known
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
383
diff
changeset
|
983 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
|
984 super(MovingObject, self).__init__(num, timeInterval) |
16 | 985 self.positions = positions |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
986 self.velocities = velocities |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
987 self.geometry = geometry |
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
988 self.userType = userType |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
989 self.features = None |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
6
diff
changeset
|
990 # compute bounding polygon from trajectory |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
991 |
555
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
992 @staticmethod |
f13220f765e0
added static methods to create trajectories and moving objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
552
diff
changeset
|
993 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
|
994 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
|
995 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
|
996 |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
997 @staticmethod |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
998 def concatenate(obj1, obj2, num = None): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
999 '''Concatenates two objects supposed to overlap temporally ''' |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1000 commonTimeInterval = obj1.commonTimeInterval(obj2) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1001 if commonTimeInterval.empty(): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1002 print('The two objects\' time intervals do not overlap: obj1 {} and obj2 {}'.format(obj1.getTimeInterval(), obj2.getTimeInterval())) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1003 return None |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1004 else: |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1005 if num is None: |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1006 newNum = obj1.getNum() |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1007 else: |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1008 newNum = num |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1009 newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval()) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1010 # positions |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1011 positions = Trajectory() |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1012 for t in newTimeInterval: |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1013 nTotal = 0. |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1014 p = Point(0.,0.) |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1015 for obj in [obj1, obj2]: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1016 if obj.existsAtInstant(t): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1017 if obj.hasFeatures(): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1018 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1019 else: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1020 n = 1. |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1021 p += obj.getPositionAtInstant(t).multiply(n) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1022 nTotal += n |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1023 assert nTotal>0, 'there should be at least one point for each instant' |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1024 positions.addPosition(p.divide(nTotal)) |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1025 # velocities: if any |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1026 if hasattr(obj1, 'velocities') and hasattr(obj2, 'velocities'): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1027 velocities = Trajectory() |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1028 for t in newTimeInterval: |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1029 nTotal = 0. |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1030 p = Point(0.,0.) |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1031 for obj in [obj1, obj2]: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1032 if obj.existsAtInstant(t): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1033 if obj.hasFeatures(): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1034 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1035 else: |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1036 n = 1. |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1037 p += obj.getVelocityAtInstant(t).multiply(n) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1038 nTotal += n |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1039 assert n>0, 'there should be at least one point for each instant' |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1040 velocities.addPosition(p.divide(nTotal)) |
623
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1041 else: |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1042 velocities = None |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1043 # TODO object envelop (polygon) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1044 # user type |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1045 if obj1.getUserType() != obj2.getUserType(): |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1046 print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()])) |
ce7133cbcdf3
implemented function to concatenate objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
622
diff
changeset
|
1047 |
663
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1048 newObject = MovingObject(newNum, newTimeInterval, positions, velocities, userType = obj1.getUserType()) |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1049 if obj1.hasFeatures() and obj2.hasFeatures(): |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1050 newObject.features = obj1.getFeatures()+obj2.getFeatures() |
65a867942eee
modified concatenate to take into account features if available
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
1051 return newObject |
622
dc8490726d06
corrected issues created with homography projection in Trajectory
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
621
diff
changeset
|
1052 |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1053 def getObjectInTimeInterval(self, inter): |
54
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1054 '''Returns a new object extracted from self, |
c354d41ef7cd
corrected code for usertype in movingobject
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
49
diff
changeset
|
1055 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
|
1056 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
|
1057 if not intersection.empty(): |
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
1058 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
|
1059 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
|
1060 if self.velocities: |
97
b3a1c26e2f22
corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
96
diff
changeset
|
1061 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval) |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1062 return obj |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1063 else: |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1064 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
|
1065 return None |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1066 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1067 def length(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1068 return self.timeInterval.length() |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1069 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1070 def getPositions(self): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1071 return self.positions |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1072 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1073 def getVelocities(self): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1074 return self.velocities |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1075 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1076 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
|
1077 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
|
1078 |
335
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1079 def getCurvilinearPositions(self): |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1080 if hasattr(self, 'curvilinearPositions'): |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1081 return self.curvilinearPositions |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1082 else: |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1083 return None |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
331
diff
changeset
|
1084 |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1085 def plotCurvilinearPositions(self, lane = None, options = '', withOrigin = False, **kwargs): |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1086 if hasattr(self, 'curvilinearPositions'): |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1087 from matplotlib.pylab import plot |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1088 from numpy import NaN |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1089 if lane is None: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1090 plot(list(self.getTimeInterval()), self.curvilinearPositions.positions[0], options, **kwargs) |
660
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1091 if withOrigin: |
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1092 plot([self.getFirstInstant()], [self.curvilinearPositions.positions[0][0]], 'ro', **kwargs) |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1093 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1094 instants = [] |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1095 coords = [] |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1096 for t, p in zip(self.getTimeInterval(), self.curvilinearPositions): |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1097 if p[2] == lane: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1098 instants.append(t) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1099 coords.append(p[0]) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1100 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1101 instants.append(NaN) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1102 coords.append(NaN) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1103 plot(instants, coords, options, **kwargs) |
660
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1104 if withOrigin and len(instants)>0: |
994dd644f6ab
corrected impact of warmUpLastInstant
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
659
diff
changeset
|
1105 plot([instants[0]], [coords[0]], 'ro', **kwargs) |
643
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1106 else: |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1107 print('Object {} has no curvilinear positions'.format(self.getNum())) |
bfaa6b95dae2
added function to plot curvilinear position as a function of time
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
1108 |
329
a70c205ebdd9
added sqlite code, in particular to load and save road user type
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
327
diff
changeset
|
1109 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
|
1110 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
|
1111 |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
1112 def setFeatures(self, features): |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
1113 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
|
1114 |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1115 def getFeatures(self): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1116 return self.features |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1117 |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1118 def hasFeatures(self): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1119 return (self.features is not None) |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1120 |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1121 def getFeature(self, i): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1122 if self.hasFeatures() and i<len(self.features): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1123 return self.features[i] |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1124 else: |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1125 return None |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1126 |
49 | 1127 def getSpeeds(self): |
1128 return self.getVelocities().norm() | |
1129 | |
633
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1130 def getSpeedIndicator(self): |
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1131 from indicators import SeverityIndicator |
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1132 return SeverityIndicator('Speed', {t:self.getVelocityAtInstant(t).norm2() for t in self.getTimeInterval()}) |
f410c8fb07b7
minor function to export speed as temporal indicator for display
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
631
diff
changeset
|
1133 |
38
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1134 def getPositionAt(self, i): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1135 return self.positions[i] |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1136 |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1137 def getVelocityAt(self, i): |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1138 return self.velocities[i] |
0d321c23d337
added norm functions for Point and accessor methods for MovingObject
Nicolas Saunier <nico@confins.net>
parents:
30
diff
changeset
|
1139 |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1140 def getPositionAtInstant(self, i): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1141 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
|
1142 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1143 def getVelocityAtInstant(self, i): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1144 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
|
1145 |
21
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1146 def getXCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1147 return self.positions.getXCoordinates() |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1148 |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1149 def getYCoordinates(self): |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1150 return self.positions.getYCoordinates() |
3c4629550f5f
added basic getters for objects
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
1151 |
517
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1152 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, **kwargs): |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1153 if withFeatures and self.hasFeatures(): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
660
diff
changeset
|
1154 for f in self.getFeatures(): |
517
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1155 f.positions.plot('r', True, timeStep, **kwargs) |
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1156 self.positions.plot('bx-', True, timeStep, **kwargs) |
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1157 else: |
47d9970ee954
added plotting of features with object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
516
diff
changeset
|
1158 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
|
1159 |
621
582508610572
merge and updated consistently that imageheight does not seem necessary anymore for plotting on world image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
diff
changeset
|
1160 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, **kwargs): |
582508610572
merge and updated consistently that imageheight does not seem necessary anymore for plotting on world image
Nicolas Saunier <nicolas.saunier@polymtl.ca>
diff
changeset
|
1161 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, **kwargs) |
22 | 1162 |
626
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
1163 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.): |
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
1164 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication) |
248
571ba5ed22e2
added utils for bus processing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
1165 |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1166 def speedDiagnostics(self, framerate = 1., display = False): |
377 | 1167 from numpy import std |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1168 from scipy.stats import scoreatpercentile |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1169 speeds = framerate*self.getSpeeds() |
377 | 1170 coef = utils.linearRegression(range(len(speeds)), speeds) |
379 | 1171 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 | 1172 if display: |
379 | 1173 from matplotlib.pyplot import figure, plot, axis |
377 | 1174 figure(1) |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
1175 self.plot() |
379 | 1176 axis('equal') |
377 | 1177 figure(2) |
1178 plot(list(self.getTimeInterval()), speeds) | |
1179 | |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1180 @staticmethod |
653
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1181 def minMaxDistance(obj1, obj2): |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1182 '''Computes the min max distance used for feature grouping''' |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1183 commonTimeInterval = obj1.commonTimeInterval(obj2) |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1184 if not commonTimeInterval.empty(): |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1185 minDistance = (obj1.getPositionAtInstant(commonTimeInterval.first)-obj2.getPositionAtInstant(commonTimeInterval.first)).norm2() |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1186 maxDistance = minDistance |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1187 for t in list(commonTimeInterval)[1:]: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1188 d = (obj1.getPositionAtInstant(t)-obj2.getPositionAtInstant(t)).norm2() |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1189 if d<minDistance: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1190 minDistance = d |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1191 elif d>maxDistance: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1192 maxDistance = d |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1193 return int(commonTimeInterval.length()), minDistance, maxDistance |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1194 else: |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1195 return int(commonTimeInterval.length()), None, None |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1196 |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1197 @staticmethod |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1198 def distances(obj1, obj2, instant1, _instant2 = None): |
653
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1199 '''Returns the distances between all features of the 2 objects |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1200 at the same instant instant1 |
107f1ad02b69
compilation with C++11 and test function for movingobject distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
643
diff
changeset
|
1201 or at instant1 and instant2''' |
378
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1202 from scipy.spatial.distance import cdist |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1203 if _instant2 is None: |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1204 instant2 = instant1 |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1205 else: |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1206 instant2 = _instant2 |
546 | 1207 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)] |
1208 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)] | |
1209 return cdist(positions1, positions2, metric = 'euclidean') | |
1210 | |
1211 @staticmethod | |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1212 def minDistance(obj1, obj2, instant1, instant2 = None): |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1213 return MovingObject.distances(obj1, obj2, instant1, instant2).min() |
546 | 1214 |
1215 @staticmethod | |
551
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1216 def maxDistance(obj1, obj2, instant, instant2 = None): |
dc3739ac2371
rearranged all distance computations between points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
550
diff
changeset
|
1217 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
|
1218 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1219 def maxSize(self): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1220 '''Returns the max distance between features |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1221 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
|
1222 if hasattr(self, 'features'): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1223 nFeatures = -1 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1224 tMaxFeatures = 0 |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1225 for t in self.getTimeInterval(): |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1226 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
|
1227 if n > nFeatures: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1228 nFeatures = n |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1229 tMaxFeatures = t |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1230 return MovingObject.maxDistance(self, self, tMaxFeatures) |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1231 else: |
3805b9639647
added tests for movement orientation, object size
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
377
diff
changeset
|
1232 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
|
1233 return None |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
diff
changeset
|
1234 |
550 | 1235 def setRoutes(self, startRouteID, endRouteID): |
1236 self.startRouteID = startRouteID | |
1237 self.endRouteID = endRouteID | |
546 | 1238 |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1239 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
|
1240 '''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
|
1241 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
|
1242 empty list if there is no crossing''' |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1243 indices, intersections = self.positions.getIntersections(p1, p2) |
83
41da2cdcd91c
re-arranged trajectory intersections
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
79
diff
changeset
|
1244 return [t+self.getFirstInstant() for t in indices] |
27
44689029a86f
updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents:
26
diff
changeset
|
1245 |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1246 @staticmethod |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1247 def computePET(obj1, obj2, collisionDistanceThreshold): |
635
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1248 '''Post-encroachment time based on distance threshold |
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1249 |
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1250 Returns the smallest time difference when the object positions are within collisionDistanceThreshold''' |
631
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1251 #for i in xrange(int(obj1.length())-1): |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1252 # for j in xrange(int(obj2.length())-1): |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1253 # inter = segmentIntersection(obj1.getPositionAt(i), obj1.getPositionAt(i+1), obj2.getPositionAt(i), obj2.getPositionAt(i+1)) |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1254 from scipy.spatial.distance import cdist |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1255 from numpy import zeros |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1256 positions1 = [p.astuple() for p in obj1.getPositions()] |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1257 positions2 = [p.astuple() for p in obj2.getPositions()] |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1258 pets = zeros((int(obj1.length()), int(obj2.length()))) |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1259 for i,t1 in enumerate(obj1.getTimeInterval()): |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1260 for j,t2 in enumerate(obj2.getTimeInterval()): |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1261 pets[i,j] = abs(t1-t2) |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1262 distances = cdist(positions1, positions2, metric = 'euclidean') |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1263 if distances.min() <= collisionDistanceThreshold: |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1264 return pets[distances <= collisionDistanceThreshold].min() |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1265 else: |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1266 return None |
2d1d33ae1c69
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
1267 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
1268 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
|
1269 '''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
|
1270 at constant speed''' |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
1271 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
|
1272 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1273 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
|
1274 ''' 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
|
1275 (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
|
1276 curvilinear coordinates using alignment metadata. |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1277 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
|
1278 ====== |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1279 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1280 Input: |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1281 ====== |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1282 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
|
1283 points (class Point). |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1284 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
|
1285 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
|
1286 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
|
1287 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1288 ''' |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1289 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1290 self.curvilinearPositions = CurvilinearTrajectory() |
567
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1291 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1292 #For each point |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1293 for i in xrange(int(self.length())): |
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1294 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
|
1295 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1296 # Error handling |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1297 if(result is None): |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1298 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
|
1299 else: |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1300 [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
|
1301 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
|
1302 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1303 ## 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
|
1304 #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
|
1305 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
|
1306 ## Recalculate projected point to new lane |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1307 lanes = self.curvilinearPositions.getLanes() |
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1308 if(lanes != smoothed_lanes): |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1309 for i in xrange(len(lanes)): |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1310 if(lanes[i] != smoothed_lanes[i]): |
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1311 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
|
1312 |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1313 # Error handling |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1314 if(result is None): |
574
e24eeb244698
first implementation of projection to curvilinear coordinates
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
573
diff
changeset
|
1315 ## 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
|
1316 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
|
1317 else: |
579
05c927c6d3cf
curvilinear projection seems operational
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
578
diff
changeset
|
1318 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result |
578
fe4e9d2b807d
finalizing transformcoordinates for each object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
577
diff
changeset
|
1319 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
|
1320 |
562
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1321 def computeSmoothTrajectory(self, minCommonIntervalLength): |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1322 '''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
|
1323 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
|
1324 |
072cedc3f33d
first integration of curvilinear transformation from Paul
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
562
diff
changeset
|
1325 Warning work in progress |
562
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1326 TODO? not use the first/last 1-.. positions''' |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1327 from numpy import array, median |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1328 nFeatures = len(self.features) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1329 if nFeatures == 0: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1330 print('Empty object features\nCannot compute smooth trajectory') |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1331 else: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1332 # compute the relative position vectors |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1333 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
|
1334 for i in xrange(nFeatures): |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1335 for j in xrange(i): |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1336 fi = self.features[i] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1337 fj = self.features[j] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1338 inter = fi.commonTimeInterval(fj) |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1339 if inter.length() >= minCommonIntervalLength: |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1340 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
|
1341 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
|
1342 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
|
1343 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
|
1344 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
|
1345 relativePositions[(j,i)] = -relativePositions[(i,j)] |
259ccb3dd962
work on object trajectory smoothing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
555
diff
changeset
|
1346 |
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
|
1347 ### |
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
|
1348 # 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
|
1349 ### |
524
1dced8932b08
corrected bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
523
diff
changeset
|
1350 def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, ignoreNInstantsAtEnds = 0): |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1351 '''Classifies slow and fast road users |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1352 slow: non-motorized -> pedestrians |
607 | 1353 fast: motorized -> cars |
608
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
1354 |
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
1355 aggregationFunc can be any function that can be applied to a vector of speeds, including percentile: |
078adacd72a4
moving.py integrating Mohamed's comments refactored
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
607
diff
changeset
|
1356 aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile''' |
501
c81cbd6953fb
update to classify speed to remove data at both ends
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
484
diff
changeset
|
1357 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
|
1358 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
|
1359 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
|
1360 speeds = 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
|
1361 if aggregationFunc(speeds) >= threshold: |
345
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1362 self.setUserType(userType2Num['car']) |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1363 else: |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1364 self.setUserType(userType2Num['pedestrian']) |
fa64b2e3a64f
added simple classification based on speed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
343
diff
changeset
|
1365 |
524
1dced8932b08
corrected bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
523
diff
changeset
|
1366 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
|
1367 '''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
|
1368 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
|
1369 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
|
1370 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
|
1371 |
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
|
1372 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
|
1373 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
|
1374 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
|
1375 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
|
1376 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
|
1377 return x''' |
626
35155ac2a294
corrected bugs, in particular to MovingObject.play
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
624
diff
changeset
|
1378 if not hasattr(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
|
1379 self.aggregatedSpeed = aggregationFunc(self.getSpeeds()) |
581
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
1380 userTypeProbabilities = {} |
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
1381 for userTypename in speedProbabilities: |
10e8a9f2bd9f
change for python 2.6
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
579
diff
changeset
|
1382 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
|
1383 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
|
1384 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
|
1385 |
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
|
1386 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
|
1387 '''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
|
1388 |
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
|
1389 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
|
1390 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
|
1391 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
|
1392 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
|
1393 |
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
|
1394 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
|
1395 '''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
|
1396 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
|
1397 from numpy import array |
fd9641cbd24b
added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
517
diff
changeset
|
1398 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
|
1399 if len(croppedImg) > 0: # != [] |
fd9641cbd24b
added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
517
diff
changeset
|
1400 hog = array([cvutils.HOG(croppedImg)], dtype = np.float32) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1401 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM is None: |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1402 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
|
1403 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
|
1404 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
|
1405 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
|
1406 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
|
1407 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
|
1408 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
|
1409 |
524
1dced8932b08
corrected bugs
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
523
diff
changeset
|
1410 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
|
1411 '''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
|
1412 (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
|
1413 |
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
|
1414 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
|
1415 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
|
1416 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
|
1417 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
|
1418 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
|
1419 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
|
1420 |
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
|
1421 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
|
1422 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
|
1423 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
|
1424 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
|
1425 # compute P(Speed|Class) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1426 if speedProbabilities is 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
|
1427 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
|
1428 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
|
1429 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
|
1430 # 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
|
1431 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
|
1432 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
|
1433 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
|
1434 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
|
1435 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
|
1436 # 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
|
1437 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
|
1438 |
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
|
1439 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
|
1440 '''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
|
1441 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
|
1442 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
|
1443 |
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
|
1444 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
|
1445 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
|
1446 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
|
1447 if not hasattr(self, projectedPositions): |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1448 if homography is not None: |
522
ce40a89bd6ae
added functions for classification refactored from Sohail s work for TRB/TRC (to be tested)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
520
diff
changeset
|
1449 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
|
1450 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
|
1451 self.projectedPositions = obj.positions |
523
ce4eaabacc26
modified internal implementation of user type for classifyUserTypeArea
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
522
diff
changeset
|
1452 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
|
1453 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
|
1454 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
|
1455 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
|
1456 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
|
1457 # 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
|
1458 return possibleUserTypes |
520
fd9641cbd24b
added function to classify object at instant from SVM
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
517
diff
changeset
|
1459 |
67
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1460 @staticmethod |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1461 def collisionCourseDotProduct(movingObject1, movingObject2, instant): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1462 '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
|
1463 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
|
1464 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
|
1465 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
|
1466 |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1467 @staticmethod |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1468 def collisionCourseCosine(movingObject1, movingObject2, instant): |
ded58c424783
added indicator computation and modified severity indicator constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
66
diff
changeset
|
1469 '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
|
1470 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
|
1471 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
|
1472 |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1473 |
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1474 ################## |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1475 # Annotations |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1476 ################## |
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1477 |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1478 class BBAnnotation(MovingObject): |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1479 '''Class for a series of ground truth annotations using bounding boxes |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1480 Its center is the center of the containing shape |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1481 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1482 By default in image space |
588
c5406edbcf12
added loading ground truth annotations (ground truth) from polytrack format
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
587
diff
changeset
|
1483 ''' |
587
cf578ba866da
added code to load bounding box corners as trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
583
diff
changeset
|
1484 |
589
5800a87f11ae
corrected one bug and changed attribute names
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
588
diff
changeset
|
1485 def __init__(self, num = None, timeInterval = None, topLeftPositions = None, bottomRightPositions = None, userType = userType2Num['unknown']): |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1486 super(BBAnnotation, self).__init__(num, timeInterval, userType = userType) |
589
5800a87f11ae
corrected one bug and changed attribute names
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
588
diff
changeset
|
1487 self.topLeftPositions = topLeftPositions.getPositions() |
5800a87f11ae
corrected one bug and changed attribute names
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
588
diff
changeset
|
1488 self.bottomRightPositions = bottomRightPositions.getPositions() |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1489 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1490 def computeCentroidTrajectory(self, homography = None): |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1491 self.positions = self.topLeftPositions.add(self.bottomRightPositions).multiply(0.5) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
635
diff
changeset
|
1492 if homography is not None: |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1493 self.positions = self.positions.project(homography) |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1494 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1495 def matches(self, obj, instant, matchingDistance): |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1496 '''Indicates if the annotation matches obj (MovingObject) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1497 with threshold matchingDistance |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1498 Returns distance if below matchingDistance, matchingDistance+1 otherwise |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1499 (returns an actual value, otherwise munkres does not terminate)''' |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1500 d = Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant)) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1501 if d < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1502 return d |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1503 else: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1504 return matchingDistance + 1 |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1505 |
595
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1506 def computeClearMOT(annotations, objects, matchingDistance, firstInstant, lastInstant, debug = False): |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1507 '''Computes the CLEAR MOT metrics |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1508 |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1509 Reference: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1510 Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008) |
590
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1511 |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1512 objects and annotations are supposed to in the same space |
0fa73cbe9fdb
annotation centroid are computed explicitly and projected at that step if required
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
589
diff
changeset
|
1513 current implementation is BBAnnotations (bounding boxes) |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1514 mathingDistance is threshold on matching between annotation and object |
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1515 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1516 TO: tracker output (objects) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1517 GT: ground truth (annotations) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1518 |
624
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1519 Output: returns motp, mota, mt, mme, fpt, gt |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1520 mt number of missed GT.frames (sum of the number of GT not detected in each frame) |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1521 mme number of mismatches |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1522 fpt number of false alarm.frames (tracker objects without match in each frame) |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1523 gt number of GT.frames |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1524 |
bac66bd536c5
added slight documentation of CLEAR MOT output
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
623
diff
changeset
|
1525 TODO: Should we use the distance as weights or just 1/0 if distance below matchingDistance? |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1526 (add argument useDistanceForWeights = False)''' |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1527 from munkres import Munkres |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1528 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1529 munk = Munkres() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1530 dist = 0. # total distance between GT and TO |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1531 ct = 0 # number of associations between GT and tracker output in each frame |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1532 gt = 0 # number of GT.frames |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1533 mt = 0 # number of missed GT.frames (sum of the number of GT not detected in each frame) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1534 fpt = 0 # number of false alarm.frames (tracker objects without match in each frame) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1535 mme = 0 # number of mismatches |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1536 matches = {} # match[i] is the tracker track associated with GT i (using object references) |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1537 for t in xrange(firstInstant, lastInstant+1): |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1538 previousMatches = matches.copy() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1539 # go through currently matched GT-TO and check if they are still matched withing matchingDistance |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1540 toDelete = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1541 for a in matches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1542 if a.existsAtInstant(t) and matches[a].existsAtInstant(t): |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1543 d = a.matches(matches[a], t, matchingDistance) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1544 if d < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1545 dist += d |
592
985a3021cff2
first match table implementation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
590
diff
changeset
|
1546 else: |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1547 toDelete.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1548 else: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1549 toDelete.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1550 for a in toDelete: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1551 del matches[a] |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
1552 |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1553 # match all unmatched GT-TO |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1554 matchedGTs = matches.keys() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1555 matchedTOs = matches.values() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1556 costs = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1557 unmatchedGTs = [a for a in annotations if a.existsAtInstant(t) and a not in matchedGTs] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1558 unmatchedTOs = [o for o in objects if o.existsAtInstant(t) and o not in matchedTOs] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1559 nGTs = len(matchedGTs)+len(unmatchedGTs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1560 nTOs = len(matchedTOs)+len(unmatchedTOs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1561 if len(unmatchedTOs) > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1562 for a in unmatchedGTs: |
595
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1563 costs.append([a.matches(o, t, matchingDistance) for o in unmatchedTOs]) |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1564 if len(costs) > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1565 newMatches = munk.compute(costs) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1566 for k,v in newMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1567 if costs[k][v] < matchingDistance: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1568 matches[unmatchedGTs[k]]=unmatchedTOs[v] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1569 dist += costs[k][v] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1570 if debug: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1571 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.iteritems()])) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1572 |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1573 # compute metrics elements |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1574 ct += len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1575 mt += nGTs-len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1576 fpt += nTOs-len(matches) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1577 gt += nGTs |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1578 # compute mismatches |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1579 # for gt that do not appear in both frames, check if the corresponding to was matched to another gt in previous/next frame |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1580 mismatches = [] |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1581 for a in matches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1582 if a in previousMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1583 if matches[a] != previousMatches[a]: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1584 mismatches.append(a) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1585 elif matches[a] in previousMatches.values(): |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1586 mismatches.append(matches[a]) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1587 for a in previousMatches: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1588 if a not in matches and previousMatches[a] in matches.values(): |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1589 mismatches.append(previousMatches[a]) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1590 if debug: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1591 for mm in set(mismatches): |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1592 print type(mm), mm.getNum() |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1593 # some object mismatches may appear twice |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1594 mme += len(set(mismatches)) |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1595 |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1596 if ct > 0: |
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1597 motp = dist/ct |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
1598 else: |
594
9e39cd95e017
first implementation of CLEAR MOT (needs formal tests)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
593
diff
changeset
|
1599 motp = None |
595
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1600 if gt > 0: |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1601 mota = 1.-float(mt+fpt+mme)/gt |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1602 else: |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1603 mota = None |
17b02c8054d0
added tests and corrected one bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
594
diff
changeset
|
1604 return motp, mota, mt, mme, fpt, gt |
593
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
1605 |
e2a873e08568
non-working clear mot metrics
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
592
diff
changeset
|
1606 |
62
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1607 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
|
1608 '''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
|
1609 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
|
1610 figure() |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1611 for obj in objects: |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
504
diff
changeset
|
1612 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
|
1613 axis('equal') |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1614 |
290fceb125d2
moved road user types and added plotting for all road users
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
58
diff
changeset
|
1615 |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1616 if __name__ == "__main__": |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1617 import doctest |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1618 import unittest |
43
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1619 suite = doctest.DocFileSuite('tests/moving.txt') |
6d11d9e7ad4e
methods for trajectories and objects
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
41
diff
changeset
|
1620 #suite = doctest.DocTestSuite() |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1621 unittest.TextTestRunner().run(suite) |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1622 #doctest.testmod() |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
1623 #doctest.testfile("example.txt") |
635
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1624 if shapelyAvailable: |
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1625 suite = doctest.DocFileSuite('tests/moving_shapely.txt') |
6ae68383071e
corrected issue with tests requiring shapely, adding a separate test file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
633
diff
changeset
|
1626 unittest.TextTestRunner().run(suite) |