Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 579:05c927c6d3cf
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 |