Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 614:5e09583275a4
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 |