Mercurial Hosting > traffic-intelligence
changeset 600:414b2e7cd873
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