Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 578:fe4e9d2b807d

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