Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 574:e24eeb244698
first implementation of projection to curvilinear coordinates
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 13 Aug 2014 00:00:08 -0400 |
parents | cae4e5f3fe9f |
children | 13df64a9ff9d |
comparison
equal
deleted
inserted
replaced
573:cae4e5f3fe9f | 574:e24eeb244698 |
---|---|
409 snapped_y = Y | 409 snapped_y = Y |
410 #Jump loop if significantly close | 410 #Jump loop if significantly close |
411 if offsetY < goodEnoughSplineDistance: | 411 if offsetY < goodEnoughSplineDistance: |
412 break | 412 break |
413 #Get sub-segment distance | 413 #Get sub-segment distance |
414 subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) | 414 if minOffsetY != float('inf'): |
415 #Get total segment distance | 415 subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) |
416 splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance | 416 #Get total segment distance |
417 orthogonalSplineVector = Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1], | 417 splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance |
418 splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x) | 418 orthogonalSplineVector = Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1], |
419 offsetVector = Point(qx-snapped_x, qy-snapped_y) | 419 splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x) |
420 if Point.dot(orthogonalSplineVector, offsetVector) < 0: | 420 offsetVector = Point(qx-snapped_x, qy-snapped_y) |
421 minOffsetY = -minOffsetY | 421 if Point.dot(orthogonalSplineVector, offsetVector) < 0: |
422 return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, minOffsetY] | 422 minOffsetY = -minOffsetY |
423 | 423 return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, minOffsetY] |
424 else: | |
425 return None | |
426 | |
424 def getXYfromSY(s, y, splineNum, splines, mode = 0): | 427 def getXYfromSY(s, y, splineNum, splines, mode = 0): |
425 ''' Find X,Y coordinate from S,Y data. | 428 ''' Find X,Y coordinate from S,Y data. |
426 if mode = 0 : return Snapped X,Y | 429 if mode = 0 : return Snapped X,Y |
427 if mode !=0 : return Real X,Y | 430 if mode !=0 : return Real X,Y |
428 ''' | 431 ''' |
1035 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): | 1038 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): |
1036 '''Predicts the position of object at instant+deltaT, | 1039 '''Predicts the position of object at instant+deltaT, |
1037 at constant speed''' | 1040 at constant speed''' |
1038 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) | 1041 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) |
1039 | 1042 |
1040 def transformToCurvilinear(objects, alignments, ln_mv_av_win=3, verbose=0): | 1043 def projectCurvilinear(self, alignments, ln_mv_av_win=3): |
1041 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' | 1044 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' |
1042 (curvilinearPositions instance) which holds information about the | 1045 (curvilinearPositions instance) which holds information about the |
1043 curvilinear coordinates using alignment metadata. | 1046 curvilinear coordinates using alignment metadata. |
1044 From Paul St-Aubin's PVA tools | 1047 From Paul St-Aubin's PVA tools |
1045 ====== | 1048 ====== |
1062 ======= | 1065 ======= |
1063 objects = modified list of objects | 1066 objects = modified list of objects |
1064 dropped_traj = list of objects or object segments that were | 1067 dropped_traj = list of objects or object segments that were |
1065 truncated. The format is identical to that of objects. | 1068 truncated. The format is identical to that of objects. |
1066 ''' | 1069 ''' |
1067 if(len(objects) <= 0): return None, None | |
1068 if(verbose >= 2): | |
1069 print(' -------------') | |
1070 print(' Transforming coordinates...') | |
1071 | |
1072 lane_readjustments = 0 | |
1073 dropped_traj = [] | |
1074 original_object_length = len(objects) | |
1075 reset_objects = 0 | |
1076 | 1070 |
1077 #if(not isinstance(objects, list)): | 1071 #if(not isinstance(objects, list)): |
1078 # objects = [objects] | 1072 # objects = [objects] |
1079 # reset_objects = 1 | 1073 # reset_objects = 1 |
1080 #if(not isinstance(objects[0], TrafIntMoving.MovingObject)): | 1074 #if(not isinstance(objects[0], TrafIntMoving.MovingObject)): |
1081 # return objects, dropped_traj | 1075 # return objects, dropped_traj |
1082 | 1076 |
1083 #For each object | 1077 #For each object |
1084 for i in range(len(objects)): | 1078 #for i in range(len(objects)): |
1085 objects[i].curvilinearPositions = CurvilinearTrajectory([],[],[]) | 1079 self.curvilinearPositions = CurvilinearTrajectory() |
1086 | 1080 |
1087 #For each point | 1081 #For each point |
1088 for point in range(len(objects[i].getXCoordinates())): | 1082 for point in range(len(self.getXCoordinates())): |
1089 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = PvaTools.Geo.getSYfromXY(objects[i].getXCoordinates()[point],objects[i].getYCoordinates()[point],alignments) | 1083 result = getSYfromXY(self.getXCoordinates()[point],self.getYCoordinates()[point],alignments) |
1090 | 1084 |
1091 # Error handling | 1085 # Error handling |
1092 if(align is False): | 1086 if(result == None): |
1093 if(verbose >= 2): print(' Warning: trajectory '+str(i)+' at point '+str(point)+' has alignment errors (spline snapping)') | 1087 print('Warning: trajectory {} at point {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), point)) |
1094 dropped_traj.append(objects[i]) | 1088 #dropped_traj.append(self) |
1095 objects[i] = None | 1089 #self = None |
1096 break | 1090 #break |
1097 else: objects[i].curvilinearPositions.addPosition(S, Y, align) | 1091 else: |
1098 | 1092 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result |
1099 if(objects[i] == None): continue | 1093 self.curvilinearPositions.addPositionSYL(S, Y, align) |
1100 | 1094 |
1101 ## Go back through points and correct lane | 1095 #if(self == None): continue |
1102 #Run through objects looking for outlier point | 1096 |
1103 smoothed_lanes = PvaTools.Math.cat_mvgavg(objects[i].curvilinearPositions.getLanes(),window=ln_mv_av_win) | 1097 ## Go back through points and correct lane |
1104 ## Recalculate smoothed lanes | 1098 #Run through objects looking for outlier point |
1105 if(objects[i].curvilinearPositions.getLanes() != smoothed_lanes): | 1099 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) |
1106 for point in range(len(objects[i].getXCoordinates())): | 1100 ## Recalculate projected point to new lane |
1107 if(objects[i].curvilinearPositions.getLanes()[point] != smoothed_lanes[point]): | 1101 if(self.curvilinearPositions.getLanes() != smoothed_lanes): |
1108 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = PvaTools.Geo.getSYfromXY(objects[i].getXCoordinates()[point],objects[i].getYCoordinates()[point],[alignments[smoothed_lanes[point]]]) | 1102 for point in range(len(self.getXCoordinates())): |
1109 | 1103 if(self.curvilinearPositions.getLanes()[point] != smoothed_lanes[point]): |
1110 # Error handling | 1104 result = getSYfromXY(self.getXCoordinates()[point],self.getYCoordinates()[point],[alignments[smoothed_lanes[point]]]) |
1111 if(align is False): | 1105 |
1112 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. | 1106 # Error handling |
1113 if(verbose >= 4): print(' Warning: trajectory '+str(i)+' at point '+str(point)+' has alignment errors during trajectory smoothing and will not be corrected.') | 1107 if(result == None): |
1114 else: | 1108 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. |
1115 objects[i].curvilinearPositions.setPosition(point, S, Y, align) | 1109 print(' Warning: trajectory '+str(i)+' at point '+str(point)+' has alignment errors during trajectory smoothing and will not be corrected.') |
1110 else: | |
1111 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result | |
1112 self.curvilinearPositions.setPosition(point, S, Y, align) | |
1116 | 1113 |
1117 #Resize objects | 1114 #Resize objects |
1118 if(len(dropped_traj) > 0): | 1115 # if(len(dropped_traj) > 0): |
1119 objects = filter(None, objects) | 1116 # objects = filter(None, objects) |
1120 if(verbose >= 2): print(' Filtering report: Trajectories dropped: '+str(len(dropped_traj))) | 1117 # if(verbose >= 2): print(' Filtering report: Trajectories dropped: '+str(len(dropped_traj))) |
1121 if(verbose >= 2): print(' Filtering report: Lane observation corrections per object: '+str(lane_readjustments/original_object_length)) | 1118 #if(verbose >= 2): print(' Filtering report: Lane observation corrections per object: '+str(lane_readjustments/original_object_length)) |
1122 | 1119 |
1123 if(reset_objects and len(objects) > 0): return objects[0], dropped_traj | 1120 #if(reset_objects and len(objects) > 0): return objects[0], dropped_traj |
1124 else: return objects, dropped_traj | 1121 #else: return objects, dropped_traj |
1125 | 1122 |
1126 | 1123 |
1127 def computeSmoothTrajectory(self, minCommonIntervalLength): | 1124 def computeSmoothTrajectory(self, minCommonIntervalLength): |
1128 '''Computes the trajectory as the mean of all features | 1125 '''Computes the trajectory as the mean of all features |
1129 if a feature exists, its position is | 1126 if a feature exists, its position is |