Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 630:69a98f84f3eb
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) |