Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 949:d6c1c05d11f5
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(): |