Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 780:1b22d81ef5ff dev

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