Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 579:05c927c6d3cf

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
curvilinear projection seems operational
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 28 Aug 2014 23:50:30 -0400
parents fe4e9d2b807d
children 10e8a9f2bd9f
comparison
equal deleted inserted replaced
578:fe4e9d2b807d 579:05c927c6d3cf
377 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) 377 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
378 import pdb; pdb.set_trace() 378 import pdb; pdb.set_trace()
379 return Point(X,Y) 379 return Point(X,Y)
380 380
381 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): 381 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
382 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). 382 ''' 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.
383
384 To force snapping to a single spline, pass the spline alone through splines (e.g. splines=[splines[splineNum]]).
385 383
386 Output: 384 Output:
387 ======= 385 =======
388 [spline index, 386 [spline index,
389 subsegment leading point index, 387 subsegment leading point index,
1082 ====== 1080 ======
1083 1081
1084 Input: 1082 Input:
1085 ====== 1083 ======
1086 alignments = a list of alignments, where each alignment is a list of 1084 alignments = a list of alignments, where each alignment is a list of
1087 points, where each point is a list of coordinates, e.g. 1085 points (class Point).
1088 alignments[alpha][1][x] is coordinate x for point number
1089 1 of the spline that represents alignment alpha.
1090 alignments can also be a compatible object that mimics a
1091 3-dimensional list using the __getitem__() method as is
1092 the case in the PVAT specification.
1093 ln_mv_av_win = moving average window (in points) in which to smooth 1086 ln_mv_av_win = moving average window (in points) in which to smooth
1094 lane changes. As per tools_math.cat_mvgavg(), this term 1087 lane changes. As per tools_math.cat_mvgavg(), this term
1095 is a search *radius* around the center of the window. 1088 is a search *radius* around the center of the window.
1096 1089
1097 ''' 1090 '''
1098 1091
1099 self.curvilinearPositions = CurvilinearTrajectory() 1092 self.curvilinearPositions = CurvilinearTrajectory()
1100 1093
1101 #For each point 1094 #For each point
1102 for p in self.getPositions():#xrange(int(self.length())): 1095 for i in xrange(int(self.length())):
1103 result = getSYfromXY(p, alignments) 1096 result = getSYfromXY(self.getPositionAt(i), alignments)
1104 1097
1105 # Error handling 1098 # Error handling
1106 if(result == None): 1099 if(result == None):
1107 print('Warning: trajectory {} at point {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), p)) 1100 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i)))
1108 else: 1101 else:
1109 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result 1102 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
1110 self.curvilinearPositions.addPositionSYL(S, Y, align) 1103 self.curvilinearPositions.addPositionSYL(S, Y, align)
1111 1104
1112 ## Go back through points and correct lane 1105 ## Go back through points and correct lane
1113 #Run through objects looking for outlier point 1106 #Run through objects looking for outlier point
1114 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) 1107 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win)
1115 ## Recalculate projected point to new lane 1108 ## Recalculate projected point to new lane
1116 lanes = self.curvilinearPositions.getLanes() 1109 lanes = self.curvilinearPositions.getLanes()
1117 if(lanes != smoothed_lanes): 1110 if(lanes != smoothed_lanes):
1118 for i in range(int(self.length())): 1111 for i in xrange(len(lanes)):
1119 if(lanes[i] != smoothed_lanes[i]): 1112 if(lanes[i] != smoothed_lanes[i]):
1120 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]]) 1113 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
1121 1114
1122 # Error handling 1115 # Error handling
1123 if(result == None): 1116 if(result == None):
1124 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. 1117 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
1125 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i))) 1118 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i)))
1126 else: 1119 else:
1127 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result 1120 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
1128 self.curvilinearPositions.setPosition(i, S, Y, align) 1121 self.curvilinearPositions.setPosition(i, S, Y, align)
1129
1130 #Resize objects
1131 # if(len(dropped_traj) > 0):
1132 # objects = filter(None, objects)
1133 # if(verbose >= 2): print(' Filtering report: Trajectories dropped: '+str(len(dropped_traj)))
1134 #if(verbose >= 2): print(' Filtering report: Lane observation corrections per object: '+str(lane_readjustments/original_object_length))
1135
1136 #if(reset_objects and len(objects) > 0): return objects[0], dropped_traj
1137 #else: return objects, dropped_traj
1138
1139 1122
1140 def computeSmoothTrajectory(self, minCommonIntervalLength): 1123 def computeSmoothTrajectory(self, minCommonIntervalLength):
1141 '''Computes the trajectory as the mean of all features 1124 '''Computes the trajectory as the mean of all features
1142 if a feature exists, its position is 1125 if a feature exists, its position is
1143 1126