Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 578:fe4e9d2b807d
finalizing transformcoordinates for each object
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 28 Aug 2014 17:42:32 -0400 |
parents | d0abd2ee17b9 |
children | 05c927c6d3cf |
comparison
equal
deleted
inserted
replaced
577:d0abd2ee17b9 | 578:fe4e9d2b807d |
---|---|
385 | 385 |
386 Output: | 386 Output: |
387 ======= | 387 ======= |
388 [spline index, | 388 [spline index, |
389 subsegment leading point index, | 389 subsegment leading point index, |
390 snapped x coordinate, | 390 snapped point, |
391 snapped y coordinate, | |
392 subsegment distance, | 391 subsegment distance, |
393 spline distance, | 392 spline distance, |
394 orthogonal point offset] | 393 orthogonal point offset] |
395 ''' | 394 ''' |
396 minOffsetY = float('inf') | 395 minOffsetY = float('inf') |
401 #Get closest point on spline | 400 #Get closest point on spline |
402 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]) | 401 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]) |
403 if closestPoint == None: | 402 if closestPoint == None: |
404 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) | 403 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) |
405 return None | 404 return None |
406 if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], p.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], p.y): | 405 # check if the |
407 offsetY = Point.distanceNorm2(closestPoint, p)#utils.pointDistanceL2(p.x,p.y,X,Y) | 406 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): |
407 offsetY = Point.distanceNorm2(closestPoint, p) | |
408 if offsetY < minOffsetY: | 408 if offsetY < minOffsetY: |
409 minOffsetY = offsetY | 409 minOffsetY = offsetY |
410 snappedSpline = spline | 410 snappedSpline = spline |
411 snappedSplineLeadingPoint = spline_p | 411 snappedSplineLeadingPoint = spline_p |
412 snappedPoint = Point(closestPoint.x, closestPoint.y) | 412 snappedPoint = Point(closestPoint.x, closestPoint.y) |
414 if offsetY < goodEnoughSplineDistance: | 414 if offsetY < goodEnoughSplineDistance: |
415 break | 415 break |
416 #Get sub-segment distance | 416 #Get sub-segment distance |
417 if minOffsetY != float('inf'): | 417 if minOffsetY != float('inf'): |
418 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint]) | 418 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint]) |
419 #subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) | |
420 #Get cumulative alignment distance (total segment distance) | 419 #Get cumulative alignment distance (total segment distance) |
421 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance | 420 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance |
422 orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal() | 421 orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal() |
423 #Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1], | 422 offsetVector = p-snappedPoint |
424 # splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x) | |
425 offsetVector = p-snappedPoint#Point(qx-snapped_x, qy-snapped_y) | |
426 if Point.dot(orthogonalSplineVector, offsetVector) < 0: | 423 if Point.dot(orthogonalSplineVector, offsetVector) < 0: |
427 minOffsetY = -minOffsetY | 424 minOffsetY = -minOffsetY |
428 return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] | 425 return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] |
429 else: | 426 else: |
430 return None | 427 return None |
1084 From Paul St-Aubin's PVA tools | 1081 From Paul St-Aubin's PVA tools |
1085 ====== | 1082 ====== |
1086 | 1083 |
1087 Input: | 1084 Input: |
1088 ====== | 1085 ====== |
1089 objects = list of objects supplied by Traffic Intelligence | |
1090 alignments = a list of alignments, where each alignment is a list of | 1086 alignments = a list of alignments, where each alignment is a list of |
1091 points, where each point is a list of coordinates, e.g. | 1087 points, where each point is a list of coordinates, e.g. |
1092 alignments[alpha][1][x] is coordinate x for point number | 1088 alignments[alpha][1][x] is coordinate x for point number |
1093 1 of the spline that represents alignment alpha. | 1089 1 of the spline that represents alignment alpha. |
1094 alignments can also be a compatible object that mimics a | 1090 alignments can also be a compatible object that mimics a |
1096 the case in the PVAT specification. | 1092 the case in the PVAT specification. |
1097 ln_mv_av_win = moving average window (in points) in which to smooth | 1093 ln_mv_av_win = moving average window (in points) in which to smooth |
1098 lane changes. As per tools_math.cat_mvgavg(), this term | 1094 lane changes. As per tools_math.cat_mvgavg(), this term |
1099 is a search *radius* around the center of the window. | 1095 is a search *radius* around the center of the window. |
1100 | 1096 |
1101 Output: (objects, dropped_traj) | |
1102 ======= | |
1103 objects = modified list of objects | |
1104 dropped_traj = list of objects or object segments that were | |
1105 truncated. The format is identical to that of objects. | |
1106 ''' | 1097 ''' |
1107 | 1098 |
1108 #if(not isinstance(objects, list)): | |
1109 # objects = [objects] | |
1110 # reset_objects = 1 | |
1111 #if(not isinstance(objects[0], TrafIntMoving.MovingObject)): | |
1112 # return objects, dropped_traj | |
1113 | |
1114 #For each object | |
1115 #for i in range(len(objects)): | |
1116 self.curvilinearPositions = CurvilinearTrajectory() | 1099 self.curvilinearPositions = CurvilinearTrajectory() |
1117 | 1100 |
1118 #For each point | 1101 #For each point |
1119 for point in range(len(self.getXCoordinates())): | 1102 for p in self.getPositions():#xrange(int(self.length())): |
1120 result = getSYfromXY(self.getXCoordinates()[point],self.getYCoordinates()[point],alignments) | 1103 result = getSYfromXY(p, alignments) |
1121 | 1104 |
1122 # Error handling | 1105 # Error handling |
1123 if(result == None): | 1106 if(result == None): |
1124 print('Warning: trajectory {} at point {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), point)) | 1107 print('Warning: trajectory {} at point {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), p)) |
1125 #dropped_traj.append(self) | |
1126 #self = None | |
1127 #break | |
1128 else: | 1108 else: |
1129 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result | 1109 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result |
1130 self.curvilinearPositions.addPositionSYL(S, Y, align) | 1110 self.curvilinearPositions.addPositionSYL(S, Y, align) |
1131 | |
1132 #if(self == None): continue | |
1133 | 1111 |
1134 ## Go back through points and correct lane | 1112 ## Go back through points and correct lane |
1135 #Run through objects looking for outlier point | 1113 #Run through objects looking for outlier point |
1136 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) | 1114 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) |
1137 ## Recalculate projected point to new lane | 1115 ## Recalculate projected point to new lane |
1138 if(self.curvilinearPositions.getLanes() != smoothed_lanes): | 1116 lanes = self.curvilinearPositions.getLanes() |
1139 for point in range(len(self.getXCoordinates())): | 1117 if(lanes != smoothed_lanes): |
1140 if(self.curvilinearPositions.getLanes()[point] != smoothed_lanes[point]): | 1118 for i in range(int(self.length())): |
1141 result = getSYfromXY(self.getXCoordinates()[point],self.getYCoordinates()[point],[alignments[smoothed_lanes[point]]]) | 1119 if(lanes[i] != smoothed_lanes[i]): |
1120 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]]) | |
1142 | 1121 |
1143 # Error handling | 1122 # Error handling |
1144 if(result == None): | 1123 if(result == None): |
1145 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. | 1124 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. |
1146 print(' Warning: trajectory '+str(i)+' at point '+str(point)+' has alignment errors during trajectory smoothing and will not be corrected.') | 1125 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i))) |
1147 else: | 1126 else: |
1148 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result | 1127 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result |
1149 self.curvilinearPositions.setPosition(point, S, Y, align) | 1128 self.curvilinearPositions.setPosition(i, S, Y, align) |
1150 | 1129 |
1151 #Resize objects | 1130 #Resize objects |
1152 # if(len(dropped_traj) > 0): | 1131 # if(len(dropped_traj) > 0): |
1153 # objects = filter(None, objects) | 1132 # objects = filter(None, objects) |
1154 # if(verbose >= 2): print(' Filtering report: Trajectories dropped: '+str(len(dropped_traj))) | 1133 # if(verbose >= 2): print(' Filtering report: Trajectories dropped: '+str(len(dropped_traj))) |