Mercurial Hosting > traffic-intelligence
changeset 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 | 99173da7afae |
children | 4b71e81e3383 |
files | python/extrapolation.py python/moving.py |
diffstat | 2 files changed, 60 insertions(+), 11 deletions(-) [+] |
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
--- a/python/moving.py Sat Jul 21 00:50:42 2012 -0400 +++ b/python/moving.py Mon Jul 23 02:50:26 2012 -0400 @@ -228,10 +228,43 @@ from matplotlib.pyplot import scatter scatter([p.x for p in points],[p.y for p in points], c=color) +class NormAngle: + 'alternate encoding of a point, by its norm and orientation' -def predictPosition(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): + def __init__(self, norm, angle): + self.norm = norm + self.angle = angle + + @staticmethod + def fromPoint(p): + from math import atan2 + norm = p.norm2() + if norm > 0: + angle = atan2(p.x, p.y) + return NormAngle(norm, angle) + + def __add__(self, other): + 'a norm cannot become negative' + return NormAngle(max(self.norm+other.norm, 0), self.orientation+other.orientation) + + def getPoint(self): + from math import cos, sin + return Point(self.norm*cos(self.orientation), self.norm*sin(self.orientation)) + + +def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): '''Predicts the position in nTimeSteps at constant speed/acceleration''' - return initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps**2) + return initialVelocity + initialAcceleration.multiply(nTimeSteps),initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps**2*0.5) + +def predictPosition(position, speedOrientation, control, maxSpeed = None): + '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta) + speedOrientation is the other encoding of velocity, (speed, orientation) + speedOrientation and control are NormAngle''' + predictedSpeedTheta = speedOrientation+control + if maxSpeed: + predictedSpeedTheta.norm = min(predictedSpeedTheta.norm, maxSpeed) + predictedPosition = position+predictedSpeedTheta.getPoint() + return predictedPosition, predictedSpeedTheta class FlowVector: