Mercurial Hosting > traffic-intelligence
diff python/prediction.py @ 942:ab13aaf41432
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 |
line wrap: on
line diff
--- a/python/prediction.py Tue Jul 18 16:21:39 2017 -0400 +++ b/python/prediction.py Tue Jul 18 18:01:16 2017 -0400 @@ -50,14 +50,6 @@ def getControl(self): return self.control - -def findNearestParams(initialPosition, prototypeTrajectory): - ''' nearest parameters are the index of minDistance and the orientation ''' - distances=[] - for position in prototypeTrajectory.positions: - distances.append(moving.Point.distanceNorm2(initialPosition, position)) - minDistanceIndex= np.argmin(distances) - return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle class PredictedTrajectoryPrototype(PredictedTrajectory): '''Predicted trajectory that follows a prototype trajectory @@ -84,36 +76,34 @@ self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition) self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) self.initialSpeed = initialVelocity.norm2() - #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[1])}#moving.NormAngle.fromPoint(initialVelocity)} + if not constantSpeed: + self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2() def predictPosition(self, nTimeSteps): if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): if self.constantSpeed: # calculate cumulative distance traj = self.prototype.getPositions() + trajLength = traj.length() traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) i = self.closestPointIdx - while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance: + while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance: i += 1 - if i == traj.length(): + if i == trajLength: v = self.prototype.getVelocityAt(-1) self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) else: self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) - else: # see c++ code, calculate ratio - speedNorm= self.predictedSpeedOrientations[0].norm - instant=findNearestParams(self.predictedPositions[0],self.prototype)[0] - prototypeSpeeds= self.prototype.getSpeeds()[instant:] - ratio=float(speedNorm)/prototypeSpeeds[0] - resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] - anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototype)[1] - if nTimeSteps<len(resampledSpeeds): - self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype) - self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) + else: + traj = self.prototype.getPositions() + trajLength = traj.length() + nSteps = self.ratio*nTimeSteps+self.closestPointIdx + i = int(np.floor(nSteps)) + if nSteps < trajLength-1: + self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i]+(traj[i+1]-traj[i])*(nSteps-i) else: - self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype) - self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) - + v = self.prototype.getVelocityAt(-1) + self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[-1]+v*(nSteps-trajLength+1) return self.predictedPositions[nTimeSteps] class PredictedTrajectoryRandomControl(PredictedTrajectory):