Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 662:72174e66aba5

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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