Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 987:f026ce2af637
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 |