Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 614:5e09583275a4

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Merged Nicolas/trafficintelligence into default
author Mohamed Gomaa <eng.m.gom3a@gmail.com>
date Fri, 05 Dec 2014 12:13:53 -0500
parents c5406edbcf12
children 5800a87f11ae 0954aaf28231
comparison
equal deleted inserted replaced
598:11f96bd08552 614:5e09583275a4
3 3
4 import utils 4 import utils
5 import cvutils 5 import cvutils
6 6
7 from math import sqrt 7 from math import sqrt
8 8 from numpy import median
9 #from shapely.geometry import Polygon 9
10 try:
11 from shapely.geometry import Polygon, Point as shapelyPoint
12 from shapely.prepared import prep
13 shapelyAvailable = True
14 except ImportError:
15 print('Shapely library could not be loaded')
16 shapelyAvailable = False
10 17
11 __metaclass__ = type 18 __metaclass__ = type
12 19
13 class Interval(object): 20 class Interval(object):
14 '''Generic interval: a subset of real numbers (not iterable)''' 21 '''Generic interval: a subset of real numbers (not iterable)'''
26 def __repr__(self): 33 def __repr__(self):
27 return self.__str__() 34 return self.__str__()
28 35
29 def empty(self): 36 def empty(self):
30 return self.first > self.last 37 return self.first > self.last
38
39 def center(self):
40 return (self.first+self.last)/2.
31 41
32 def length(self): 42 def length(self):
33 '''Returns the length of the interval''' 43 '''Returns the length of the interval'''
34 return float(max(0,self.last-self.first)) 44 return float(max(0,self.last-self.first))
35 45
88 def fromInterval(inter): 98 def fromInterval(inter):
89 return TimeInterval(inter.first, inter.last) 99 return TimeInterval(inter.first, inter.last)
90 100
91 def __getitem__(self, i): 101 def __getitem__(self, i):
92 if not self.empty(): 102 if not self.empty():
93 return self.first+i 103 if isinstance(i, int):
104 return self.first+i
105 else:
106 raise TypeError, "Invalid argument type."
107 #elif isinstance( key, slice ):
94 108
95 def __iter__(self): 109 def __iter__(self):
96 self.iterInstantNum = -1 110 self.iterInstantNum = -1
97 return self 111 return self
98 112
126 self.boundingPolygon = boundingPolygon 140 self.boundingPolygon = boundingPolygon
127 141
128 def empty(self): 142 def empty(self):
129 return self.timeInterval.empty() or not self.boudingPolygon 143 return self.timeInterval.empty() or not self.boudingPolygon
130 144
131 def getId(self): 145 def getNum(self):
132 return self.num 146 return self.num
133 147
134 def getFirstInstant(self): 148 def getFirstInstant(self):
135 return self.timeInterval.first 149 return self.timeInterval.first
136 150
161 return Point(self.x+other.x, self.y+other.y) 175 return Point(self.x+other.x, self.y+other.y)
162 176
163 def __sub__(self, other): 177 def __sub__(self, other):
164 return Point(self.x-other.x, self.y-other.y) 178 return Point(self.x-other.x, self.y-other.y)
165 179
180 def __neg__(self):
181 return Point(-self.x, -self.y)
182
183 def __getitem__(self, i):
184 if i == 0:
185 return self.x
186 elif i == 1:
187 return self.y
188 else:
189 raise IndexError()
190
191 def orthogonal(self):
192 return Point(self.y, -self.x)
193
166 def multiply(self, alpha): 194 def multiply(self, alpha):
195 'Warning, returns a new Point'
167 return Point(self.x*alpha, self.y*alpha) 196 return Point(self.x*alpha, self.y*alpha)
168 197
169 def draw(self, options = 'o', **kwargs): 198 def plot(self, options = 'o', **kwargs):
170 from matplotlib.pylab import plot 199 from matplotlib.pylab import plot
171 plot([self.x], [self.y], options, **kwargs) 200 plot([self.x], [self.y], options, **kwargs)
172 201
173 def norm2Squared(self): 202 def norm2Squared(self):
174 '''2-norm distance (Euclidean distance)''' 203 '''2-norm distance (Euclidean distance)'''
175 return self.x*self.x+self.y*self.y 204 return self.x**2+self.y**2
176 205
177 def norm2(self): 206 def norm2(self):
178 '''2-norm distance (Euclidean distance)''' 207 '''2-norm distance (Euclidean distance)'''
179 return sqrt(self.norm2Squared()) 208 return sqrt(self.norm2Squared())
180 209
190 def astuple(self): 219 def astuple(self):
191 return (self.x, self.y) 220 return (self.x, self.y)
192 221
193 def asint(self): 222 def asint(self):
194 return Point(int(self.x), int(self.y)) 223 return Point(int(self.x), int(self.y))
224
225 if shapelyAvailable:
226 def asShapely(self):
227 return shapelyPoint(self.x, self.y)
195 228
196 def project(self, homography): 229 def project(self, homography):
197 from numpy.core.multiarray import array 230 from numpy.core.multiarray import array
198 projected = cvutils.projectArray(homography, array([[self.x], [self.y]])) 231 projected = cvutils.projectArray(homography, array([[self.x], [self.y]]))
199 return Point(projected[0], projected[1]) 232 return Point(projected[0], projected[1])
200 233
201 def inPolygon(self, poly): 234 def inPolygonNoShapely(self, polygon):
202 '''Returns if the point x, y is inside the polygon. 235 '''Indicates if the point x, y is inside the polygon
203 The polygon is defined by the ordered list of points in poly 236 (array of Nx2 coordinates of the polygon vertices)
204 237
205 taken from http://www.ariel.com.au/a/python-point-int-poly.html 238 taken from http://www.ariel.com.au/a/python-point-int-poly.html
206 239
207 Use points_inside_poly from matplotlib.nxutils''' 240 Use Polygon.contains if Shapely is installed'''
208 241
209 n = len(poly); 242 n = polygon.shape[0];
210 counter = 0; 243 counter = 0;
211 244
212 p1 = poly[0]; 245 p1 = polygon[0,:];
213 for i in range(n+1): 246 for i in range(n+1):
214 p2 = poly[i % n]; 247 p2 = polygon[i % n,:];
215 if self.y > min(p1.y,p2.y): 248 if self.y > min(p1[1],p2[1]):
216 if self.y <= max(p1.y,p2.y): 249 if self.y <= max(p1[1],p2[1]):
217 if self.x <= max(p1.x,p2.x): 250 if self.x <= max(p1[0],p2[0]):
218 if p1.y != p2.y: 251 if p1[1] != p2[1]:
219 xinters = (self.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x; 252 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0];
220 if p1.x == p2.x or self.x <= xinters: 253 if p1[0] == p2[0] or self.x <= xinters:
221 counter+=1; 254 counter+=1;
222 p1=p2 255 p1=p2
223 return (counter%2 == 1); 256 return (counter%2 == 1);
224 257
225 @staticmethod 258 @staticmethod
259 def fromList(p):
260 return Point(p[0], p[1])
261
262 @staticmethod
226 def dot(p1, p2): 263 def dot(p1, p2):
227 'Scalar product' 264 'Scalar product'
228 return p1.x*p2.x+p1.y*p2.y 265 return p1.x*p2.x+p1.y*p2.y
229 266
230 @staticmethod 267 @staticmethod
231 def cross(p1, p2): 268 def cross(p1, p2):
232 'Cross product' 269 'Cross product'
233 return p1.x*p2.y-p1.y*p2.x 270 return p1.x*p2.y-p1.y*p2.x
234 271
235 @staticmethod 272 @staticmethod
273 def cosine(p1, p2):
274 return Point.dot(p1,p2)/(p1.norm2()*p2.norm2())
275
276 @staticmethod
236 def distanceNorm2(p1, p2): 277 def distanceNorm2(p1, p2):
237 return (p1-p2).norm2() 278 return (p1-p2).norm2()
238 279
239 @staticmethod 280 @staticmethod
240 def plotAll(points, color='r'): 281 def plotAll(points, **kwargs):
241 from matplotlib.pyplot import scatter 282 from matplotlib.pyplot import scatter
242 scatter([p.x for p in points],[p.y for p in points], c=color) 283 scatter([p.x for p in points],[p.y for p in points], **kwargs)
284
285 def similarOrientation(self, refDirection, cosineThreshold):
286 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold'
287 return Point.cosine(self, refDirection) >= cosineThreshold
288
289 @staticmethod
290 def timeToCollision(p1, p2, v1, v2, collisionThreshold):
291 '''Computes exact time to collision with a distance threshold
292 The unknown of the equation is the time to reach the intersection
293 between the relative trajectory of one road user
294 and the circle of radius collisionThreshold around the other road user'''
295 from math import sqrt
296 dv = v1-v2
297 dp = p1-p2
298 a = dv.norm2Squared()#(v1.x-v2.x)**2 + (v1.y-v2.y)**2
299 b = 2*Point.dot(dv, dp)#2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y))
300 c = dp.norm2Squared() - collisionThreshold**2#(p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2
301
302 delta = b**2 - 4*a*c
303 if delta >= 0:
304 deltaRoot = sqrt(delta)
305 ttc1 = (-b + deltaRoot)/(2*a)
306 ttc2 = (-b - deltaRoot)/(2*a)
307 if ttc1 >= 0 and ttc2 >= 0:
308 ttc = min(ttc1,ttc2)
309 elif ttc1 >= 0:
310 ttc = ttc1
311 elif ttc2 >= 0:
312 ttc = ttc2
313 else: # ttc1 < 0 and ttc2 < 0:
314 ttc = None
315 else:
316 ttc = None
317 return ttc
318
319 @staticmethod
320 def midPoint(p1, p2):
321 'Returns the middle of the segment [p1, p2]'
322 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y)
323
324 if shapelyAvailable:
325 def pointsInPolygon(points, polygon):
326 '''Optimized tests of a series of points within (Shapely) polygon '''
327 prepared_polygon = prep(polygon)
328 return filter(prepared_polygon.contains, points)
329
330 # Functions for coordinate transformation
331 # From Paul St-Aubin's PVA tools
332 def subsec_spline_dist(splines):
333 ''' Prepare list of spline subsegments from a spline list.
334
335 Output:
336 =======
337 ss_spline_d[spline #][mode][station]
338
339 where:
340 mode=0: incremental distance
341 mode=1: cumulative distance
342 mode=2: cumulative distance with trailing distance
343 '''
344
345 from numpy import zeros
346 ss_spline_d = []
347 #Prepare subsegment distances
348 for spline in range(len(splines)):
349 ss_spline_d.append([[],[],[]])
350 ss_spline_d[spline][0] = zeros(len(splines[spline])-1) #Incremental distance
351 ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance
352 ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance
353 for spline_p in range(len(splines[spline])):
354 if spline_p > (len(splines[spline]) - 2):
355 break
356 ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1])
357 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p])
358 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p])
359
360 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1]
361
362 return ss_spline_d
363
364 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
365 ''' Point-projection (Q) on line defined by 2 points (P0,P1).
366 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
367 '''
368 if(p0x == p1x and p0y == p1y):
369 return None
370 try:
371 #Approximate slope singularity by giving some slope roundoff; account for roundoff error
372 if(round(p0x, 10) == round(p1x, 10)):
373 p1x += 0.0000000001
374 if(round(p0y, 10) == round(p1y, 10)):
375 p1y += 0.0000000001
376 #make the calculation
377 Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x))
378 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
379 except ZeroDivisionError:
380 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
381 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
382 import pdb; pdb.set_trace()
383 return Point(X,Y)
384
385 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
386 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). A spline is a list of points (class Point), most likely a trajectory.
387
388 Output:
389 =======
390 [spline index,
391 subsegment leading point index,
392 snapped point,
393 subsegment distance,
394 spline distance,
395 orthogonal point offset]
396 '''
397 minOffsetY = float('inf')
398 #For each spline
399 for spline in range(len(splines)):
400 #For each spline point index
401 for spline_p in range(len(splines[spline])-1):
402 #Get closest point on spline
403 closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1])
404 if closestPoint == None:
405 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
406 return None
407 # check if the
408 if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], closestPoint.y):
409 offsetY = Point.distanceNorm2(closestPoint, p)
410 if offsetY < minOffsetY:
411 minOffsetY = offsetY
412 snappedSpline = spline
413 snappedSplineLeadingPoint = spline_p
414 snappedPoint = Point(closestPoint.x, closestPoint.y)
415 #Jump loop if significantly close
416 if offsetY < goodEnoughSplineDistance:
417 break
418 #Get sub-segment distance
419 if minOffsetY != float('inf'):
420 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
421 #Get cumulative alignment distance (total segment distance)
422 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
423 orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal()
424 offsetVector = p-snappedPoint
425 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
426 minOffsetY = -minOffsetY
427 return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
428 else:
429 return None
430
431 def getXYfromSY(s, y, splineNum, splines, mode = 0):
432 ''' Find X,Y coordinate from S,Y data.
433 if mode = 0 : return Snapped X,Y
434 if mode !=0 : return Real X,Y
435 '''
436
437 #(buckle in, it gets ugly from here on out)
438 ss_spline_d = subsec_spline_dist(splines)
439
440 #Find subsegment
441 snapped_x = None
442 snapped_y = None
443 for spline_ss_index in range(len(ss_spline_d[splineNum][1])):
444 if(s < ss_spline_d[splineNum][1][spline_ss_index]):
445 ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1]
446 #Get normal vector and then snap
447 vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0])
448 vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1])
449 magnitude = sqrt(vector_l_x**2 + vector_l_y**2)
450 n_vector_x = vector_l_x/magnitude
451 n_vector_y = vector_l_y/magnitude
452 snapped_x = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x
453 snapped_y = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y
454
455 #Real values (including orthogonal projection of y))
456 real_x = snapped_x - y*n_vector_y
457 real_y = snapped_y + y*n_vector_x
458 break
459
460 if mode == 0 or (not snapped_x):
461 if(not snapped_x):
462 snapped_x = splines[splineNum][-1][0]
463 snapped_y = splines[splineNum][-1][1]
464 return [snapped_x,snapped_y]
465 else:
466 return [real_x,real_y]
467
243 468
244 class NormAngle(object): 469 class NormAngle(object):
245 '''Alternate encoding of a point, by its norm and orientation''' 470 '''Alternate encoding of a point, by its norm and orientation'''
246 471
247 def __init__(self, norm, angle): 472 def __init__(self, norm, angle):
252 def fromPoint(p): 477 def fromPoint(p):
253 from math import atan2 478 from math import atan2
254 norm = p.norm2() 479 norm = p.norm2()
255 if norm > 0: 480 if norm > 0:
256 angle = atan2(p.y, p.x) 481 angle = atan2(p.y, p.x)
482 else:
483 angle = 0.
257 return NormAngle(norm, angle) 484 return NormAngle(norm, angle)
258 485
259 def __add__(self, other): 486 def __add__(self, other):
260 'a norm cannot become negative' 487 'a norm cannot become negative'
261 return NormAngle(max(self.norm+other.norm, 0), self.angle+other.angle) 488 return NormAngle(max(self.norm+other.norm, 0), self.angle+other.angle)
292 return FlowVector(self.position+other.position, self.velocity+other.velocity) 519 return FlowVector(self.position+other.position, self.velocity+other.velocity)
293 520
294 def multiply(self, alpha): 521 def multiply(self, alpha):
295 return FlowVector(self.position.multiply(alpha), self.velocity.multiply(alpha)) 522 return FlowVector(self.position.multiply(alpha), self.velocity.multiply(alpha))
296 523
297 def draw(self, options = '', **kwargs): 524 def plot(self, options = '', **kwargs):
298 from matplotlib.pylab import plot 525 from matplotlib.pylab import plot
299 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs) 526 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs)
300 self.position.draw(options+'x', **kwargs) 527 self.position.plot(options+'x', **kwargs)
301 528
302 @staticmethod 529 @staticmethod
303 def similar(f1, f2, maxDistance2, maxDeltavelocity2): 530 def similar(f1, f2, maxDistance2, maxDeltavelocity2):
304 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 531 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2
305 532
533 def intersection(p1, p2, p3, p4):
534 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4
535 http://paulbourke.net/geometry/pointlineplane/'''
536 dp12 = p2-p1
537 dp34 = p4-p3
538 #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y)
539 det = dp34.y*dp12.x-dp34.x*dp12.y
540 if det == 0:
541 return None
542 else:
543 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det
544 return p1+dp12.multiply(ua)
545
546 # def intersection(p1, p2, dp1, dp2):
547 # '''Returns the intersection point between the two lines
548 # defined by the respective vectors (dp) and origin points (p)'''
549 # from numpy import matrix
550 # from numpy.linalg import linalg
551 # A = matrix([[dp1.y, -dp1.x],
552 # [dp2.y, -dp2.x]])
553 # B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
554 # [dp2.y*p2.x-dp2.x*p2.y]])
555
556 # if linalg.det(A) == 0:
557 # return None
558 # else:
559 # intersection = linalg.solve(A,B)
560 # return Point(intersection[0,0], intersection[1,0])
561
306 def segmentIntersection(p1, p2, p3, p4): 562 def segmentIntersection(p1, p2, p3, p4):
307 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' 563 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise'''
308 from numpy import matrix
309 from numpy.linalg import linalg, det
310 564
311 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()): 565 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()):
312 return None 566 return None
313 else: 567 else:
314 dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] 568 inter = intersection(p1, p2, p3, p4)
315 dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] 569 if (inter != None
316 570 and utils.inBetween(p1.x, p2.x, inter.x)
317 A = matrix([[dp1.y, -dp1.x], 571 and utils.inBetween(p3.x, p4.x, inter.x)
318 [dp2.y, -dp2.x]]) 572 and utils.inBetween(p1.y, p2.y, inter.y)
319 B = matrix([[dp1.y*p1.x-dp1.x*p1.y], 573 and utils.inBetween(p3.y, p4.y, inter.y)):
320 [dp2.y*p3.x-dp2.x*p3.y]]) 574 return inter
321 575 else:
322 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0:
323 return None 576 return None
324 else:
325 intersection = linalg.solve(A,B)
326 if (utils.inBetween(p1.x, p2.x, intersection[0,0])
327 and utils.inBetween(p3.x, p4.x, intersection[0,0])
328 and utils.inBetween(p1.y, p2.y, intersection[1,0])
329 and utils.inBetween(p3.y, p4.y, intersection[1,0])):
330 return Point(intersection[0,0], intersection[1,0])
331 else:
332 return None
333
334 # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection
335 577
336 class Trajectory(object): 578 class Trajectory(object):
337 '''Class for trajectories: temporal sequence of positions 579 '''Class for trajectories: temporal sequence of positions
338 580
339 The class is iterable''' 581 The class is iterable'''
343 self.positions = positions 585 self.positions = positions
344 else: 586 else:
345 self.positions = [[],[]] 587 self.positions = [[],[]]
346 588
347 @staticmethod 589 @staticmethod
590 def generate(p, v, nPoints):
591 t = Trajectory()
592 p0 = Point(p.x, p.y)
593 t.addPosition(p0)
594 for i in xrange(nPoints-1):
595 p0 += v
596 t.addPosition(p0)
597 return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints])
598
599 @staticmethod
348 def load(line1, line2): 600 def load(line1, line2):
349 return Trajectory([[float(n) for n in line1.split(' ')], 601 return Trajectory([[float(n) for n in line1.split(' ')],
350 [float(n) for n in line2.split(' ')]]) 602 [float(n) for n in line2.split(' ')]])
351 603
352 @staticmethod 604 @staticmethod
353 def fromPointList(points): 605 def fromPointList(points):
354 t = Trajectory() 606 t = Trajectory()
355 for p in points: 607 if isinstance(points[0], list) or isinstance(points[0], tuple):
356 t.addPosition(p) 608 for p in points:
609 t.addPositionXY(p[0],p[1])
610 else:
611 for p in points:
612 t.addPosition(p)
357 return t 613 return t
614
615 def __len__(self):
616 return len(self.positions[0])
617
618 def length(self):
619 return self.__len__()
620
621 def empty(self):
622 return self.__len__() == 0
623
624 def __getitem__(self, i):
625 if isinstance(i, int):
626 return Point(self.positions[0][i], self.positions[1][i])
627 else:
628 raise TypeError, "Invalid argument type."
629 #elif isinstance( key, slice ):
358 630
359 def __str__(self): 631 def __str__(self):
360 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())]) 632 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())])
361 633
362 def __repr__(self): 634 def __repr__(self):
363 return str(self) 635 return self.__str__()
364 636
365 def __getitem__(self, i):
366 return Point(self.positions[0][i], self.positions[1][i])
367 637
368 def __iter__(self): 638 def __iter__(self):
369 self.iterInstantNum = 0 639 self.iterInstantNum = 0
370 return self 640 return self
371 641
374 raise StopIteration 644 raise StopIteration
375 else: 645 else:
376 self.iterInstantNum += 1 646 self.iterInstantNum += 1
377 return self[self.iterInstantNum-1] 647 return self[self.iterInstantNum-1]
378 648
379 def length(self): 649 def setPositionXY(self, i, x, y):
380 return len(self.positions[0]) 650 if i < self.__len__():
381 651 self.positions[0][i] = x
382 def __len__(self): 652 self.positions[1][i] = y
383 return self.length() 653
654 def setPosition(self, i, p):
655 self.setPositionXY(i, p.x, p.y)
384 656
385 def addPositionXY(self, x, y): 657 def addPositionXY(self, x, y):
386 self.positions[0].append(x) 658 self.positions[0].append(x)
387 self.positions[1].append(y) 659 self.positions[1].append(y)
388 660
389 def addPosition(self, p): 661 def addPosition(self, p):
390 self.addPositionXY(p.x, p.y) 662 self.addPositionXY(p.x, p.y)
391 663
392 @staticmethod 664 def duplicateLastPosition(self):
393 def _draw(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, **kwargs): 665 self.positions[0].append(self.positions[0][-1])
666 self.positions[1].append(self.positions[1][-1])
667
668 @staticmethod
669 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, **kwargs):
394 from matplotlib.pylab import plot 670 from matplotlib.pylab import plot
395 if lastCoordinate == None: 671 if lastCoordinate == None:
396 plot(positions[0][::timeStep], positions[1][::timeStep], options, **kwargs) 672 plot(positions[0][::timeStep], positions[1][::timeStep], options, **kwargs)
397 elif 0 <= lastCoordinate <= len(positions[0]): 673 elif 0 <= lastCoordinate <= len(positions[0]):
398 plot(positions[0][:lastCoordinate:timeStep], positions[1][:lastCoordinate:timeStep], options, **kwargs) 674 plot(positions[0][:lastCoordinate:timeStep], positions[1][:lastCoordinate:timeStep], options, **kwargs)
399 if withOrigin: 675 if withOrigin:
400 plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs) 676 plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs)
401 677
402 def project(self, homography): 678 def project(self, homography):
403 from numpy.core.multiarray import array 679 return Trajectory(cvutils.projectTrajectory(homography, self.positions))
404 projected = cvutils.projectArray(homography, array(self.positions)) 680
405 return Trajectory(projected) 681 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
406 682 Trajectory._plot(self.positions, options, withOrigin, None, timeStep, **kwargs)
407 def draw(self, options = '', withOrigin = False, timeStep = 1, **kwargs): 683
408 Trajectory._draw(self.positions, options, withOrigin, None, timeStep, **kwargs) 684 def plotAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, **kwargs):
409 685 Trajectory._plot(self.positions, options, withOrigin, lastCoordinate, timeStep, **kwargs)
410 def drawAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, **kwargs): 686
411 Trajectory._draw(self.positions, options, withOrigin, lastCoordinate, timeStep, **kwargs) 687 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
412
413 def drawOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
414 from matplotlib.pylab import plot 688 from matplotlib.pylab import plot
415 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]], 689 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]],
416 [-x*nPixelsPerUnitDistance+imageHeight for x in self.positions[1]]] 690 [-x*nPixelsPerUnitDistance+imageHeight for x in self.positions[1]]]
417 Trajectory._draw(imgPositions, options, withOrigin, timeStep, **kwargs) 691 Trajectory._plot(imgPositions, options, withOrigin, timeStep, **kwargs)
418 692
419 def getXCoordinates(self): 693 def getXCoordinates(self):
420 return self.positions[0] 694 return self.positions[0]
421 695
422 def getYCoordinates(self): 696 def getYCoordinates(self):
449 print 'Trajectories of different lengths' 723 print 'Trajectories of different lengths'
450 return None 724 return None
451 else: 725 else:
452 return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], 726 return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())],
453 [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) 727 [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]])
728
729 def differentiate(self, doubleLastPosition = False):
730 diff = Trajectory()
731 for i in xrange(1, self.length()):
732 diff.addPosition(self[i]-self[i-1])
733 if doubleLastPosition:
734 diff.addPosition(diff[-1])
735 return diff
454 736
455 def norm(self): 737 def norm(self):
456 '''Returns the list of the norms at each instant''' 738 '''Returns the list of the norms at each instant'''
457 # def add(x, y): return x+y 739 # def add(x, y): return x+y
458 # sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]]) 740 # sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]])
459 # return sqrt(sq) 741 # return sqrt(sq)
460 from numpy import hypot 742 from numpy import hypot
461 return hypot(self.positions[0], self.positions[1]) 743 return hypot(self.positions[0], self.positions[1])
462 744
463 def cumulatedDisplacement(self): 745 # def cumulatedDisplacement(self):
464 'Returns the sum of the distances between each successive point' 746 # 'Returns the sum of the distances between each successive point'
465 displacement = 0 747 # displacement = 0
748 # for i in xrange(self.length()-1):
749 # displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1))
750 # return displacement
751
752 def computeCumulativeDistances(self):
753 '''Computes the distance from each point to the next and the cumulative distance up to the point
754 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)'''
755 self.distances = []
756 self.cumulativeDistances = [0.]
757 p1 = self[0]
758 cumulativeDistance = 0.
466 for i in xrange(self.length()-1): 759 for i in xrange(self.length()-1):
467 displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1)) 760 p2 = self[i+1]
468 return displacement 761 self.distances.append(Point.distanceNorm2(p1,p2))
762 cumulativeDistance += self.distances[-1]
763 self.cumulativeDistances.append(cumulativeDistance)
764 p1 = p2
765
766 def getDistance(self,i):
767 '''Return the distance between points i and i+1'''
768 if i < self.length()-1:
769 return self.distances[i]
770 else:
771 print('Index {} beyond trajectory length {}-1'.format(i, self.length()))
772
773 def getCumulativeDistance(self, i):
774 '''Return the cumulative distance between the beginning and point i'''
775 if i < self.length():
776 return self.cumulativeDistances[i]
777 else:
778 print('Index {} beyond trajectory length {}'.format(i, self.length()))
779
780 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
781 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity)
782 have a cosine with refDirection is smaller than cosineThreshold'''
783 count = 0
784 lengthThreshold = float(self.length())*minProportion
785 for p in self:
786 if p.similarOrientation(refDirection, cosineThreshold):
787 count += 1
788 if count > lengthThreshold:
789 return True
790 return False
469 791
470 def wiggliness(self): 792 def wiggliness(self):
471 return self.cumulatedDisplacement()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))) 793 return self.cumulatedDisplacement()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)))
472 794
473 def getIntersections(self, p1, p2): 795 def getIntersections(self, p1, p2):
495 return Trajectory([self.positions[0][inter.first:inter.last], 817 return Trajectory([self.positions[0][inter.first:inter.last],
496 self.positions[1][inter.first:inter.last]]) 818 self.positions[1][inter.first:inter.last]])
497 else: 819 else:
498 return None 820 return None
499 821
500 def getTrajectoryInPolygon(self, polygon): 822 def getTrajectoryInPolygonNoShapely(self, polygon):
501 '''Returns the set of points inside the polygon 823 '''Returns the trajectory built with the set of points inside the polygon
502 (array of Nx2 coordinates of the polygon vertices)''' 824 (array of Nx2 coordinates of the polygon vertices)'''
503 import matplotlib.nxutils as nx
504 traj = Trajectory() 825 traj = Trajectory()
505 result = nx.points_inside_poly(self.asArray().T, polygon) 826 for p in self:
506 for i in xrange(self.length()): 827 if p.inPolygonNoShapely(polygon):
507 if result[i]: 828 traj.addPosition(p)
508 traj.addPositionXY(self.positions[0][i], self.positions[1][i])
509 return traj 829 return traj
510 830
511 # version 2: use shapely polygon contains 831 if shapelyAvailable:
512 832 def getTrajectoryInPolygon(self, polygon):
513 @staticmethod 833 '''Returns the trajectory built with the set of points inside the (shapely) polygon'''
514 def norm2LCSS(t1, t2, threshold): 834 traj = Trajectory()
515 return utils.LCSS(t1, t2, threshold, Point.distanceNorm2) 835 points = [p.asShapely() for p in self]
516 836 for p in pointsInPolygon(points, polygon):
517 @staticmethod 837 traj.addPositionXY(p.x, p.y)
518 def normMaxLCSS(t1, t2, threshold): 838 return traj
519 return utils.LCSS(t1, t2, threshold, lambda p1, p2: (p1-p2).normMax()) 839
840 @staticmethod
841 def lcss(t1, t2, lcss):
842 return lcss.compute(t1, t2)
843
844 class CurvilinearTrajectory(Trajectory):
845 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements
846 longitudinal coordinate is stored as first coordinate (exterior name S)
847 lateral coordiante is stored as second coordinate'''
848
849 def __init__(self, S = None, Y = None, lanes = None):
850 if S == None or Y == None or len(S) != len(Y):
851 self.positions = [[],[]]
852 if S != None and Y != None and len(S) != len(Y):
853 print("S and Y coordinates of different lengths\nInitializing to empty lists")
854 else:
855 self.positions = [S,Y]
856 if lanes == None or len(lanes) != self.length():
857 self.lanes = []
858 else:
859 self.lanes = lanes
860
861 def __getitem__(self,i):
862 if isinstance(i, int):
863 return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
864 else:
865 raise TypeError, "Invalid argument type."
866 #elif isinstance( key, slice ):
867
868 def getSCoordinates(self):
869 return self.getXCoordinates()
870
871 def getLanes(self):
872 return self.lanes
873
874 def addPositionSYL(self, s, y, lane):
875 self.addPositionXY(s,y)
876 self.lanes.append(lane)
877
878 def addPosition(self, p):
879 'Adds position in the point format for curvilinear of list with 3 values'
880 self.addPositionSYL(p[0], p[1], p[2])
881
882 def setPosition(self, i, s, y, lane):
883 self.setPositionXY(i, s, y)
884 if i < self.__len__():
885 self.lanes[i] = lane
886
887 def differentiate(self, doubleLastPosition = False):
888 diff = CurvilinearTrajectory()
889 p1 = self[0]
890 for i in xrange(1, self.length()):
891 p2 = self[i]
892 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2])
893 p1=p2
894 if doubleLastPosition and self.length() > 1:
895 diff.addPosition(diff[-1])
896 return diff
897
898 def getIntersections(self, S1, lane = None):
899 '''Returns a list of the indices at which the trajectory
900 goes past the curvilinear coordinate S1
901 (in provided lane if lane != None)
902 the list is empty if there is no crossing'''
903 indices = []
904 for i in xrange(self.length()-1):
905 q1=self.__getitem__(i)
906 q2=self.__getitem__(i+1)
907 if q1[0] <= S1 < q2[0] and (lane == None or (self.lanes[i] == lane and self.lanes[i+1] == lane)):
908 indices.append(i+(S1-q1[0])/(q2[0]-q1[0]))
909 return indices
520 910
521 ################## 911 ##################
522 # Moving Objects 912 # Moving Objects
523 ################## 913 ##################
524 914
525 userTypeNames = ['car', 915 userTypeNames = ['unknown',
916 'car',
526 'pedestrian', 917 'pedestrian',
527 'twowheels', 918 'motorcycle',
919 'bicycle',
528 'bus', 920 'bus',
529 'truck'] 921 'truck']
530 922
531 userType2Num = utils.inverseEnumeration(userTypeNames) 923 userType2Num = utils.inverseEnumeration(userTypeNames)
532 924
533 class MovingObject(STObject): 925 class MovingObject(STObject):
534 '''Class for moving objects: a spatio-temporal object 926 '''Class for moving objects: a spatio-temporal object
535 with a trajectory and a geometry (constant volume over time) and a usertype (e.g. road user) 927 with a trajectory and a geometry (constant volume over time)
928 and a usertype (e.g. road user) coded as a number (see userTypeNames)
536 ''' 929 '''
537 930
538 def __init__(self, num = None, timeInterval = None, positions = None, geometry = None, userType = None): 931 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown']):
539 super(MovingObject, self).__init__(num, timeInterval) 932 super(MovingObject, self).__init__(num, timeInterval)
540 self.positions = positions 933 self.positions = positions
934 self.velocities = velocities
541 self.geometry = geometry 935 self.geometry = geometry
542 self.userType = userType 936 self.userType = userType
937 self.features = []
543 # compute bounding polygon from trajectory 938 # compute bounding polygon from trajectory
544 939
940 @staticmethod
941 def generate(p, v, timeInterval):
942 positions, velocities = Trajectory.generate(p, v, int(timeInterval.length()))
943 return MovingObject(timeInterval = timeInterval, positions = positions, velocities = velocities)
944
545 def getObjectInTimeInterval(self, inter): 945 def getObjectInTimeInterval(self, inter):
546 '''Returns a new object extracted from self, 946 '''Returns a new object extracted from self,
547 restricted to time interval inter''' 947 restricted to time interval inter'''
548 intersection = TimeInterval.intersection(inter, self.getTimeInterval()) 948 intersection = TimeInterval.intersection(inter, self.getTimeInterval())
549 if not intersection.empty(): 949 if not intersection.empty():
563 return self.positions 963 return self.positions
564 964
565 def getVelocities(self): 965 def getVelocities(self):
566 return self.velocities 966 return self.velocities
567 967
968 def getUserType(self):
969 return self.userType
970
971 def getCurvilinearPositions(self):
972 if hasattr(self, 'curvilinearPositions'):
973 return self.curvilinearPositions
974 else:
975 return None
976
977 def setUserType(self, userType):
978 self.userType = userType
979
568 def setFeatures(self, features): 980 def setFeatures(self, features):
569 self.features = [features[i] for i in self.featureNumbers] 981 self.features = [features[i] for i in self.featureNumbers]
570 982
571 def getSpeeds(self): 983 def getSpeeds(self):
572 return self.getVelocities().norm() 984 return self.getVelocities().norm()
587 return self.positions.getXCoordinates() 999 return self.positions.getXCoordinates()
588 1000
589 def getYCoordinates(self): 1001 def getYCoordinates(self):
590 return self.positions.getYCoordinates() 1002 return self.positions.getYCoordinates()
591 1003
592 def draw(self, options = '', withOrigin = False, timeStep = 1, **kwargs): 1004 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, **kwargs):
593 self.positions.draw(options, withOrigin, timeStep, **kwargs) 1005 if withFeatures:
594 1006 for f in self.features:
595 def drawOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs): 1007 f.positions.plot('r', True, timeStep, **kwargs)
596 self.positions.drawOnWorldImage(nPixelsPerUnitDistance, imageHeight, options, withOrigin, timeStep, **kwargs) 1008 self.positions.plot('bx-', True, timeStep, **kwargs)
1009 else:
1010 self.positions.plot(options, withOrigin, timeStep, **kwargs)
1011
1012 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
1013 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, imageHeight, options, withOrigin, timeStep, **kwargs)
597 1014
598 def play(self, videoFilename, homography = None): 1015 def play(self, videoFilename, homography = None):
599 cvutils.displayTrajectories(videoFilename, [self], homography, self.getFirstInstant(), self.getLastInstant()) 1016 cvutils.displayTrajectories(videoFilename, [self], homography, self.getFirstInstant(), self.getLastInstant())
600 1017
1018 def speedDiagnostics(self, framerate = 1., display = False):
1019 from numpy import std
1020 from scipy.stats import scoreatpercentile
1021 speeds = framerate*self.getSpeeds()
1022 coef = utils.linearRegression(range(len(speeds)), speeds)
1023 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), scoreatpercentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0]))
1024 if display:
1025 from matplotlib.pyplot import figure, plot, axis
1026 figure(1)
1027 self.plot()
1028 axis('equal')
1029 figure(2)
1030 plot(list(self.getTimeInterval()), speeds)
1031
1032 @staticmethod
1033 def distances(obj1, obj2, instant1, _instant2 = None):
1034 from scipy.spatial.distance import cdist
1035 if _instant2 == None:
1036 instant2 = instant1
1037 else:
1038 instant2 = _instant2
1039 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)]
1040 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)]
1041 return cdist(positions1, positions2, metric = 'euclidean')
1042
1043 @staticmethod
1044 def minDistance(obj1, obj2, instant1, instant2 = None):
1045 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
1046
1047 @staticmethod
1048 def maxDistance(obj1, obj2, instant, instant2 = None):
1049 return MovingObject.distances(obj1, obj2, instant1, instant2).max()
1050
1051 def maxSize(self):
1052 '''Returns the max distance between features
1053 at instant there are the most features'''
1054 if hasattr(self, 'features'):
1055 nFeatures = -1
1056 tMaxFeatures = 0
1057 for t in self.getTimeInterval():
1058 n = len([f for f in self.features if f.existsAtInstant(t)])
1059 if n > nFeatures:
1060 nFeatures = n
1061 tMaxFeatures = t
1062 return MovingObject.maxDistance(self, self, tMaxFeatures)
1063 else:
1064 print('Load features to compute a maximum size')
1065 return None
1066
1067 def setRoutes(self, startRouteID, endRouteID):
1068 self.startRouteID = startRouteID
1069 self.endRouteID = endRouteID
1070
601 def getInstantsCrossingLane(self, p1, p2): 1071 def getInstantsCrossingLane(self, p1, p2):
602 '''Returns the instant(s) 1072 '''Returns the instant(s)
603 at which the object passes from one side of the segment to the other 1073 at which the object passes from one side of the segment to the other
604 empty list if there is no crossing''' 1074 empty list if there is no crossing'''
605 indices = self.positions.getIntersections(p1, p2) 1075 indices = self.positions.getIntersections(p1, p2)
608 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): 1078 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
609 '''Predicts the position of object at instant+deltaT, 1079 '''Predicts the position of object at instant+deltaT,
610 at constant speed''' 1080 at constant speed'''
611 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) 1081 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration)
612 1082
1083 def projectCurvilinear(self, alignments, ln_mv_av_win=3):
1084 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
1085 (curvilinearPositions instance) which holds information about the
1086 curvilinear coordinates using alignment metadata.
1087 From Paul St-Aubin's PVA tools
1088 ======
1089
1090 Input:
1091 ======
1092 alignments = a list of alignments, where each alignment is a list of
1093 points (class Point).
1094 ln_mv_av_win = moving average window (in points) in which to smooth
1095 lane changes. As per tools_math.cat_mvgavg(), this term
1096 is a search *radius* around the center of the window.
1097
1098 '''
1099
1100 self.curvilinearPositions = CurvilinearTrajectory()
1101
1102 #For each point
1103 for i in xrange(int(self.length())):
1104 result = getSYfromXY(self.getPositionAt(i), alignments)
1105
1106 # Error handling
1107 if(result == None):
1108 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i)))
1109 else:
1110 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
1111 self.curvilinearPositions.addPositionSYL(S, Y, align)
1112
1113 ## Go back through points and correct lane
1114 #Run through objects looking for outlier point
1115 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win)
1116 ## Recalculate projected point to new lane
1117 lanes = self.curvilinearPositions.getLanes()
1118 if(lanes != smoothed_lanes):
1119 for i in xrange(len(lanes)):
1120 if(lanes[i] != smoothed_lanes[i]):
1121 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
1122
1123 # Error handling
1124 if(result == None):
1125 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
1126 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i)))
1127 else:
1128 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
1129 self.curvilinearPositions.setPosition(i, S, Y, align)
1130
1131 def computeSmoothTrajectory(self, minCommonIntervalLength):
1132 '''Computes the trajectory as the mean of all features
1133 if a feature exists, its position is
1134
1135 Warning work in progress
1136 TODO? not use the first/last 1-.. positions'''
1137 from numpy import array, median
1138 nFeatures = len(self.features)
1139 if nFeatures == 0:
1140 print('Empty object features\nCannot compute smooth trajectory')
1141 else:
1142 # compute the relative position vectors
1143 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i
1144 for i in xrange(nFeatures):
1145 for j in xrange(i):
1146 fi = self.features[i]
1147 fj = self.features[j]
1148 inter = fi.commonTimeInterval(fj)
1149 if inter.length() >= minCommonIntervalLength:
1150 xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)])
1151 yi = array(fi.getYCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)])
1152 xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
1153 yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
1154 relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi))
1155 relativePositions[(j,i)] = -relativePositions[(i,j)]
1156
1157 ###
1158 # User Type Classification
1159 ###
1160 def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, ignoreNInstantsAtEnds = 0):
1161 '''Classifies slow and fast road users
1162 slow: non-motorized -> pedestrians
1163 fast: motorized -> cars'''
1164 if ignoreNInstantsAtEnds > 0:
1165 speeds = self.getSpeeds()[ignoreNInstantsAtEnds:-ignoreNInstantsAtEnds]
1166 else:
1167 speeds = self.getSpeeds()
1168 if aggregationFunc(speeds) >= threshold:
1169 self.setUserType(userType2Num['car'])
1170 else:
1171 self.setUserType(userType2Num['pedestrian'])
1172
1173 def classifyUserTypeSpeed(self, speedProbabilities, aggregationFunc = median):
1174 '''Classifies road user per road user type
1175 speedProbabilities are functions return P(speed|class)
1176 in a dictionary indexed by user type names
1177 Returns probabilities for each class
1178
1179 for simple threshold classification, simply pass non-overlapping indicator functions (membership)
1180 e.g. def indic(x):
1181 if abs(x-mu) < sigma:
1182 return 1
1183 else:
1184 return x'''
1185 if not hasattr(self, aggregatedSpeed):
1186 self.aggregatedSpeed = aggregationFunc(self.getSpeeds())
1187 userTypeProbabilities = {}
1188 for userTypename in speedProbabilities:
1189 userTypeProbabilities[userType2Num[userTypename]] = speedProbabilities[userTypename](self.aggregatedSpeed)
1190 self.setUserType(utils.argmaxDict(userTypeProbabilities))
1191 return userTypeProbabilities
1192
1193 def initClassifyUserTypeHoGSVM(self, aggregationFunc = median):
1194 '''Initializes the data structures for classification
1195
1196 TODO? compute speed for longest feature?
1197 Skip beginning and end of feature for speed? Offer options instead of median'''
1198 self.aggregatedSpeed = aggregationFunc(self.getSpeeds())
1199 self.userTypes = {}
1200
1201 def classifyUserTypeHoGSVMAtInstant(self, img, pedBikeCarSVM, instant, homography, width, height, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), px = 0.2, py = 0.2, pixelThreshold = 800):
1202 '''Extract the image box around the object and
1203 applies the SVM model on it'''
1204 from numpy import array
1205 croppedImg, yCropMin, yCropMax, xCropMin, xCropMax = imageBox(img, self, instant, homography, width, height, px, py, pixelThreshold)
1206 if len(croppedImg) > 0: # != []
1207 hog = array([cvutils.HOG(croppedImg)], dtype = np.float32)
1208 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM == None:
1209 self.userTypes[instant] = int(pedBikeCarSVM.predict(hog))
1210 elif self.aggregatedSpeed < bikeCarSpeedTreshold:
1211 self.userTypes[instant] = int(bikeCarSVM.predict(hog))
1212 else:
1213 self.userTypes[instant] = userType2Num['car']
1214 else:
1215 self.userTypes[instant] = userType2Num['unknown']
1216
1217 def classifyUserTypeHoGSVM(self, images, pedBikeCarSVM, homography, width, height, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), speedProbabilities = None, aggregationFunc = median, px = 0.2, py = 0.2, pixelThreshold = 800):
1218 '''Agregates SVM detections in each image and returns probability
1219 (proportion of instants with classification in each category)
1220
1221 iamges is a dictionary of images indexed by instant
1222 With default parameters, the general (ped-bike-car) classifier will be used
1223 TODO? consider all categories?'''
1224 if not hasattr(self, aggregatedSpeed) or not hasattr(self, userTypes):
1225 print('Initilize the data structures for classification by HoG-SVM')
1226 self.initClassifyUserTypeHoGSVM(aggregationFunc)
1227
1228 if len(self.userTypes) != self.length(): # if classification has not been done previously
1229 for t in self.getTimeInterval():
1230 if t not in self.userTypes:
1231 self.classifyUserTypeHoGSVMAtInstant(images[t], pedBikeCarSVM, t, homography, width, height, bikeCarSVM, pedBikeSpeedTreshold, bikeCarSpeedThreshold, px, py, pixelThreshold)
1232 # compute P(Speed|Class)
1233 if speedProbabilities == None: # equiprobable information from speed
1234 userTypeProbabilities = {userType2Num['car']: 1., userType2Num['pedestrian']: 1., userType2Num['bicycle']: 1.}
1235 else:
1236 userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities}
1237 # result is P(Class|Appearance) x P(Speed|Class)
1238 nInstantsUserType = {userType2Num[userTypename]: 0 for userTypename in userTypeProbabilities}# number of instants the object is classified as userTypename
1239 for t in self.userTypes:
1240 nInstantsUserType[self.userTypes[t]] += 1
1241 for userTypename in userTypeProbabilities:
1242 userTypeProbabilities[userTypename] *= nInstantsUserType[userTypename]
1243 # class is the user type that maximizes usertype probabilities
1244 self.setUserType(utils.argmaxDict(userTypeProbabilities))
1245
1246 def classifyUserTypeArea(self, areas, homography):
1247 '''Classifies the object based on its location (projected to image space)
1248 areas is a dictionary of matrix of the size of the image space
1249 for different road users possible locations, indexed by road user type names
1250
1251 TODO: areas could be a wrapper object with a contains method that would work for polygons and images (with wrapper class)
1252 skip frames at beginning/end?'''
1253 print('not implemented/tested yet')
1254 if not hasattr(self, projectedPositions):
1255 if homography != None:
1256 self.projectedPositions = obj.positions.project(homography)
1257 else:
1258 self.projectedPositions = obj.positions
1259 possibleUserTypes = {userType: 0 for userType in range(len(userTypenames))}
1260 for p in self.projectedPositions:
1261 for userTypename in areas:
1262 if areas[userTypename][p.x, p.y] != 0:
1263 possibleUserTypes[userType2Enum[userTypename]] += 1
1264 # what to do: threshold for most common type? self.setUserType()
1265 return possibleUserTypes
1266
613 @staticmethod 1267 @staticmethod
614 def collisionCourseDotProduct(movingObject1, movingObject2, instant): 1268 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
615 'A positive result indicates that the road users are getting closer' 1269 'A positive result indicates that the road users are getting closer'
616 deltap = movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant) 1270 deltap = movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant)
617 deltav = movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant) 1271 deltav = movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)
618 return Point.dot(deltap, deltav) 1272 return Point.dot(deltap, deltav)
619 1273
620 @staticmethod 1274 @staticmethod
621 def collisionCourseCosine(movingObject1, movingObject2, instant): 1275 def collisionCourseCosine(movingObject1, movingObject2, instant):
622 'A positive result indicates that the road users are getting closer' 1276 'A positive result indicates that the road users are getting closer'
623 deltap = movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant) 1277 return Point.cosine(movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant), #deltap
624 deltav = movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant) 1278 movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)) #deltav
625 return Point.dot(deltap, deltav)/(deltap.norm2()*deltav.norm2()) 1279
1280
1281 ##################
1282 # Annotations
1283 ##################
1284
1285 class BBAnnotation(MovingObject):
1286 '''Class for : a series of ground truth annotations using bounding boxes
1287 Its center is the center of the containing shape
1288 '''
1289
1290 def __init__(self, num = None, timeInterval = None, topPositions = None, bottomPositions = None, userType = userType2Num['unknown']):
1291 super(BBAnnotation, self).__init__(num, timeInterval, Trajectory(), userType = userType)
1292 self.topPositions = topPositions.getPositions()
1293 self.bottomPositions = bottomPositions.getPositions()
1294 for i in xrange(int(topPositions.length())):
1295 self.positions.addPosition((topPositions.getPositionAt(i) + bottomPositions.getPositionAt(i)).multiply(0.5))
1296
626 1297
627 def plotRoadUsers(objects, colors): 1298 def plotRoadUsers(objects, colors):
628 '''Colors is a PlottingPropertyValues instance''' 1299 '''Colors is a PlottingPropertyValues instance'''
629 from matplotlib.pyplot import figure, axis 1300 from matplotlib.pyplot import figure, axis
630 figure() 1301 figure()
631 for obj in objects: 1302 for obj in objects:
632 obj.draw(colors.get(obj.userType)) 1303 obj.plot(colors.get(obj.userType))
633 axis('equal') 1304 axis('equal')
634 1305
635 1306
636 if __name__ == "__main__": 1307 if __name__ == "__main__":
637 import doctest 1308 import doctest