Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/prediction.py @ 1269:ca70a79688ae

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
adding a speed threshold to avoid computing TTC at very low speeds
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 05 Jun 2024 10:12:43 -0400
parents 1b472cddf9b1
children
comparison
equal deleted inserted replaced
1268:27b206d118b7 1269:ca70a79688ae
353 if debug: 353 if debug:
354 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) 354 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
355 355
356 return collisionPoints, crossingZones 356 return collisionPoints, crossingZones
357 357
358 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):#, nProcesses = 1): 358 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, speedThreshold = 0.):
359 '''Computes all crossing and collision points at each common instant for two road users. ''' 359 '''Computes all crossing and collision points at each common instant for two road users.
360 No movement prediction below a certain speedThreshold for both objects'''
360 collisionPoints = {} 361 collisionPoints = {}
361 if computeCZ: 362 if computeCZ:
362 crossingZones = {} 363 crossingZones = {}
363 else: 364 else:
364 crossingZones = None 365 crossingZones = None
365 if timeInterval is not None: 366 if timeInterval is not None:
366 commonTimeInterval = timeInterval 367 commonTimeInterval = timeInterval
367 else: 368 else:
368 commonTimeInterval = obj1.commonTimeInterval(obj2) 369 commonTimeInterval = obj1.commonTimeInterval(obj2)
369 #if nProcesses == 1: 370 speedThreshold2 = max(0,speedThreshold)**2
370 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors 371 for t in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
371 cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) 372 if obj1.getVelocityAtInstant(t).norm2Squared() > speedThreshold2 and obj2.getVelocityAtInstant(t).norm2Squared() > speedThreshold2:
372 if len(cp) != 0: 373 cp, cz = self.computeCrossingsCollisionsAtInstant(t, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
373 collisionPoints[i] = cp 374 if len(cp) != 0:
374 if computeCZ and len(cz) != 0: 375 collisionPoints[t] = cp
375 crossingZones[i] = cz 376 if computeCZ and len(cz) != 0:
377 crossingZones[t] = cz
376 return collisionPoints, crossingZones 378 return collisionPoints, crossingZones
377 379
378 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): 380 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
379 '''Computes only collision probabilities 381 '''Computes only collision probabilities
380 Returns for each instant the collision probability and number of samples drawn''' 382 Returns for each instant the collision probability and number of samples drawn'''