nsaunier/traffic-intelligence: python/prediction.py comparison

comparison python/prediction.py @ 600:414b2e7cd873

merge feature-minDistance-based collisionPoints calculation in prediction file

author	Mohamed Gomaa
date	Thu, 18 Apr 2013 17:12:53 -0400
parents	f65b828e5521
children	480c8edf177e

comparison

equal deleted inserted replaced

-:4b5fe2de1e8d
+:414b2e7cd873
 def computeExpectedIndicator(points):
 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
+if asWholeVehicle:
-p1 = predictedTrajectory1.predictPosition(t)
+from scipy.spatial.distance import cdist
-p2 = predictedTrajectory2.predictPosition(t)
+import numpy as np
-while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
+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
+while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
-return t, p1, p2
+p1 = predictedTrajectory1.predictPosition(t)
+p2 = predictedTrajectory2.predictPosition(t)
-def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
+t += 1
-'''returns the lists of collision points and crossing zones
+return t, p1, p2
-Check: Predicting all the points together, as if they represent the whole vehicle'''
+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:
+if asWholeVehicle:
-for et2 in predictedTrajectories2:
+t, p1, p2 = computeCollisionTime(predictedTrajectories1, predictedTrajectories2, collisionDistanceThreshold, timeHorizon,asWholeVehicle)
-t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
+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 debug:
 from matplotlib.pyplot import figure, axis, title
 figure()
 for et in predictedTrajectories1:
 title('instant {0}'.format(i))
 axis('equal')
 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={}
 if timeInterval:
 commonTimeInterval = timeInterval
 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
 def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
 '''Computes only collision probabilities
 title('instant {0}'.format(i))
 axis('equal')
 return collisionProbabilities
 if __name__ == "__main__":
 import doctest
 import unittest
 suite = doctest.DocFileSuite('tests/prediction.txt')
 #suite = doctest.DocTestSuite()

Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 600:414b2e7cd873