Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 619:dc2d0a0d7fe1
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') |