nsaunier/traffic-intelligence: trafficintelligence/moving.py comparison

comparison trafficintelligence/moving.py @ 1086:8734742c08c0

major refactoring of curvilinear trajectory projections

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 16 Oct 2018 12:46:29 -0400
parents	7853106677b7
children	c96388c696ac

comparison

equal deleted inserted replaced

-:7853106677b7
+:8734742c08c0
 def projectLocal(self, v, clockwise = True):
 'Projects point projected on v, v.orthogonal()'
 e1 = v/v.norm2()
 e2 = e1.orthogonal(clockwise)
-return Point(Point.dot(self, e1), Point.doc(self, e2))
+return Point(Point.dot(self, e1), Point.dot(self, e2))
 def rotate(self, theta):
 return Point(self.x*cos(theta)-self.y*sin(theta), self.x*sin(theta)+self.y*cos(theta))
 def __mul__(self, alpha):
 prepared_polygon = prep(polygon)
 return list(filter(prepared_polygon.contains, points))
 # Functions for coordinate transformation
 # From Paul St-Aubin's PVA tools
-def subsec_spline_dist(splines):
+def prepareAlignments(alignments):
-''' Prepare list of spline subsegments from a spline list.
+'''Prepares alignments (list of splines, each typically represented as a Trajectory)
+- computes cumulative distances
-Output:
+- approximates slope singularity by giving some slope roundoff (account for roundoff error)'''
-=======
+for alignment in alignments:
-ss_spline_d[spline #][mode][station]
+alignment.computeCumulativeDistances()
+p1 = alignment[0]
-where:
+for i in range(len(alignment)-1):
-mode=0: incremental distance
+p2 = alignment[i+1]
-mode=1: cumulative distance
-mode=2: cumulative distance with trailing distance
-'''
-ss_spline_d = []
-#Prepare subsegment distances
-for spline in range(len(splines)):
-ss_spline_d[spline]=[]#.append([[],[],[]])
-ss_spline_d[spline].append(zeros(len(splines[spline])-1))  #Incremental distance
-ss_spline_d[spline].append(zeros(len(splines[spline])-1))  #Cumulative distance
-ss_spline_d[spline].append(zeros(len(splines[spline])))  #Cumulative distance with trailing distance
-for spline_p in range(len(splines[spline])):
-if spline_p > (len(splines[spline]) - 2):
-break
-ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1])
-ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p])
-ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p])
-ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1]
-return ss_spline_d
-def prepareSplines(splines):
-'Approximates slope singularity by giving some slope roundoff; account for roundoff error'
-for spline in splines:
-p1 = spline[0]
-for i in range(len(spline)-1):
-p2 = spline[i+1]
 if(round(p1.x, 10) == round(p2.x, 10)):
 p2.x += 0.0000000001
 if(round(p1.y, 10) == round(p2.y, 10)):
 p2.y += 0.0000000001
 p1 = p2
 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
 import pdb; pdb.set_trace()
 return Point(X,Y)
-def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
+def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
-''' Snap a point p to its nearest subsegment of it's nearest spline (from the list splines).
+''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments).
-A spline is a list of points (class Point), most likely a trajectory.
+A alignment is a list of points (class Point), most likely a trajectory.
 Output:
 =======
-[spline index,
+[alignment index,
 subsegment leading point index,
 snapped point,
 subsegment distance,
-spline distance,
+alignment distance,
 orthogonal point offset]
 or None
 '''
 minOffsetY = float('inf')
-#For each spline
+#For each alignment
-for splineIdx in range(len(splines)):
+for alignmentIdx in range(len(alignments)):
-#For each spline point index
+#For each alignment point index
-for spline_p in range(len(splines[splineIdx])-1):
+for alignment_p in range(len(alignments[alignmentIdx])-1):
-#Get closest point on spline
+#Get closest point on alignment
-closestPoint = ppldb2p(p.x,p.y,splines[splineIdx][spline_p][0],splines[splineIdx][spline_p][1],splines[splineIdx][spline_p+1][0],splines[splineIdx][spline_p+1][1])
+closestPoint = ppldb2p(p.x,p.y,alignments[alignmentIdx][alignment_p][0],alignments[alignmentIdx][alignment_p][1],alignments[alignmentIdx][alignment_p+1][0],alignments[alignmentIdx][alignment_p+1][1])
 if closestPoint is None:
-print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p))
+print('Error: Alignment {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(alignmentIdx, alignment_p))
 return None
 # check if the projected point is in between the current segment of the alignment bounds
-if utils.inBetween(splines[splineIdx][spline_p][0], splines[splineIdx][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[splineIdx][spline_p][1], splines[splineIdx][spline_p+1][1], closestPoint.y):
+if utils.inBetween(alignments[alignmentIdx][alignment_p][0], alignments[alignmentIdx][alignment_p+1][0], closestPoint.x) and utils.inBetween(alignments[alignmentIdx][alignment_p][1], alignments[alignmentIdx][alignment_p+1][1], closestPoint.y):
 offsetY = Point.distanceNorm2(closestPoint, p)
 if offsetY < minOffsetY:
 minOffsetY = offsetY
-snappedSplineIdx = splineIdx
+snappedAlignmentIdx = alignmentIdx
-snappedSplineLeadingPoint = spline_p
+snappedAlignmentLeadingPoint = alignment_p
 snappedPoint = Point(closestPoint.x, closestPoint.y)
 #Jump loop if significantly close
-if offsetY < goodEnoughSplineDistance:
+if offsetY < goodEnoughAlignmentDistance:
 break
 #Get sub-segment distance
 if minOffsetY != float('inf'):
-subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint])
+subsegmentDistance = Point.distanceNorm2(snappedPoint, alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint])
 #Get cumulative alignment distance (total segment distance)
-splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
+alignmentDistanceS = alignments[snappedAlignmentIdx].getCumulativeDistance(snappedAlignmentLeadingPoint) + subsegmentDistance
-orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal()
+orthogonalAlignmentVector = (alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint+1]-alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint]).orthogonal()
 offsetVector = p-snappedPoint
-if Point.dot(orthogonalSplineVector, offsetVector) < 0:
+if Point.dot(orthogonalAlignmentVector, offsetVector) < 0:
 minOffsetY = -minOffsetY
-return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
+return [snappedAlignmentIdx, snappedAlignmentLeadingPoint, snappedPoint, subsegmentDistance, alignmentDistanceS, minOffsetY]
 else:
-print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p))
+print('Offset for point {} is infinite (check with prepareAlignments if some alignment segments are aligned with axes)'.format(p))
 return None
-def getXYfromSY(s, y, splineNum, splines, mode = 0):
+def getXYfromSY(s, y, alignmentNum, alignments):
 ''' Find X,Y coordinate from S,Y data.
 if mode = 0 : return Snapped X,Y
 if mode !=0 : return Real X,Y
 '''
+alignment = alignments[alignmentNum]
-#(buckle in, it gets ugly from here on out)
+i = 1
-ss_spline_d = subsec_spline_dist(splines)
+while s > alignment.getCumulativeDistance(i) and i < len(alignment):
+i += 1
-#Find subsegment
+if i < len(alignment):
-snapped_x = None
+d = s - alignment.getCumulativeDistance(i-1) # distance on subsegment
-snapped_y = None
+#Get difference vector and then snap
-for spline_ss_index in range(len(ss_spline_d[splineNum][1])):
+dv = alignment[i] - alignment[i-1]
-if(s < ss_spline_d[splineNum][1][spline_ss_index]):
+magnitude  = dv.norm2()
-ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1]
+normalizedV = dv.divide(magnitude)
-#Get normal vector and then snap
+#snapped = alignment[i-1] + normalizedV*d # snapped point coordinate along alignment
-vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0])
+# add offset finally
-vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1])
+orthoNormalizedV = normalizedV.orthogonal()
-magnitude  = sqrt(vector_l_x**2 + vector_l_y**2)
+return alignment[i-1] + normalizedV*d + orthoNormalizedV*y
-n_vector_x = vector_l_x/magnitude
-n_vector_y = vector_l_y/magnitude
-snapped_x  = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x
-snapped_y  = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y
-#Real values (including orthogonal projection of y))
-real_x = snapped_x - y*n_vector_y
-real_y = snapped_y + y*n_vector_x
-break
-if mode == 0 or (not snapped_x):
-if(not snapped_x):
-snapped_x = splines[splineNum][-1][0]
-snapped_y = splines[splineNum][-1][1]
-return [snapped_x,snapped_y]
 else:
-return [real_x,real_y]
+print('Curvilinear point {} is past the end of the alignement'.format((s, y, alignmentNum)))
+return None
 class NormAngle(object):
 '''Alternate encoding of a point, by its norm and orientation'''
 def __init__(self, norm, angle):
 self.norm = norm
 p0 = Point(p.x, p.y)
 t.addPosition(p0)
 for i in range(nPoints-1):
 p0 += v
 t.addPosition(p0)
-return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints])
+return t
 @staticmethod
 def load(line1, line2):
 return Trajectory([[float(n) for n in line1.split(' ')],
 [float(n) for n in line2.split(' ')]])
 S.append(S[-1]+v)
 Y = [y]*nPoints
 lanes = [lane]*nPoints
 return CurvilinearTrajectory(S, Y, lanes)
+@staticmethod
+def fromTrajectoryProjection(t, alignments, halfWidth = 3):
+''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
+(curvilinearPositions instance) which holds information about the
+curvilinear coordinates using alignment metadata.
+From Paul St-Aubin's PVA tools
+======
+Input:
+======
+alignments   = a list of alignments, where each alignment is a list of
+points (class Point).
+halfWidth = moving average window (in points) in which to smooth
+lane changes. As per tools_math.cat_mvgavg(), this term
+is a search *radius* around the center of the window.
+'''
+curvilinearPositions = CurvilinearTrajectory()
+#For each point
+for i in range(int(t.length())):
+result = getSYfromXY(t[i], alignments)
+# Error handling
+if(result is None):
+print('Warning: trajectory at point {} {} has alignment errors (alignment snapping)\nCurvilinear trajectory could not be computed'.format(i, t[i]))
+else:
+[align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
+curvilinearPositions.addPositionSYL(S, Y, align)
+## Go back through points and correct lane
+#Run through objects looking for outlier point
+smoothed_lanes = utils.filterCategoricalMovingWindow(curvilinearPositions.getLanes(), halfWidth)
+## Recalculate projected point to new lane
+lanes = curvilinearPositions.getLanes()
+if(lanes != smoothed_lanes):
+for i in range(len(lanes)):
+if(lanes[i] != smoothed_lanes[i]):
+result = getSYfromXY(t[i],[alignments[smoothed_lanes[i]]])
+# Error handling
+if(result is None):
+## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
+print('    Warning: trajectory at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(i, t[i]))
+else:
+[align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
+curvilinearPositions.setPosition(i, S, Y, align)
+return curvilinearPositions
 def __getitem__(self,i):
 if isinstance(i, int):
 return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
 else:
 raise TypeError("Invalid argument type.")
 velocities.addPosition(Point.agg(vels, aggFunc))
 return inter, positions, velocities
 @staticmethod
 def generate(num, p, v, timeInterval):
-positions, velocities = Trajectory.generate(p, v, int(timeInterval.length()))
+nPoints = int(timeInterval.length())
-return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = velocities)
+positions = Trajectory.generate(p, v, nPoints)
+return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints]))
 def updatePositions(self):
 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
 @staticmethod
 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
 '''Predicts the position of object at instant+deltaT,
 at constant speed'''
 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration)
-def projectCurvilinear(self, alignments, ln_mv_av_win=3):
+def projectCurvilinear(self, alignments, halfWidth = 3):
-''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
+self.curvilinearPositions = CurvilinearTrajectory.fromTrajectoryProjection(self.getPositions(), alignments, halfWidth)
-(curvilinearPositions instance) which holds information about the
-curvilinear coordinates using alignment metadata.
-From Paul St-Aubin's PVA tools
-======
-Input:
-======
-alignments   = a list of alignments, where each alignment is a list of
-points (class Point).
-ln_mv_av_win = moving average window (in points) in which to smooth
-lane changes. As per tools_math.cat_mvgavg(), this term
-is a search *radius* around the center of the window.
-'''
-self.curvilinearPositions = CurvilinearTrajectory()
-#For each point
-for i in range(int(self.length())):
-result = getSYfromXY(self.getPositionAt(i), alignments)
-# Error handling
-if(result is None):
-print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i)))
-else:
-[align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
-self.curvilinearPositions.addPositionSYL(S, Y, align)
-## Go back through points and correct lane
-#Run through objects looking for outlier point
-smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win)
-## Recalculate projected point to new lane
-lanes = self.curvilinearPositions.getLanes()
-if(lanes != smoothed_lanes):
-for i in range(len(lanes)):
-if(lanes[i] != smoothed_lanes[i]):
-result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
-# Error handling
-if(result is None):
-## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
-print('    Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i)))
-else:
-[align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
-self.curvilinearPositions.setPosition(i, S, Y, align)
 def computeSmoothTrajectory(self, minCommonIntervalLength):
 '''Computes the trajectory as the mean of all features
 if a feature exists, its position is

Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1086:8734742c08c0