Mercurial Hosting > traffic-intelligence
comparison 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 |
comparison
equal
deleted
inserted
replaced
249:99173da7afae | 250:59f547aebaac |
---|---|
1 #! /usr/bin/env python | 1 #! /usr/bin/env python |
2 '''Library for moving object extrapolation hypotheses''' | 2 '''Library for moving object extrapolation hypotheses''' |
3 | 3 |
4 import moving | 4 import moving |
5 import math | |
5 | 6 |
6 class ExtrapolatedTrajectory: | 7 class ExtrapolatedTrajectory: |
7 '''Class for extrapolated trajectories with lazy evaluation | 8 '''Class for extrapolated trajectories with lazy evaluation |
8 if the predicted position has not been already computed, compute it | 9 if the predicted position has not been already computed, compute it |
9 | 10 |
14 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): | 15 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): |
15 '''Extrapolated trajectory at constant speed or acceleration | 16 '''Extrapolated trajectory at constant speed or acceleration |
16 TODO add limits if acceleration | 17 TODO add limits if acceleration |
17 TODO generalize by passing a series of velocities/accelerations''' | 18 TODO generalize by passing a series of velocities/accelerations''' |
18 | 19 |
19 def __init__(self, initialPosition, initialVelocity, initialAccleration = 0, probability = 1): | 20 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None): |
20 self.initialPosition = initialPosition | 21 self.control = control |
21 self.initialVelocity = initialVelocity | 22 self.maxSpeed = maxSpeed |
22 self.initialAccleration = initialAccleration | |
23 self.probability = probability | 23 self.probability = probability |
24 self.predictedPositions = {} | 24 self.predictedPositions = {0: initialPosition} |
25 self.predictedVelocities = {} | 25 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
26 | 26 |
27 def predictPosition(self, nTimeSteps): | 27 def predictPosition(self, nTimeSteps): |
28 if not nTimeSteps in self.predictedPositions.keys(): | 28 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
29 self.predictedPositions[nTimeSteps] = moving.predictPosition(nTimeSteps, self.initialPosition, self.initialVelocity, self.initialAcceleration) | 29 self.predictedPositions[nTimeSteps], self.predictedspeedOrientations[nTimeSteps] = predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedspeedOrientations[nTimeSteps-1], self.control, maxSpeed) |
30 return self.predictedPositions[nTimeSteps] | 30 return self.predictedPositions[nTimeSteps] |
31 | |
32 class ExtrapolatedTrajectoryAdaptation(ExtrapolatedTrajectoryConstant): | |
33 '''Random small adaptation of vehicle control ''' | |
34 def __init__(self, initialPosition, initialVelocity, accelerationBounds, steeringBounds, probability = 1, maxSpeed = None): | |
35 self.accelerationBounds = accelerationBounds | |
36 self.steeringBounds = steeringBounds | |
37 self.maxSpeed = maxSpeed | |
38 self.probability = probability | |
39 self.predictedPositions = {0: initialPosition} | |
40 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} | |
41 | |
42 def predictPosition(self, nTimeSteps): | |
43 # draw acceleration and steering | |
44 # TODO should the predictPosition be in the base class ?? | |
45 self.control = moving.NormAngle(0,0) | |
46 return ExtrapolatedTrajectoryConstant.predictPosition(self, nTimeSteps) | |
31 | 47 |
32 # Default values: to remove because we cannot tweak that from a script where the value may be different | 48 # Default values: to remove because we cannot tweak that from a script where the value may be different |
33 FPS= 25 # No. of frame per second (FPS) | 49 FPS= 25 # No. of frame per second (FPS) |
34 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec | 50 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec |
35 deltaT= FPS*5 # extrapolatation time Horizon = 5 second | 51 deltaT= FPS*5 # extrapolatation time Horizon = 5 second |