Mercurial Hosting > traffic-intelligence

changeset 600:414b2e7cd873

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merge feature-minDistance-based collisionPoints calculation in prediction file
author Mohamed Gomaa
date Thu, 18 Apr 2013 17:12:53 -0400
parents 4b5fe2de1e8d
children e1f3b789c632
files python/events.py python/prediction.py
diffstat 2 files changed, 66 insertions(+), 37 deletions(-) [+]
line wrap: on
line diff
--- a/python/events.py	Thu Apr 18 15:46:21 2013 -0400
+++ b/python/events.py	Thu Apr 18 17:12:53 2013 -0400
@@ -77,7 +77,7 @@
 
     def computeCollisionPoints(self, predictionParameters, collisionDistanceThreshold, timeHorizon):
         if self.movingObject1.features and self.movingObject2.features:
-            collisionPoints = prediction.computeCollisions(self.movingObject1, self.movingObject2, predictionParameters, collisionDistanceThreshold, timeHorizon)
+            collisionPoints,crossingZones = prediction.computeCrossingsCollisions(self.movingObject1, self.movingObject2, predictionParameters, collisionDistanceThreshold, timeHorizon,asWholeVehicle=True)
             self.addIndicator(indicators.SeverityIndicator('collisionPoints', collisionPoints))	
         else:
             print('Features not associated with objects')
--- a/python/prediction.py	Thu Apr 18 15:46:21 2013 -0400
+++ b/python/prediction.py	Thu Apr 18 17:12:53 2013 -0400
@@ -179,48 +179,78 @@
     from numpy import sum
     return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
 
-def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
+def getPredictedSetPositions (predictedTrajectory,instant):
+    positions=[]
+    for p in predictedTrajectory:
+        positions.append(p.predictPosition(instant))
+    return positions
+
+def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon,asWholeVehicle=False ):
     '''Computes the first instant at which two predicted trajectories are within some distance threshold'''
-    t = 1
-    p1 = predictedTrajectory1.predictPosition(t)
-    p2 = predictedTrajectory2.predictPosition(t)
-    while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
+    if asWholeVehicle:
+        from scipy.spatial.distance import cdist
+        import numpy as np
+        t = 1
+        p1= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory1,t)]
+        p2= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory2,t)]
+        distance= cdist(p1,p2, metric='euclidean')
+        minDist= distance.min()
+        while t <= timeHorizon and minDist > collisionDistanceThreshold:
+            p1= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory1,t)]
+            p2= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory2,t)]
+            distance= cdist(p1,p2, metric='euclidean')
+            minDist= distance.min()
+            t += 1
+        Index=np.unravel_index(distance.argmin(), distance.shape)
+        involvedPosition1= p1[Index[0]]
+        involvedPosition2= p2[Index[1]]
+        return t, involvedPosition1, involvedPosition2
+    else:
+        t = 1
         p1 = predictedTrajectory1.predictPosition(t)
         p2 = predictedTrajectory2.predictPosition(t)
-        t += 1
-    return t, p1, p2
-
-def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
-    '''returns the lists of collision points and crossing zones
-    
-    Check: Predicting all the points together, as if they represent the whole vehicle'''
+        while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
+            p1 = predictedTrajectory1.predictPosition(t)
+            p2 = predictedTrajectory2.predictPosition(t)
+            t += 1
+        return t, p1, p2
+   
+def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, asWholeVehicle=False):
+    '''returns the lists of collision points and crossing zones   
+    Check: Predicting all the points together, as if they represent the whole vehicle''' # Done
+	
     predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant)
     predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant)
 
     collisionPoints = []
     crossingZones = []
-    for et1 in predictedTrajectories1:
-        for et2 in predictedTrajectories2:
-            t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
+    if asWholeVehicle:
+        t, p1, p2 = computeCollisionTime(predictedTrajectories1, predictedTrajectories2, collisionDistanceThreshold, timeHorizon,asWholeVehicle)        
+        if t <= timeHorizon:
+            collisionPoints.append(SafetyPoint((moving.Point(p1[0]+p2[0],p1[1]+p2[1])).multiply(0.5), 1, t))                       
+    else:
+        for et1 in predictedTrajectories1:
+            for et2 in predictedTrajectories2:
+                t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon,asWholeVehicle)
             
-            if t <= timeHorizon:
-                collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
-            elif computeCZ: # check if there is a crossing zone
-                # TODO? zone should be around the points at which the traj are the closest
-                # look for CZ at different times, otherwise it would be a collision
-                # an approximation would be to look for close points at different times, ie the complementary of collision points
-                cz = None
-                t1 = 0
-                while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
-                    t2 = 0
-                    while not cz and t2 < timeHorizon:
-                        #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
-                        #    cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
-                        cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
-                        if cz:
-                            crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
-                        t2 += 1
-                    t1 += 1                        
+                if t <= timeHorizon:
+                    collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
+                elif computeCZ: # check if there is a crossing zone
+                    # TODO? zone should be around the points at which the traj are the closest
+                    # look for CZ at different times, otherwise it would be a collision
+                    # an approximation would be to look for close points at different times, ie the complementary of collision points
+                    cz = None
+                    t1 = 0
+                    while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
+                        t2 = 0
+                        while not cz and t2 < timeHorizon:
+                            #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
+                            #    cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
+                            cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
+                            if cz:
+                                crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
+                            t2 += 1
+                        t1 += 1                        
 
     if debug:
         from matplotlib.pyplot import figure, axis, title
@@ -239,7 +269,7 @@
 
     return collisionPoints, crossingZones
     
-def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):
+def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,asWholeVehicle=False):
     '''Computes all crossing and collision points at each common instant for two road users. '''
     collisionPoints={}
     crossingZones={}
@@ -248,7 +278,7 @@
     else:
         commonTimeInterval = obj1.commonTimeInterval(obj2)
     for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
-        collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
+        collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug,asWholeVehicle)
 
     return collisionPoints, crossingZones
  
@@ -291,7 +321,6 @@
 
     return collisionProbabilities
 
-
 if __name__ == "__main__":
     import doctest
     import unittest