Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 574:e24eeb244698

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