Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 987:f026ce2af637

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
found bug with direct ttc computation
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 07 Mar 2018 23:37:00 -0500
parents 668a85c963c3
children 933670761a57
comparison
equal deleted inserted replaced
986:3be8aaa47651 987:f026ce2af637
262 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) 262 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
263 else: 263 else:
264 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) 264 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
265 return prototypeTrajectories 265 return prototypeTrajectories
266 266
267 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
268 '''returns the lists of collision points and crossing zones'''
269 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
270 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
271
272 collisionPoints = []
273 crossingZones = []
274 for et1 in predictedTrajectories1:
275 for et2 in predictedTrajectories2:
276 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
277 if collision:
278 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
279 elif computeCZ: # check if there is a crossing zone
280 # TODO same computation as PET with metric + concatenate past trajectory with future trajectory
281 cz = None
282 t1 = 0
283 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
284 t2 = 0
285 while not cz and t2 < timeHorizon:
286 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
287 if cz is not None:
288 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
289 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV)))
290 t2 += 1
291 t1 += 1
292
293 if debug:
294 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
295
296 return currentInstant, collisionPoints, crossingZones
297
298 267
299 class PredictionParameters(object): 268 class PredictionParameters(object):
300 def __init__(self, name, maxSpeed): 269 def __init__(self, name, maxSpeed):
301 self.name = name 270 self.name = name
302 self.maxSpeed = maxSpeed 271 self.maxSpeed = maxSpeed
303 272
304 def __str__(self): 273 def __str__(self):
305 return '{0} {1}'.format(self.name, self.maxSpeed) 274 return '{0} {1}'.format(self.name, self.maxSpeed)
306 275
307 def generatePredictedTrajectories(self, obj, instant): 276 def generatePredictedTrajectories(self, obj, instant):
308 return [] 277 return None
278
279 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
280 '''returns the lists of collision points and crossing zones'''
281 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant)
282 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant)
283
284 collisionPoints = []
285 if computeCZ:
286 crossingZones = []
287 else:
288 crossingZones = None
289 for et1 in predictedTrajectories1:
290 for et2 in predictedTrajectories2:
291 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
292 if collision:
293 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
294 elif computeCZ: # check if there is a crossing zone
295 # TODO same computation as PET with metric + concatenate past trajectory with future trajectory
296 cz = None
297 t1 = 0
298 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
299 t2 = 0
300 while not cz and t2 < timeHorizon:
301 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
302 if cz is not None:
303 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
304 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV)))
305 t2 += 1
306 t1 += 1
307
308 if debug:
309 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
310
311 return collisionPoints, crossingZones
309 312
310 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):#, nProcesses = 1): 313 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):#, nProcesses = 1):
311 '''Computes all crossing and collision points at each common instant for two road users. ''' 314 '''Computes all crossing and collision points at each common instant for two road users. '''
312 collisionPoints={} 315 collisionPoints = {}
313 crossingZones={} 316 if computeCZ:
317 crossingZones = {}
318 else:
319 crossingZones = None
314 if timeInterval: 320 if timeInterval:
315 commonTimeInterval = timeInterval 321 commonTimeInterval = timeInterval
316 else: 322 else:
317 commonTimeInterval = obj1.commonTimeInterval(obj2) 323 commonTimeInterval = obj1.commonTimeInterval(obj2)
318 #if nProcesses == 1: 324 #if nProcesses == 1:
319 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors 325 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
320 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) 326 cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
321 if len(cp) != 0: 327 if len(cp) != 0:
322 collisionPoints[i] = cp 328 collisionPoints[i] = cp
323 if len(cz) != 0: 329 if computeCZ and len(cz) != 0:
324 crossingZones[i] = cz 330 crossingZones[i] = cz
325 # else:
326 # pool = Pool(processes = nProcesses)
327 # jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]]
328 # for j in jobs:
329 # i, cp, cz = j.get()
330 # if len(cp) != 0:
331 # collisionPoints[i] = cp
332 # if len(cz) != 0:
333 # crossingZones[i] = cz
334 # pool.close()
335 return collisionPoints, crossingZones 331 return collisionPoints, crossingZones
336 332
337 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): 333 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
338 '''Computes only collision probabilities 334 '''Computes only collision probabilities
339 Returns for each instant the collision probability and number of samples drawn''' 335 Returns for each instant the collision probability and number of samples drawn'''
473 def __init__(self): 469 def __init__(self):
474 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None) 470 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None)
475 471
476 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): 472 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
477 collisionPoints = [] 473 collisionPoints = []
478 crossingZones = [] 474 if computeCZ:
475 crossingZones = []
476 else:
477 crossingZones = None
479 478
480 p1 = obj1.getPositionAtInstant(currentInstant) 479 p1 = obj1.getPositionAtInstant(currentInstant)
481 p2 = obj2.getPositionAtInstant(currentInstant) 480 p2 = obj2.getPositionAtInstant(currentInstant)
482 if (p1-p2).norm2() <= collisionDistanceThreshold: 481 if (p1-p2).norm2() <= collisionDistanceThreshold:
483 collisionPoints = [SafetyPoint((p1+p2)*0.5, 1., 0.)] 482 collisionPoints = [SafetyPoint((p1+p2)*0.5, 1., 0.)]
516 obj1.plot('r') 515 obj1.plot('r')
517 obj2.plot('b') 516 obj2.plot('b')
518 title('instant {0}'.format(currentInstant)) 517 title('instant {0}'.format(currentInstant))
519 axis('equal') 518 axis('equal')
520 519
521 return currentInstant, collisionPoints, crossingZones 520 return collisionPoints, crossingZones
522 521
523 class CVExactPredictionParameters(PredictionParameters): 522 class CVExactPredictionParameters(PredictionParameters):
524 '''Prediction parameters of prediction at constant velocity 523 '''Prediction parameters of prediction at constant velocity
525 using direct computation of the intersecting point (solving the equation) 524 using direct computation of the intersecting point (solving the equation)
526 Warning: the computed time to collision may be higher than timeHorizon (not used)''' 525 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
535 534
536 p1 = obj1.getPositionAtInstant(currentInstant) 535 p1 = obj1.getPositionAtInstant(currentInstant)
537 p2 = obj2.getPositionAtInstant(currentInstant) 536 p2 = obj2.getPositionAtInstant(currentInstant)
538 v1 = obj1.getVelocityAtInstant(currentInstant) 537 v1 = obj1.getVelocityAtInstant(currentInstant)
539 v2 = obj2.getVelocityAtInstant(currentInstant) 538 v2 = obj2.getVelocityAtInstant(currentInstant)
540 intersection = moving.intersection(p1, p1+v1, p2, p2+v2) 539 #intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
541 540
542 if intersection is not None: 541 if not moving.Point.parallel(v1, v2):
543 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) 542 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
544 if ttc is not None: 543 if ttc is not None:
545 collisionPoints = [SafetyPoint(intersection, 1., ttc)] 544 collisionPoints = [SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)]
546 else: 545 else:
547 pass # compute pPET 546 pass # compute pPET
548 547
549 return currentInstant, collisionPoints, crossingZones 548 return collisionPoints, crossingZones
550 549
551 class PrototypePredictionParameters(PredictionParameters): 550 class PrototypePredictionParameters(PredictionParameters):
552 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): 551 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):
553 PredictionParameters.__init__(self, 'prototypes', None) 552 PredictionParameters.__init__(self, 'prototypes', None)
554 self.prototypes = prototypes 553 self.prototypes = prototypes