Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 780:1b22d81ef5ff dev
cleaned and checked storage with functions for curvilinear trajectories
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 08 Feb 2016 12:24:26 -0500 |
parents | 84420159c5f4 |
children | 7c38250ddfc7 |
comparison
equal
deleted
inserted
replaced
779:670bd6a35417 | 780:1b22d81ef5ff |
---|---|
368 mode=2: cumulative distance with trailing distance | 368 mode=2: cumulative distance with trailing distance |
369 ''' | 369 ''' |
370 ss_spline_d = [] | 370 ss_spline_d = [] |
371 #Prepare subsegment distances | 371 #Prepare subsegment distances |
372 for spline in range(len(splines)): | 372 for spline in range(len(splines)): |
373 ss_spline_d.append([[],[],[]]) | 373 ss_spline_d[spline]=[]#.append([[],[],[]]) |
374 ss_spline_d[spline][0] = zeros(len(splines[spline])-1) #Incremental distance | 374 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Incremental distance |
375 ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance | 375 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Cumulative distance |
376 ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance | 376 ss_spline_d[spline].append(zeros(len(splines[spline]))) #Cumulative distance with trailing distance |
377 for spline_p in range(len(splines[spline])): | 377 for spline_p in range(len(splines[spline])): |
378 if spline_p > (len(splines[spline]) - 2): | 378 if spline_p > (len(splines[spline]) - 2): |
379 break | 379 break |
380 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]) | 380 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]) |
381 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) | 381 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) |
382 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p]) | 382 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p]) |
383 | 383 |
384 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] | 384 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] |
385 | 385 |
386 return ss_spline_d | 386 return ss_spline_d |
387 | |
388 def prepareSplines(splines): | |
389 'Approximates slope singularity by giving some slope roundoff; account for roundoff error' | |
390 for spline in splines: | |
391 p1 = spline[0] | |
392 for i in xrange(len(spline)-1): | |
393 p2 = spline[i+1] | |
394 if(round(p1.x, 10) == round(p2.x, 10)): | |
395 p2.x += 0.0000000001 | |
396 if(round(p1.y, 10) == round(p2.y, 10)): | |
397 p2.y += 0.0000000001 | |
398 p1 = p2 | |
387 | 399 |
388 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): | 400 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): |
389 ''' Point-projection (Q) on line defined by 2 points (P0,P1). | 401 ''' Point-projection (Q) on line defined by 2 points (P0,P1). |
390 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf | 402 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf |
391 ''' | 403 ''' |
392 if(p0x == p1x and p0y == p1y): | 404 if(p0x == p1x and p0y == p1y): |
393 return None | 405 return None |
394 try: | 406 try: |
395 #Approximate slope singularity by giving some slope roundoff; account for roundoff error | 407 #Approximate slope singularity by giving some slope roundoff; account for roundoff error |
396 if(round(p0x, 10) == round(p1x, 10)): | 408 # if(round(p0x, 10) == round(p1x, 10)): |
397 p1x += 0.0000000001 | 409 # p1x += 0.0000000001 |
398 if(round(p0y, 10) == round(p1y, 10)): | 410 # if(round(p0y, 10) == round(p1y, 10)): |
399 p1y += 0.0000000001 | 411 # p1y += 0.0000000001 |
400 #make the calculation | 412 #make the calculation |
401 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)) | 413 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)) |
402 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) | 414 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) |
403 except ZeroDivisionError: | 415 except ZeroDivisionError: |
404 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') | 416 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') |
405 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) | 417 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) |
406 import pdb; pdb.set_trace() | 418 import pdb; pdb.set_trace() |
407 return Point(X,Y) | 419 return Point(X,Y) |
408 | 420 |
409 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): | 421 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): |
410 ''' 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. | 422 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). |
411 | 423 A spline is a list of points (class Point), most likely a trajectory. |
424 | |
412 Output: | 425 Output: |
413 ======= | 426 ======= |
414 [spline index, | 427 [spline index, |
415 subsegment leading point index, | 428 subsegment leading point index, |
416 snapped point, | 429 snapped point, |
420 | 433 |
421 or None | 434 or None |
422 ''' | 435 ''' |
423 minOffsetY = float('inf') | 436 minOffsetY = float('inf') |
424 #For each spline | 437 #For each spline |
425 for spline in range(len(splines)): | 438 for splineIdx in range(len(splines)): |
426 #For each spline point index | 439 #For each spline point index |
427 for spline_p in range(len(splines[spline])-1): | 440 for spline_p in range(len(splines[splineIdx])-1): |
428 #Get closest point on spline | 441 #Get closest point on spline |
429 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]) | 442 closestPoint = ppldb2p(p.x,p.y,splines[splineIdx][spline_p][0],splines[splineIdx][spline_p][1],splines[splineIdx][spline_p+1][0],splines[splineIdx][spline_p+1][1]) |
430 if closestPoint is None: | 443 if closestPoint is None: |
431 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) | 444 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p)) |
432 return None | 445 return None |
433 # check if the | 446 # check if the projected point is in between the current segment of the alignment bounds |
434 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): | 447 if utils.inBetween(splines[splineIdx][spline_p][0], splines[splineIdx][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[splineIdx][spline_p][1], splines[splineIdx][spline_p+1][1], closestPoint.y): |
435 offsetY = Point.distanceNorm2(closestPoint, p) | 448 offsetY = Point.distanceNorm2(closestPoint, p) |
436 if offsetY < minOffsetY: | 449 if offsetY < minOffsetY: |
437 minOffsetY = offsetY | 450 minOffsetY = offsetY |
438 snappedSpline = spline | 451 snappedSplineIdx = splineIdx |
439 snappedSplineLeadingPoint = spline_p | 452 snappedSplineLeadingPoint = spline_p |
440 snappedPoint = Point(closestPoint.x, closestPoint.y) | 453 snappedPoint = Point(closestPoint.x, closestPoint.y) |
441 #Jump loop if significantly close | 454 #Jump loop if significantly close |
442 if offsetY < goodEnoughSplineDistance: | 455 if offsetY < goodEnoughSplineDistance: |
443 break | 456 break |
457 | |
444 #Get sub-segment distance | 458 #Get sub-segment distance |
445 if minOffsetY != float('inf'): | 459 if minOffsetY != float('inf'): |
446 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint]) | 460 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint]) |
447 #Get cumulative alignment distance (total segment distance) | 461 #Get cumulative alignment distance (total segment distance) |
448 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance | 462 splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance |
449 orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal() | 463 orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal() |
450 offsetVector = p-snappedPoint | 464 offsetVector = p-snappedPoint |
451 if Point.dot(orthogonalSplineVector, offsetVector) < 0: | 465 if Point.dot(orthogonalSplineVector, offsetVector) < 0: |
452 minOffsetY = -minOffsetY | 466 minOffsetY = -minOffsetY |
453 return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] | 467 return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] |
454 else: | 468 else: |
469 print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p)) | |
455 return None | 470 return None |
456 | 471 |
457 def getXYfromSY(s, y, splineNum, splines, mode = 0): | 472 def getXYfromSY(s, y, splineNum, splines, mode = 0): |
458 ''' Find X,Y coordinate from S,Y data. | 473 ''' Find X,Y coordinate from S,Y data. |
459 if mode = 0 : return Snapped X,Y | 474 if mode = 0 : return Snapped X,Y |