Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 630:69a98f84f3eb

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
corrected major issue with pPET, only for CVDirect prediction for now
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 17 Feb 2015 02:21:31 +0100
parents 0a5e89d6fc62
children 3058e00887bc
comparison
equal deleted inserted replaced
629:0a5e89d6fc62 630:69a98f84f3eb
285 if debug: 285 if debug:
286 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) 286 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
287 287
288 return currentInstant, collisionPoints, crossingZones 288 return currentInstant, collisionPoints, crossingZones
289 289
290 def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): 290
291 '''Computes all crossing and collision points at each common instant for two road users. ''' 291 class PredictionParameters:
292 collisionPoints={} 292 def __init__(self, name, maxSpeed):
293 crossingZones={} 293 self.name = name
294 if timeInterval: 294 self.maxSpeed = maxSpeed
295 commonTimeInterval = timeInterval 295
296 else: 296 def __str__(self):
297 commonTimeInterval = obj1.commonTimeInterval(obj2) 297 return '{0} {1}'.format(self.name, self.maxSpeed)
298 if nProcesses == 1: 298
299 if usePrototypes: 299 def generatePredictedTrajectories(self, obj, instant):
300 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) 300 return []
301 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors 301
302 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors 302 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
303 for i in commonTimeIntervalList2: 303 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
304 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) 304
305 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
306 #def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
307 '''Computes all crossing and collision points at each common instant for two road users. '''
308 collisionPoints={}
309 crossingZones={}
310 if timeInterval:
311 commonTimeInterval = timeInterval
312 else:
313 commonTimeInterval = obj1.commonTimeInterval(obj2)
314 if nProcesses == 1:
315 if usePrototypes:
316 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
317 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors
318 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors
319 for i in commonTimeIntervalList2:
320 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
321 if len(cp) != 0:
322 collisionPoints[i] = cp
323 if len(cz) != 0:
324 crossingZones[i] = cz
325 if collisionPoints!={} or crossingZones!={}:
326 for i in commonTimeIntervalList1:
327 if i not in commonTimeIntervalList2:
328 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
329 if len(cp) != 0:
330 collisionPoints[i] = cp
331 if len(cz) != 0:
332 crossingZones[i] = cz
333 else:
334 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
335 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
336 if len(cp) != 0:
337 collisionPoints[i] = cp
338 if len(cz) != 0:
339 crossingZones[i] = cz
340 else:
341 from multiprocessing import Pool
342 pool = Pool(processes = nProcesses)
343 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
344 #results = [j.get() for j in jobs]
345 #results.sort()
346 for j in jobs:
347 i, cp, cz = j.get()
348 #if len(cp) != 0 or len(cz) != 0:
305 if len(cp) != 0: 349 if len(cp) != 0:
306 collisionPoints[i] = cp 350 collisionPoints[i] = cp
307 if len(cz) != 0: 351 if len(cz) != 0:
308 crossingZones[i] = cz 352 crossingZones[i] = cz
309 if collisionPoints!={} or crossingZones!={}: 353 pool.close()
310 for i in commonTimeIntervalList1: 354 return collisionPoints, crossingZones
311 if i not in commonTimeIntervalList2: 355 #return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
312 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
313 if len(cp) != 0:
314 collisionPoints[i] = cp
315 if len(cz) != 0:
316 crossingZones[i] = cz
317 else:
318 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
319 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
320 if len(cp) != 0:
321 collisionPoints[i] = cp
322 if len(cz) != 0:
323 crossingZones[i] = cz
324 else:
325 from multiprocessing import Pool
326 pool = Pool(processes = nProcesses)
327 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
328 #results = [j.get() for j in jobs]
329 #results.sort()
330 for j in jobs:
331 i, cp, cz = j.get()
332 #if len(cp) != 0 or len(cz) != 0:
333 if len(cp) != 0:
334 collisionPoints[i] = cp
335 if len(cz) != 0:
336 crossingZones[i] = cz
337 pool.close()
338 return collisionPoints, crossingZones
339
340 class PredictionParameters:
341 def __init__(self, name, maxSpeed):
342 self.name = name
343 self.maxSpeed = maxSpeed
344
345 def __str__(self):
346 return '{0} {1}'.format(self.name, self.maxSpeed)
347
348 def generatePredictedTrajectories(self, obj, instant):
349 return []
350
351 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
352 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
353
354 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
355 return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
356 356
357 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): 357 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
358 '''Computes only collision probabilities 358 '''Computes only collision probabilities
359 Returns for each instant the collision probability and number of samples drawn''' 359 Returns for each instant the collision probability and number of samples drawn'''
360 collisionProbabilities = {} 360 collisionProbabilities = {}
485 using direct computation of the intersecting point''' 485 using direct computation of the intersecting point'''
486 486
487 def __init__(self): 487 def __init__(self):
488 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None) 488 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None)
489 489
490 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): 490 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
491 collisionPoints = [] 491 collisionPoints = []
492 crossingZones = [] 492 crossingZones = []
493 493
494 p1 = obj1.getPositionAtInstant(currentInstant) 494 p1 = obj1.getPositionAtInstant(currentInstant)
495 p2 = obj2.getPositionAtInstant(currentInstant) 495 p2 = obj2.getPositionAtInstant(currentInstant)
496 if (p1-p2).norm2() <= collisionDistanceThreshold: 496 if (p1-p2).norm2() <= collisionDistanceThreshold:
497 collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)] 497 collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)]
498 else: 498 else:
499 v1 = obj1.getVelocityAtInstant(currentInstant) 499 v1 = obj1.getVelocityAtInstant(currentInstant)
500 v2 = obj2.getVelocityAtInstant(currentInstant) 500 v2 = obj2.getVelocityAtInstant(currentInstant)
501 intersection = moving.intersection(p1, p2, v1, v2) 501 intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
502 502
503 if intersection != None: 503 if intersection != None:
504 dp1 = intersection-p1 504 dp1 = intersection-p1
505 dp2 = intersection-p2 505 dp2 = intersection-p2
506 if moving.Point.dot(dp1, v1) > 0 and moving.Point.dot(dp2, v2) > 0: # if the road users are moving towards the intersection 506 dot1 = moving.Point.dot(dp1, v1)
507 dot2 = moving.Point.dot(dp2, v2)
508 #print dot1, dot2
509 # (computeCZ and (dot1 > 0 or dot2 > 0)) or (
510 if (computeCZ and (dot1 > 0 or dot2 > 0)) or (dot1 > 0 and dot2 > 0): # if the road users are moving towards the intersection or if computing pPET
507 dist1 = dp1.norm2() 511 dist1 = dp1.norm2()
508 dist2 = dp2.norm2() 512 dist2 = dp2.norm2()
509 s1 = v1.norm2() 513 s1 = math.copysign(v1.norm2(), dot1)
510 s2 = v2.norm2() 514 s2 = math.copysign(v2.norm2(), dot2)
511 halfCollisionDistanceThreshold = collisionDistanceThreshold/2. 515 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
512 timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1) 516 timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1)
513 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2) 517 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2)
514 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2) 518 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2)
515 if computeCZ and collisionTimeInterval.empty(): 519
516 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] 520 if collisionTimeInterval.empty():
521 if computeCZ:
522 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
517 else: 523 else:
518 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] 524 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
525 # elif computeCZ and (dot1 > 0 or dot2 > 0):
526 # if dot1 > 0:
527 # firstUser = obj2 # first through crossingzone
528 # secondUser = obj1 # second through crossingzone
529 # elif dot2 > 0:
530 # firstUser = obj1
531 # secondUser = obj2
532 # p2 = secondUser.getPositionAtInstant(currentInstant)
533 # v2 = secondUser.getVelocityAtInstant(currentInstant)
534 # indices, intersections = firstUser.getPositions().getLineIntersections(p2, p2+v2)
535 # if indices != None:
536 # pass
537 # else: # one has to predict !!!
519 538
520 if debug and intersection!= None: 539 if debug and intersection!= None:
521 from matplotlib.pyplot import plot, figure, axis, title 540 from matplotlib.pyplot import plot, figure, axis, title
522 figure() 541 figure()
523 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') 542 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r')
526 obj1.plot('r') 545 obj1.plot('r')
527 obj2.plot('b') 546 obj2.plot('b')
528 title('instant {0}'.format(currentInstant)) 547 title('instant {0}'.format(currentInstant))
529 axis('equal') 548 axis('equal')
530 549
531 return collisionPoints, crossingZones 550 return currentInstant, collisionPoints, crossingZones
532 551
533 class CVExactPredictionParameters(PredictionParameters): 552 class CVExactPredictionParameters(PredictionParameters):
534 '''Prediction parameters of prediction at constant velocity 553 '''Prediction parameters of prediction at constant velocity
535 using direct computation of the intersecting point (solving for the equation''' 554 using direct computation of the intersecting point (solving for the equation'''
536 555
544 563
545 p1 = obj1.getPositionAtInstant(currentInstant) 564 p1 = obj1.getPositionAtInstant(currentInstant)
546 p2 = obj2.getPositionAtInstant(currentInstant) 565 p2 = obj2.getPositionAtInstant(currentInstant)
547 v1 = obj1.getVelocityAtInstant(currentInstant) 566 v1 = obj1.getVelocityAtInstant(currentInstant)
548 v2 = obj2.getVelocityAtInstant(currentInstant) 567 v2 = obj2.getVelocityAtInstant(currentInstant)
549 intersection = moving.intersection(p1, p2, v1, v2) 568 intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
550 569
551 if intersection != None: 570 if intersection != None:
552 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) 571 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
553 if ttc: 572 if ttc:
554 return [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5) 573 return [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5)