def __init__(self,
initialPosition,
initialVelocity,
control=moving.NormAngle(0, 0),
probability=1.,
maxSpeed=None):
self.control = control
self.maxSpeed = maxSpeed
self.probability = probability
self.predictedPositions = {0: initialPosition}
self.predictedSpeedOrientations = {
0: moving.NormAngle.fromPoint(initialVelocity)
}
def predictPosition(self, nTimeSteps):
if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
if self.constantSpeed:
# calculate cumulative distance
speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm
anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype)
self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
else: # see c++ code, calculate ratio
speedNorm= self.predictedSpeedOrientations[0].norm
instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]
prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:]
ratio=float(speedNorm)/prototypeSpeeds[0]
resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[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:
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)
return self.predictedPositions[nTimeSteps]
def __init__(self,
initialPosition,
initialVelocity,
prototypeTrajectory,
constantSpeed=True,
probability=1.):
self.prototypeTrajectory = prototypeTrajectory
self.constantSpeed = constantSpeed
self.probability = probability
self.predictedPositions = {0: initialPosition}
self.predictedSpeedOrientations = {
0:
moving.NormAngle(
moving.NormAngle.fromPoint(initialVelocity).norm,
findNearestParams(initialPosition, prototypeTrajectory)[1])
} # moving.NormAngle.fromPoint(initialVelocity)}
def generatePredictedTrajectories(self, obj, instant):
predictedTrajectories = []
if self.useFeatures and obj.hasFeatures():
features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
positions = [f.getPositionAtInstant(instant) for f in features]
velocities = [f.getVelocityAtInstant(instant) for f in features]
else:
positions = [obj.getPositionAtInstant(instant)]
velocities = [obj.getVelocityAtInstant(instant)]
probability = 1./float(self.nPredictedTrajectories)
for i in xrange(self.nPredictedTrajectories):
for initialPosition,initialVelocity in zip(positions, velocities):
predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
initialVelocity,
moving.NormAngle(self.accelerationDistribution(),
self.steeringDistribution()),
probability,
self.maxSpeed))
return predictedTrajectories
def getControl(self):
return moving.NormAngle(self.accelerationDistribution(),
self.steeringDistribution())