comparison python/prediction.py @ 943:b1e8453c207c

work on motion prediction using motion patterns
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 19 Jul 2017 18:02:38 -0400
parents ab13aaf41432
children 84ebe1b031f1
comparison
equal deleted inserted replaced
942:ab13aaf41432 943:b1e8453c207c
80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2() 80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
81 81
82 def predictPosition(self, nTimeSteps): 82 def predictPosition(self, nTimeSteps):
83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
84 if self.constantSpeed: 84 if self.constantSpeed:
85 # calculate cumulative distance
86 traj = self.prototype.getPositions() 85 traj = self.prototype.getPositions()
87 trajLength = traj.length() 86 trajLength = traj.length()
88 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) 87 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
89 i = self.closestPointIdx 88 i = self.closestPointIdx
90 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance: 89 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
159 p2 = predictedTrajectory2.predictPosition(t) 158 p2 = predictedTrajectory2.predictPosition(t)
160 collision = (p1-p2).norm2() <= collisionDistanceThreshold 159 collision = (p1-p2).norm2() <= collisionDistanceThreshold
161 return collision, t, p1, p2 160 return collision, t, p1, p2
162 161
163 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): 162 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon):
164 from matplotlib.pyplot import figure, axis, title, close, savefig 163 from matplotlib.pyplot import figure, axis, title, clf, savefig
165 figure() 164 figure()
165 #clf()
166 for et in predictedTrajectories1: 166 for et in predictedTrajectories1:
167 et.predictPosition(int(np.round(timeHorizon))) 167 et.predictPosition(int(np.round(timeHorizon)))
168 et.plot('rx') 168 et.plot('rx')
169 169
170 for et in predictedTrajectories2: 170 for et in predictedTrajectories2:
171 et.predictPosition(int(np.round(timeHorizon))) 171 et.predictPosition(int(np.round(timeHorizon)))
172 et.plot('bx') 172 et.plot('bx')
173 obj1.plot('r') 173 obj1.plot('r', withOrigin = True)
174 obj2.plot('b') 174 obj2.plot('b', withOrigin = True)
175 title('instant {0}'.format(currentInstant)) 175 title('instant {0}'.format(currentInstant))
176 axis('equal') 176 axis('equal')
177 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) 177 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
178 close()
179 178
180 def calculateProbability(nMatching,similarity,objects): 179 def calculateProbability(nMatching,similarity,objects):
181 sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) 180 sumFrequencies=sum([nMatching[p] for p in similarity.keys()])
182 prototypeProbability={} 181 prototypeProbability={}
183 for i in similarity.keys(): 182 for i in similarity.keys():
253 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) 252 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
254 else: 253 else:
255 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) 254 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
256 return prototypeTrajectories 255 return prototypeTrajectories
257 256
258 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): 257 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
259 '''returns the lists of collision points and crossing zones''' 258 '''returns the lists of collision points and crossing zones'''
260 if usePrototypes: 259 # if usePrototypes:
261 prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) 260 # prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
262 prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) 261 # prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
263 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) 262 # predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1)
264 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) 263 # predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)
265 else: 264 # else:
266 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) 265 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
267 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) 266 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
268 267
269 collisionPoints = [] 268 collisionPoints = []
270 crossingZones = [] 269 crossingZones = []
271 for et1 in predictedTrajectories1: 270 for et1 in predictedTrajectories1:
272 for et2 in predictedTrajectories2: 271 for et2 in predictedTrajectories2:
273 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) 272 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
274
275 if collision: 273 if collision:
276 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) 274 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
277 elif computeCZ: # check if there is a crossing zone 275 elif computeCZ: # check if there is a crossing zone
278 # TODO? zone should be around the points at which the traj are the closest 276 # TODO? zone should be around the points at which the traj are the closest
279 # look for CZ at different times, otherwise it would be a collision 277 # look for CZ at different times, otherwise it would be a collision
307 return '{0} {1}'.format(self.name, self.maxSpeed) 305 return '{0} {1}'.format(self.name, self.maxSpeed)
308 306
309 def generatePredictedTrajectories(self, obj, instant): 307 def generatePredictedTrajectories(self, obj, instant):
310 return [] 308 return []
311 309
312 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): 310 # def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
313 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) 311 # return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
314 312
315 def computeCrossingsCollisions(self, 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): 313 def computeCrossingsCollisions(self, 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):
316 #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): 314 #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):
317 '''Computes all crossing and collision points at each common instant for two road users. ''' 315 '''Computes all crossing and collision points at each common instant for two road users. '''
318 collisionPoints={} 316 collisionPoints={}
319 crossingZones={} 317 crossingZones={}
320 if timeInterval: 318 if timeInterval:
321 commonTimeInterval = timeInterval 319 commonTimeInterval = timeInterval
322 else: 320 else:
323 commonTimeInterval = obj1.commonTimeInterval(obj2) 321 commonTimeInterval = obj1.commonTimeInterval(obj2)
324 if nProcesses == 1: 322 if nProcesses == 1:
325 if usePrototypes: 323 # if usePrototypes:
326 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) 324 # firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
327 commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors 325 # commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors
328 commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors 326 # commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors
329 for i in commonTimeIntervalList2: 327 # for i in commonTimeIntervalList2:
330 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 # 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)
331 if len(cp) != 0: 329 # if len(cp) != 0:
332 collisionPoints[i] = cp 330 # collisionPoints[i] = cp
333 if len(cz) != 0: 331 # if len(cz) != 0:
334 crossingZones[i] = cz 332 # crossingZones[i] = cz
335 if collisionPoints!={} or crossingZones!={}: 333 # if collisionPoints!={} or crossingZones!={}:
336 for i in commonTimeIntervalList1: 334 # for i in commonTimeIntervalList1:
337 if i not in commonTimeIntervalList2: 335 # if i not in commonTimeIntervalList2:
338 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 # 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)
339 if len(cp) != 0: 337 # if len(cp) != 0:
340 collisionPoints[i] = cp 338 # collisionPoints[i] = cp
341 if len(cz) != 0: 339 # if len(cz) != 0:
342 crossingZones[i] = cz 340 # crossingZones[i] = cz
343 else: 341 # else:
344 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors 342 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
345 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) 343 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
346 if len(cp) != 0: 344 if len(cp) != 0:
347 collisionPoints[i] = cp 345 collisionPoints[i] = cp
348 if len(cz) != 0: 346 if len(cz) != 0:
349 crossingZones[i] = cz 347 crossingZones[i] = cz
350 else: 348 else:
351 pool = Pool(processes = nProcesses) 349 pool = Pool(processes = nProcesses)
352 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]] 350 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] #,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype
353 #results = [j.get() for j in jobs] 351 #results = [j.get() for j in jobs]
354 #results.sort() 352 #results.sort()
355 for j in jobs: 353 for j in jobs:
356 i, cp, cz = j.get() 354 i, cp, cz = j.get()
357 #if len(cp) != 0 or len(cz) != 0: 355 #if len(cp) != 0 or len(cz) != 0:
435 probability, 433 probability,
436 maxSpeed = self.maxSpeed)) 434 maxSpeed = self.maxSpeed))
437 return predictedTrajectories 435 return predictedTrajectories
438 436
439 class PointSetPredictionParameters(PredictionParameters): 437 class PointSetPredictionParameters(PredictionParameters):
440 # todo generate several trajectories with normal adaptatoins from each position (feature)
441 def __init__(self, maxSpeed): 438 def __init__(self, maxSpeed):
442 PredictionParameters.__init__(self, 'point set', maxSpeed) 439 PredictionParameters.__init__(self, 'point set', maxSpeed)
443 #self.nPredictedTrajectories = nPredictedTrajectories 440 #self.nPredictedTrajectories = nPredictedTrajectories
444 441
445 def generatePredictedTrajectories(self, obj, instant): 442 def generatePredictedTrajectories(self, obj, instant):
454 return predictedTrajectories 451 return predictedTrajectories
455 else: 452 else:
456 print('Object {} has no features'.format(obj.getNum())) 453 print('Object {} has no features'.format(obj.getNum()))
457 return None 454 return None
458 455
456
459 class EvasiveActionPredictionParameters(PredictionParameters): 457 class EvasiveActionPredictionParameters(PredictionParameters):
460 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False): 458 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
461 '''Suggested acceleration distribution may not be symmetric, eg 459 '''Suggested acceleration distribution may not be symmetric, eg
462 lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)''' 460 lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)'''
463 461
578 else: 576 else:
579 pass # compute pPET 577 pass # compute pPET
580 578
581 return currentInstant, collisionPoints, crossingZones 579 return currentInstant, collisionPoints, crossingZones
582 580
583 ####
584 # Other Methods
585 ####
586 class PrototypePredictionParameters(PredictionParameters): 581 class PrototypePredictionParameters(PredictionParameters):
587 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True): 582 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): # lcss parameters
588 name = 'prototype' 583 PredictionParameters.__init__(self, 'prototypes', None)
589 PredictionParameters.__init__(self, name, maxSpeed) 584 self.prototypes = prototypes
590 self.nPredictedTrajectories = nPredictedTrajectories 585 self.nPredictedTrajectories = nPredictedTrajectories
586 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance)
587 #self.similarityFunc = lambda x,y : self.lcss.computeNormalized(x, y)
588 self.minSimilarity = minSimilarity
589 self.minFeatureTime = minFeatureTime
591 self.constantSpeed = constantSpeed 590 self.constantSpeed = constantSpeed
591 self.useFeatures = useFeatures
592 592
593 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories): 593 def generatePredictedTrajectories(self, obj, instant):
594 predictedTrajectories = [] 594 predictedTrajectories = []
595 initialPosition = obj.getPositionAtInstant(instant) 595 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime:
596 initialVelocity = obj.getVelocityAtInstant(instant) 596 if self.useFeatures and obj.hasFeatures():
597 for prototypeTraj in prototypeTrajectories.keys(): 597 # get current features existing for the most time, sort on first instant of feature and take n first
598 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj])) 598 pass
599 else:
600 if not hasattr(obj, 'prototypeSimilarities'):
601 obj.prototypeSimilarities = []
602 for proto in self.prototypes:
603 self.lcss.similarities(proto.getMovingObject().getPositions().asArray().T, obj.getPositions().asArray().T)
604 similarities = self.lcss.similarityTable[-1, :-1].astype(float)
605 obj.prototypeSimilarities.append(similarities/np.minimum(np.arange(1.,len(similarities)+1), proto.getMovingObject().length()*np.ones(len(similarities))))
606 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities):
607 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
608 initialPosition = obj.getPositionAtInstant(instant)
609 initialVelocity = obj.getVelocityAtInstant(instant)
610 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings()))
599 return predictedTrajectories 611 return predictedTrajectories
600 612
601 if __name__ == "__main__": 613 if __name__ == "__main__":
602 import doctest 614 import doctest
603 import unittest 615 import unittest