Mercurial Hosting > traffic-intelligence

comparison python/prediction.py @ 610:0dc36203973d

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 = {}