Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 708:a37c565f4b68

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merged dev
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 22 Jul 2015 14:17:44 -0400
parents 4cc56ff82c3c
children 8b74a5176549
comparison
equal deleted inserted replaced
707:7efa36b9bcfd 708:a37c565f4b68
4 import moving 4 import moving
5 from utils import LCSS 5 from utils import LCSS
6 6
7 import math, random 7 import math, random
8 import numpy as np 8 import numpy as np
9 from multiprocessing import Pool
9 10
10 11
11 class PredictedTrajectory(object): 12 class PredictedTrajectory(object):
12 '''Class for predicted trajectories with lazy evaluation 13 '''Class for predicted trajectories with lazy evaluation
13 if the predicted position has not been already computed, compute it 14 if the predicted position has not been already computed, compute it
135 for p in points: 136 for p in points:
136 out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p)) 137 out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p))
137 138
138 @staticmethod 139 @staticmethod
139 def computeExpectedIndicator(points): 140 def computeExpectedIndicator(points):
140 from numpy import sum 141 return np.sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
141 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
142 142
143 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): 143 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
144 '''Computes the first instant 144 '''Computes the first instant
145 at which two predicted trajectories are within some distance threshold 145 at which two predicted trajectories are within some distance threshold
146 Computes all the times including timeHorizon 146 Computes all the times including timeHorizon
159 159
160 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): 160 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon):
161 from matplotlib.pyplot import figure, axis, title, close, savefig 161 from matplotlib.pyplot import figure, axis, title, close, savefig
162 figure() 162 figure()
163 for et in predictedTrajectories1: 163 for et in predictedTrajectories1:
164 et.predictPosition(timeHorizon) 164 et.predictPosition(int(np.round(timeHorizon)))
165 et.plot('rx') 165 et.plot('rx')
166 166
167 for et in predictedTrajectories2: 167 for et in predictedTrajectories2:
168 et.predictPosition(timeHorizon) 168 et.predictPosition(int(np.round(timeHorizon)))
169 et.plot('bx') 169 et.plot('bx')
170 obj1.plot('r') 170 obj1.plot('r')
171 obj2.plot('b') 171 obj2.plot('b')
172 title('instant {0}'.format(currentInstant)) 172 title('instant {0}'.format(currentInstant))
173 axis('equal') 173 axis('equal')
319 else: 319 else:
320 commonTimeInterval = obj1.commonTimeInterval(obj2) 320 commonTimeInterval = obj1.commonTimeInterval(obj2)
321 if nProcesses == 1: 321 if nProcesses == 1:
322 if usePrototypes: 322 if usePrototypes:
323 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) 323 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
324 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors 324 commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors
325 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors 325 commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors
326 for i in commonTimeIntervalList2: 326 for i in commonTimeIntervalList2:
327 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) 327 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)
328 if len(cp) != 0: 328 if len(cp) != 0:
329 collisionPoints[i] = cp 329 collisionPoints[i] = cp
330 if len(cz) != 0: 330 if len(cz) != 0:
343 if len(cp) != 0: 343 if len(cp) != 0:
344 collisionPoints[i] = cp 344 collisionPoints[i] = cp
345 if len(cz) != 0: 345 if len(cz) != 0:
346 crossingZones[i] = cz 346 crossingZones[i] = cz
347 else: 347 else:
348 from multiprocessing import Pool
349 pool = Pool(processes = nProcesses) 348 pool = Pool(processes = nProcesses)
350 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]] 349 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]]
351 #results = [j.get() for j in jobs] 350 #results = [j.get() for j in jobs]
352 #results.sort() 351 #results.sort()
353 for j in jobs: 352 for j in jobs:
491 return predictedTrajectories 490 return predictedTrajectories
492 491
493 492
494 class CVDirectPredictionParameters(PredictionParameters): 493 class CVDirectPredictionParameters(PredictionParameters):
495 '''Prediction parameters of prediction at constant velocity 494 '''Prediction parameters of prediction at constant velocity
496 using direct computation of the intersecting point''' 495 using direct computation of the intersecting point
496 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
497 497
498 def __init__(self): 498 def __init__(self):
499 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None) 499 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None)
500 500
501 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): 501 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
531 if collisionTimeInterval.empty(): 531 if collisionTimeInterval.empty():
532 if computeCZ: 532 if computeCZ:
533 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] 533 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
534 else: 534 else:
535 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] 535 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
536 # elif computeCZ and (dot1 > 0 or dot2 > 0):
537 # if dot1 > 0:
538 # firstUser = obj2 # first through crossingzone
539 # secondUser = obj1 # second through crossingzone
540 # elif dot2 > 0:
541 # firstUser = obj1
542 # secondUser = obj2
543 # p2 = secondUser.getPositionAtInstant(currentInstant)
544 # v2 = secondUser.getVelocityAtInstant(currentInstant)
545 # indices, intersections = firstUser.getPositions().getLineIntersections(p2, p2+v2)
546 # if indices is not None:
547 # pass
548 # else: # one has to predict !!!
549 536
550 if debug and intersection is not None: 537 if debug and intersection is not None:
551 from matplotlib.pyplot import plot, figure, axis, title 538 from matplotlib.pyplot import plot, figure, axis, title
552 figure() 539 figure()
553 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') 540 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r')
560 547
561 return currentInstant, collisionPoints, crossingZones 548 return currentInstant, collisionPoints, crossingZones
562 549
563 class CVExactPredictionParameters(PredictionParameters): 550 class CVExactPredictionParameters(PredictionParameters):
564 '''Prediction parameters of prediction at constant velocity 551 '''Prediction parameters of prediction at constant velocity
565 using direct computation of the intersecting point (solving for the equation''' 552 using direct computation of the intersecting point (solving the equation)
553 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
566 554
567 def __init__(self): 555 def __init__(self):
568 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None) 556 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None)
569 557
570 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): 558 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
571 'TODO add collision point coordinates, compute pPET' 559 'TODO add collision point coordinates, compute pPET'
572 #collisionPoints = [] 560 #collisionPoints = []
573 #crossingZones = [] 561 #crossingZones = []
574 562
575 p1 = obj1.getPositionAtInstant(currentInstant) 563 p1 = obj1.getPositionAtInstant(currentInstant)
579 intersection = moving.intersection(p1, p1+v1, p2, p2+v2) 567 intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
580 568
581 if intersection is not None: 569 if intersection is not None:
582 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) 570 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
583 if ttc: 571 if ttc:
584 return [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5) 572 return currentInstant, [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5)
585 else: 573 else:
586 return [],[] 574 return currentInstant, [],[]
587 575
588 #### 576 ####
589 # Other Methods 577 # Other Methods
590 #### 578 ####
591 class PrototypePredictionParameters(PredictionParameters): 579 class PrototypePredictionParameters(PredictionParameters):