Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 998:933670761a57
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()), |