Mercurial Hosting > traffic-intelligence

comparison python/prediction.py @ 949:d6c1c05d11f5

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
modified multithreading at the interaction level for safety computations
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 21 Jul 2017 17:52:56 -0400
parents 584b9405e494
children 668a85c963c3
comparison
equal deleted inserted replaced
948:584b9405e494 949:d6c1c05d11f5
5 from utils import LCSS 5 from utils import LCSS
6 6
7 import math, random 7 import math, random
8 from copy import copy 8 from copy import copy
9 import numpy as np 9 import numpy as np
10 from multiprocessing import Pool 10 #from multiprocessing import Pool
11 11
12 12
13 class PredictedTrajectory(object): 13 class PredictedTrajectory(object):
14 '''Class for predicted trajectories with lazy evaluation 14 '''Class for predicted trajectories with lazy evaluation
15 if the predicted position has not been already computed, compute it 15 if the predicted position has not been already computed, compute it
62 1. at constant speed (the instantaneous user speed) 62 1. at constant speed (the instantaneous user speed)
63 2. following the trajectory path, at the speed of the user 63 2. following the trajectory path, at the speed of the user
64 (applying a constant ratio equal 64 (applying a constant ratio equal
65 to the ratio of the user instantaneous speed and the trajectory closest speed)''' 65 to the ratio of the user instantaneous speed and the trajectory closest speed)'''
66 66
67 def __init__(self, initialPosition, initialVelocity, prototype, constantSpeed = False, probability = 1.): 67 def __init__(self, initialPosition, initialVelocity, prototype, constantSpeed = False, nFramesIgnore = 3, probability = 1.):
68 ''' prototype is a MovingObject 68 ''' prototype is a MovingObject
69 69
70 Prediction at constant speed will not work for unrealistic trajectories 70 Prediction at constant speed will not work for unrealistic trajectories
71 that do not follow a slowly changing velocity (eg moving object trajectories, 71 that do not follow a slowly changing velocity (eg moving object trajectories,
72 but is good for realistic motion (eg features)''' 72 but is good for realistic motion (eg features)'''
73 self.prototype = prototype 73 self.prototype = prototype
74 self.constantSpeed = constantSpeed 74 self.constantSpeed = constantSpeed
75 self.nFramesIgnore = nFramesIgnore
75 self.probability = probability 76 self.probability = probability
76 self.predictedPositions = {0: initialPosition} 77 self.predictedPositions = {0: initialPosition}
77 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition) 78 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
78 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) #should be computed in relative coordinates to position 79 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) #should be computed in relative coordinates to position
79 self.theta = prototype.getVelocityAt(self.closestPointIdx).angle() 80 self.theta = prototype.getVelocityAt(self.closestPointIdx).angle()
90 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) 91 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
91 i = self.closestPointIdx 92 i = self.closestPointIdx
92 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance: 93 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
93 i += 1 94 i += 1
94 if i == trajLength: 95 if i == trajLength:
95 v = self.prototype.getVelocityAt(-1) 96 v = self.prototype.getVelocityAt(-1-self.nFramesIgnore)
96 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) 97 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
97 else: 98 else:
98 v = self.prototype.getVelocityAt(i-1) 99 v = self.prototype.getVelocityAt(min(i-1, int(self.prototype.length())-1-self.nFramesIgnore))
99 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) 100 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1))
100 else: 101 else:
101 traj = self.prototype.getPositions() 102 traj = self.prototype.getPositions()
102 trajLength = traj.length() 103 trajLength = traj.length()
103 nSteps = self.ratio*nTimeSteps+self.closestPointIdx 104 nSteps = self.ratio*nTimeSteps+self.closestPointIdx
104 i = int(np.floor(nSteps)) 105 i = int(np.floor(nSteps))
105 if nSteps < trajLength-1: 106 if nSteps < trajLength-1:
106 v = self.prototype.getVelocityAt(i) 107 v = self.prototype.getVelocityAt(min(i, int(self.prototype.length())-1-self.nFramesIgnore))
107 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i]+(traj[i+1]-traj[i])*(nSteps-i) 108 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
108 else: 109 else:
109 v = self.prototype.getVelocityAt(-1) 110 v = self.prototype.getVelocityAt(-1-self.nFramesIgnore)
110 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[-1]+v*(nSteps-trajLength+1) 111 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[-1]+v*(nSteps-trajLength+1)
111 return self.predictedPositions[nTimeSteps] 112 return self.predictedPositions[nTimeSteps]
112 113
113 class PredictedTrajectoryRandomControl(PredictedTrajectory): 114 class PredictedTrajectoryRandomControl(PredictedTrajectory):
114 '''Random vehicle control: suitable for normal adaptation''' 115 '''Random vehicle control: suitable for normal adaptation'''
304 return '{0} {1}'.format(self.name, self.maxSpeed) 305 return '{0} {1}'.format(self.name, self.maxSpeed)
305 306
306 def generatePredictedTrajectories(self, obj, instant): 307 def generatePredictedTrajectories(self, obj, instant):
307 return [] 308 return []
308 309
309 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): 310 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):#, nProcesses = 1):
310 '''Computes all crossing and collision points at each common instant for two road users. ''' 311 '''Computes all crossing and collision points at each common instant for two road users. '''
311 collisionPoints={} 312 collisionPoints={}
312 crossingZones={} 313 crossingZones={}
313 if timeInterval: 314 if timeInterval:
314 commonTimeInterval = timeInterval 315 commonTimeInterval = timeInterval
315 else: 316 else:
316 commonTimeInterval = obj1.commonTimeInterval(obj2) 317 commonTimeInterval = obj1.commonTimeInterval(obj2)
317 if nProcesses == 1: 318 #if nProcesses == 1:
318 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors 319 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
319 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) 320 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
320 if len(cp) != 0: 321 if len(cp) != 0:
321 collisionPoints[i] = cp 322 collisionPoints[i] = cp
322 if len(cz) != 0: 323 if len(cz) != 0:
323 crossingZones[i] = cz 324 crossingZones[i] = cz
324 else: 325 # else:
325 pool = Pool(processes = nProcesses) 326 # pool = Pool(processes = nProcesses)
326 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] 327 # jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]]
327 for j in jobs: 328 # for j in jobs:
328 i, cp, cz = j.get() 329 # i, cp, cz = j.get()
329 if len(cp) != 0: 330 # if len(cp) != 0:
330 collisionPoints[i] = cp 331 # collisionPoints[i] = cp
331 if len(cz) != 0: 332 # if len(cz) != 0:
332 crossingZones[i] = cz 333 # crossingZones[i] = cz
333 pool.close() 334 # pool.close()
334 return collisionPoints, crossingZones 335 return collisionPoints, crossingZones
335 336
336 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): 337 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
337 '''Computes only collision probabilities 338 '''Computes only collision probabilities
338 Returns for each instant the collision probability and number of samples drawn''' 339 Returns for each instant the collision probability and number of samples drawn'''
556 self.minSimilarity = minSimilarity 557 self.minSimilarity = minSimilarity
557 self.minFeatureTime = minFeatureTime 558 self.minFeatureTime = minFeatureTime
558 self.constantSpeed = constantSpeed 559 self.constantSpeed = constantSpeed
559 self.useFeatures = useFeatures 560 self.useFeatures = useFeatures
560 561
562 def getLcss(self):
563 return self.lcss
564
561 def addPredictedTrajectories(self, predictedTrajectories, obj, instant): 565 def addPredictedTrajectories(self, predictedTrajectories, obj, instant):
562 if not hasattr(obj, 'prototypeSimilarities'): 566 obj.computeTrajectorySimilarities(self.prototypes, self.lcss)
563 obj.prototypeSimilarities = []
564 for proto in self.prototypes:
565 self.lcss.similarities(proto.getMovingObject().getPositions().asArray().T, obj.getPositions().asArray().T)
566 similarities = self.lcss.similarityTable[-1, :-1].astype(float)
567 obj.prototypeSimilarities.append(similarities/np.minimum(np.arange(1.,len(similarities)+1), proto.getMovingObject().length()*np.ones(len(similarities))))
568 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities): 567 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities):
569 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity: 568 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
570 initialPosition = obj.getPositionAtInstant(instant) 569 initialPosition = obj.getPositionAtInstant(instant)
571 initialVelocity = obj.getVelocityAtInstant(instant) 570 initialVelocity = obj.getVelocityAtInstant(instant)
572 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings())) 571 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings()))
573 572
574 def generatePredictedTrajectories(self, obj, instant): 573 def generatePredictedTrajectories(self, obj, instant):
575 predictedTrajectories = [] 574 predictedTrajectories = []
576 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime: 575 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime:
577 if self.useFeatures and obj.hasFeatures(): 576 if self.useFeatures and obj.hasFeatures():