Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 610:0dc36203973d
remove dublicated code for collision/crossing computations
author | MohamedGomaa |
---|---|
date | Wed, 03 Dec 2014 22:57:08 -0500 |
parents | 84690dfe5560 |
children | 233867934190 |
comparison
equal
deleted
inserted
replaced
607:84690dfe5560 | 610:0dc36203973d |
---|---|
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 | 7 import numpy as np |
8 import trajLearning | |
8 | 9 |
9 class PredictedTrajectory: | 10 class PredictedTrajectory: |
10 '''Class for predicted trajectories with lazy evaluation | 11 '''Class for predicted trajectories with lazy evaluation |
11 if the predicted position has not been already computed, compute it | 12 if the predicted position has not been already computed, compute it |
12 | 13 |
164 title('instant {0}'.format(currentInstant)) | 165 title('instant {0}'.format(currentInstant)) |
165 axis('equal') | 166 axis('equal') |
166 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) | 167 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) |
167 close() | 168 close() |
168 | 169 |
169 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,prototypeTrajectories1=None,prototypeTrajectories2=None): | 170 def getPrototypeTrajectory(obj,route,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): |
171 partialInterval=moving.Interval(obj.getFirstInstant(),currentInstant) | |
172 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions | |
173 if useSpeedPrototype: | |
174 prototypeTrajectories=trajLearning.findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) | |
175 else: | |
176 prototypeTrajectories=trajLearning.findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) | |
177 return prototypeTrajectories | |
178 | |
179 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): | |
170 '''returns the lists of collision points and crossing zones''' | 180 '''returns the lists of collision points and crossing zones''' |
171 if prototypeTrajectories1==None: | 181 if usePrototypes: |
182 prototypeTrajectories1=getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | |
183 prototypeTrajectories2= getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | |
184 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) | |
185 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) | |
186 else: | |
172 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) | 187 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) |
173 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) | 188 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) |
174 else: | |
175 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) | |
176 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) | |
177 | 189 |
178 collisionPoints = [] | 190 collisionPoints = [] |
179 crossingZones = [] | 191 crossingZones = [] |
180 for et1 in predictedTrajectories1: | 192 for et1 in predictedTrajectories1: |
181 for et2 in predictedTrajectories2: | 193 for et2 in predictedTrajectories2: |
203 | 215 |
204 if debug: | 216 if debug: |
205 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) | 217 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) |
206 return currentInstant,collisionPoints, crossingZones | 218 return currentInstant,collisionPoints, crossingZones |
207 | 219 |
208 def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1,prototypeTrajectories1=None,prototypeTrajectories2=None): | 220 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): |
209 '''Computes all crossing and collision points at each common instant for two road users. ''' | 221 '''Computes all crossing and collision points at each common instant for two road users. ''' |
210 collisionPoints={} | 222 collisionPoints={} |
211 crossingZones={} | 223 crossingZones={} |
212 if timeInterval: | 224 if timeInterval: |
213 commonTimeInterval = timeInterval | 225 commonTimeInterval = timeInterval |
214 else: | 226 else: |
215 commonTimeInterval = obj1.commonTimeInterval(obj2) | 227 commonTimeInterval = obj1.commonTimeInterval(obj2) |
216 if nProcesses == 1: | 228 if nProcesses == 1: |
229 if usePrototypes: | |
230 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) | |
231 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last)) | |
232 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last,step)) | |
233 for i in commonTimeIntervalList2[:-1]: # do not look at the 1 last position/velocities, often with errors | |
234 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) | |
235 if len(cp) != 0: | |
236 collisionPoints[i] = cp | |
237 if len(cz) != 0: | |
238 crossingZones[i] = cz | |
239 if collisionPoints!={} or crossingZones!={}: | |
240 for i in commonTimeIntervalList1[:-1]: | |
241 if i not in commonTimeIntervalList2: | |
242 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) | |
243 if len(cp) != 0: | |
244 collisionPoints[i] = cp | |
245 if len(cz) != 0: | |
246 crossingZones[i] = cz | |
247 | |
217 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 248 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
218 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2) | 249 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) |
219 if len(cp) != 0: | 250 if len(cp) != 0: |
220 collisionPoints[i] = cp | 251 collisionPoints[i] = cp |
221 if len(cz) != 0: | 252 if len(cz) != 0: |
222 crossingZones[i] = cz | 253 crossingZones[i] = cz |
223 else: | 254 else: |
224 from multiprocessing import Pool | 255 from multiprocessing import Pool |
225 pool = Pool(processes = nProcesses) | 256 pool = Pool(processes = nProcesses) |
226 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2)) for i in list(commonTimeInterval)[:-1]] | 257 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]] |
227 #results = [j.get() for j in jobs] | 258 #results = [j.get() for j in jobs] |
228 #results.sort() | 259 #results.sort() |
229 for j in jobs: | 260 for j in jobs: |
230 i, cp, cz = j.get() | 261 i, cp, cz = j.get() |
231 #if len(cp) != 0 or len(cz) != 0: | 262 #if len(cp) != 0 or len(cz) != 0: |
245 return '{0} {1}'.format(self.name, self.maxSpeed) | 276 return '{0} {1}'.format(self.name, self.maxSpeed) |
246 | 277 |
247 def generatePredictedTrajectories(self, obj, instant): | 278 def generatePredictedTrajectories(self, obj, instant): |
248 return [] | 279 return [] |
249 | 280 |
250 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,prototypeTrajectories1=None,prototypeTrajectories2=None): | 281 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): |
251 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2) | 282 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
252 | 283 |
253 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,prototypeTrajectories1=None,prototypeTrajectories2=None): | 284 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): |
254 return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,prototypeTrajectories1,prototypeTrajectories2) | 285 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) |
255 | 286 |
256 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): | 287 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
257 '''Computes only collision probabilities | 288 '''Computes only collision probabilities |
258 Returns for each instant the collision probability and number of samples drawn''' | 289 Returns for each instant the collision probability and number of samples drawn''' |
259 collisionProbabilities = {} | 290 collisionProbabilities = {} |