Mercurial Hosting > traffic-intelligence

changeset 630:69a98f84f3eb

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
corrected major issue with pPET, only for CVDirect prediction for now
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 17 Feb 2015 02:21:31 +0100
parents 0a5e89d6fc62
children 2d1d33ae1c69
files python/events.py python/moving.py python/prediction.py
diffstat 3 files changed, 111 insertions(+), 61 deletions(-) [+]
line wrap: on
line diff
--- a/python/events.py	Tue Feb 17 00:56:47 2015 +0100
+++ b/python/events.py	Tue Feb 17 02:21:31 2015 +0100
@@ -196,7 +196,7 @@
             commonTimeInterval = timeInterval
         else:
             commonTimeInterval = self.timeInterval
-        self.collisionPoints, self.crossingZones = prediction.computeCrossingsCollisions(predictionParameters, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
+        self.collisionPoints, self.crossingZones = predictionParameters.computeCrossingsCollisions(self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
         for i, cp in self.collisionPoints.iteritems():
             TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cp)
         # add probability of collision, and probability of successful evasive action
--- a/python/moving.py	Tue Feb 17 00:56:47 2015 +0100
+++ b/python/moving.py	Tue Feb 17 02:21:31 2015 +0100
@@ -579,6 +579,15 @@
         else:
             return None
 
+def segmentLineIntersection(p1, p2, p3, p4):
+    '''Indicates if the line going through p1 and p2 intersects inside p3, p4'''
+    inter = intersection(p1, p2, p3, p4)
+    if inter != None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y)):
+        return inter
+    else:
+        return None
+        
+
 class Trajectory(object):
     '''Class for trajectories: temporal sequence of positions
 
@@ -808,7 +817,7 @@
             q1=self.__getitem__(i)
             q2=self.__getitem__(i+1)
             p = utils.segmentIntersection(q1, q2, p1, p2)
-            if p:
+            if p != None:
                 if q1.x != q2.x:
                     ratio = (p.x-q1.x)/(q2.x-q1.x)
                 elif q1.y != q2.y:
@@ -818,6 +827,28 @@
                 indices.append(i+ratio)
         return indices
 
+    def getLineIntersections(self, p1, p2):
+         '''Returns a list of the indices at which the trajectory 
+        intersects with the segment of extremities p1 and p2 
+        the list is empty if there is no crossing'''
+        indices = []
+        intersections = []
+
+        for i in xrange(self.length()-1):
+            q1=self.__getitem__(i)
+            q2=self.__getitem__(i+1)
+            p = utils.segmentLineIntersection(p1, p2, q1, q2)
+            if p != None:
+                if q1.x != q2.x:
+                    ratio = (p.x-q1.x)/(q2.x-q1.x)
+                elif q1.y != q2.y:
+                    ratio = (p.y-q1.y)/(q2.y-q1.y)
+                else:
+                    ratio = 0
+                indices.append(i+ratio)
+                intersections.append(p)
+        return indices, intersections
+
     def getTrajectoryInInterval(self, inter):
         'Returns all position between index inter.first and index.last (included)'
         if inter.first >=0 and inter.last<= self.length():
--- a/python/prediction.py	Tue Feb 17 00:56:47 2015 +0100
+++ b/python/prediction.py	Tue Feb 17 02:21:31 2015 +0100
@@ -287,55 +287,6 @@
 
     return currentInstant, collisionPoints, crossingZones
 
-def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
-    '''Computes all crossing and collision points at each common instant for two road users. '''
-    collisionPoints={}
-    crossingZones={}
-    if timeInterval:
-        commonTimeInterval = timeInterval
-    else:
-        commonTimeInterval = obj1.commonTimeInterval(obj2)
-    if nProcesses == 1:
-        if usePrototypes:
-            firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
-            commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors
-            commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors
-            for i in commonTimeIntervalList2: 
-                i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-                if len(cp) != 0:
-                    collisionPoints[i] = cp
-                if len(cz) != 0:
-                    crossingZones[i] = cz
-            if collisionPoints!={} or crossingZones!={}:
-                for i in commonTimeIntervalList1:
-                    if i not in commonTimeIntervalList2:
-                        i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-                        if len(cp) != 0:
-                            collisionPoints[i] = cp
-                        if len(cz) != 0:
-                            crossingZones[i] = cz                        
-        else:
-            for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
-                i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-                if len(cp) != 0:
-                    collisionPoints[i] = cp
-                if len(cz) != 0:
-                    crossingZones[i] = cz
-    else:
-        from multiprocessing import Pool
-        pool = Pool(processes = nProcesses)
-        jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
-        #results = [j.get() for j in jobs]
-        #results.sort()
-        for j in jobs:
-            i, cp, cz = j.get()
-            #if len(cp) != 0 or len(cz) != 0:
-            if len(cp) != 0:
-                collisionPoints[i] = cp
-            if len(cz) != 0:
-                crossingZones[i] = cz
-        pool.close()
-    return collisionPoints, crossingZones
 
 class PredictionParameters:
     def __init__(self, name, maxSpeed):
@@ -352,7 +303,56 @@
         return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
 
     def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
-        return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
+        #def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
+        '''Computes all crossing and collision points at each common instant for two road users. '''
+        collisionPoints={}
+        crossingZones={}
+        if timeInterval:
+            commonTimeInterval = timeInterval
+        else:
+            commonTimeInterval = obj1.commonTimeInterval(obj2)
+        if nProcesses == 1:
+            if usePrototypes:
+                firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
+                commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors
+                commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors
+                for i in commonTimeIntervalList2: 
+                    i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+                    if len(cp) != 0:
+                        collisionPoints[i] = cp
+                    if len(cz) != 0:
+                        crossingZones[i] = cz
+                if collisionPoints!={} or crossingZones!={}:
+                    for i in commonTimeIntervalList1:
+                        if i not in commonTimeIntervalList2:
+                            i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+                            if len(cp) != 0:
+                                collisionPoints[i] = cp
+                            if len(cz) != 0:
+                                crossingZones[i] = cz                        
+            else:
+                for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
+                    i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+                    if len(cp) != 0:
+                        collisionPoints[i] = cp
+                    if len(cz) != 0:
+                        crossingZones[i] = cz
+        else:
+            from multiprocessing import Pool
+            pool = Pool(processes = nProcesses)
+            jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
+            #results = [j.get() for j in jobs]
+            #results.sort()
+            for j in jobs:
+                i, cp, cz = j.get()
+                #if len(cp) != 0 or len(cz) != 0:
+                if len(cp) != 0:
+                    collisionPoints[i] = cp
+                if len(cz) != 0:
+                    crossingZones[i] = cz
+            pool.close()
+        return collisionPoints, crossingZones
+#return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
 
     def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
         '''Computes only collision probabilities
@@ -487,7 +487,7 @@
     def __init__(self):
         PredictionParameters.__init__(self, 'constant velocity (direct computation)', None)
 
-    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
+    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
         collisionPoints = []
         crossingZones = []
 
@@ -498,24 +498,43 @@
         else:
             v1 = obj1.getVelocityAtInstant(currentInstant)
             v2 = obj2.getVelocityAtInstant(currentInstant)
-            intersection = moving.intersection(p1, p2, v1, v2)
+            intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
 
             if intersection != None:
                 dp1 = intersection-p1
                 dp2 = intersection-p2
-                if moving.Point.dot(dp1, v1) > 0 and moving.Point.dot(dp2, v2) > 0: # if the road users are moving towards the intersection
+                dot1 = moving.Point.dot(dp1, v1)
+                dot2 = moving.Point.dot(dp2, v2)
+                #print dot1, dot2
+                # (computeCZ and (dot1 > 0 or dot2 > 0)) or (
+                if (computeCZ and (dot1 > 0 or dot2 > 0)) or (dot1 > 0 and dot2 > 0): # if the road users are moving towards the intersection or if computing pPET
                     dist1 = dp1.norm2()
                     dist2 = dp2.norm2()
-                    s1 = v1.norm2()
-                    s2 = v2.norm2()
+                    s1 = math.copysign(v1.norm2(), dot1)
+                    s2 = math.copysign(v2.norm2(), dot2)
                     halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
                     timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1)
                     timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2)
                     collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2)
-                    if computeCZ and collisionTimeInterval.empty():
-                        crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
+                    
+                    if collisionTimeInterval.empty():
+                        if computeCZ:
+                            crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
                     else:
                         collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
+            # elif computeCZ and (dot1 > 0 or dot2 > 0):
+            #     if dot1 > 0:
+            #         firstUser = obj2 # first through crossingzone
+            #         secondUser = obj1 # second through crossingzone
+            #     elif dot2 > 0:
+            #         firstUser = obj1
+            #         secondUser = obj2
+            #     p2 = secondUser.getPositionAtInstant(currentInstant)
+            #     v2 = secondUser.getVelocityAtInstant(currentInstant)
+            #     indices, intersections = firstUser.getPositions().getLineIntersections(p2, p2+v2)
+            #     if indices != None:
+            #         pass
+            #     else: # one has to predict !!!
     
         if debug and intersection!= None:
             from matplotlib.pyplot import plot, figure, axis, title
@@ -528,7 +547,7 @@
             title('instant {0}'.format(currentInstant))
             axis('equal')
 
-        return collisionPoints, crossingZones
+        return currentInstant, collisionPoints, crossingZones
 
 class CVExactPredictionParameters(PredictionParameters):
     '''Prediction parameters of prediction at constant velocity
@@ -546,7 +565,7 @@
         p2 = obj2.getPositionAtInstant(currentInstant)
         v1 = obj1.getVelocityAtInstant(currentInstant)
         v2 = obj2.getVelocityAtInstant(currentInstant)
-        intersection = moving.intersection(p1, p2, v1, v2)
+        intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
 
         if intersection != None:
             ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)