Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 619:dc2d0a0d7fe1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 10 Dec 2014 15:27:08 -0500
parents 306db0f3c7a2
children 0a5e89d6fc62
comparison
equal deleted inserted replaced
596:04a8304e13f0 619:dc2d0a0d7fe1
2 '''Library for motion prediction methods''' 2 '''Library for motion prediction methods'''
3 3
4 import moving 4 import moving
5 import math 5 import math
6 import random 6 import random
7 import numpy as np
8 from utils import LCSS
7 9
8 class PredictedTrajectory: 10 class PredictedTrajectory:
9 '''Class for predicted trajectories with lazy evaluation 11 '''Class for predicted trajectories with lazy evaluation
10 if the predicted position has not been already computed, compute it 12 if the predicted position has not been already computed, compute it
11 13
44 self.predictedPositions = {0: initialPosition} 46 self.predictedPositions = {0: initialPosition}
45 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} 47 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
46 48
47 def getControl(self): 49 def getControl(self):
48 return self.control 50 return self.control
51
52 def findNearestParams(initialPosition,prototypeTrajectory):
53 ''' nearest parameters are the index of minDistance and the orientation '''
54 distances=[]
55 for position in prototypeTrajectory.positions:
56 distances.append(moving.Point.distanceNorm2(initialPosition, position))
57 minDistanceIndex= np.argmin(distances)
58 return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle
49 59
50 class PredictedTrajectoryPrototype(PredictedTrajectory): 60 class PredictedTrajectoryPrototype(PredictedTrajectory):
51 '''Predicted trajectory that follows a prototype trajectory 61 '''Predicted trajectory that follows a prototype trajectory
52 The prototype is in the format of a moving.Trajectory: it could be 62 The prototype is in the format of a moving.Trajectory: it could be
53 1. an observed trajectory (extracted from video) 63 1. an observed trajectory (extracted from video)
62 def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = True, probability = 1.): 72 def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = True, probability = 1.):
63 self.prototypeTrajectory = prototypeTrajectory 73 self.prototypeTrajectory = prototypeTrajectory
64 self.constantSpeed = constantSpeed 74 self.constantSpeed = constantSpeed
65 self.probability = probability 75 self.probability = probability
66 self.predictedPositions = {0: initialPosition} 76 self.predictedPositions = {0: initialPosition}
67 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} 77 self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
68 78
69 def predictPosition(self, nTimeSteps): 79 def predictPosition(self, nTimeSteps):
70 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 80 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
71 if constantSpeed: 81 if self.constantSpeed:
72 # calculate cumulative distance 82 # calculate cumulative distance
73 pass 83 speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm
84 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
85 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype)
86 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
87
74 else: # see c++ code, calculate ratio 88 else: # see c++ code, calculate ratio
75 pass 89 speedNorm= self.predictedSpeedOrientations[0].norm
90 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]
91 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:]
92 ratio=float(speedNorm)/prototypeSpeeds[0]
93 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
94 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
95 if nTimeSteps<len(resampledSpeeds):
96 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype)
97 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
98 else:
99 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype)
100 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
101
76 return self.predictedPositions[nTimeSteps] 102 return self.predictedPositions[nTimeSteps]
77 103
78 class PredictedTrajectoryRandomControl(PredictedTrajectory): 104 class PredictedTrajectoryRandomControl(PredictedTrajectory):
79 '''Random vehicle control: suitable for normal adaptation''' 105 '''Random vehicle control: suitable for normal adaptation'''
80 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None): 106 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None):
139 title('instant {0}'.format(currentInstant)) 165 title('instant {0}'.format(currentInstant))
140 axis('equal') 166 axis('equal')
141 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) 167 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
142 close() 168 close()
143 169
144 def computeCrossingsCollisionsAtInstant(predictionParams, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): 170 def calculateProbability(nMatching,similarity,objects):
171 sumFrequencies=sum([nMatching[p] for p in similarity.keys()])
172 prototypeProbability={}
173 for i in similarity.keys():
174 prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies
175 sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability.keys()])
176 probabilities={}
177 for i in prototypeProbability.keys():
178 probabilities[objects[i]]= float(prototypeProbability[i])/sumProbabilities
179 return probabilities
180
181 def findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,spatialThreshold=1.0, delta=180):
182 ''' behaviour prediction first step'''
183 if route[0] not in noiseEntryNums:
184 prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]]
185 elif route[1] not in noiseExitNums:
186 prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]]
187 else:
188 prototypesRoutes=[x for x in sorted(prototypes.keys())]
189 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta)
190 similarity={}
191 for y in prototypesRoutes:
192 if y in prototypes.keys():
193 prototypesIDs=prototypes[y]
194 for x in prototypesIDs:
195 s=lcss.computeNormalized(partialObjPositions, objects[x].positions)
196 if s >= minSimilarity:
197 similarity[x]=s
198
199 if mostMatched==None:
200 probabilities= calculateProbability(nMatching,similarity,objects)
201 return probabilities
202 else:
203 mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched]
204 keys=[k for k in similarity.keys() if similarity[k] in mostMatchedValues]
205 newSimilarity={}
206 for i in keys:
207 newSimilarity[i]=similarity[i]
208 probabilities= calculateProbability(nMatching,newSimilarity,objects)
209 return probabilities
210
211 def findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,spatialThreshold=1.0, delta=180):
212 if useDestination:
213 prototypesRoutes=[route]
214 else:
215 if route[0] not in noiseEntryNums:
216 prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]]
217 elif route[1] not in noiseExitNums:
218 prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]]
219 else:
220 prototypesRoutes=[x for x in sorted(prototypes.keys())]
221 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta)
222 similarity={}
223 for y in prototypesRoutes:
224 if y in prototypes.keys():
225 prototypesIDs=prototypes[y]
226 for x in prototypesIDs:
227 s=lcss.computeNormalized(partialObjPositions, objects[x].positions)
228 if s >= minSimilarity:
229 similarity[x]=s
230
231 newSimilarity={}
232 for i in similarity.keys():
233 if i in secondStepPrototypes.keys():
234 for j in secondStepPrototypes[i]:
235 newSimilarity[j]=similarity[i]
236 probabilities= calculateProbability(nMatching,newSimilarity,objects)
237 return probabilities
238
239 def getPrototypeTrajectory(obj,route,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
240 partialInterval=moving.Interval(obj.getFirstInstant(),currentInstant)
241 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions
242 if useSpeedPrototype:
243 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
244 else:
245 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
246 return prototypeTrajectories
247
248 def computeCrossingsCollisionsAtInstant(predictionParams,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):
145 '''returns the lists of collision points and crossing zones''' 249 '''returns the lists of collision points and crossing zones'''
146 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) 250 if usePrototypes:
147 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) 251 prototypeTrajectories1=getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
252 prototypeTrajectories2= getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
253 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1)
254 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)
255 else:
256 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
257 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
148 258
149 collisionPoints = [] 259 collisionPoints = []
150 crossingZones = [] 260 crossingZones = []
151 for et1 in predictedTrajectories1: 261 for et1 in predictedTrajectories1:
152 for et2 in predictedTrajectories2: 262 for et2 in predictedTrajectories2:
165 while not cz and t2 < timeHorizon: 275 while not cz and t2 < timeHorizon:
166 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: 276 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
167 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) 277 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
168 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) 278 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
169 if cz != None: 279 if cz != None:
170 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) 280 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
281 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV)))
171 t2 += 1 282 t2 += 1
172 t1 += 1 283 t1 += 1
173 284
174 if debug: 285 if debug:
175 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) 286 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
176 287
177 return currentInstant, collisionPoints, crossingZones 288 return currentInstant, collisionPoints, crossingZones
178 289
179 def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): 290 def computeCrossingsCollisions(predictionParams, 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):
180 '''Computes all crossing and collision points at each common instant for two road users. ''' 291 '''Computes all crossing and collision points at each common instant for two road users. '''
181 collisionPoints={} 292 collisionPoints={}
182 crossingZones={} 293 crossingZones={}
183 if timeInterval: 294 if timeInterval:
184 commonTimeInterval = timeInterval 295 commonTimeInterval = timeInterval
185 else: 296 else:
186 commonTimeInterval = obj1.commonTimeInterval(obj2) 297 commonTimeInterval = obj1.commonTimeInterval(obj2)
187 if nProcesses == 1: 298 if nProcesses == 1:
188 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors 299 if usePrototypes:
189 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) 300 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
190 if len(cp) != 0: 301 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors
191 collisionPoints[i] = cp 302 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors
192 if len(cz) != 0: 303 for i in commonTimeIntervalList2:
193 crossingZones[i] = cz 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)
305 if len(cp) != 0:
306 collisionPoints[i] = cp
307 if len(cz) != 0:
308 crossingZones[i] = cz
309 if collisionPoints!={} or crossingZones!={}:
310 for i in commonTimeIntervalList1:
311 if i not in commonTimeIntervalList2:
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
194 else: 324 else:
195 from multiprocessing import Pool 325 from multiprocessing import Pool
196 pool = Pool(processes = nProcesses) 326 pool = Pool(processes = nProcesses)
197 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] 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]]
198 #results = [j.get() for j in jobs] 328 #results = [j.get() for j in jobs]
199 #results.sort() 329 #results.sort()
200 for j in jobs: 330 for j in jobs:
201 i, cp, cz = j.get() 331 i, cp, cz = j.get()
202 #if len(cp) != 0 or len(cz) != 0: 332 #if len(cp) != 0 or len(cz) != 0:
216 return '{0} {1}'.format(self.name, self.maxSpeed) 346 return '{0} {1}'.format(self.name, self.maxSpeed)
217 347
218 def generatePredictedTrajectories(self, obj, instant): 348 def generatePredictedTrajectories(self, obj, instant):
219 return [] 349 return []
220 350
221 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): 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):
222 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) 352 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
223 353
224 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): 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):
225 return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses) 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)
226 356
227 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): 357 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
228 '''Computes only collision probabilities 358 '''Computes only collision probabilities
229 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'''
230 collisionProbabilities = {} 360 collisionProbabilities = {}
426 return [],[] 556 return [],[]
427 557
428 #### 558 ####
429 # Other Methods 559 # Other Methods
430 #### 560 ####
431 561 class PrototypePredictionParameters(PredictionParameters):
432 562 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True):
433 563 name = 'prototype'
434 564 PredictionParameters.__init__(self, name, maxSpeed)
565 self.nPredictedTrajectories = nPredictedTrajectories
566 self.constantSpeed = constantSpeed
567
568 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories):
569 predictedTrajectories = []
570 initialPosition = obj.getPositionAtInstant(instant)
571 initialVelocity = obj.getVelocityAtInstant(instant)
572 for prototypeTraj in prototypeTrajectories.keys():
573 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj]))
574 return predictedTrajectories
435 575
436 if __name__ == "__main__": 576 if __name__ == "__main__":
437 import doctest 577 import doctest
438 import unittest 578 import unittest
439 suite = doctest.DocFileSuite('tests/prediction.txt') 579 suite = doctest.DocFileSuite('tests/prediction.txt')