Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 945:05d4302bf67e

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
working motion pattern prediction with rotation and features
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 20 Jul 2017 14:29:46 -0400
parents 84ebe1b031f1
children e5970606066f
comparison
equal deleted inserted replaced
944:84ebe1b031f1 945:05d4302bf67e
3 3
4 import moving 4 import moving
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 import numpy as np 9 import numpy as np
9 from multiprocessing import Pool 10 from multiprocessing import Pool
10 11
11 12
12 class PredictedTrajectory(object): 13 class PredictedTrajectory(object):
72 self.prototype = prototype 73 self.prototype = prototype
73 self.constantSpeed = constantSpeed 74 self.constantSpeed = constantSpeed
74 self.probability = probability 75 self.probability = probability
75 self.predictedPositions = {0: initialPosition} 76 self.predictedPositions = {0: initialPosition}
76 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition) 77 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
77 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) 78 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) #should be computed in relative coordinates to position
79 self.theta = prototype.getVelocityAt(self.closestPointIdx).angle()
78 self.initialSpeed = initialVelocity.norm2() 80 self.initialSpeed = initialVelocity.norm2()
79 if not constantSpeed: 81 if not constantSpeed:
80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2() 82 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
81 83
82 def predictPosition(self, nTimeSteps): 84 def predictPosition(self, nTimeSteps):
83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 85 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
86 deltaPosition = copy(self.deltaPosition)
84 if self.constantSpeed: 87 if self.constantSpeed:
85 traj = self.prototype.getPositions() 88 traj = self.prototype.getPositions()
86 trajLength = traj.length() 89 trajLength = traj.length()
87 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) 90 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
88 i = self.closestPointIdx 91 i = self.closestPointIdx
89 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance: 92 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
90 i += 1 93 i += 1
91 if i == trajLength: 94 if i == trajLength:
92 v = self.prototype.getVelocityAt(-1) 95 v = self.prototype.getVelocityAt(-1)
93 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) 96 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
94 else: 97 else:
95 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) 98 v = self.prototype.getVelocityAt(i-1)
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))
96 else: 100 else:
97 traj = self.prototype.getPositions() 101 traj = self.prototype.getPositions()
98 trajLength = traj.length() 102 trajLength = traj.length()
99 nSteps = self.ratio*nTimeSteps+self.closestPointIdx 103 nSteps = self.ratio*nTimeSteps+self.closestPointIdx
100 i = int(np.floor(nSteps)) 104 i = int(np.floor(nSteps))
101 if nSteps < trajLength-1: 105 if nSteps < trajLength-1:
102 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i]+(traj[i+1]-traj[i])*(nSteps-i) 106 v = self.prototype.getVelocityAt(i)
107 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
103 else: 108 else:
104 v = self.prototype.getVelocityAt(-1) 109 v = self.prototype.getVelocityAt(-1)
105 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[-1]+v*(nSteps-trajLength+1) 110 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[-1]+v*(nSteps-trajLength+1)
106 return self.predictedPositions[nTimeSteps] 111 return self.predictedPositions[nTimeSteps]
107 112
108 class PredictedTrajectoryRandomControl(PredictedTrajectory): 113 class PredictedTrajectoryRandomControl(PredictedTrajectory):
109 '''Random vehicle control: suitable for normal adaptation''' 114 '''Random vehicle control: suitable for normal adaptation'''
110 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None): 115 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None):
550 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance) 555 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance)
551 self.minSimilarity = minSimilarity 556 self.minSimilarity = minSimilarity
552 self.minFeatureTime = minFeatureTime 557 self.minFeatureTime = minFeatureTime
553 self.constantSpeed = constantSpeed 558 self.constantSpeed = constantSpeed
554 self.useFeatures = useFeatures 559 self.useFeatures = useFeatures
560
561 def addPredictedTrajectories(self, predictedTrajectories, obj, instant):
562 if not hasattr(obj, 'prototypeSimilarities'):
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):
569 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
570 initialPosition = obj.getPositionAtInstant(instant)
571 initialVelocity = obj.getVelocityAtInstant(instant)
572 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings()))
555 573
556 def generatePredictedTrajectories(self, obj, instant): 574 def generatePredictedTrajectories(self, obj, instant):
557 predictedTrajectories = [] 575 predictedTrajectories = []
558 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime: 576 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime:
559 if self.useFeatures and obj.hasFeatures(): 577 if self.useFeatures and obj.hasFeatures():
560 # get current features existing for the most time, sort on first instant of feature and take n first 578 # get current features existing for the most time, sort on first instant of feature and take n first
561 pass 579 firstInstants = [(f,f.getFirstInstant()) for f in obj.getFeatures() if f.existsAtInstant(instant)]
580 firstInstants.sort(key = lambda t: t[1])
581 for f,t1 in firstInstants[:min(self.nPredictedTrajectories, len(firstInstants))]:
582 self.addPredictedTrajectories(predictedTrajectories, f, instant)
562 else: 583 else:
563 if not hasattr(obj, 'prototypeSimilarities'): 584 self.addPredictedTrajectories(predictedTrajectories, obj, instant)
564 obj.prototypeSimilarities = []
565 for proto in self.prototypes:
566 self.lcss.similarities(proto.getMovingObject().getPositions().asArray().T, obj.getPositions().asArray().T)
567 similarities = self.lcss.similarityTable[-1, :-1].astype(float)
568 obj.prototypeSimilarities.append(similarities/np.minimum(np.arange(1.,len(similarities)+1), proto.getMovingObject().length()*np.ones(len(similarities))))
569 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities):
570 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
571 initialPosition = obj.getPositionAtInstant(instant)
572 initialVelocity = obj.getVelocityAtInstant(instant)
573 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings()))
574 return predictedTrajectories 585 return predictedTrajectories
575 586
576 if __name__ == "__main__": 587 if __name__ == "__main__":
577 import doctest 588 import doctest
578 import unittest 589 import unittest