diff python/moving.py @ 578:fe4e9d2b807d

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
line wrap: on
line diff
--- a/python/moving.py	Thu Aug 28 17:22:38 2014 -0400
+++ b/python/moving.py	Thu Aug 28 17:42:32 2014 -0400
@@ -387,8 +387,7 @@
         =======
         [spline index, 
         subsegment leading point index, 
-        snapped x coordinate, 
-        snapped y coordinate, 
+        snapped point, 
         subsegment distance, 
         spline distance,
         orthogonal point offset]
@@ -403,8 +402,9 @@
             if closestPoint == None:
                 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
                 return None
-            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): 
-                offsetY = Point.distanceNorm2(closestPoint, p)#utils.pointDistanceL2(p.x,p.y,X,Y)
+            # 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): 
+                offsetY = Point.distanceNorm2(closestPoint, p)
                 if offsetY < minOffsetY:
                     minOffsetY = offsetY
                     snappedSpline = spline
@@ -416,13 +416,10 @@
     #Get sub-segment distance
     if minOffsetY != float('inf'):
         subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
-        #subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y)
         #Get cumulative alignment distance (total segment distance)
         splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
         orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal()
-#Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1],
-#                                       splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x)
-        offsetVector = p-snappedPoint#Point(qx-snapped_x, qy-snapped_y)
+        offsetVector = p-snappedPoint
         if Point.dot(orthogonalSplineVector, offsetVector) < 0:
             minOffsetY = -minOffsetY
         return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
@@ -1086,7 +1083,6 @@
 
             Input:
             ======
-            objects      = list of objects supplied by Traffic Intelligence
             alignments   = a list of alignments, where each alignment is a list of
                            points, where each point is a list of coordinates, e.g.
                            alignments[alpha][1][x] is coordinate x for point number
@@ -1098,55 +1094,38 @@
                            lane changes. As per tools_math.cat_mvgavg(), this term
                            is a search *radius* around the center of the window.
 
-            Output: (objects, dropped_traj)
-            =======
-            objects        = modified list of objects
-            dropped_traj   = list of objects or object segments that were
-                             truncated. The format is identical to that of objects.
             '''
 
-        #if(not isinstance(objects, list)):
-        #    objects = [objects]
-        #    reset_objects = 1
-        #if(not isinstance(objects[0], TrafIntMoving.MovingObject)):
-        #    return objects, dropped_traj
-
-        #For each object
-        #for i in range(len(objects)):
         self.curvilinearPositions = CurvilinearTrajectory()
 
         #For each point
-        for point in range(len(self.getXCoordinates())):
-            result = getSYfromXY(self.getXCoordinates()[point],self.getYCoordinates()[point],alignments)
+        for p in self.getPositions():#xrange(int(self.length())):
+            result = getSYfromXY(p, alignments)
 
             # Error handling
             if(result == None):
-                print('Warning: trajectory {} at point {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), point))
-                #dropped_traj.append(self)
-                #self = None
-                #break
+                print('Warning: trajectory {} at point {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), p))
             else:
-                [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result
+                [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
                 self.curvilinearPositions.addPositionSYL(S, Y, align)
 
-        #if(self == None): continue
-
         ## 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
-        if(self.curvilinearPositions.getLanes() != smoothed_lanes):
-            for point in range(len(self.getXCoordinates())):
-                if(self.curvilinearPositions.getLanes()[point] != smoothed_lanes[point]):
-                    result = getSYfromXY(self.getXCoordinates()[point],self.getYCoordinates()[point],[alignments[smoothed_lanes[point]]])
+        lanes = self.curvilinearPositions.getLanes()
+        if(lanes != smoothed_lanes):
+            for i in range(int(self.length())):
+                if(lanes[i] != smoothed_lanes[i]):
+                    result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
 
                     # Error handling
                     if(result == None):
                         ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
-                        print('    Warning: trajectory '+str(i)+' at point '+str(point)+' has alignment errors during trajectory smoothing and will not be corrected.')
+                        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, snapped_x, snapped_y, subsegmentDistance, S, Y] = result
-                        self.curvilinearPositions.setPosition(point, S, Y, align)
+                        self.curvilinearPositions.setPosition(i, S, Y, align)
 
         #Resize objects
         # if(len(dropped_traj) > 0):