diff python/prediction.py @ 630:69a98f84f3eb

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 3058e00887bc
line wrap: on
line diff
--- 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)