Mercurial Hosting > traffic-intelligence
diff python/extrapolation.py @ 250:59f547aebaac
modified prediction functions, added norm/angle representation of Points
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 23 Jul 2012 02:50:26 -0400 |
parents | bd8ab323c198 |
children | 13ec22bec5d4 |
line wrap: on
line diff
--- a/python/extrapolation.py Sat Jul 21 00:50:42 2012 -0400 +++ b/python/extrapolation.py Mon Jul 23 02:50:26 2012 -0400 @@ -1,7 +1,8 @@ #! /usr/bin/env python '''Library for moving object extrapolation hypotheses''' -import moving +import moving +import math class ExtrapolatedTrajectory: '''Class for extrapolated trajectories with lazy evaluation @@ -16,19 +17,34 @@ TODO add limits if acceleration TODO generalize by passing a series of velocities/accelerations''' - def __init__(self, initialPosition, initialVelocity, initialAccleration = 0, probability = 1): - self.initialPosition = initialPosition - self.initialVelocity = initialVelocity - self.initialAccleration = initialAccleration + 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 = {} - self.predictedVelocities = {} + self.predictedPositions = {0: initialPosition} + self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} def predictPosition(self, nTimeSteps): - if not nTimeSteps in self.predictedPositions.keys(): - self.predictedPositions[nTimeSteps] = moving.predictPosition(nTimeSteps, self.initialPosition, self.initialVelocity, self.initialAcceleration) + if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): + self.predictedPositions[nTimeSteps], self.predictedspeedOrientations[nTimeSteps] = predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedspeedOrientations[nTimeSteps-1], self.control, maxSpeed) return self.predictedPositions[nTimeSteps] +class ExtrapolatedTrajectoryAdaptation(ExtrapolatedTrajectoryConstant): + '''Random small adaptation of vehicle control ''' + def __init__(self, initialPosition, initialVelocity, accelerationBounds, steeringBounds, probability = 1, maxSpeed = None): + self.accelerationBounds = accelerationBounds + self.steeringBounds = steeringBounds + self.maxSpeed = maxSpeed + self.probability = probability + self.predictedPositions = {0: initialPosition} + self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} + + def predictPosition(self, nTimeSteps): + # draw acceleration and steering + # TODO should the predictPosition be in the base class ?? + self.control = moving.NormAngle(0,0) + return ExtrapolatedTrajectoryConstant.predictPosition(self, nTimeSteps) + # Default values: to remove because we cannot tweak that from a script where the value may be different FPS= 25 # No. of frame per second (FPS) vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec