Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 662:72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 18 May 2015 17:17:06 +0200 |
parents | dc70d9e711f5 |
children | 15e244d2a1b5 |
comparison
equal
deleted
inserted
replaced
661:dc70d9e711f5 | 662:72174e66aba5 |
---|---|
138 def computeExpectedIndicator(points): | 138 def computeExpectedIndicator(points): |
139 from numpy import sum | 139 from numpy import sum |
140 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) | 140 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) |
141 | 141 |
142 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): | 142 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): |
143 '''Computes the first instant at which two predicted trajectories are within some distance threshold''' | 143 '''Computes the first instant |
144 at which two predicted trajectories are within some distance threshold | |
145 Computes all the times including timeHorizon | |
146 | |
147 User has to check the first variable collision to know about a collision''' | |
144 t = 1 | 148 t = 1 |
145 p1 = predictedTrajectory1.predictPosition(t) | 149 p1 = predictedTrajectory1.predictPosition(t) |
146 p2 = predictedTrajectory2.predictPosition(t) | 150 p2 = predictedTrajectory2.predictPosition(t) |
147 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: | 151 collision = (p1-p2).norm2() <= collisionDistanceThreshold |
152 while t < timeHorizon and not collision: | |
153 t += 1 | |
148 p1 = predictedTrajectory1.predictPosition(t) | 154 p1 = predictedTrajectory1.predictPosition(t) |
149 p2 = predictedTrajectory2.predictPosition(t) | 155 p2 = predictedTrajectory2.predictPosition(t) |
150 t += 1 | 156 collision = (p1-p2).norm2() <= collisionDistanceThreshold |
151 return t, p1, p2 | 157 return collision, t, p1, p2 |
152 | 158 |
153 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): | 159 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): |
154 from matplotlib.pyplot import figure, axis, title, close, savefig | 160 from matplotlib.pyplot import figure, axis, title, close, savefig |
155 figure() | 161 figure() |
156 for et in predictedTrajectories1: | 162 for et in predictedTrajectories1: |
246 return prototypeTrajectories | 252 return prototypeTrajectories |
247 | 253 |
248 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): | 254 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): |
249 '''returns the lists of collision points and crossing zones''' | 255 '''returns the lists of collision points and crossing zones''' |
250 if usePrototypes: | 256 if usePrototypes: |
251 prototypeTrajectories1=getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 257 prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
252 prototypeTrajectories2= getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 258 prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
253 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) | 259 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) |
254 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) | 260 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) |
255 else: | 261 else: |
256 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) | 262 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) |
257 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) | 263 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) |
258 | 264 |
259 collisionPoints = [] | 265 collisionPoints = [] |
260 crossingZones = [] | 266 crossingZones = [] |
261 for et1 in predictedTrajectories1: | 267 for et1 in predictedTrajectories1: |
262 for et2 in predictedTrajectories2: | 268 for et2 in predictedTrajectories2: |
263 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 269 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
264 | 270 |
265 if t <= timeHorizon: | 271 if collision: |
266 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) | 272 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) |
267 elif computeCZ: # check if there is a crossing zone | 273 elif computeCZ: # check if there is a crossing zone |
268 # TODO? zone should be around the points at which the traj are the closest | 274 # TODO? zone should be around the points at which the traj are the closest |
269 # look for CZ at different times, otherwise it would be a collision | 275 # look for CZ at different times, otherwise it would be a collision |
270 # an approximation would be to look for close points at different times, ie the complementary of collision points | 276 # an approximation would be to look for close points at different times, ie the complementary of collision points |
366 nCollisions = 0 | 372 nCollisions = 0 |
367 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i) | 373 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i) |
368 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i) | 374 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i) |
369 for et1 in predictedTrajectories1: | 375 for et1 in predictedTrajectories1: |
370 for et2 in predictedTrajectories2: | 376 for et2 in predictedTrajectories2: |
371 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 377 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
372 if t <= timeHorizon: | 378 if collision: |
373 nCollisions += 1 | 379 nCollisions += 1 |
374 # take into account probabilities ?? | 380 # take into account probabilities ?? |
375 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) | 381 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) |
376 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] | 382 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] |
377 | 383 |