Mercurial Hosting > traffic-intelligence

comparison trafficintelligence/moving.py @ 1028:cc5cb04b04b0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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)