Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1028:cc5cb04b04b0
major update using the trafficintelligence package name and install through pip
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 15 Jun 2018 11:19:10 -0400 |
parents | python/moving.py@5d2f6afae35b |
children | c6cf75a2ed08 |
comparison
equal
deleted
inserted
replaced
1027:6129296848d3 | 1028:cc5cb04b04b0 |
---|---|
1 #! /usr/bin/env python | |
2 '''Libraries for moving objects, trajectories...''' | |
3 | |
4 from trafficintelligence import utils, cvutils | |
5 from trafficintelligence.base import VideoFilenameAddable | |
6 | |
7 from math import sqrt, atan2, cos, sin | |
8 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, float32, argwhere, minimum | |
9 from matplotlib.pyplot import plot, text | |
10 from scipy.stats import scoreatpercentile | |
11 from scipy.spatial.distance import cdist | |
12 from scipy.signal import savgol_filter | |
13 import copy | |
14 | |
15 try: | |
16 from shapely.geometry import Polygon, Point as shapelyPoint | |
17 from shapely.prepared import prep, PreparedGeometry | |
18 shapelyAvailable = True | |
19 except ImportError: | |
20 print('Shapely library could not be loaded') | |
21 shapelyAvailable = False | |
22 | |
23 | |
24 class Interval(object): | |
25 '''Generic interval: a subset of real numbers (not iterable)''' | |
26 def __init__(self, first=0, last=-1, revert = False): | |
27 if revert and last<first: | |
28 self.first=last | |
29 self.last=first | |
30 else: | |
31 self.first=first | |
32 self.last=last | |
33 | |
34 def __str__(self): | |
35 return '[{0}, {1}]'.format(self.first, self.last) | |
36 | |
37 def __repr__(self): | |
38 return self.__str__() | |
39 | |
40 def __eq__(self, other): | |
41 return ((self.first == other.first) and (self.last == other.last)) or ((self.first == other.last) and (self.last == other.first)) | |
42 | |
43 def empty(self): | |
44 return self.first > self.last | |
45 | |
46 def center(self): | |
47 return (self.first+self.last)/2. | |
48 | |
49 def length(self): | |
50 '''Returns the length of the interval''' | |
51 return float(max(0,self.last-self.first)) | |
52 | |
53 def equal(self, i2): | |
54 return self.first==i2.first and self.last == i2.last | |
55 | |
56 def getList(self): | |
57 return [self.first, self.last] | |
58 | |
59 def contains(self, instant): | |
60 return (self.first<=instant and self.last>=instant) | |
61 | |
62 def inside(self, interval2): | |
63 '''Indicates if the temporal interval of self is comprised in interval2''' | |
64 return (self.first >= interval2.first) and (self.last <= interval2.last) | |
65 | |
66 def shift(self, offset): | |
67 self.first += offset | |
68 self.last += offset | |
69 | |
70 @classmethod | |
71 def union(cls, interval1, interval2): | |
72 '''Smallest interval comprising self and interval2''' | |
73 return cls(min(interval1.first, interval2.first), max(interval2.last, interval2.last)) | |
74 | |
75 @classmethod | |
76 def intersection(cls, interval1, interval2): | |
77 '''Largest interval comprised in both self and interval2''' | |
78 return cls(max(interval1.first, interval2.first), min(interval1.last, interval2.last)) | |
79 | |
80 def distance(self, interval2): | |
81 if not Interval.intersection(self, interval2).empty(): | |
82 return 0 | |
83 elif self.first > interval2.last: | |
84 return self.first - interval2.last | |
85 elif self.last < interval2.first: | |
86 return interval2.first - self.last | |
87 else: | |
88 return None | |
89 | |
90 @classmethod | |
91 def unionIntervals(cls, intervals): | |
92 'returns the smallest interval containing all intervals' | |
93 inter = cls(intervals[0].first, intervals[0].last) | |
94 for i in intervals[1:]: | |
95 inter = cls.union(inter, i) | |
96 return inter | |
97 | |
98 | |
99 class TimeInterval(Interval): | |
100 '''Temporal interval: set of instants at fixed time step, between first and last, included | |
101 | |
102 For example: based on frame numbers (hence the modified length method) | |
103 It may be modified directly by setting first and last | |
104 It also (mostly) works with datetime.datetime''' | |
105 | |
106 def __init__(self, first=0, last=-1, revert = False): | |
107 super(TimeInterval, self).__init__(first, last, revert) | |
108 | |
109 @staticmethod | |
110 def fromInterval(inter): | |
111 return TimeInterval(inter.first, inter.last) | |
112 | |
113 def __getitem__(self, i): | |
114 if not self.empty(): | |
115 if isinstance(i, int): | |
116 return self.first+i | |
117 else: | |
118 raise TypeError("Invalid argument type.") | |
119 #elif isinstance( key, slice ): | |
120 | |
121 def __iter__(self): | |
122 self.iterInstantNum = -1 | |
123 return self | |
124 | |
125 def __next__(self): | |
126 if self.iterInstantNum >= self.length()-1: | |
127 raise StopIteration | |
128 else: | |
129 self.iterInstantNum += 1 | |
130 return self[self.iterInstantNum] | |
131 | |
132 def length(self): | |
133 '''Returns the length of the interval''' | |
134 return float(max(0,self.last-self.first+1)) | |
135 | |
136 def __len__(self): | |
137 return self.length() | |
138 | |
139 # class BoundingPolygon: | |
140 # '''Class for a polygon bounding a set of points | |
141 # with methods to create intersection, unions... | |
142 # ''' | |
143 # We will use the polygon class of Shapely | |
144 | |
145 class STObject(object): | |
146 '''Class for spatio-temporal object, i.e. with temporal and spatial existence | |
147 (time interval and bounding polygon for positions (e.g. rectangle)). | |
148 | |
149 It may not mean that the object is defined | |
150 for all time instants within the time interval''' | |
151 | |
152 def __init__(self, num = None, timeInterval = None, boundingPolygon = None): | |
153 self.num = num | |
154 self.timeInterval = timeInterval | |
155 self.boundingPolygon = boundingPolygon | |
156 | |
157 def empty(self): | |
158 return self.timeInterval.empty()# or not self.boudingPolygon | |
159 | |
160 def getNum(self): | |
161 return self.num | |
162 | |
163 def __len__(self): | |
164 return self.timeInterval.length() | |
165 | |
166 def length(self): | |
167 return self.timeInterval.length() | |
168 | |
169 def getFirstInstant(self): | |
170 return self.timeInterval.first | |
171 | |
172 def getLastInstant(self): | |
173 return self.timeInterval.last | |
174 | |
175 def setFirstInstant(self, t): | |
176 if t <= self.timeInterval.last: | |
177 self.timeInterval.first = t | |
178 else: | |
179 print('new first instant is after last, not changing') | |
180 | |
181 def setLastInstant(self, t): | |
182 if t >= self.timeInterval.first: | |
183 self.timeInterval.last = t | |
184 else: | |
185 print('new last instant is before first, not changing') | |
186 | |
187 def getTimeInterval(self): | |
188 return self.timeInterval | |
189 | |
190 def existsAtInstant(self, t): | |
191 return self.timeInterval.contains(t) | |
192 | |
193 def commonTimeInterval(self, obj2): | |
194 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval()) | |
195 | |
196 def shiftTimeInterval(self, offset): | |
197 self.timeInterval.shift(offset) | |
198 | |
199 class Point(object): | |
200 def __init__(self, x, y): | |
201 self.x = x | |
202 self.y = y | |
203 | |
204 def __str__(self): | |
205 return '({:f},{:f})'.format(self.x,self.y) | |
206 | |
207 def __repr__(self): | |
208 return self.__str__() | |
209 | |
210 def __eq__(self, other): | |
211 return (self.x == other.x) and (self.y == other.y) | |
212 | |
213 def __add__(self, other): | |
214 return Point(self.x+other.x, self.y+other.y) | |
215 | |
216 def __sub__(self, other): | |
217 return Point(self.x-other.x, self.y-other.y) | |
218 | |
219 def __neg__(self): | |
220 return Point(-self.x, -self.y) | |
221 | |
222 def __getitem__(self, i): | |
223 if i == 0: | |
224 return self.x | |
225 elif i == 1: | |
226 return self.y | |
227 else: | |
228 raise IndexError() | |
229 | |
230 def orthogonal(self, clockwise = True): | |
231 'Returns the orthogonal vector' | |
232 if clockwise: | |
233 return Point(self.y, -self.x) | |
234 else: | |
235 return Point(-self.y, self.x) | |
236 | |
237 def projectLocal(self, v, clockwise = True): | |
238 'Projects point projected on v, v.orthogonal()' | |
239 e1 = v/v.norm2() | |
240 e2 = e1.orthogonal(clockwise) | |
241 return Point(Point.dot(self, e1), Point.doc(self, e2)) | |
242 | |
243 def rotate(self, theta): | |
244 return Point(self.x*cos(theta)-self.y*sin(theta), self.x*sin(theta)+self.y*cos(theta)) | |
245 | |
246 def __mul__(self, alpha): | |
247 'Warning, returns a new Point' | |
248 return Point(self.x*alpha, self.y*alpha) | |
249 | |
250 def divide(self, alpha): | |
251 'Warning, returns a new Point' | |
252 return Point(self.x/alpha, self.y/alpha) | |
253 | |
254 def plot(self, options = 'o', **kwargs): | |
255 plot([self.x], [self.y], options, **kwargs) | |
256 | |
257 @staticmethod | |
258 def plotSegment(p1, p2, options = 'o', **kwargs): | |
259 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs) | |
260 | |
261 def angle(self): | |
262 return atan2(self.y, self.x) | |
263 | |
264 def norm2Squared(self): | |
265 '''2-norm distance (Euclidean distance)''' | |
266 return self.x**2+self.y**2 | |
267 | |
268 def norm2(self): | |
269 '''2-norm distance (Euclidean distance)''' | |
270 return sqrt(self.norm2Squared()) | |
271 | |
272 def norm1(self): | |
273 return abs(self.x)+abs(self.y) | |
274 | |
275 def normMax(self): | |
276 return max(abs(self.x),abs(self.y)) | |
277 | |
278 def aslist(self): | |
279 return [self.x, self.y] | |
280 | |
281 def astuple(self): | |
282 return (self.x, self.y) | |
283 | |
284 def asint(self): | |
285 return Point(int(self.x), int(self.y)) | |
286 | |
287 if shapelyAvailable: | |
288 def asShapely(self): | |
289 return shapelyPoint(self.x, self.y) | |
290 | |
291 def homographyProject(self, homography): | |
292 projected = cvutils.homographyProject(array([[self.x], [self.y]]), homography) | |
293 return Point(projected[0], projected[1]) | |
294 | |
295 def inPolygon(self, polygon): | |
296 '''Indicates if the point x, y is inside the polygon | |
297 (array of Nx2 coordinates of the polygon vertices) | |
298 | |
299 taken from http://www.ariel.com.au/a/python-point-int-poly.html | |
300 | |
301 Use Polygon.contains if Shapely is installed''' | |
302 | |
303 n = polygon.shape[0]; | |
304 counter = 0; | |
305 | |
306 p1 = polygon[0,:]; | |
307 for i in range(n+1): | |
308 p2 = polygon[i % n,:]; | |
309 if self.y > min(p1[1],p2[1]): | |
310 if self.y <= max(p1[1],p2[1]): | |
311 if self.x <= max(p1[0],p2[0]): | |
312 if p1[1] != p2[1]: | |
313 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0]; | |
314 if p1[0] == p2[0] or self.x <= xinters: | |
315 counter+=1; | |
316 p1=p2 | |
317 return (counter%2 == 1); | |
318 | |
319 @staticmethod | |
320 def fromList(p): | |
321 return Point(p[0], p[1]) | |
322 | |
323 @staticmethod | |
324 def dot(p1, p2): | |
325 'Scalar product' | |
326 return p1.x*p2.x+p1.y*p2.y | |
327 | |
328 @staticmethod | |
329 def cross(p1, p2): | |
330 'Cross product' | |
331 return p1.x*p2.y-p1.y*p2.x | |
332 | |
333 @staticmethod | |
334 def parallel(p1, p2): | |
335 return Point.cross(p1, p2) == 0. | |
336 | |
337 @staticmethod | |
338 def cosine(p1, p2): | |
339 return Point.dot(p1,p2)/(p1.norm2()*p2.norm2()) | |
340 | |
341 @staticmethod | |
342 def distanceNorm2(p1, p2): | |
343 return (p1-p2).norm2() | |
344 | |
345 @staticmethod | |
346 def plotAll(points, **kwargs): | |
347 from matplotlib.pyplot import scatter | |
348 scatter([p.x for p in points],[p.y for p in points], **kwargs) | |
349 | |
350 def similarOrientation(self, refDirection, cosineThreshold): | |
351 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold' | |
352 return Point.cosine(self, refDirection) >= cosineThreshold | |
353 | |
354 @staticmethod | |
355 def timeToCollision(p1, p2, v1, v2, collisionThreshold): | |
356 '''Computes exact time to collision with a distance threshold | |
357 The unknown of the equation is the time to reach the intersection | |
358 between the relative trajectory of one road user | |
359 and the circle of radius collisionThreshold around the other road user''' | |
360 dv = v1-v2 | |
361 dp = p1-p2 | |
362 a = dv.norm2Squared()#(v1.x-v2.x)**2 + (v1.y-v2.y)**2 | |
363 b = 2*Point.dot(dv, dp)#2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) | |
364 c = dp.norm2Squared() - collisionThreshold**2#(p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 | |
365 | |
366 delta = b**2 - 4*a*c | |
367 if delta >= 0: | |
368 deltaRoot = sqrt(delta) | |
369 ttc1 = (-b + deltaRoot)/(2*a) | |
370 ttc2 = (-b - deltaRoot)/(2*a) | |
371 if ttc1 >= 0 and ttc2 >= 0: | |
372 return min(ttc1,ttc2) | |
373 elif ttc1 >= 0: | |
374 return ttc1 | |
375 elif ttc2 >= 0: | |
376 return ttc2 | |
377 else: # ttc1 < 0 and ttc2 < 0: | |
378 return None | |
379 else: | |
380 return None | |
381 | |
382 @staticmethod | |
383 def midPoint(p1, p2): | |
384 'Returns the middle of the segment [p1, p2]' | |
385 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) | |
386 | |
387 @staticmethod | |
388 def agg(points, aggFunc = mean): | |
389 return Point(aggFunc([p.x for p in points]), aggFunc([p.y for p in points])) | |
390 | |
391 if shapelyAvailable: | |
392 def pointsInPolygon(points, polygon): | |
393 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)''' | |
394 if type(polygon) == PreparedGeometry: | |
395 prepared_polygon = polygon | |
396 else: | |
397 prepared_polygon = prep(polygon) | |
398 return list(filter(prepared_polygon.contains, points)) | |
399 | |
400 # Functions for coordinate transformation | |
401 # From Paul St-Aubin's PVA tools | |
402 def subsec_spline_dist(splines): | |
403 ''' Prepare list of spline subsegments from a spline list. | |
404 | |
405 Output: | |
406 ======= | |
407 ss_spline_d[spline #][mode][station] | |
408 | |
409 where: | |
410 mode=0: incremental distance | |
411 mode=1: cumulative distance | |
412 mode=2: cumulative distance with trailing distance | |
413 ''' | |
414 ss_spline_d = [] | |
415 #Prepare subsegment distances | |
416 for spline in range(len(splines)): | |
417 ss_spline_d[spline]=[]#.append([[],[],[]]) | |
418 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Incremental distance | |
419 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Cumulative distance | |
420 ss_spline_d[spline].append(zeros(len(splines[spline]))) #Cumulative distance with trailing distance | |
421 for spline_p in range(len(splines[spline])): | |
422 if spline_p > (len(splines[spline]) - 2): | |
423 break | |
424 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]) | |
425 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) | |
426 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p]) | |
427 | |
428 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] | |
429 | |
430 return ss_spline_d | |
431 | |
432 def prepareSplines(splines): | |
433 'Approximates slope singularity by giving some slope roundoff; account for roundoff error' | |
434 for spline in splines: | |
435 p1 = spline[0] | |
436 for i in range(len(spline)-1): | |
437 p2 = spline[i+1] | |
438 if(round(p1.x, 10) == round(p2.x, 10)): | |
439 p2.x += 0.0000000001 | |
440 if(round(p1.y, 10) == round(p2.y, 10)): | |
441 p2.y += 0.0000000001 | |
442 p1 = p2 | |
443 | |
444 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): | |
445 ''' Point-projection (Q) on line defined by 2 points (P0,P1). | |
446 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf | |
447 ''' | |
448 if(p0x == p1x and p0y == p1y): | |
449 return None | |
450 try: | |
451 #Approximate slope singularity by giving some slope roundoff; account for roundoff error | |
452 # if(round(p0x, 10) == round(p1x, 10)): | |
453 # p1x += 0.0000000001 | |
454 # if(round(p0y, 10) == round(p1y, 10)): | |
455 # p1y += 0.0000000001 | |
456 #make the calculation | |
457 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)) | |
458 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) | |
459 except ZeroDivisionError: | |
460 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') | |
461 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) | |
462 import pdb; pdb.set_trace() | |
463 return Point(X,Y) | |
464 | |
465 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): | |
466 ''' Snap a point p to its nearest subsegment of it's nearest spline (from the list splines). | |
467 A spline is a list of points (class Point), most likely a trajectory. | |
468 | |
469 Output: | |
470 ======= | |
471 [spline index, | |
472 subsegment leading point index, | |
473 snapped point, | |
474 subsegment distance, | |
475 spline distance, | |
476 orthogonal point offset] | |
477 | |
478 or None | |
479 ''' | |
480 minOffsetY = float('inf') | |
481 #For each spline | |
482 for splineIdx in range(len(splines)): | |
483 #For each spline point index | |
484 for spline_p in range(len(splines[splineIdx])-1): | |
485 #Get closest point on spline | |
486 closestPoint = ppldb2p(p.x,p.y,splines[splineIdx][spline_p][0],splines[splineIdx][spline_p][1],splines[splineIdx][spline_p+1][0],splines[splineIdx][spline_p+1][1]) | |
487 if closestPoint is None: | |
488 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p)) | |
489 return None | |
490 # check if the projected point is in between the current segment of the alignment bounds | |
491 if utils.inBetween(splines[splineIdx][spline_p][0], splines[splineIdx][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[splineIdx][spline_p][1], splines[splineIdx][spline_p+1][1], closestPoint.y): | |
492 offsetY = Point.distanceNorm2(closestPoint, p) | |
493 if offsetY < minOffsetY: | |
494 minOffsetY = offsetY | |
495 snappedSplineIdx = splineIdx | |
496 snappedSplineLeadingPoint = spline_p | |
497 snappedPoint = Point(closestPoint.x, closestPoint.y) | |
498 #Jump loop if significantly close | |
499 if offsetY < goodEnoughSplineDistance: | |
500 break | |
501 | |
502 #Get sub-segment distance | |
503 if minOffsetY != float('inf'): | |
504 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint]) | |
505 #Get cumulative alignment distance (total segment distance) | |
506 splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance | |
507 orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal() | |
508 offsetVector = p-snappedPoint | |
509 if Point.dot(orthogonalSplineVector, offsetVector) < 0: | |
510 minOffsetY = -minOffsetY | |
511 return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] | |
512 else: | |
513 print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p)) | |
514 return None | |
515 | |
516 def getXYfromSY(s, y, splineNum, splines, mode = 0): | |
517 ''' Find X,Y coordinate from S,Y data. | |
518 if mode = 0 : return Snapped X,Y | |
519 if mode !=0 : return Real X,Y | |
520 ''' | |
521 | |
522 #(buckle in, it gets ugly from here on out) | |
523 ss_spline_d = subsec_spline_dist(splines) | |
524 | |
525 #Find subsegment | |
526 snapped_x = None | |
527 snapped_y = None | |
528 for spline_ss_index in range(len(ss_spline_d[splineNum][1])): | |
529 if(s < ss_spline_d[splineNum][1][spline_ss_index]): | |
530 ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1] | |
531 #Get normal vector and then snap | |
532 vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0]) | |
533 vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1]) | |
534 magnitude = sqrt(vector_l_x**2 + vector_l_y**2) | |
535 n_vector_x = vector_l_x/magnitude | |
536 n_vector_y = vector_l_y/magnitude | |
537 snapped_x = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x | |
538 snapped_y = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y | |
539 | |
540 #Real values (including orthogonal projection of y)) | |
541 real_x = snapped_x - y*n_vector_y | |
542 real_y = snapped_y + y*n_vector_x | |
543 break | |
544 | |
545 if mode == 0 or (not snapped_x): | |
546 if(not snapped_x): | |
547 snapped_x = splines[splineNum][-1][0] | |
548 snapped_y = splines[splineNum][-1][1] | |
549 return [snapped_x,snapped_y] | |
550 else: | |
551 return [real_x,real_y] | |
552 | |
553 | |
554 class NormAngle(object): | |
555 '''Alternate encoding of a point, by its norm and orientation''' | |
556 | |
557 def __init__(self, norm, angle): | |
558 self.norm = norm | |
559 self.angle = angle | |
560 | |
561 @staticmethod | |
562 def fromPoint(p): | |
563 norm = p.norm2() | |
564 if norm > 0: | |
565 angle = p.angle() | |
566 else: | |
567 angle = 0. | |
568 return NormAngle(norm, angle) | |
569 | |
570 def __add__(self, other): | |
571 'a norm cannot become negative' | |
572 return NormAngle(max(self.norm+other.norm, 0), self.angle+other.angle) | |
573 | |
574 def getPoint(self): | |
575 return Point(self.norm*cos(self.angle), self.norm*sin(self.angle)) | |
576 | |
577 | |
578 def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): | |
579 '''Predicts the position in nTimeSteps at constant speed/acceleration''' | |
580 return initialVelocity + initialAcceleration.__mul__(nTimeSteps),initialPosition+initialVelocity.__mul__(nTimeSteps) + initialAcceleration.__mul__(nTimeSteps**2*0.5) | |
581 | |
582 def predictPosition(position, speedOrientation, control, maxSpeed = None): | |
583 '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta) | |
584 speedOrientation is the other encoding of velocity, (speed, orientation) | |
585 speedOrientation and control are NormAngle''' | |
586 predictedSpeedTheta = speedOrientation+control | |
587 if maxSpeed is not None: | |
588 predictedSpeedTheta.norm = min(predictedSpeedTheta.norm, maxSpeed) | |
589 predictedPosition = position+predictedSpeedTheta.getPoint() | |
590 return predictedPosition, predictedSpeedTheta | |
591 | |
592 | |
593 class FlowVector(object): | |
594 '''Class to represent 4-D flow vectors, | |
595 ie a position and a velocity''' | |
596 def __init__(self, position, velocity): | |
597 'position and velocity should be Point instances' | |
598 self.position = position | |
599 self.velocity = velocity | |
600 | |
601 def __add__(self, other): | |
602 return FlowVector(self.position+other.position, self.velocity+other.velocity) | |
603 | |
604 def __mul__(self, alpha): | |
605 return FlowVector(self.position.__mul__(alpha), self.velocity.__mul__(alpha)) | |
606 | |
607 def plot(self, options = '', **kwargs): | |
608 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs) | |
609 self.position.plot(options+'x', **kwargs) | |
610 | |
611 @staticmethod | |
612 def similar(f1, f2, maxDistance2, maxDeltavelocity2): | |
613 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 | |
614 | |
615 def intersection(p1, p2, p3, p4): | |
616 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 | |
617 http://paulbourke.net/geometry/pointlineplane/''' | |
618 dp12 = p2-p1 | |
619 dp34 = p4-p3 | |
620 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y) | |
621 det = float(dp34.y*dp12.x-dp34.x*dp12.y) | |
622 if det == 0.: | |
623 return None | |
624 else: | |
625 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det | |
626 return p1+dp12.__mul__(ua) | |
627 | |
628 # def intersection(p1, p2, dp1, dp2): | |
629 # '''Returns the intersection point between the two lines | |
630 # defined by the respective vectors (dp) and origin points (p)''' | |
631 # from numpy import matrix | |
632 # from numpy.linalg import linalg | |
633 # A = matrix([[dp1.y, -dp1.x], | |
634 # [dp2.y, -dp2.x]]) | |
635 # B = matrix([[dp1.y*p1.x-dp1.x*p1.y], | |
636 # [dp2.y*p2.x-dp2.x*p2.y]]) | |
637 | |
638 # if linalg.det(A) == 0: | |
639 # return None | |
640 # else: | |
641 # intersection = linalg.solve(A,B) | |
642 # return Point(intersection[0,0], intersection[1,0]) | |
643 | |
644 def segmentIntersection(p1, p2, p3, p4): | |
645 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' | |
646 | |
647 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()): | |
648 return None | |
649 else: | |
650 inter = intersection(p1, p2, p3, p4) | |
651 if (inter is not None | |
652 and utils.inBetween(p1.x, p2.x, inter.x) | |
653 and utils.inBetween(p3.x, p4.x, inter.x) | |
654 and utils.inBetween(p1.y, p2.y, inter.y) | |
655 and utils.inBetween(p3.y, p4.y, inter.y)): | |
656 return inter | |
657 else: | |
658 return None | |
659 | |
660 def segmentLineIntersection(p1, p2, p3, p4): | |
661 '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' | |
662 inter = intersection(p1, p2, p3, p4) | |
663 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y): | |
664 return inter | |
665 else: | |
666 return None | |
667 | |
668 | |
669 class Trajectory(object): | |
670 '''Class for trajectories: temporal sequence of positions | |
671 | |
672 The class is iterable''' | |
673 | |
674 def __init__(self, positions=None): | |
675 if positions is not None: | |
676 self.positions = positions | |
677 else: | |
678 self.positions = [[],[]] | |
679 | |
680 @staticmethod | |
681 def generate(p, v, nPoints): | |
682 t = Trajectory() | |
683 p0 = Point(p.x, p.y) | |
684 t.addPosition(p0) | |
685 for i in range(nPoints-1): | |
686 p0 += v | |
687 t.addPosition(p0) | |
688 return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints]) | |
689 | |
690 @staticmethod | |
691 def load(line1, line2): | |
692 return Trajectory([[float(n) for n in line1.split(' ')], | |
693 [float(n) for n in line2.split(' ')]]) | |
694 | |
695 @staticmethod | |
696 def fromPointList(points): | |
697 t = Trajectory() | |
698 if isinstance(points[0], list) or isinstance(points[0], tuple): | |
699 for p in points: | |
700 t.addPositionXY(p[0],p[1]) | |
701 else: | |
702 for p in points: | |
703 t.addPosition(p) | |
704 return t | |
705 | |
706 def __len__(self): | |
707 return len(self.positions[0]) | |
708 | |
709 def length(self): | |
710 return self.__len__() | |
711 | |
712 def empty(self): | |
713 return self.__len__() == 0 | |
714 | |
715 def __getitem__(self, i): | |
716 if isinstance(i, int): | |
717 return Point(self.positions[0][i], self.positions[1][i]) | |
718 elif isinstance(i, slice): | |
719 return Trajectory([self.positions[0][i],self.positions[1][i]]) | |
720 else: | |
721 raise TypeError("Invalid argument type.") | |
722 | |
723 def __str__(self): | |
724 return ' '.join([self.__getitem__(i).__str__() for i in range(self.length())]) | |
725 | |
726 def __repr__(self): | |
727 return self.__str__() | |
728 | |
729 def __iter__(self): | |
730 self.iterInstantNum = 0 | |
731 return self | |
732 | |
733 def __next__(self): | |
734 if self.iterInstantNum >= self.length(): | |
735 raise StopIteration | |
736 else: | |
737 self.iterInstantNum += 1 | |
738 return self[self.iterInstantNum-1] | |
739 | |
740 def __eq__(self, other): | |
741 if self.length() == other.length(): | |
742 result = True | |
743 for p, po in zip(self, other): | |
744 result = result and (p == po) | |
745 return result | |
746 else: | |
747 return False | |
748 | |
749 def setPositionXY(self, i, x, y): | |
750 if i < self.__len__(): | |
751 self.positions[0][i] = x | |
752 self.positions[1][i] = y | |
753 | |
754 def setPosition(self, i, p): | |
755 self.setPositionXY(i, p.x, p.y) | |
756 | |
757 def addPositionXY(self, x, y): | |
758 self.positions[0].append(x) | |
759 self.positions[1].append(y) | |
760 | |
761 def addPosition(self, p): | |
762 self.addPositionXY(p.x, p.y) | |
763 | |
764 def duplicateLastPosition(self): | |
765 self.positions[0].append(self.positions[0][-1]) | |
766 self.positions[1].append(self.positions[1][-1]) | |
767 | |
768 @staticmethod | |
769 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, objNum = None, **kwargs): | |
770 if lastCoordinate is None: | |
771 plot(positions[0][::timeStep], positions[1][::timeStep], options, **kwargs) | |
772 elif 0 <= lastCoordinate <= len(positions[0]): | |
773 plot(positions[0][:lastCoordinate:timeStep], positions[1][:lastCoordinate:timeStep], options, **kwargs) | |
774 if withOrigin: | |
775 plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs) | |
776 if objNum is not None: | |
777 text(positions[0][0], positions[1][0], '{}'.format(objNum)) | |
778 | |
779 def homographyProject(self, homography): | |
780 return Trajectory(cvutils.homographyProject(array(self.positions), homography).tolist()) | |
781 | |
782 def newCameraProject(self, newCameraMatrix): | |
783 return Trajectory(cvutils.newCameraProject(array(self.positions), newCameraMatrix).tolist()) | |
784 | |
785 def plot(self, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs): | |
786 Trajectory._plot(self.positions, options, withOrigin, None, timeStep, objNum, **kwargs) | |
787 | |
788 def plotAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs): | |
789 Trajectory._plot(self.positions, options, withOrigin, lastCoordinate, timeStep, objNum, **kwargs) | |
790 | |
791 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs): | |
792 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]], | |
793 [x*nPixelsPerUnitDistance for x in self.positions[1]]] | |
794 Trajectory._plot(imgPositions, options, withOrigin, None, timeStep, objNum, **kwargs) | |
795 | |
796 def getXCoordinates(self): | |
797 return self.positions[0] | |
798 | |
799 def getYCoordinates(self): | |
800 return self.positions[1] | |
801 | |
802 def asArray(self): | |
803 return array(self.positions) | |
804 | |
805 def xBounds(self): | |
806 # look for function that does min and max in one pass | |
807 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates())) | |
808 | |
809 def yBounds(self): | |
810 # look for function that does min and max in one pass | |
811 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates())) | |
812 | |
813 def add(self, traj2): | |
814 '''Returns a new trajectory of the same length''' | |
815 if self.length() != traj2.length(): | |
816 print('Trajectories of different lengths') | |
817 return None | |
818 else: | |
819 return Trajectory([[a+b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], | |
820 [a+b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) | |
821 | |
822 def subtract(self, traj2): | |
823 '''Returns a new trajectory of the same length''' | |
824 if self.length() != traj2.length(): | |
825 print('Trajectories of different lengths') | |
826 return None | |
827 else: | |
828 return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], | |
829 [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) | |
830 | |
831 def __mul__(self, alpha): | |
832 '''Returns a new trajectory of the same length''' | |
833 return Trajectory([[alpha*x for x in self.getXCoordinates()], | |
834 [alpha*y for y in self.getYCoordinates()]]) | |
835 | |
836 def differentiate(self, doubleLastPosition = False): | |
837 diff = Trajectory() | |
838 for i in range(1, self.length()): | |
839 diff.addPosition(self[i]-self[i-1]) | |
840 if doubleLastPosition: | |
841 diff.addPosition(diff[-1]) | |
842 return diff | |
843 | |
844 def differentiateSG(self, window_length, polyorder, deriv=0, delta=1.0, axis=-1, mode='interp', cval=0.0, nInstantsIgnoredAtEnds = 2): | |
845 '''Differentiates the trajectory using the Savitsky Golay filter | |
846 | |
847 window_length : The length of the filter window (i.e. the number of coefficients). window_length must be a positive odd integer. | |
848 polyorder : The order of the polynomial used to fit the samples. polyorder must be less than window_length. | |
849 deriv : The order of the derivative to compute. This must be a nonnegative integer. The default is 0, which means to filter the data without differentiating. | |
850 delta : The spacing of the samples to which the filter will be applied. This is only used if deriv > 0. Default is 1.0. | |
851 axis : The axis of the array x along which the filter is to be applied. Default is -1. | |
852 mode : Must be mirror, constant, nearest, wrap or interp. This determines the type of extension to use for the padded signal to which the filter is applied. When mode is constant, the padding value is given by cval. See the Notes for more details on mirror, constant, wrap, and nearest. When the interp mode is selected (the default), no extension is used. Instead, a degree polyorder polynomial is fit to the last window_length values of the edges, and this polynomial is used to evaluate the last window_length // 2 output values. | |
853 cval : Value to fill past the edges of the input if mode is constant. Default is 0.0. | |
854 | |
855 https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.savgol_filter.html#scipy.signal.savgol_filter''' | |
856 if removeBothEnds >=1: | |
857 pos = [self.positions[0][nInstantsIgnoredAtEnds:-nInstantsIgnoredAtEnds], | |
858 self.positions[1][nInstantsIgnoredAtEnds:-nInstantsIgnoredAtEnds]] | |
859 else: | |
860 pos = self.positions | |
861 filtered = savgol_filter(pos, window_length, polyorder, deriv, delta, axis, mode, cval) | |
862 return Trajectory(filtered) | |
863 | |
864 def norm(self): | |
865 '''Returns the list of the norms at each instant''' | |
866 return hypot(self.positions[0], self.positions[1]) | |
867 | |
868 def computeCumulativeDistances(self): | |
869 '''Computes the distance from each point to the next and the cumulative distance up to the point | |
870 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)''' | |
871 self.distances = [] | |
872 self.cumulativeDistances = [0.] | |
873 p1 = self[0] | |
874 cumulativeDistance = 0. | |
875 for i in range(self.length()-1): | |
876 p2 = self[i+1] | |
877 self.distances.append(Point.distanceNorm2(p1,p2)) | |
878 cumulativeDistance += self.distances[-1] | |
879 self.cumulativeDistances.append(cumulativeDistance) | |
880 p1 = p2 | |
881 | |
882 def getDistance(self,i): | |
883 '''Return the distance between points i and i+1''' | |
884 if i < self.length()-1: | |
885 return self.distances[i] | |
886 else: | |
887 print('Index {} beyond trajectory length {}-1'.format(i, self.length())) | |
888 | |
889 def getCumulativeDistance(self, i): | |
890 '''Return the cumulative distance between the beginning and point i''' | |
891 if i < self.length(): | |
892 return self.cumulativeDistances[i] | |
893 else: | |
894 print('Index {} beyond trajectory length {}'.format(i, self.length())) | |
895 | |
896 def getMaxDistance(self, metric): | |
897 'Returns the maximum distance between points in the trajectory' | |
898 positions = self.getPositions().asArray().T | |
899 return cdist(positions, positions, metric = metric).max() | |
900 | |
901 def getClosestPoint(self, p1, maxDist2 = None): | |
902 '''Returns the instant of the closest position in trajectory to p1 (and the point) | |
903 if maxDist is not None, will check the distance is smaller | |
904 TODO: could use cdist for different metrics''' | |
905 distances2 = [] | |
906 minDist2 = float('inf') | |
907 i = -1 | |
908 for p2 in self: | |
909 distances2.append(Point.distanceNorm2(p1, p2)) | |
910 if distances2[-1] < minDist2: | |
911 minDist2 = distances2[-1] | |
912 i = len(distances2)-1 | |
913 if maxDist2 is not None and minDist2 < maxDist2: | |
914 return None | |
915 else: | |
916 return i | |
917 | |
918 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): | |
919 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) | |
920 have a cosine with refDirection is smaller than cosineThreshold''' | |
921 count = 0 | |
922 lengthThreshold = float(self.length())*minProportion | |
923 for p in self: | |
924 if p.similarOrientation(refDirection, cosineThreshold): | |
925 count += 1 | |
926 return count >= lengthThreshold | |
927 | |
928 def wiggliness(self): | |
929 straightDistance = Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)) | |
930 if straightDistance > 0: | |
931 return self.getCumulativeDistance(self.length()-1)/float(straightDistance) | |
932 else: | |
933 return None | |
934 | |
935 def getIntersections(self, p1, p2): | |
936 '''Returns a list of the indices at which the trajectory | |
937 intersects with the segment of extremities p1 and p2 | |
938 Returns an empty list if there is no crossing''' | |
939 indices = [] | |
940 intersections = [] | |
941 | |
942 for i in range(self.length()-1): | |
943 q1=self.__getitem__(i) | |
944 q2=self.__getitem__(i+1) | |
945 p = segmentIntersection(q1, q2, p1, p2) | |
946 if p is not None: | |
947 if q1.x != q2.x: | |
948 ratio = (p.x-q1.x)/(q2.x-q1.x) | |
949 elif q1.y != q2.y: | |
950 ratio = (p.y-q1.y)/(q2.y-q1.y) | |
951 else: | |
952 ratio = 0 | |
953 indices.append(i+ratio) | |
954 intersections.append(p) | |
955 return indices, intersections | |
956 | |
957 def getLineIntersections(self, p1, p2): | |
958 '''Returns a list of the indices at which the trajectory | |
959 intersects with the line going through p1 and p2 | |
960 Returns an empty list if there is no crossing''' | |
961 indices = [] | |
962 intersections = [] | |
963 | |
964 for i in range(self.length()-1): | |
965 q1=self.__getitem__(i) | |
966 q2=self.__getitem__(i+1) | |
967 p = segmentLineIntersection(p1, p2, q1, q2) | |
968 if p is not None: | |
969 if q1.x != q2.x: | |
970 ratio = (p.x-q1.x)/(q2.x-q1.x) | |
971 elif q1.y != q2.y: | |
972 ratio = (p.y-q1.y)/(q2.y-q1.y) | |
973 else: | |
974 ratio = 0 | |
975 indices.append(i+ratio) | |
976 intersections.append(p) | |
977 return indices, intersections | |
978 | |
979 def getTrajectoryInInterval(self, inter): | |
980 'Returns all position between index inter.first and index.last (included)' | |
981 if inter.first >=0 and inter.last<= self.length(): | |
982 return Trajectory([self.positions[0][inter.first:inter.last+1], | |
983 self.positions[1][inter.first:inter.last+1]]) | |
984 else: | |
985 return None | |
986 | |
987 def subSample(self, step): | |
988 'Returns the positions very step' | |
989 return Trajectory([self.positions[0][::step], | |
990 self.positions[1][::step]]) | |
991 | |
992 if shapelyAvailable: | |
993 def getInstantsInPolygon(self, polygon): | |
994 '''Returns the list of instants at which the trajectory is in the polygon''' | |
995 instants = [] | |
996 n = self.length() | |
997 for t, x, y in zip(range(n), self.positions[0], self.positions[1]): | |
998 if polygon.contains(shapelyPoint(x, y)): | |
999 instants.append(t) | |
1000 return instants | |
1001 | |
1002 def getTrajectoryInPolygon(self, polygon, t2 = None): | |
1003 '''Returns the trajectory built with the set of points inside the (shapely) polygon | |
1004 The polygon could be a prepared polygon (faster) from prepared.prep | |
1005 | |
1006 t2 is another trajectory (could be velocities) | |
1007 which is filtered based on the first (self) trajectory''' | |
1008 traj = Trajectory() | |
1009 inPolygon = [] | |
1010 for x, y in zip(self.positions[0], self.positions[1]): | |
1011 inPolygon.append(polygon.contains(shapelyPoint(x, y))) | |
1012 if inPolygon[-1]: | |
1013 traj.addPositionXY(x, y) | |
1014 traj2 = Trajectory() | |
1015 if t2 is not None: | |
1016 for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]): | |
1017 if inp: | |
1018 traj2.addPositionXY(x, y) | |
1019 return traj, traj2 | |
1020 | |
1021 def proportionInPolygon(self, polygon, minProportion = 0.5): | |
1022 instants = self.getInstantsInPolygon(polygon) | |
1023 lengthThreshold = float(self.length())*minProportion | |
1024 return len(instants) >= lengthThreshold | |
1025 else: | |
1026 def getTrajectoryInPolygon(self, polygon, t2 = None): | |
1027 '''Returns the trajectory built with the set of points inside the polygon | |
1028 (array of Nx2 coordinates of the polygon vertices)''' | |
1029 traj = Trajectory() | |
1030 inPolygon = [] | |
1031 for p in self: | |
1032 inPolygon.append(p.inPolygon(polygon)) | |
1033 if inPolygon[-1]: | |
1034 traj.addPosition(p) | |
1035 traj2 = Trajectory() | |
1036 if t2 is not None: | |
1037 for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]): | |
1038 if inp: | |
1039 traj2.addPositionXY(p.x, p.y) | |
1040 return traj, traj2 | |
1041 | |
1042 def proportionInPolygon(self, polygon, minProportion = 0.5): | |
1043 inPolygon = [p.inPolygon(polygon) for p in self] | |
1044 lengthThreshold = float(self.length())*minProportion | |
1045 return sum(inPolygon) >= lengthThreshold | |
1046 | |
1047 @staticmethod | |
1048 def lcss(t1, t2, lcss): | |
1049 return lcss.compute(t1, t2) | |
1050 | |
1051 class CurvilinearTrajectory(Trajectory): | |
1052 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements | |
1053 longitudinal coordinate is stored as first coordinate (exterior name S) | |
1054 lateral coordiante is stored as second coordinate''' | |
1055 | |
1056 def __init__(self, S = None, Y = None, lanes = None): | |
1057 if S is None or Y is None or len(S) != len(Y): | |
1058 self.positions = [[],[]] | |
1059 if S is not None and Y is not None and len(S) != len(Y): | |
1060 print("S and Y coordinates of different lengths\nInitializing to empty lists") | |
1061 else: | |
1062 self.positions = [S,Y] | |
1063 if lanes is None or len(lanes) != self.length(): | |
1064 self.lanes = [] | |
1065 else: | |
1066 self.lanes = lanes | |
1067 | |
1068 def __getitem__(self,i): | |
1069 if isinstance(i, int): | |
1070 return [self.positions[0][i], self.positions[1][i], self.lanes[i]] | |
1071 else: | |
1072 raise TypeError("Invalid argument type.") | |
1073 #elif isinstance( key, slice ): | |
1074 | |
1075 def getSCoordinates(self): | |
1076 return self.getXCoordinates() | |
1077 | |
1078 def getLanes(self): | |
1079 return self.lanes | |
1080 | |
1081 def addPositionSYL(self, s, y, lane): | |
1082 self.addPositionXY(s,y) | |
1083 self.lanes.append(lane) | |
1084 | |
1085 def addPosition(self, p): | |
1086 'Adds position in the point format for curvilinear of list with 3 values' | |
1087 self.addPositionSYL(p[0], p[1], p[2]) | |
1088 | |
1089 def setPosition(self, i, s, y, lane): | |
1090 self.setPositionXY(i, s, y) | |
1091 if i < self.__len__(): | |
1092 self.lanes[i] = lane | |
1093 | |
1094 def differentiate(self, doubleLastPosition = False): | |
1095 diff = CurvilinearTrajectory() | |
1096 p1 = self[0] | |
1097 for i in range(1, self.length()): | |
1098 p2 = self[i] | |
1099 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2]) | |
1100 p1=p2 | |
1101 if doubleLastPosition and self.length() > 1: | |
1102 diff.addPosition(diff[-1]) | |
1103 return diff | |
1104 | |
1105 def getIntersections(self, S1, lane = None): | |
1106 '''Returns a list of the indices at which the trajectory | |
1107 goes past the curvilinear coordinate S1 | |
1108 (in provided lane if lane is not None) | |
1109 Returns an empty list if there is no crossing''' | |
1110 indices = [] | |
1111 for i in range(self.length()-1): | |
1112 q1=self.__getitem__(i) | |
1113 q2=self.__getitem__(i+1) | |
1114 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)): | |
1115 indices.append(i+(S1-q1[0])/(q2[0]-q1[0])) | |
1116 return indices | |
1117 | |
1118 ################## | |
1119 # Moving Objects | |
1120 ################## | |
1121 | |
1122 userTypeNames = ['unknown', | |
1123 'car', | |
1124 'pedestrian', | |
1125 'motorcycle', | |
1126 'bicycle', | |
1127 'bus', | |
1128 'truck'] | |
1129 | |
1130 userType2Num = utils.inverseEnumeration(userTypeNames) | |
1131 | |
1132 class CarClassifier: | |
1133 def predict(self, hog): | |
1134 return userType2Num['car'] | |
1135 carClassifier = CarClassifier() | |
1136 | |
1137 class MovingObject(STObject, VideoFilenameAddable): | |
1138 '''Class for moving objects: a spatio-temporal object | |
1139 with a trajectory and a geometry (constant volume over time) | |
1140 and a usertype (e.g. road user) coded as a number (see userTypeNames) | |
1141 ''' | |
1142 | |
1143 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown']): | |
1144 super(MovingObject, self).__init__(num, timeInterval) | |
1145 self.positions = positions | |
1146 self.velocities = velocities | |
1147 self.geometry = geometry | |
1148 self.userType = userType | |
1149 self.features = None | |
1150 # compute bounding polygon from trajectory | |
1151 | |
1152 @staticmethod | |
1153 def aggregateTrajectories(features, interval = None, aggFunc = mean): | |
1154 'Computes the aggregate trajectory from list of MovingObject features' | |
1155 positions = Trajectory() | |
1156 velocities = Trajectory() | |
1157 if interval is None: | |
1158 inter = TimeInterval.unionIntervals([f.getTimeInterval() for f in features]) | |
1159 else: | |
1160 inter = interval | |
1161 for t in inter: | |
1162 points = [] | |
1163 vels = [] | |
1164 for f in features: | |
1165 if f.existsAtInstant(t): | |
1166 points.append(f.getPositionAtInstant(t)) | |
1167 vels.append(f.getVelocityAtInstant(t)) | |
1168 positions.addPosition(Point.agg(points, aggFunc)) | |
1169 velocities.addPosition(Point.agg(vels, aggFunc)) | |
1170 return inter, positions, velocities | |
1171 | |
1172 @staticmethod | |
1173 def generate(num, p, v, timeInterval): | |
1174 positions, velocities = Trajectory.generate(p, v, int(timeInterval.length())) | |
1175 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = velocities) | |
1176 | |
1177 def updatePositions(self): | |
1178 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) | |
1179 | |
1180 @staticmethod | |
1181 def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False): | |
1182 '''Concatenates two objects, whether overlapping temporally or not | |
1183 | |
1184 Positions will be recomputed only if computePositions is True | |
1185 Otherwise, only featureNumbers and/or features will be merged''' | |
1186 if num is None: | |
1187 newNum = obj1.getNum() | |
1188 else: | |
1189 newNum = num | |
1190 commonTimeInterval = obj1.commonTimeInterval(obj2) | |
1191 if commonTimeInterval.empty(): | |
1192 #print('The two objects\' time intervals do not overlap: obj1 {} and obj2 {}'.format(obj1.getTimeInterval(), obj2.getTimeInterval())) | |
1193 emptyInterval = TimeInterval(min(obj1.getLastInstant(),obj2.getLastInstant()), max(obj1.getFirstInstant(),obj2.getFirstInstant())) | |
1194 if obj1.existsAtInstant(emptyInterval.last): | |
1195 firstObject = obj2 | |
1196 secondObject = obj1 | |
1197 else: | |
1198 firstObject = obj1 | |
1199 secondObject = obj2 | |
1200 v = (secondObject.getPositionAtInstant(emptyInterval.last)-firstObject.getPositionAtInstant(emptyInterval.first)).divide(emptyInterval.length()-1) | |
1201 positions = copy.deepcopy(firstObject.getPositions()) | |
1202 velocities = copy.deepcopy(firstObject.getPositions()) | |
1203 featurePositions = Trajectory() | |
1204 featureVelocities = Trajectory() | |
1205 p = firstObject.getPositionAtInstant(emptyInterval.first)+v | |
1206 for t in range(emptyInterval.first+1, emptyInterval.last): | |
1207 positions.addPosition(p) | |
1208 velocities.addPosition(v) | |
1209 featurePositions.addPosition(p) | |
1210 featureVelocities.addPosition(v) | |
1211 p=p+v | |
1212 for t in secondObject.getTimeInterval(): | |
1213 positions.addPosition(secondObject.getPositionAtInstant(t)) | |
1214 velocities.addPosition(secondObject.getVelocityAtInstant(t)) | |
1215 newObject = MovingObject(newNum, TimeInterval(firstObject.getFirstInstant(), secondObject.getLastInstant()), positions, velocities) | |
1216 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'): | |
1217 if newFeatureNum is not None: | |
1218 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers+[newFeatureNum] | |
1219 else: | |
1220 print('Issue, new created feature has no num id') | |
1221 if obj1.hasFeatures() and obj2.hasFeatures(): | |
1222 newObject.features = obj1.getFeatures()+obj2.getFeatures()+[MovingObject(newFeatureNum, TimeInterval(emptyInterval.first+1, emptyInterval.last-1), featurePositions, featureVelocities)] | |
1223 else: # time intervals overlap | |
1224 newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval()) | |
1225 newObject = MovingObject(newNum, newTimeInterval) | |
1226 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'): | |
1227 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers | |
1228 if obj1.hasFeatures() and obj2.hasFeatures(): | |
1229 newObject.features = obj1.getFeatures()+obj2.getFeatures() | |
1230 newObject.updatePositions() | |
1231 else: | |
1232 print('Cannot update object positions without features') | |
1233 # user type | |
1234 if obj1.getUserType() != obj2.getUserType(): | |
1235 print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()])) | |
1236 newObject.setUserType(obj1.getUserType()) | |
1237 return newObject | |
1238 | |
1239 def getObjectInTimeInterval(self, inter): | |
1240 '''Returns a new object extracted from self, | |
1241 restricted to time interval inter''' | |
1242 intersection = TimeInterval.intersection(inter, self.getTimeInterval()) | |
1243 if not intersection.empty(): | |
1244 trajectoryInterval = TimeInterval(intersection.first-self.getFirstInstant(), intersection.last-self.getFirstInstant()) | |
1245 obj = MovingObject(self.num, intersection, self.positions.getTrajectoryInInterval(trajectoryInterval), self.geometry, self.userType) | |
1246 if self.velocities is not None: | |
1247 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval) | |
1248 return obj | |
1249 else: | |
1250 print('The object does not exist at {}'.format(inter)) | |
1251 return None | |
1252 | |
1253 def getObjectsInMask(self, mask, homography = None, minLength = 1): | |
1254 '''Returns new objects made of the positions in the mask | |
1255 mask is in the destination of the homography space''' | |
1256 if homography is not None: | |
1257 self.projectedPositions = self.positions.homographyProject(homography) | |
1258 else: | |
1259 self.projectedPositions = self.positions | |
1260 def inMask(positions, i, mask): | |
1261 p = positions[i] | |
1262 return mask[int(p.y), int(p.x)] != 0. | |
1263 | |
1264 #subTimeIntervals self.getFirstInstant()+i | |
1265 filteredIndices = [inMask(self.projectedPositions, i, mask) for i in range(int(self.length()))] | |
1266 # 'connected components' in subTimeIntervals | |
1267 l = 0 | |
1268 intervalLabels = [] | |
1269 prev = True | |
1270 for i in filteredIndices: | |
1271 if i: | |
1272 if not prev: # new interval | |
1273 l += 1 | |
1274 intervalLabels.append(l) | |
1275 else: | |
1276 intervalLabels.append(-1) | |
1277 prev = i | |
1278 intervalLabels = array(intervalLabels) | |
1279 subObjects = [] | |
1280 for l in set(intervalLabels): | |
1281 if l >= 0: | |
1282 if sum(intervalLabels == l) >= minLength: | |
1283 times = [self.getFirstInstant()+i for i in range(len(intervalLabels)) if intervalLabels[i] == l] | |
1284 subTimeInterval = TimeInterval(min(times), max(times)) | |
1285 subObjects.append(self.getObjectInTimeInterval(subTimeInterval)) | |
1286 | |
1287 return subObjects | |
1288 | |
1289 def getPositions(self): | |
1290 return self.positions | |
1291 | |
1292 def getVelocities(self): | |
1293 return self.velocities | |
1294 | |
1295 def getUserType(self): | |
1296 return self.userType | |
1297 | |
1298 def computeCumulativeDistances(self): | |
1299 self.positions.computeCumulativeDistances() | |
1300 | |
1301 def getCurvilinearPositions(self): | |
1302 if hasattr(self, 'curvilinearPositions'): | |
1303 return self.curvilinearPositions | |
1304 else: | |
1305 return None | |
1306 | |
1307 def plotCurvilinearPositions(self, lane = None, options = '', withOrigin = False, **kwargs): | |
1308 if hasattr(self, 'curvilinearPositions'): | |
1309 if lane is None: | |
1310 plot(list(self.getTimeInterval()), self.curvilinearPositions.positions[0], options, **kwargs) | |
1311 if withOrigin: | |
1312 plot([self.getFirstInstant()], [self.curvilinearPositions.positions[0][0]], 'ro', **kwargs) | |
1313 else: | |
1314 instants = [] | |
1315 coords = [] | |
1316 for t, p in zip(self.getTimeInterval(), self.curvilinearPositions): | |
1317 if p[2] == lane: | |
1318 instants.append(t) | |
1319 coords.append(p[0]) | |
1320 else: | |
1321 instants.append(NaN) | |
1322 coords.append(NaN) | |
1323 plot(instants, coords, options, **kwargs) | |
1324 if withOrigin and len(instants)>0: | |
1325 plot([instants[0]], [coords[0]], 'ro', **kwargs) | |
1326 else: | |
1327 print('Object {} has no curvilinear positions'.format(self.getNum())) | |
1328 | |
1329 def setUserType(self, userType): | |
1330 self.userType = userType | |
1331 | |
1332 def setFeatures(self, features, featuresOrdered = False): | |
1333 '''Sets the features in the features field based on featureNumbers | |
1334 if not all features are loaded from 0, one needs to renumber in a dict''' | |
1335 if featuresOrdered: | |
1336 tmp = features | |
1337 else: | |
1338 tmp = {f.getNum():f for f in features} | |
1339 self.features = [tmp[i] for i in self.featureNumbers] | |
1340 | |
1341 def getFeatures(self): | |
1342 return self.features | |
1343 | |
1344 def hasFeatures(self): | |
1345 return (self.features is not None) | |
1346 | |
1347 def getFeature(self, i): | |
1348 if self.hasFeatures() and i<len(self.features): | |
1349 return self.features[i] | |
1350 else: | |
1351 return None | |
1352 | |
1353 def getNLongestFeatures(self, nFeatures = 1): | |
1354 if self.features is None: | |
1355 return [] | |
1356 else: | |
1357 tmp = utils.sortByLength(self.getFeatures(), reverse = True) | |
1358 return tmp[:min(len(tmp), nFeatures)] | |
1359 | |
1360 def getFeatureNumbers(self): | |
1361 '''Returns the number of features at each instant | |
1362 dict instant -> number of features''' | |
1363 if self.hasFeatures(): | |
1364 featureNumbers = {} | |
1365 for t in self.getTimeInterval(): | |
1366 n = 0 | |
1367 for f in self.getFeatures(): | |
1368 if f.existsAtInstant(t): | |
1369 n += 1 | |
1370 featureNumbers[t]=n | |
1371 return featureNumbers | |
1372 else: | |
1373 print('Object {} has no features loaded.'.format(self.getNum())) | |
1374 return None | |
1375 | |
1376 def getSpeeds(self, nInstantsIgnoredAtEnds = 0): | |
1377 speeds = self.getVelocities().norm() | |
1378 if nInstantsIgnoredAtEnds > 0: | |
1379 n = min(nInstantsIgnoredAtEnds, int(floor(self.length()/2.))) | |
1380 return speeds[n:-n] | |
1381 else: | |
1382 return speeds | |
1383 | |
1384 def getAccelerations(self, window_length, polyorder, delta=1.0, axis=-1, mode='interp', cval=0.0, speeds = None, nInstantsIgnoredAtEnds = 0): | |
1385 '''Returns the 1-D acceleration from the 1-D speeds | |
1386 Caution about previously filtered data''' | |
1387 if speeds is None: | |
1388 speeds = self.getSpeeds(nInstantsIgnoredAtEnds) | |
1389 return savgol_filter(speeds, window_length, polyorder, 1, delta, axis, mode, cval) | |
1390 | |
1391 def getSpeedIndicator(self): | |
1392 from indicators import SeverityIndicator | |
1393 return SeverityIndicator('Speed', {t:self.getVelocityAtInstant(t).norm2() for t in self.getTimeInterval()}) | |
1394 | |
1395 def getPositionAt(self, i): | |
1396 return self.positions[i] | |
1397 | |
1398 def getVelocityAt(self, i): | |
1399 return self.velocities[i] | |
1400 | |
1401 def getPositionAtInstant(self, i): | |
1402 return self.positions[i-self.getFirstInstant()] | |
1403 | |
1404 def getVelocityAtInstant(self, i): | |
1405 return self.velocities[i-self.getFirstInstant()] | |
1406 | |
1407 def getXCoordinates(self): | |
1408 return self.positions.getXCoordinates() | |
1409 | |
1410 def getYCoordinates(self): | |
1411 return self.positions.getYCoordinates() | |
1412 | |
1413 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, withIds = False, **kwargs): | |
1414 if withIds: | |
1415 objNum = self.getNum() | |
1416 else: | |
1417 objNum = None | |
1418 if withFeatures and self.hasFeatures(): | |
1419 for f in self.getFeatures(): | |
1420 f.positions.plot('r', True, timeStep, **kwargs) | |
1421 self.positions.plot('bx-', True, timeStep, objNum, **kwargs) | |
1422 else: | |
1423 self.positions.plot(options, withOrigin, timeStep, objNum, **kwargs) | |
1424 | |
1425 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, withIds = False, **kwargs): | |
1426 if withIds: | |
1427 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, self.getNum(), **kwargs) | |
1428 else: | |
1429 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs) | |
1430 | |
1431 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.): | |
1432 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication) | |
1433 | |
1434 def speedDiagnostics(self, framerate = 1., display = False, nInstantsIgnoredAtEnds=0): | |
1435 speeds = framerate*self.getSpeeds(nInstantsIgnoredAtEnds) | |
1436 coef = utils.linearRegression(list(range(len(speeds))), speeds) | |
1437 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), scoreatpercentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0])) | |
1438 if display: | |
1439 from matplotlib.pyplot import figure, axis | |
1440 figure(1) | |
1441 self.plot() | |
1442 axis('equal') | |
1443 figure(2) | |
1444 plot(list(self.getTimeInterval()), speeds) | |
1445 figure(3) | |
1446 plot(list(self.getTimeInterval()), self.getAccelerations(9, 3, speeds = speeds)) # arbitrary parameter | |
1447 | |
1448 @staticmethod | |
1449 def minMaxDistance(obj1, obj2): | |
1450 '''Computes the min max distance used for feature grouping''' | |
1451 commonTimeInterval = obj1.commonTimeInterval(obj2) | |
1452 if not commonTimeInterval.empty(): | |
1453 minDistance = (obj1.getPositionAtInstant(commonTimeInterval.first)-obj2.getPositionAtInstant(commonTimeInterval.first)).norm2() | |
1454 maxDistance = minDistance | |
1455 for t in list(commonTimeInterval)[1:]: | |
1456 d = (obj1.getPositionAtInstant(t)-obj2.getPositionAtInstant(t)).norm2() | |
1457 if d<minDistance: | |
1458 minDistance = d | |
1459 elif d>maxDistance: | |
1460 maxDistance = d | |
1461 return int(commonTimeInterval.length()), minDistance, maxDistance | |
1462 else: | |
1463 return int(commonTimeInterval.length()), None, None | |
1464 | |
1465 @staticmethod | |
1466 def distances(obj1, obj2, instant1, _instant2 = None): | |
1467 '''Returns the distances between all features of the 2 objects | |
1468 at the same instant instant1 | |
1469 or at instant1 and instant2''' | |
1470 if _instant2 is None: | |
1471 instant2 = instant1 | |
1472 else: | |
1473 instant2 = _instant2 | |
1474 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)] | |
1475 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)] | |
1476 return cdist(positions1, positions2, metric = 'euclidean') | |
1477 | |
1478 @staticmethod | |
1479 def minDistance(obj1, obj2, instant1, instant2 = None): | |
1480 return MovingObject.distances(obj1, obj2, instant1, instant2).min() | |
1481 | |
1482 @staticmethod | |
1483 def maxDistance(obj1, obj2, instant, instant2 = None): | |
1484 return MovingObject.distances(obj1, obj2, instant1, instant2).max() | |
1485 | |
1486 def maxSize(self): | |
1487 '''Returns the max distance between features | |
1488 at instant there are the most features''' | |
1489 if hasattr(self, 'features'): | |
1490 nFeatures = -1 | |
1491 tMaxFeatures = 0 | |
1492 for t in self.getTimeInterval(): | |
1493 n = len([f for f in self.features if f.existsAtInstant(t)]) | |
1494 if n > nFeatures: | |
1495 nFeatures = n | |
1496 tMaxFeatures = t | |
1497 return MovingObject.maxDistance(self, self, tMaxFeatures) | |
1498 else: | |
1499 print('Load features to compute a maximum size') | |
1500 return None | |
1501 | |
1502 def setRoutes(self, startRouteID, endRouteID): | |
1503 self.startRouteID = startRouteID | |
1504 self.endRouteID = endRouteID | |
1505 | |
1506 def getInstantsCrossingLane(self, p1, p2): | |
1507 '''Returns the instant(s) | |
1508 at which the object passes from one side of the segment to the other | |
1509 empty list if there is no crossing''' | |
1510 indices, intersections = self.positions.getIntersections(p1, p2) | |
1511 return [t+self.getFirstInstant() for t in indices] | |
1512 | |
1513 def computeTrajectorySimilarities(self, prototypes, lcss): | |
1514 'Computes the similarities to the prototypes using the LCSS' | |
1515 if not hasattr(self, 'prototypeSimilarities'): | |
1516 self.prototypeSimilarities = [] | |
1517 for proto in prototypes: | |
1518 lcss.similarities(proto.getMovingObject().getPositions().asArray().T, self.getPositions().asArray().T) | |
1519 similarities = lcss.similarityTable[-1, :-1].astype(float) | |
1520 self.prototypeSimilarities.append(similarities/minimum(arange(1.,len(similarities)+1), proto.getMovingObject().length()*ones(len(similarities)))) | |
1521 | |
1522 @staticmethod | |
1523 def computePET(obj1, obj2, collisionDistanceThreshold): | |
1524 '''Post-encroachment time based on distance threshold | |
1525 | |
1526 Returns the smallest time difference when the object positions are within collisionDistanceThreshold | |
1527 and the instants at which each object is passing through its corresponding position''' | |
1528 positions1 = [p.astuple() for p in obj1.getPositions()] | |
1529 positions2 = [p.astuple() for p in obj2.getPositions()] | |
1530 n1 = len(positions1) | |
1531 n2 = len(positions2) | |
1532 pets = zeros((n1, n2)) | |
1533 for i,t1 in enumerate(obj1.getTimeInterval()): | |
1534 for j,t2 in enumerate(obj2.getTimeInterval()): | |
1535 pets[i,j] = abs(t1-t2) | |
1536 distances = cdist(positions1, positions2, metric = 'euclidean') | |
1537 smallDistances = (distances <= collisionDistanceThreshold) | |
1538 if smallDistances.any(): | |
1539 smallPets = pets[smallDistances] | |
1540 petIdx = smallPets.argmin() | |
1541 distanceIndices = argwhere(smallDistances)[petIdx] | |
1542 return smallPets[petIdx], obj1.getFirstInstant()+distanceIndices[0], obj2.getFirstInstant()+distanceIndices[1] | |
1543 else: | |
1544 return None, None, None | |
1545 | |
1546 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): | |
1547 '''Predicts the position of object at instant+deltaT, | |
1548 at constant speed''' | |
1549 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) | |
1550 | |
1551 def projectCurvilinear(self, alignments, ln_mv_av_win=3): | |
1552 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' | |
1553 (curvilinearPositions instance) which holds information about the | |
1554 curvilinear coordinates using alignment metadata. | |
1555 From Paul St-Aubin's PVA tools | |
1556 ====== | |
1557 | |
1558 Input: | |
1559 ====== | |
1560 alignments = a list of alignments, where each alignment is a list of | |
1561 points (class Point). | |
1562 ln_mv_av_win = moving average window (in points) in which to smooth | |
1563 lane changes. As per tools_math.cat_mvgavg(), this term | |
1564 is a search *radius* around the center of the window. | |
1565 | |
1566 ''' | |
1567 | |
1568 self.curvilinearPositions = CurvilinearTrajectory() | |
1569 | |
1570 #For each point | |
1571 for i in range(int(self.length())): | |
1572 result = getSYfromXY(self.getPositionAt(i), alignments) | |
1573 | |
1574 # Error handling | |
1575 if(result is None): | |
1576 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i))) | |
1577 else: | |
1578 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result | |
1579 self.curvilinearPositions.addPositionSYL(S, Y, align) | |
1580 | |
1581 ## Go back through points and correct lane | |
1582 #Run through objects looking for outlier point | |
1583 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) | |
1584 ## Recalculate projected point to new lane | |
1585 lanes = self.curvilinearPositions.getLanes() | |
1586 if(lanes != smoothed_lanes): | |
1587 for i in range(len(lanes)): | |
1588 if(lanes[i] != smoothed_lanes[i]): | |
1589 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]]) | |
1590 | |
1591 # Error handling | |
1592 if(result is None): | |
1593 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. | |
1594 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i))) | |
1595 else: | |
1596 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result | |
1597 self.curvilinearPositions.setPosition(i, S, Y, align) | |
1598 | |
1599 def computeSmoothTrajectory(self, minCommonIntervalLength): | |
1600 '''Computes the trajectory as the mean of all features | |
1601 if a feature exists, its position is | |
1602 | |
1603 Warning work in progress | |
1604 TODO? not use the first/last 1-.. positions''' | |
1605 nFeatures = len(self.features) | |
1606 if nFeatures == 0: | |
1607 print('Empty object features\nCannot compute smooth trajectory') | |
1608 else: | |
1609 # compute the relative position vectors | |
1610 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i | |
1611 for i in range(nFeatures): | |
1612 for j in range(i): | |
1613 fi = self.features[i] | |
1614 fj = self.features[j] | |
1615 inter = fi.commonTimeInterval(fj) | |
1616 if inter.length() >= minCommonIntervalLength: | |
1617 xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)]) | |
1618 yi = array(fi.getYCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)]) | |
1619 xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) | |
1620 yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) | |
1621 relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi)) | |
1622 relativePositions[(j,i)] = -relativePositions[(i,j)] | |
1623 | |
1624 ### | |
1625 # User Type Classification | |
1626 ### | |
1627 def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, nInstantsIgnoredAtEnds = 0): | |
1628 '''Classifies slow and fast road users | |
1629 slow: non-motorized -> pedestrians | |
1630 fast: motorized -> cars | |
1631 | |
1632 aggregationFunc can be any function that can be applied to a vector of speeds, including percentile: | |
1633 aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile''' | |
1634 speeds = self.getSpeeds(nInstantsIgnoredAtEnds) | |
1635 if aggregationFunc(speeds) >= threshold: | |
1636 self.setUserType(userType2Num['car']) | |
1637 else: | |
1638 self.setUserType(userType2Num['pedestrian']) | |
1639 | |
1640 def classifyUserTypeSpeed(self, speedProbabilities, aggregationFunc = median, nInstantsIgnoredAtEnds = 0): | |
1641 '''Classifies road user per road user type | |
1642 speedProbabilities are functions return P(speed|class) | |
1643 in a dictionary indexed by user type names | |
1644 Returns probabilities for each class | |
1645 | |
1646 for simple threshold classification, simply pass non-overlapping indicator functions (membership) | |
1647 e.g. def indic(x): | |
1648 if abs(x-mu) < sigma: | |
1649 return 1 | |
1650 else: | |
1651 return x''' | |
1652 if not hasattr(self, 'aggregatedSpeed'): | |
1653 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds)) | |
1654 userTypeProbabilities = {} | |
1655 for userTypename in speedProbabilities: | |
1656 userTypeProbabilities[userType2Num[userTypename]] = speedProbabilities[userTypename](self.aggregatedSpeed) | |
1657 self.setUserType(utils.argmaxDict(userTypeProbabilities)) | |
1658 return userTypeProbabilities | |
1659 | |
1660 def initClassifyUserTypeHoGSVM(self, aggregationFunc, pedBikeCarSVM, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), nInstantsIgnoredAtEnds = 0, homography = None, intrinsicCameraMatrix = None, distortionCoefficients = None): | |
1661 '''Initializes the data structures for classification | |
1662 | |
1663 TODO? compute speed for longest feature?''' | |
1664 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds)) | |
1665 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM is None: | |
1666 self.appearanceClassifier = pedBikeCarSVM | |
1667 elif self.aggregatedSpeed < bikeCarSpeedThreshold: | |
1668 self.appearanceClassifier = bikeCarSVM | |
1669 else: | |
1670 self.appearanceClassifier = carClassifier | |
1671 # project feature positions | |
1672 if self.hasFeatures(): | |
1673 for f in self.getFeatures(): | |
1674 pp = cvutils.worldToImageProject(f.getPositions().asArray(), intrinsicCameraMatrix, distortionCoefficients, homography).tolist() | |
1675 f.positions = Trajectory(pp) | |
1676 self.userTypes = {} | |
1677 | |
1678 def classifyUserTypeHoGSVMAtInstant(self, img, instant, width, height, px, py, minNPixels, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm): | |
1679 '''Extracts the image box around the object | |
1680 (of square size max(width, height) of the box around the features, | |
1681 with an added px or py for width and height (around the box)) | |
1682 computes HOG on this cropped image (with parameters rescaleSize, orientations, pixelsPerCell, cellsPerBlock) | |
1683 and applies the SVM model on it''' | |
1684 croppedImg = cvutils.imageBox(img, self, instant, width, height, px, py, minNPixels) | |
1685 if croppedImg is not None and len(croppedImg) > 0: | |
1686 hog = cvutils.HOG(croppedImg, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm) | |
1687 self.userTypes[instant] = self.appearanceClassifier.predict(hog.reshape(1,hog.size)) | |
1688 else: | |
1689 self.userTypes[instant] = userType2Num['unknown'] | |
1690 | |
1691 def classifyUserTypeHoGSVM(self, pedBikeCarSVM = None, width = 0, height = 0, homography = None, images = None, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), minSpeedEquiprobable = -1, speedProbabilities = None, aggregationFunc = median, maxPercentUnknown = 0.5, nInstantsIgnoredAtEnds = 0, px = 0.2, py = 0.2, minNPixels = 800, rescaleSize = (64, 64), orientations = 9, pixelsPerCell = (8,8), cellsPerBlock = (2,2)): | |
1692 '''Agregates SVM detections in each image and returns probability | |
1693 (proportion of instants with classification in each category) | |
1694 | |
1695 images is a dictionary of images indexed by instant | |
1696 With default parameters, the general (ped-bike-car) classifier will be used | |
1697 | |
1698 Considered categories are the keys of speedProbabilities''' | |
1699 if not hasattr(self, 'aggregatedSpeed') or not hasattr(self, 'userTypes'): | |
1700 print('Initializing the data structures for classification by HoG-SVM') | |
1701 self.initClassifyUserTypeHoGSVM(aggregationFunc, pedBikeCarSVM, bikeCarSVM, pedBikeSpeedTreshold, bikeCarSpeedThreshold, nInstantsIgnoredAtEnds) | |
1702 | |
1703 if len(self.userTypes) != self.length() and images is not None: # if classification has not been done previously | |
1704 for t in self.getTimeInterval(): | |
1705 if t not in self.userTypes: | |
1706 self.classifyUserTypeHoGSVMAtInstant(images[t], t, homography, width, height, px, py, minNPixels, rescaleSize, orientations, pixelsPerCell, cellsPerBlock) | |
1707 # compute P(Speed|Class) | |
1708 if speedProbabilities is None or self.aggregatedSpeed < minSpeedEquiprobable: # equiprobable information from speed | |
1709 userTypeProbabilities = {userType2Num['car']: 1., userType2Num['pedestrian']: 1., userType2Num['bicycle']: 1.} | |
1710 else: | |
1711 userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities} | |
1712 # compute P(Class|Appearance) | |
1713 nInstantsUserType = {userTypeNum: 0 for userTypeNum in userTypeProbabilities}# number of instants the object is classified as userTypename | |
1714 nInstantsUserType[userType2Num['unknown']] = 0 | |
1715 for t in self.userTypes: | |
1716 nInstantsUserType[self.userTypes[t]] += 1 #nInstantsUserType.get(self.userTypes[t], 0) + 1 | |
1717 # result is P(Class|Appearance) x P(Speed|Class) | |
1718 if nInstantsUserType[userType2Num['unknown']] < maxPercentUnknown*self.length(): # if not too many unknowns | |
1719 for userTypeNum in userTypeProbabilities: | |
1720 userTypeProbabilities[userTypeNum] *= nInstantsUserType[userTypeNum] | |
1721 # class is the user type that maximizes usertype probabilities | |
1722 if nInstantsUserType[userType2Num['unknown']] >= maxPercentUnknown*self.length() and (speedProbabilities is None or self.aggregatedSpeed < minSpeedEquiprobable): # if no speed information and too many unknowns | |
1723 self.setUserType(userType2Num['unknown']) | |
1724 else: | |
1725 self.setUserType(utils.argmaxDict(userTypeProbabilities)) | |
1726 | |
1727 def classifyUserTypeArea(self, areas, homography): | |
1728 '''Classifies the object based on its location (projected to image space) | |
1729 areas is a dictionary of matrix of the size of the image space | |
1730 for different road users possible locations, indexed by road user type names | |
1731 | |
1732 TODO: areas could be a wrapper object with a contains method that would work for polygons and images (with wrapper class) | |
1733 skip frames at beginning/end?''' | |
1734 print('not implemented/tested yet') | |
1735 if not hasattr(self, projectedPositions): | |
1736 if homography is not None: | |
1737 self.projectedPositions = obj.positions.homographyProject(homography) | |
1738 else: | |
1739 self.projectedPositions = obj.positions | |
1740 possibleUserTypes = {userType: 0 for userType in range(len(userTypenames))} | |
1741 for p in self.projectedPositions: | |
1742 for userTypename in areas: | |
1743 if areas[userTypename][p.x, p.y] != 0: | |
1744 possibleUserTypes[userType2Enum[userTypename]] += 1 | |
1745 # what to do: threshold for most common type? self.setUserType() | |
1746 return possibleUserTypes | |
1747 | |
1748 @staticmethod | |
1749 def collisionCourseDotProduct(movingObject1, movingObject2, instant): | |
1750 'A positive result indicates that the road users are getting closer' | |
1751 deltap = movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant) | |
1752 deltav = movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant) | |
1753 return Point.dot(deltap, deltav) | |
1754 | |
1755 @staticmethod | |
1756 def collisionCourseCosine(movingObject1, movingObject2, instant): | |
1757 'A positive result indicates that the road users are getting closer' | |
1758 return Point.cosine(movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant), #deltap | |
1759 movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)) #deltav | |
1760 | |
1761 | |
1762 class Prototype(object): | |
1763 'Class for a prototype' | |
1764 | |
1765 def __init__(self, filename, num, trajectoryType, nMatchings = None): | |
1766 self.filename = filename | |
1767 self.num = num | |
1768 self.trajectoryType = trajectoryType | |
1769 self.nMatchings = nMatchings | |
1770 self.movingObject = None | |
1771 | |
1772 def getFilename(self): | |
1773 return self.filename | |
1774 def getNum(self): | |
1775 return self.num | |
1776 def getTrajectoryType(self): | |
1777 return self.trajectoryType | |
1778 def getNMatchings(self): | |
1779 return self.nMatchings | |
1780 def getMovingObject(self): | |
1781 return self.movingObject | |
1782 def setMovingObject(self, o): | |
1783 self.movingObject = o | |
1784 | |
1785 | |
1786 ################## | |
1787 # Annotations | |
1788 ################## | |
1789 | |
1790 class BBMovingObject(MovingObject): | |
1791 '''Class for a moving object represented as a bounding box | |
1792 used for series of ground truth annotations using bounding boxes | |
1793 and for the output of Urban Tracker http://www.jpjodoin.com/urbantracker/ | |
1794 | |
1795 By default in image space | |
1796 | |
1797 Its center is the center of the box (generalize to other shapes?) | |
1798 (computed after projecting if homography available) | |
1799 ''' | |
1800 | |
1801 def __init__(self, num = None, timeInterval = None, topLeftPositions = None, bottomRightPositions = None, userType = userType2Num['unknown']): | |
1802 super(BBMovingObject, self).__init__(num, timeInterval, userType = userType) | |
1803 self.topLeftPositions = topLeftPositions.getPositions() | |
1804 self.bottomRightPositions = bottomRightPositions.getPositions() | |
1805 | |
1806 def computeCentroidTrajectory(self, homography = None): | |
1807 self.positions = self.topLeftPositions.add(self.bottomRightPositions).__mul__(0.5) | |
1808 if homography is not None: | |
1809 self.positions = self.positions.homographyProject(homography) | |
1810 | |
1811 def matches(self, obj, instant, matchingDistance): | |
1812 '''Indicates if the annotation matches obj (MovingObject) | |
1813 with threshold matchingDistance | |
1814 Returns distance if below matchingDistance, matchingDistance+1 otherwise | |
1815 (returns an actual value, otherwise munkres does not terminate)''' | |
1816 d = Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant)) | |
1817 if d < matchingDistance: | |
1818 return d | |
1819 else: | |
1820 return matchingDistance + 1 | |
1821 | |
1822 def computeClearMOT(annotations, objects, matchingDistance, firstInstant, lastInstant, returnMatches = False, debug = False): | |
1823 '''Computes the CLEAR MOT metrics | |
1824 | |
1825 Reference: | |
1826 Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008) | |
1827 | |
1828 objects and annotations are supposed to in the same space | |
1829 current implementation is BBMovingObject (bounding boxes) | |
1830 mathingDistance is threshold on matching between annotation and object | |
1831 | |
1832 TO: tracker output (objects) | |
1833 GT: ground truth (annotations) | |
1834 | |
1835 Output: returns motp, mota, mt, mme, fpt, gt | |
1836 mt number of missed GT.frames (sum of the number of GT not detected in each frame) | |
1837 mme number of mismatches | |
1838 fpt number of false alarm.frames (tracker objects without match in each frame) | |
1839 gt number of GT.frames | |
1840 | |
1841 if returnMatches is True, return as 2 new arguments the GT and TO matches | |
1842 matches is a dict | |
1843 matches[i] is the list of matches for GT/TO i | |
1844 the list of matches is a dict, indexed by time, for the TO/GT id matched at time t | |
1845 (an instant t not present in matches[i] at which GT/TO exists means a missed detection or false alarm) | |
1846 | |
1847 TODO: Should we use the distance as weights or just 1/0 if distance below matchingDistance? | |
1848 (add argument useDistanceForWeights = False)''' | |
1849 from munkres import Munkres | |
1850 | |
1851 munk = Munkres() | |
1852 dist = 0. # total distance between GT and TO | |
1853 ct = 0 # number of associations between GT and tracker output in each frame | |
1854 gt = 0 # number of GT.frames | |
1855 mt = 0 # number of missed GT.frames (sum of the number of GT not detected in each frame) | |
1856 fpt = 0 # number of false alarm.frames (tracker objects without match in each frame) | |
1857 mme = 0 # number of mismatches | |
1858 matches = {} # match[i] is the tracker track associated with GT i (using object references) | |
1859 if returnMatches: | |
1860 gtMatches = {a.getNum():{} for a in annotations} | |
1861 toMatches = {o.getNum():{} for o in objects} | |
1862 else: | |
1863 gtMatches = None | |
1864 toMatches = None | |
1865 for t in range(firstInstant, lastInstant+1): | |
1866 previousMatches = matches.copy() | |
1867 # go through currently matched GT-TO and check if they are still matched withing matchingDistance | |
1868 toDelete = [] | |
1869 for a in matches: | |
1870 if a.existsAtInstant(t) and matches[a].existsAtInstant(t): | |
1871 d = a.matches(matches[a], t, matchingDistance) | |
1872 if d < matchingDistance: | |
1873 dist += d | |
1874 else: | |
1875 toDelete.append(a) | |
1876 else: | |
1877 toDelete.append(a) | |
1878 for a in toDelete: | |
1879 del matches[a] | |
1880 | |
1881 # match all unmatched GT-TO | |
1882 matchedGTs = list(matches.keys()) | |
1883 matchedTOs = list(matches.values()) | |
1884 costs = [] | |
1885 unmatchedGTs = [a for a in annotations if a.existsAtInstant(t) and a not in matchedGTs] | |
1886 unmatchedTOs = [o for o in objects if o.existsAtInstant(t) and o not in matchedTOs] | |
1887 nGTs = len(matchedGTs)+len(unmatchedGTs) | |
1888 nTOs = len(matchedTOs)+len(unmatchedTOs) | |
1889 if len(unmatchedTOs) > 0: | |
1890 for a in unmatchedGTs: | |
1891 costs.append([a.matches(o, t, matchingDistance) for o in unmatchedTOs]) | |
1892 if len(costs) > 0: | |
1893 newMatches = munk.compute(costs) | |
1894 for k,v in newMatches: | |
1895 if costs[k][v] < matchingDistance: | |
1896 matches[unmatchedGTs[k]]=unmatchedTOs[v] | |
1897 dist += costs[k][v] | |
1898 if debug: | |
1899 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.items()])) | |
1900 if returnMatches: | |
1901 for a,o in matches.items(): | |
1902 gtMatches[a.getNum()][t] = o.getNum() | |
1903 toMatches[o.getNum()][t] = a.getNum() | |
1904 | |
1905 # compute metrics elements | |
1906 ct += len(matches) | |
1907 mt += nGTs-len(matches) | |
1908 fpt += nTOs-len(matches) | |
1909 gt += nGTs | |
1910 # compute mismatches | |
1911 # for gt that do not appear in both frames, check if the corresponding to was matched to another gt in previous/next frame | |
1912 mismatches = [] | |
1913 for a in matches: | |
1914 if a in previousMatches: | |
1915 if matches[a] != previousMatches[a]: | |
1916 mismatches.append(a) | |
1917 elif matches[a] in list(previousMatches.values()): | |
1918 mismatches.append(matches[a]) | |
1919 for a in previousMatches: | |
1920 if a not in matches and previousMatches[a] in list(matches.values()): | |
1921 mismatches.append(previousMatches[a]) | |
1922 if debug: | |
1923 for mm in set(mismatches): | |
1924 print('{} {}'.format(type(mm), mm.getNum())) | |
1925 # some object mismatches may appear twice | |
1926 mme += len(set(mismatches)) | |
1927 | |
1928 if ct > 0: | |
1929 motp = dist/ct | |
1930 else: | |
1931 motp = None | |
1932 if gt > 0: | |
1933 mota = 1.-float(mt+fpt+mme)/gt | |
1934 else: | |
1935 mota = None | |
1936 return motp, mota, mt, mme, fpt, gt, gtMatches, toMatches | |
1937 | |
1938 def plotRoadUsers(objects, colors): | |
1939 '''Colors is a PlottingPropertyValues instance''' | |
1940 from matplotlib.pyplot import figure, axis | |
1941 figure() | |
1942 for obj in objects: | |
1943 obj.plot(colors.get(obj.userType)) | |
1944 axis('equal') | |
1945 | |
1946 | |
1947 if __name__ == "__main__": | |
1948 import doctest | |
1949 import unittest | |
1950 suite = doctest.DocFileSuite('tests/moving.txt') | |
1951 #suite = doctest.DocTestSuite() | |
1952 unittest.TextTestRunner().run(suite) | |
1953 #doctest.testmod() | |
1954 #doctest.testfile("example.txt") | |
1955 if shapelyAvailable: | |
1956 suite = doctest.DocFileSuite('tests/moving_shapely.txt') | |
1957 unittest.TextTestRunner().run(suite) |