Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 942:ab13aaf41432

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
implemented motion prediction with prototypes at constant ratio, with tests
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 18 Jul 2017 18:01:16 -0400
parents d8ab183a7351
children b1e8453c207c
comparison
equal deleted inserted replaced
941:c5191acb025f 942:ab13aaf41432
48 self.predictedPositions = {0: initialPosition} 48 self.predictedPositions = {0: initialPosition}
49 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} 49 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
50 50
51 def getControl(self): 51 def getControl(self):
52 return self.control 52 return self.control
53
54 def findNearestParams(initialPosition, prototypeTrajectory):
55 ''' nearest parameters are the index of minDistance and the orientation '''
56 distances=[]
57 for position in prototypeTrajectory.positions:
58 distances.append(moving.Point.distanceNorm2(initialPosition, position))
59 minDistanceIndex= np.argmin(distances)
60 return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle
61 53
62 class PredictedTrajectoryPrototype(PredictedTrajectory): 54 class PredictedTrajectoryPrototype(PredictedTrajectory):
63 '''Predicted trajectory that follows a prototype trajectory 55 '''Predicted trajectory that follows a prototype trajectory
64 The prototype is in the format of a moving.Trajectory: it could be 56 The prototype is in the format of a moving.Trajectory: it could be
65 1. an observed trajectory (extracted from video) 57 1. an observed trajectory (extracted from video)
82 self.probability = probability 74 self.probability = probability
83 self.predictedPositions = {0: initialPosition} 75 self.predictedPositions = {0: initialPosition}
84 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition) 76 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
85 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) 77 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx)
86 self.initialSpeed = initialVelocity.norm2() 78 self.initialSpeed = initialVelocity.norm2()
87 #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[1])}#moving.NormAngle.fromPoint(initialVelocity)} 79 if not constantSpeed:
80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
88 81
89 def predictPosition(self, nTimeSteps): 82 def predictPosition(self, nTimeSteps):
90 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
91 if self.constantSpeed: 84 if self.constantSpeed:
92 # calculate cumulative distance 85 # calculate cumulative distance
93 traj = self.prototype.getPositions() 86 traj = self.prototype.getPositions()
87 trajLength = traj.length()
94 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) 88 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
95 i = self.closestPointIdx 89 i = self.closestPointIdx
96 while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance: 90 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
97 i += 1 91 i += 1
98 if i == traj.length(): 92 if i == trajLength:
99 v = self.prototype.getVelocityAt(-1) 93 v = self.prototype.getVelocityAt(-1)
100 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) 94 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
101 else: 95 else:
102 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) 96 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1))
103 else: # see c++ code, calculate ratio 97 else:
104 speedNorm= self.predictedSpeedOrientations[0].norm 98 traj = self.prototype.getPositions()
105 instant=findNearestParams(self.predictedPositions[0],self.prototype)[0] 99 trajLength = traj.length()
106 prototypeSpeeds= self.prototype.getSpeeds()[instant:] 100 nSteps = self.ratio*nTimeSteps+self.closestPointIdx
107 ratio=float(speedNorm)/prototypeSpeeds[0] 101 i = int(np.floor(nSteps))
108 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] 102 if nSteps < trajLength-1:
109 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototype)[1] 103 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
110 if nTimeSteps<len(resampledSpeeds):
111 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype)
112 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
113 else: 104 else:
114 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype) 105 v = self.prototype.getVelocityAt(-1)
115 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) 106 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[-1]+v*(nSteps-trajLength+1)
116
117 return self.predictedPositions[nTimeSteps] 107 return self.predictedPositions[nTimeSteps]
118 108
119 class PredictedTrajectoryRandomControl(PredictedTrajectory): 109 class PredictedTrajectoryRandomControl(PredictedTrajectory):
120 '''Random vehicle control: suitable for normal adaptation''' 110 '''Random vehicle control: suitable for normal adaptation'''
121 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None): 111 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None):