Mercurial Hosting > traffic-intelligence
diff python/moving.py @ 780:1b22d81ef5ff dev
cleaned and checked storage with functions for curvilinear trajectories
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Mon, 08 Feb 2016 12:24:26 -0500 |
| parents | 84420159c5f4 |
| children | 7c38250ddfc7 |
line wrap: on
line diff
--- a/python/moving.py Mon Feb 08 12:07:47 2016 -0500 +++ b/python/moving.py Mon Feb 08 12:24:26 2016 -0500 @@ -370,10 +370,10 @@ ss_spline_d = [] #Prepare subsegment distances for spline in range(len(splines)): - ss_spline_d.append([[],[],[]]) - ss_spline_d[spline][0] = zeros(len(splines[spline])-1) #Incremental distance - ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance - ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance + 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 @@ -385,6 +385,18 @@ 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 xrange(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 + def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): ''' Point-projection (Q) on line defined by 2 points (P0,P1). http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf @@ -393,10 +405,10 @@ return None try: #Approximate slope singularity by giving some slope roundoff; account for roundoff error - if(round(p0x, 10) == round(p1x, 10)): - p1x += 0.0000000001 - if(round(p0y, 10) == round(p1y, 10)): - p1y += 0.0000000001 + # if(round(p0x, 10) == round(p1x, 10)): + # p1x += 0.0000000001 + # if(round(p0y, 10) == round(p1y, 10)): + # p1y += 0.0000000001 #make the calculation Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x)) X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) @@ -407,8 +419,9 @@ return Point(X,Y) def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): - ''' 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. - + ''' 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. + Output: ======= [spline index, @@ -422,36 +435,38 @@ ''' minOffsetY = float('inf') #For each spline - for spline in range(len(splines)): + for splineIdx in range(len(splines)): #For each spline point index - for spline_p in range(len(splines[spline])-1): + for spline_p in range(len(splines[splineIdx])-1): #Get closest point on spline - 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]) + 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]) if closestPoint is None: - print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) + print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p)) return None - # check if the - 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): + # 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): offsetY = Point.distanceNorm2(closestPoint, p) if offsetY < minOffsetY: minOffsetY = offsetY - snappedSpline = spline + snappedSplineIdx = splineIdx snappedSplineLeadingPoint = spline_p snappedPoint = Point(closestPoint.x, closestPoint.y) #Jump loop if significantly close if offsetY < goodEnoughSplineDistance: break + #Get sub-segment distance if minOffsetY != float('inf'): - subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint]) + subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint]) #Get cumulative alignment distance (total segment distance) - splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance - orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal() + splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance + orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal() offsetVector = p-snappedPoint if Point.dot(orthogonalSplineVector, offsetVector) < 0: minOffsetY = -minOffsetY - return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] + return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] else: + print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p)) return None def getXYfromSY(s, y, splineNum, splines, mode = 0):