Mercurial Hosting > traffic-intelligence
changeset 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 | c5191acb025f |
children | b1e8453c207c |
files | python/prediction.py python/tests/prediction.txt |
diffstat | 2 files changed, 27 insertions(+), 25 deletions(-) [+] |
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):
--- a/python/tests/prediction.txt Tue Jul 18 16:21:39 2017 -0400 +++ b/python/tests/prediction.txt Tue Jul 18 18:01:16 2017 -0400 @@ -1,5 +1,5 @@ >>> from prediction import * ->>> import moving, storage +>>> import moving, storage, utils >>> from numpy import absolute, array >>> et = PredictedTrajectoryConstant(moving.Point(0,0), moving.Point(1,0)) @@ -57,3 +57,15 @@ >>> (absolute(array(traj.distances) - et.initialSpeed) < 1e-5).all() True +>>> et = PredictedTrajectoryPrototype(proto.getPositionAt(10)+moving.Point(0.6, 0.6), proto.getVelocityAt(10)*0.7, proto, False) +>>> absolute(et.initialSpeed - proto.getVelocityAt(10).norm2()*0.7) < 1e-5 +True +>>> proto = moving.MovingObject.generate(1, moving.Point(-5.,0.), moving.Point(1.,0.), moving.TimeInterval(0,10)) +>>> et = PredictedTrajectoryPrototype(proto.getPositionAt(0)+moving.Point(0., 1.), proto.getVelocityAt(0)*0.5, proto, False) +>>> for t in xrange(int(proto.length()/0.5)): x=et.predictPosition(t) +>>> et.predictPosition(10) # doctest:+ELLIPSIS +(0.0...,1.0...) +>>> et.predictPosition(12) # doctest:+ELLIPSIS +(1.0...,1.0...) + +