Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1086:8734742c08c0
major refactoring of curvilinear trajectory projections
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 16 Oct 2018 12:46:29 -0400 |
parents | 7853106677b7 |
children | c96388c696ac |
comparison
equal
deleted
inserted
replaced
1085:7853106677b7 | 1086:8734742c08c0 |
---|---|
246 | 246 |
247 def projectLocal(self, v, clockwise = True): | 247 def projectLocal(self, v, clockwise = True): |
248 'Projects point projected on v, v.orthogonal()' | 248 'Projects point projected on v, v.orthogonal()' |
249 e1 = v/v.norm2() | 249 e1 = v/v.norm2() |
250 e2 = e1.orthogonal(clockwise) | 250 e2 = e1.orthogonal(clockwise) |
251 return Point(Point.dot(self, e1), Point.doc(self, e2)) | 251 return Point(Point.dot(self, e1), Point.dot(self, e2)) |
252 | 252 |
253 def rotate(self, theta): | 253 def rotate(self, theta): |
254 return Point(self.x*cos(theta)-self.y*sin(theta), self.x*sin(theta)+self.y*cos(theta)) | 254 return Point(self.x*cos(theta)-self.y*sin(theta), self.x*sin(theta)+self.y*cos(theta)) |
255 | 255 |
256 def __mul__(self, alpha): | 256 def __mul__(self, alpha): |
407 prepared_polygon = prep(polygon) | 407 prepared_polygon = prep(polygon) |
408 return list(filter(prepared_polygon.contains, points)) | 408 return list(filter(prepared_polygon.contains, points)) |
409 | 409 |
410 # Functions for coordinate transformation | 410 # Functions for coordinate transformation |
411 # From Paul St-Aubin's PVA tools | 411 # From Paul St-Aubin's PVA tools |
412 def subsec_spline_dist(splines): | 412 def prepareAlignments(alignments): |
413 ''' Prepare list of spline subsegments from a spline list. | 413 '''Prepares alignments (list of splines, each typically represented as a Trajectory) |
414 | 414 - computes cumulative distances |
415 Output: | 415 - approximates slope singularity by giving some slope roundoff (account for roundoff error)''' |
416 ======= | 416 for alignment in alignments: |
417 ss_spline_d[spline #][mode][station] | 417 alignment.computeCumulativeDistances() |
418 | 418 p1 = alignment[0] |
419 where: | 419 for i in range(len(alignment)-1): |
420 mode=0: incremental distance | 420 p2 = alignment[i+1] |
421 mode=1: cumulative distance | |
422 mode=2: cumulative distance with trailing distance | |
423 ''' | |
424 ss_spline_d = [] | |
425 #Prepare subsegment distances | |
426 for spline in range(len(splines)): | |
427 ss_spline_d[spline]=[]#.append([[],[],[]]) | |
428 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Incremental distance | |
429 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Cumulative distance | |
430 ss_spline_d[spline].append(zeros(len(splines[spline]))) #Cumulative distance with trailing distance | |
431 for spline_p in range(len(splines[spline])): | |
432 if spline_p > (len(splines[spline]) - 2): | |
433 break | |
434 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]) | |
435 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) | |
436 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p]) | |
437 | |
438 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] | |
439 | |
440 return ss_spline_d | |
441 | |
442 def prepareSplines(splines): | |
443 'Approximates slope singularity by giving some slope roundoff; account for roundoff error' | |
444 for spline in splines: | |
445 p1 = spline[0] | |
446 for i in range(len(spline)-1): | |
447 p2 = spline[i+1] | |
448 if(round(p1.x, 10) == round(p2.x, 10)): | 421 if(round(p1.x, 10) == round(p2.x, 10)): |
449 p2.x += 0.0000000001 | 422 p2.x += 0.0000000001 |
450 if(round(p1.y, 10) == round(p2.y, 10)): | 423 if(round(p1.y, 10) == round(p2.y, 10)): |
451 p2.y += 0.0000000001 | 424 p2.y += 0.0000000001 |
452 p1 = p2 | 425 p1 = p2 |
470 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') | 443 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') |
471 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) | 444 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) |
472 import pdb; pdb.set_trace() | 445 import pdb; pdb.set_trace() |
473 return Point(X,Y) | 446 return Point(X,Y) |
474 | 447 |
475 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): | 448 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5): |
476 ''' Snap a point p to its nearest subsegment of it's nearest spline (from the list splines). | 449 ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments). |
477 A spline is a list of points (class Point), most likely a trajectory. | 450 A alignment is a list of points (class Point), most likely a trajectory. |
478 | 451 |
479 Output: | 452 Output: |
480 ======= | 453 ======= |
481 [spline index, | 454 [alignment index, |
482 subsegment leading point index, | 455 subsegment leading point index, |
483 snapped point, | 456 snapped point, |
484 subsegment distance, | 457 subsegment distance, |
485 spline distance, | 458 alignment distance, |
486 orthogonal point offset] | 459 orthogonal point offset] |
487 | 460 |
488 or None | 461 or None |
489 ''' | 462 ''' |
490 minOffsetY = float('inf') | 463 minOffsetY = float('inf') |
491 #For each spline | 464 #For each alignment |
492 for splineIdx in range(len(splines)): | 465 for alignmentIdx in range(len(alignments)): |
493 #For each spline point index | 466 #For each alignment point index |
494 for spline_p in range(len(splines[splineIdx])-1): | 467 for alignment_p in range(len(alignments[alignmentIdx])-1): |
495 #Get closest point on spline | 468 #Get closest point on alignment |
496 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]) | 469 closestPoint = ppldb2p(p.x,p.y,alignments[alignmentIdx][alignment_p][0],alignments[alignmentIdx][alignment_p][1],alignments[alignmentIdx][alignment_p+1][0],alignments[alignmentIdx][alignment_p+1][1]) |
497 if closestPoint is None: | 470 if closestPoint is None: |
498 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p)) | 471 print('Error: Alignment {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(alignmentIdx, alignment_p)) |
499 return None | 472 return None |
500 # check if the projected point is in between the current segment of the alignment bounds | 473 # check if the projected point is in between the current segment of the alignment bounds |
501 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): | 474 if utils.inBetween(alignments[alignmentIdx][alignment_p][0], alignments[alignmentIdx][alignment_p+1][0], closestPoint.x) and utils.inBetween(alignments[alignmentIdx][alignment_p][1], alignments[alignmentIdx][alignment_p+1][1], closestPoint.y): |
502 offsetY = Point.distanceNorm2(closestPoint, p) | 475 offsetY = Point.distanceNorm2(closestPoint, p) |
503 if offsetY < minOffsetY: | 476 if offsetY < minOffsetY: |
504 minOffsetY = offsetY | 477 minOffsetY = offsetY |
505 snappedSplineIdx = splineIdx | 478 snappedAlignmentIdx = alignmentIdx |
506 snappedSplineLeadingPoint = spline_p | 479 snappedAlignmentLeadingPoint = alignment_p |
507 snappedPoint = Point(closestPoint.x, closestPoint.y) | 480 snappedPoint = Point(closestPoint.x, closestPoint.y) |
508 #Jump loop if significantly close | 481 #Jump loop if significantly close |
509 if offsetY < goodEnoughSplineDistance: | 482 if offsetY < goodEnoughAlignmentDistance: |
510 break | 483 break |
511 | 484 |
512 #Get sub-segment distance | 485 #Get sub-segment distance |
513 if minOffsetY != float('inf'): | 486 if minOffsetY != float('inf'): |
514 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint]) | 487 subsegmentDistance = Point.distanceNorm2(snappedPoint, alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint]) |
515 #Get cumulative alignment distance (total segment distance) | 488 #Get cumulative alignment distance (total segment distance) |
516 splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance | 489 alignmentDistanceS = alignments[snappedAlignmentIdx].getCumulativeDistance(snappedAlignmentLeadingPoint) + subsegmentDistance |
517 orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal() | 490 orthogonalAlignmentVector = (alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint+1]-alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint]).orthogonal() |
518 offsetVector = p-snappedPoint | 491 offsetVector = p-snappedPoint |
519 if Point.dot(orthogonalSplineVector, offsetVector) < 0: | 492 if Point.dot(orthogonalAlignmentVector, offsetVector) < 0: |
520 minOffsetY = -minOffsetY | 493 minOffsetY = -minOffsetY |
521 return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] | 494 return [snappedAlignmentIdx, snappedAlignmentLeadingPoint, snappedPoint, subsegmentDistance, alignmentDistanceS, minOffsetY] |
522 else: | 495 else: |
523 print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p)) | 496 print('Offset for point {} is infinite (check with prepareAlignments if some alignment segments are aligned with axes)'.format(p)) |
524 return None | 497 return None |
525 | 498 |
526 def getXYfromSY(s, y, splineNum, splines, mode = 0): | 499 def getXYfromSY(s, y, alignmentNum, alignments): |
527 ''' Find X,Y coordinate from S,Y data. | 500 ''' Find X,Y coordinate from S,Y data. |
528 if mode = 0 : return Snapped X,Y | 501 if mode = 0 : return Snapped X,Y |
529 if mode !=0 : return Real X,Y | 502 if mode !=0 : return Real X,Y |
530 ''' | 503 ''' |
531 | 504 alignment = alignments[alignmentNum] |
532 #(buckle in, it gets ugly from here on out) | 505 i = 1 |
533 ss_spline_d = subsec_spline_dist(splines) | 506 while s > alignment.getCumulativeDistance(i) and i < len(alignment): |
534 | 507 i += 1 |
535 #Find subsegment | 508 if i < len(alignment): |
536 snapped_x = None | 509 d = s - alignment.getCumulativeDistance(i-1) # distance on subsegment |
537 snapped_y = None | 510 #Get difference vector and then snap |
538 for spline_ss_index in range(len(ss_spline_d[splineNum][1])): | 511 dv = alignment[i] - alignment[i-1] |
539 if(s < ss_spline_d[splineNum][1][spline_ss_index]): | 512 magnitude = dv.norm2() |
540 ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1] | 513 normalizedV = dv.divide(magnitude) |
541 #Get normal vector and then snap | 514 #snapped = alignment[i-1] + normalizedV*d # snapped point coordinate along alignment |
542 vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0]) | 515 # add offset finally |
543 vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1]) | 516 orthoNormalizedV = normalizedV.orthogonal() |
544 magnitude = sqrt(vector_l_x**2 + vector_l_y**2) | 517 return alignment[i-1] + normalizedV*d + orthoNormalizedV*y |
545 n_vector_x = vector_l_x/magnitude | |
546 n_vector_y = vector_l_y/magnitude | |
547 snapped_x = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x | |
548 snapped_y = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y | |
549 | |
550 #Real values (including orthogonal projection of y)) | |
551 real_x = snapped_x - y*n_vector_y | |
552 real_y = snapped_y + y*n_vector_x | |
553 break | |
554 | |
555 if mode == 0 or (not snapped_x): | |
556 if(not snapped_x): | |
557 snapped_x = splines[splineNum][-1][0] | |
558 snapped_y = splines[splineNum][-1][1] | |
559 return [snapped_x,snapped_y] | |
560 else: | 518 else: |
561 return [real_x,real_y] | 519 print('Curvilinear point {} is past the end of the alignement'.format((s, y, alignmentNum))) |
562 | 520 return None |
563 | 521 |
522 | |
564 class NormAngle(object): | 523 class NormAngle(object): |
565 '''Alternate encoding of a point, by its norm and orientation''' | 524 '''Alternate encoding of a point, by its norm and orientation''' |
566 | 525 |
567 def __init__(self, norm, angle): | 526 def __init__(self, norm, angle): |
568 self.norm = norm | 527 self.norm = norm |
693 p0 = Point(p.x, p.y) | 652 p0 = Point(p.x, p.y) |
694 t.addPosition(p0) | 653 t.addPosition(p0) |
695 for i in range(nPoints-1): | 654 for i in range(nPoints-1): |
696 p0 += v | 655 p0 += v |
697 t.addPosition(p0) | 656 t.addPosition(p0) |
698 return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints]) | 657 return t |
699 | 658 |
700 @staticmethod | 659 @staticmethod |
701 def load(line1, line2): | 660 def load(line1, line2): |
702 return Trajectory([[float(n) for n in line1.split(' ')], | 661 return Trajectory([[float(n) for n in line1.split(' ')], |
703 [float(n) for n in line2.split(' ')]]) | 662 [float(n) for n in line2.split(' ')]]) |
1088 S.append(S[-1]+v) | 1047 S.append(S[-1]+v) |
1089 Y = [y]*nPoints | 1048 Y = [y]*nPoints |
1090 lanes = [lane]*nPoints | 1049 lanes = [lane]*nPoints |
1091 return CurvilinearTrajectory(S, Y, lanes) | 1050 return CurvilinearTrajectory(S, Y, lanes) |
1092 | 1051 |
1052 @staticmethod | |
1053 def fromTrajectoryProjection(t, alignments, halfWidth = 3): | |
1054 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' | |
1055 (curvilinearPositions instance) which holds information about the | |
1056 curvilinear coordinates using alignment metadata. | |
1057 From Paul St-Aubin's PVA tools | |
1058 ====== | |
1059 | |
1060 Input: | |
1061 ====== | |
1062 alignments = a list of alignments, where each alignment is a list of | |
1063 points (class Point). | |
1064 halfWidth = moving average window (in points) in which to smooth | |
1065 lane changes. As per tools_math.cat_mvgavg(), this term | |
1066 is a search *radius* around the center of the window. | |
1067 | |
1068 ''' | |
1069 curvilinearPositions = CurvilinearTrajectory() | |
1070 | |
1071 #For each point | |
1072 for i in range(int(t.length())): | |
1073 result = getSYfromXY(t[i], alignments) | |
1074 | |
1075 # Error handling | |
1076 if(result is None): | |
1077 print('Warning: trajectory at point {} {} has alignment errors (alignment snapping)\nCurvilinear trajectory could not be computed'.format(i, t[i])) | |
1078 else: | |
1079 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result | |
1080 curvilinearPositions.addPositionSYL(S, Y, align) | |
1081 | |
1082 ## Go back through points and correct lane | |
1083 #Run through objects looking for outlier point | |
1084 smoothed_lanes = utils.filterCategoricalMovingWindow(curvilinearPositions.getLanes(), halfWidth) | |
1085 ## Recalculate projected point to new lane | |
1086 lanes = curvilinearPositions.getLanes() | |
1087 if(lanes != smoothed_lanes): | |
1088 for i in range(len(lanes)): | |
1089 if(lanes[i] != smoothed_lanes[i]): | |
1090 result = getSYfromXY(t[i],[alignments[smoothed_lanes[i]]]) | |
1091 | |
1092 # Error handling | |
1093 if(result is None): | |
1094 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. | |
1095 print(' Warning: trajectory at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(i, t[i])) | |
1096 else: | |
1097 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result | |
1098 curvilinearPositions.setPosition(i, S, Y, align) | |
1099 return curvilinearPositions | |
1100 | |
1093 def __getitem__(self,i): | 1101 def __getitem__(self,i): |
1094 if isinstance(i, int): | 1102 if isinstance(i, int): |
1095 return [self.positions[0][i], self.positions[1][i], self.lanes[i]] | 1103 return [self.positions[0][i], self.positions[1][i], self.lanes[i]] |
1096 else: | 1104 else: |
1097 raise TypeError("Invalid argument type.") | 1105 raise TypeError("Invalid argument type.") |
1218 velocities.addPosition(Point.agg(vels, aggFunc)) | 1226 velocities.addPosition(Point.agg(vels, aggFunc)) |
1219 return inter, positions, velocities | 1227 return inter, positions, velocities |
1220 | 1228 |
1221 @staticmethod | 1229 @staticmethod |
1222 def generate(num, p, v, timeInterval): | 1230 def generate(num, p, v, timeInterval): |
1223 positions, velocities = Trajectory.generate(p, v, int(timeInterval.length())) | 1231 nPoints = int(timeInterval.length()) |
1224 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = velocities) | 1232 positions = Trajectory.generate(p, v, nPoints) |
1233 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints])) | |
1225 | 1234 |
1226 def updatePositions(self): | 1235 def updatePositions(self): |
1227 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) | 1236 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) |
1228 | 1237 |
1229 @staticmethod | 1238 @staticmethod |
1606 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): | 1615 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): |
1607 '''Predicts the position of object at instant+deltaT, | 1616 '''Predicts the position of object at instant+deltaT, |
1608 at constant speed''' | 1617 at constant speed''' |
1609 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) | 1618 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) |
1610 | 1619 |
1611 def projectCurvilinear(self, alignments, ln_mv_av_win=3): | 1620 def projectCurvilinear(self, alignments, halfWidth = 3): |
1612 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' | 1621 self.curvilinearPositions = CurvilinearTrajectory.fromTrajectoryProjection(self.getPositions(), alignments, halfWidth) |
1613 (curvilinearPositions instance) which holds information about the | |
1614 curvilinear coordinates using alignment metadata. | |
1615 From Paul St-Aubin's PVA tools | |
1616 ====== | |
1617 | |
1618 Input: | |
1619 ====== | |
1620 alignments = a list of alignments, where each alignment is a list of | |
1621 points (class Point). | |
1622 ln_mv_av_win = moving average window (in points) in which to smooth | |
1623 lane changes. As per tools_math.cat_mvgavg(), this term | |
1624 is a search *radius* around the center of the window. | |
1625 | |
1626 ''' | |
1627 | |
1628 self.curvilinearPositions = CurvilinearTrajectory() | |
1629 | |
1630 #For each point | |
1631 for i in range(int(self.length())): | |
1632 result = getSYfromXY(self.getPositionAt(i), alignments) | |
1633 | |
1634 # Error handling | |
1635 if(result is None): | |
1636 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i))) | |
1637 else: | |
1638 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result | |
1639 self.curvilinearPositions.addPositionSYL(S, Y, align) | |
1640 | |
1641 ## Go back through points and correct lane | |
1642 #Run through objects looking for outlier point | |
1643 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) | |
1644 ## Recalculate projected point to new lane | |
1645 lanes = self.curvilinearPositions.getLanes() | |
1646 if(lanes != smoothed_lanes): | |
1647 for i in range(len(lanes)): | |
1648 if(lanes[i] != smoothed_lanes[i]): | |
1649 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]]) | |
1650 | |
1651 # Error handling | |
1652 if(result is None): | |
1653 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. | |
1654 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i))) | |
1655 else: | |
1656 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result | |
1657 self.curvilinearPositions.setPosition(i, S, Y, align) | |
1658 | 1622 |
1659 def computeSmoothTrajectory(self, minCommonIntervalLength): | 1623 def computeSmoothTrajectory(self, minCommonIntervalLength): |
1660 '''Computes the trajectory as the mean of all features | 1624 '''Computes the trajectory as the mean of all features |
1661 if a feature exists, its position is | 1625 if a feature exists, its position is |
1662 | 1626 |