Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 998:933670761a57

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Sun, 27 May 2018 23:22:48 -0400
parents f026ce2af637
children
comparison
equal deleted inserted replaced
997:4f3387a242a1 998:933670761a57
22 self.predictedSpeedOrientations = {} 22 self.predictedSpeedOrientations = {}
23 #self.collisionPoints = {} 23 #self.collisionPoints = {}
24 #self.crossingZones = {} 24 #self.crossingZones = {}
25 25
26 def predictPosition(self, nTimeSteps): 26 def predictPosition(self, nTimeSteps):
27 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 27 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
28 self.predictPosition(nTimeSteps-1) 28 self.predictPosition(nTimeSteps-1)
29 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed) 29 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed)
30 return self.predictedPositions[nTimeSteps] 30 return self.predictedPositions[nTimeSteps]
31 31
32 def getPredictedTrajectory(self): 32 def getPredictedTrajectory(self):
33 return moving.Trajectory.fromPointList(self.predictedPositions.values()) 33 return moving.Trajectory.fromPointList(list(self.predictedPositions.values()))
34 34
35 def getPredictedSpeeds(self): 35 def getPredictedSpeeds(self):
36 return [so.norm for so in self.predictedSpeedOrientations.values()] 36 return [so.norm for so in self.predictedSpeedOrientations.values()]
37 37
38 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs): 38 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
81 self.initialSpeed = initialVelocity.norm2() 81 self.initialSpeed = initialVelocity.norm2()
82 if not constantSpeed: 82 if not constantSpeed:
83 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2() 83 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
84 84
85 def predictPosition(self, nTimeSteps): 85 def predictPosition(self, nTimeSteps):
86 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 86 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
87 deltaPosition = copy(self.deltaPosition) 87 deltaPosition = copy(self.deltaPosition)
88 if self.constantSpeed: 88 if self.constantSpeed:
89 traj = self.prototype.getPositions() 89 traj = self.prototype.getPositions()
90 trajLength = traj.length() 90 trajLength = traj.length()
91 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) 91 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
170 if printFigure: 170 if printFigure:
171 clf() 171 clf()
172 else: 172 else:
173 figure() 173 figure()
174 for et in predictedTrajectories1: 174 for et in predictedTrajectories1:
175 for t in xrange(int(np.round(timeHorizon))): 175 for t in range(int(np.round(timeHorizon))):
176 et.predictPosition(t) 176 et.predictPosition(t)
177 et.plot('rx') 177 et.plot('rx')
178 for et in predictedTrajectories2: 178 for et in predictedTrajectories2:
179 for t in xrange(int(np.round(timeHorizon))): 179 for t in range(int(np.round(timeHorizon))):
180 et.predictPosition(t) 180 et.predictPosition(t)
181 et.plot('bx') 181 et.plot('bx')
182 obj1.plot('r', withOrigin = True) 182 obj1.plot('r', withOrigin = True)
183 obj2.plot('b', withOrigin = True) 183 obj2.plot('b', withOrigin = True)
184 title('instant {0}'.format(currentInstant)) 184 title('instant {0}'.format(currentInstant))
185 axis('equal') 185 axis('equal')
186 if printFigure: 186 if printFigure:
187 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) 187 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
188 188
189 def calculateProbability(nMatching,similarity,objects): 189 def calculateProbability(nMatching,similarity,objects):
190 sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) 190 sumFrequencies=sum([nMatching[p] for p in similarity])
191 prototypeProbability={} 191 prototypeProbability={}
192 for i in similarity.keys(): 192 for i in similarity:
193 prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies 193 prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies
194 sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability.keys()]) 194 sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability])
195 probabilities={} 195 probabilities={}
196 for i in prototypeProbability.keys(): 196 for i in prototypeProbability:
197 probabilities[objects[i]]= float(prototypeProbability[i])/sumProbabilities 197 probabilities[objects[i]]= float(prototypeProbability[i])/sumProbabilities
198 return probabilities 198 return probabilities
199 199
200 def findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,spatialThreshold=1.0, delta=180): 200 def findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,spatialThreshold=1.0, delta=180):
201 ''' behaviour prediction first step''' 201 ''' behaviour prediction first step'''
206 else: 206 else:
207 prototypesRoutes=[x for x in sorted(prototypes.keys())] 207 prototypesRoutes=[x for x in sorted(prototypes.keys())]
208 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta) 208 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta)
209 similarity={} 209 similarity={}
210 for y in prototypesRoutes: 210 for y in prototypesRoutes:
211 if y in prototypes.keys(): 211 if y in prototypes:
212 prototypesIDs=prototypes[y] 212 prototypesIDs=prototypes[y]
213 for x in prototypesIDs: 213 for x in prototypesIDs:
214 s=lcss.computeNormalized(partialObjPositions, objects[x].positions) 214 s=lcss.computeNormalized(partialObjPositions, objects[x].positions)
215 if s >= minSimilarity: 215 if s >= minSimilarity:
216 similarity[x]=s 216 similarity[x]=s
218 if mostMatched==None: 218 if mostMatched==None:
219 probabilities= calculateProbability(nMatching,similarity,objects) 219 probabilities= calculateProbability(nMatching,similarity,objects)
220 return probabilities 220 return probabilities
221 else: 221 else:
222 mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched] 222 mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched]
223 keys=[k for k in similarity.keys() if similarity[k] in mostMatchedValues] 223 keys=[k for k in similarity if similarity[k] in mostMatchedValues]
224 newSimilarity={} 224 newSimilarity={}
225 for i in keys: 225 for i in keys:
226 newSimilarity[i]=similarity[i] 226 newSimilarity[i]=similarity[i]
227 probabilities= calculateProbability(nMatching,newSimilarity,objects) 227 probabilities= calculateProbability(nMatching,newSimilarity,objects)
228 return probabilities 228 return probabilities
238 else: 238 else:
239 prototypesRoutes=[x for x in sorted(prototypes.keys())] 239 prototypesRoutes=[x for x in sorted(prototypes.keys())]
240 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta) 240 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta)
241 similarity={} 241 similarity={}
242 for y in prototypesRoutes: 242 for y in prototypesRoutes:
243 if y in prototypes.keys(): 243 if y in prototypes:
244 prototypesIDs=prototypes[y] 244 prototypesIDs=prototypes[y]
245 for x in prototypesIDs: 245 for x in prototypesIDs:
246 s=lcss.computeNormalized(partialObjPositions, objects[x].positions) 246 s=lcss.computeNormalized(partialObjPositions, objects[x].positions)
247 if s >= minSimilarity: 247 if s >= minSimilarity:
248 similarity[x]=s 248 similarity[x]=s
249 249
250 newSimilarity={} 250 newSimilarity={}
251 for i in similarity.keys(): 251 for i in similarity:
252 if i in secondStepPrototypes.keys(): 252 if i in secondStepPrototypes:
253 for j in secondStepPrototypes[i]: 253 for j in secondStepPrototypes[i]:
254 newSimilarity[j]=similarity[i] 254 newSimilarity[j]=similarity[i]
255 probabilities= calculateProbability(nMatching,newSimilarity,objects) 255 probabilities= calculateProbability(nMatching,newSimilarity,objects)
256 return probabilities 256 return probabilities
257 257
315 collisionPoints = {} 315 collisionPoints = {}
316 if computeCZ: 316 if computeCZ:
317 crossingZones = {} 317 crossingZones = {}
318 else: 318 else:
319 crossingZones = None 319 crossingZones = None
320 if timeInterval: 320 if timeInterval is not None:
321 commonTimeInterval = timeInterval 321 commonTimeInterval = timeInterval
322 else: 322 else:
323 commonTimeInterval = obj1.commonTimeInterval(obj2) 323 commonTimeInterval = obj1.commonTimeInterval(obj2)
324 #if nProcesses == 1: 324 #if nProcesses == 1:
325 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors 325 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
332 332
333 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): 333 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
334 '''Computes only collision probabilities 334 '''Computes only collision probabilities
335 Returns for each instant the collision probability and number of samples drawn''' 335 Returns for each instant the collision probability and number of samples drawn'''
336 collisionProbabilities = {} 336 collisionProbabilities = {}
337 if timeInterval: 337 if timeInterval is not None:
338 commonTimeInterval = timeInterval 338 commonTimeInterval = timeInterval
339 else: 339 else:
340 commonTimeInterval = obj1.commonTimeInterval(obj2) 340 commonTimeInterval = obj1.commonTimeInterval(obj2)
341 for i in list(commonTimeInterval)[:-1]: 341 for i in list(commonTimeInterval)[:-1]:
342 nCollisions = 0 342 nCollisions = 0
391 velocities = [f.getVelocityAtInstant(instant) for f in features] 391 velocities = [f.getVelocityAtInstant(instant) for f in features]
392 else: 392 else:
393 positions = [obj.getPositionAtInstant(instant)] 393 positions = [obj.getPositionAtInstant(instant)]
394 velocities = [obj.getVelocityAtInstant(instant)] 394 velocities = [obj.getVelocityAtInstant(instant)]
395 probability = 1./float(len(positions)*self.nPredictedTrajectories) 395 probability = 1./float(len(positions)*self.nPredictedTrajectories)
396 for i in xrange(self.nPredictedTrajectories): 396 for i in range(self.nPredictedTrajectories):
397 for initialPosition,initialVelocity in zip(positions, velocities): 397 for initialPosition,initialVelocity in zip(positions, velocities):
398 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition, 398 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
399 initialVelocity, 399 initialVelocity,
400 self.accelerationDistribution, 400 self.accelerationDistribution,
401 self.steeringDistribution, 401 self.steeringDistribution,
448 velocities = [f.getVelocityAtInstant(instant) for f in features] 448 velocities = [f.getVelocityAtInstant(instant) for f in features]
449 else: 449 else:
450 positions = [obj.getPositionAtInstant(instant)] 450 positions = [obj.getPositionAtInstant(instant)]
451 velocities = [obj.getVelocityAtInstant(instant)] 451 velocities = [obj.getVelocityAtInstant(instant)]
452 probability = 1./float(self.nPredictedTrajectories) 452 probability = 1./float(self.nPredictedTrajectories)
453 for i in xrange(self.nPredictedTrajectories): 453 for i in range(self.nPredictedTrajectories):
454 for initialPosition,initialVelocity in zip(positions, velocities): 454 for initialPosition,initialVelocity in zip(positions, velocities):
455 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, 455 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
456 initialVelocity, 456 initialVelocity,
457 moving.NormAngle(self.accelerationDistribution(), 457 moving.NormAngle(self.accelerationDistribution(),
458 self.steeringDistribution()), 458 self.steeringDistribution()),