Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 244:5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 17 Jul 2012 00:15:42 -0400 |
parents | e0988a8ace0c |
children | bd8ab323c198 |
comparison
equal
deleted
inserted
replaced
243:e0988a8ace0c | 244:5027c174ab90 |
---|---|
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 sys | 4 import moving |
5 | 5 |
6 import moving | 6 class ExtrapolatedTrajectory: |
7 '''Class for extrapolated trajectories with lazy evaluation | |
8 if the predicted position has not been already computed, compute it | |
9 | |
10 it should also have a probability''' | |
11 def predictPosition(self, nTimeSteps): | |
12 return None | |
13 | |
14 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): | |
15 '''Extrapolated trajectory at constant speed or acceleration | |
16 TODO add limits if acceleration | |
17 TODO generalize by passing a series of velocities/accelerations''' | |
18 | |
19 def __init__(self, initialPosition, initialVelocity, initialAccleration = 0, probability = 1): | |
20 self.initialPosition = initialPosition | |
21 self.initialVelocity = initialVelocity | |
22 self.initialAccleration = initialAccleration | |
23 self.probability = probability | |
24 self.predictedPositions = {} | |
25 self.predictedVelocities = {} | |
26 | |
27 def predictPosition(self, nTimeSteps): | |
28 if not nTimeSteps in self.predictedPositions.keys(): | |
29 self.predictedPositions[nTimeSteps] = moving.Point.predictPosition(nTimeSteps, self.initialPosition, self.initialVelocity, self.initialAcceleration) | |
30 return self.predictedPositions[nTimeSteps] | |
7 | 31 |
8 # Default values: to remove because we cannot tweak that from a script where the value may be different | 32 # Default values: to remove because we cannot tweak that from a script where the value may be different |
9 FPS= 25 # No. of frame per second (FPS) | 33 FPS= 25 # No. of frame per second (FPS) |
10 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec | 34 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec |
11 deltaT= FPS*5 # extrapolatation time Horizon = 5 second | 35 deltaT= FPS*5 # extrapolatation time Horizon = 5 second |
12 | 36 |
13 def motion (position, velocity, acceleration): | 37 def motion (position, velocity, acceleration): |
14 ''' extrapolation hypothesis: constant acceleration''' | 38 ''' extrapolation hypothesis: constant acceleration''' |
15 from math import atan2,cos,sin | 39 from math import atan2,cos,sin |
16 vInit= velocity | 40 vInit= velocity |
17 vInitial= velocity.norm2() | 41 vInitial= velocity.norm2() |
18 theta= atan2(velocity.y,velocity.x) | 42 theta= atan2(velocity.y,velocity.x) |
19 vFinal= vInitial+acceleration | 43 vFinal= vInitial+acceleration |
20 | 44 |
21 if acceleration<= 0: | 45 if acceleration<= 0: |
22 v= max(0,vFinal) | 46 v= max(0,vFinal) |
23 velocity= moving.Point(v* cos(theta),v* sin(theta)) | 47 velocity= moving.Point(v* cos(theta),v* sin(theta)) |
24 position= position+ (velocity+vInit). multiply(0.5) | 48 position= position+ (velocity+vInit). multiply(0.5) |
25 else: | 49 else: |
26 v= min(vLimit,vFinal) | 50 v= min(vLimit,vFinal) |
27 velocity= moving.Point(v* cos(theta),v* sin(theta)) | 51 velocity= moving.Point(v* cos(theta),v* sin(theta)) |
28 position= position+ (velocity+vInit). multiply(0.5) | 52 position= position+ (velocity+vInit). multiply(0.5) |
29 return(position,velocity) | 53 return(position,velocity) |
30 | 54 |
31 def motionPET (position, velocity, acceleration, deltaT): | 55 def motionPET (position, velocity, acceleration, deltaT): |
32 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' | 56 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' |
33 from math import atan2,cos,sin,fabs | 57 from math import atan2,cos,sin,fabs |
34 vInit= velocity | 58 vInit= velocity |
35 vInitial= velocity.norm2() | 59 vInitial= velocity.norm2() |
36 theta= atan2(velocity.y,velocity.x) | 60 theta= atan2(velocity.y,velocity.x) |
37 vFinal= vInitial+acceleration * deltaT | 61 vFinal= vInitial+acceleration * deltaT |
38 if acceleration< 0: | 62 if acceleration< 0: |
39 if vFinal> 0: | 63 if vFinal> 0: |
40 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | 64 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
41 position= position+ (vInit+ velocity). multiply(0.5*deltaT) | 65 position= position+ (vInit+ velocity). multiply(0.5*deltaT) |
42 else: | 66 else: |
43 T= fabs(vInitial/acceleration) | 67 T= fabs(vInitial/acceleration) |
44 position= position + vInit. multiply(0.5*T) | 68 position= position + vInit. multiply(0.5*T) |
45 elif acceleration> 0 : | 69 elif acceleration> 0 : |
46 if vFinal<= vLimit: | 70 if vFinal<= vLimit: |
47 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | 71 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
48 position= position+ (vInit+ velocity). multiply(0.5*deltaT) | 72 position= position+ (vInit+ velocity). multiply(0.5*deltaT) |
49 else: | 73 else: |
50 time1= fabs((vLimit-vInitial)/acceleration) | 74 time1= fabs((vLimit-vInitial)/acceleration) |
51 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) | 75 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) |
52 position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1)) | 76 position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1)) |
53 elif acceleration == 0: | 77 elif acceleration == 0: |
54 position= position + velocity. multiply(deltaT) | 78 position= position + velocity. multiply(deltaT) |
55 | 79 |
56 return position | 80 return position |
57 | 81 |
58 def timePET (position, velocity, acceleration, intersectedPoint ): | 82 def timePET (position, velocity, acceleration, intersectedPoint ): |
59 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' | 83 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' |
60 from math import atan2,cos,sin,fabs | 84 from math import atan2,cos,sin,fabs |
61 vInit= velocity | 85 vInit= velocity |
62 vInitial= velocity.norm2() | 86 vInitial= velocity.norm2() |
63 theta= atan2(velocity.y,velocity.x) | 87 theta= atan2(velocity.y,velocity.x) |
64 vFinal= vInitial+acceleration * deltaT | 88 vFinal= vInitial+acceleration * deltaT |
65 if acceleration< 0: | 89 if acceleration< 0: |
66 if vFinal> 0: | 90 if vFinal> 0: |
67 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | 91 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
68 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) | 92 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) |
69 else: | 93 else: |
70 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x))) | 94 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x))) |
71 elif acceleration> 0 : | 95 elif acceleration> 0 : |
72 if vFinal<= vLimit: | 96 if vFinal<= vLimit: |
73 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | 97 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
74 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) | 98 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) |
75 else: | 99 else: |
76 time1= fabs((vLimit-vInitial)/acceleration) | 100 time1= fabs((vLimit-vInitial)/acceleration) |
77 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) | 101 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) |
78 time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) | 102 time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) |
79 if time2<=time1: | 103 if time2<=time1: |
80 time= time2 | 104 time= time2 |
81 else: | 105 else: |
82 position2= (position+ (velocity+vInit). multiply(0.5*time1)) | 106 position2= (position+ (velocity+vInit). multiply(0.5*time1)) |
83 time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x)) | 107 time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x)) |
84 elif acceleration == 0: | 108 elif acceleration == 0: |
85 time= fabs((intersectedPoint.x-position.x)/(velocity.x)) | 109 time= fabs((intersectedPoint.x-position.x)/(velocity.x)) |
86 | 110 |
87 return time | 111 return time |
88 | 112 |
89 def motionSteering (position, velocity, deltaTheta, deltaT ): | 113 def motionSteering (position, velocity, deltaTheta, deltaT ): |
90 ''' extrapolation hypothesis: steering with deltaTheta''' | 114 ''' extrapolation hypothesis: steering with deltaTheta''' |
91 from math import atan2,cos,sin | 115 from math import atan2,cos,sin |
92 vInitial= velocity.norm2() | 116 vInitial= velocity.norm2() |
93 theta= atan2(velocity.y,velocity.x) | 117 theta= atan2(velocity.y,velocity.x) |
94 newTheta= theta + deltaTheta | 118 newTheta= theta + deltaTheta |
95 velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) | 119 velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) |
96 position= position+ (velocity). multiply(deltaT) | 120 position= position+ (velocity). multiply(deltaT) |
97 return position | 121 return position |
98 | 122 |
99 def MonteCarlo(movingObject1,movingObject2, instant): | 123 def MonteCarlo(movingObject1,movingObject2, instant): |
100 ''' Monte Carlo Simulation : estimate the probability of collision''' | 124 ''' Monte Carlo Simulation : estimate the probability of collision''' |
101 from random import uniform | 125 from random import uniform |
102 from math import pow, sqrt, sin, cos,atan2 | 126 from math import pow, sqrt, sin, cos,atan2 |
103 N=1000 | 127 N=1000 |
104 ProbOfCollision = 0 | 128 ProbOfCollision = 0 |
105 for n in range (1, N): | 129 for n in range (1, N): |
106 # acceleration limit | 130 # acceleration limit |
107 acc1 = uniform(-0.040444,0) | 131 acc1 = uniform(-0.040444,0) |
108 acc2 = uniform(-0.040444,0) | 132 acc2 = uniform(-0.040444,0) |
109 p1= movingObject1.getPositionAtInstant(instant) | 133 p1= movingObject1.getPositionAtInstant(instant) |
110 p2= movingObject2.getPositionAtInstant(instant) | 134 p2= movingObject2.getPositionAtInstant(instant) |
111 v1= movingObject1.getVelocityAtInstant(instant) | 135 v1= movingObject1.getVelocityAtInstant(instant) |
112 v2= movingObject2.getVelocityAtInstant(instant) | 136 v2= movingObject2.getVelocityAtInstant(instant) |
113 distance= (p1-p2).norm2() | 137 distance= (p1-p2).norm2() |
114 distanceThreshold= 1.8 | 138 distanceThreshold= 1.8 |
115 t=1 | 139 t=1 |
116 while distance > distanceThreshold and t <= deltaT: | 140 while distance > distanceThreshold and t <= deltaT: |
117 # Extrapolation position | 141 # Extrapolation position |
118 (p1,v1) = motion(p1,v1,acc1) | 142 (p1,v1) = motion(p1,v1,acc1) |
119 (p2,v2) = motion(p2,v2,acc2) | 143 (p2,v2) = motion(p2,v2,acc2) |
120 distance= (p1-p2).norm2() | 144 distance= (p1-p2).norm2() |
121 if distance <=distanceThreshold: | 145 if distance <=distanceThreshold: |
122 ProbOfCollision= ProbOfCollision+1 | 146 ProbOfCollision= ProbOfCollision+1 |
123 t+=1 | 147 t+=1 |
124 POC= float(ProbOfCollision)/N | 148 POC= float(ProbOfCollision)/N |
125 return POC | 149 return POC |
126 | 150 |
127 def velocitySteering(velocity,steering): | 151 def velocitySteering(velocity,steering): |
128 from math import atan2,cos,sin | 152 from math import atan2,cos,sin |
129 vInitial= velocity.norm2() | 153 vInitial= velocity.norm2() |
130 theta= atan2(velocity.y,velocity.x) | 154 theta= atan2(velocity.y,velocity.x) |
131 newTheta= theta + steering | 155 newTheta= theta + steering |
132 v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) | 156 v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) |
133 return v | 157 return v |
134 | 158 |
135 def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2): | 159 def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2): |
136 ''' Monte Carlo Simulation : estimate the probability of collision in case of steering''' | 160 ''' Monte Carlo Simulation : estimate the probability of collision in case of steering''' |
137 from random import uniform | 161 from random import uniform |
138 from math import pow, sqrt, sin, cos,atan2 | 162 from math import pow, sqrt, sin, cos,atan2 |
139 N=1000 | 163 N=1000 |
140 L= 2.4 | 164 L= 2.4 |
141 ProbOfCollision = 0 | 165 ProbOfCollision = 0 |
142 for n in range (1, N): | 166 for n in range (1, N): |
143 # acceleration limit | 167 # acceleration limit |
144 acc1 = uniform(-0.040444,0) | 168 acc1 = uniform(-0.040444,0) |
145 acc2 = uniform(-0.040444,0) | 169 acc2 = uniform(-0.040444,0) |
146 p1= movingObject1.getPositionAtInstant(instant) | 170 p1= movingObject1.getPositionAtInstant(instant) |
147 p2= movingObject2.getPositionAtInstant(instant) | 171 p2= movingObject2.getPositionAtInstant(instant) |
148 vInit1= movingObject1.getVelocityAtInstant(instant) | 172 vInit1= movingObject1.getVelocityAtInstant(instant) |
149 v1= velocitySteering (vInit1,steering1) | 173 v1= velocitySteering (vInit1,steering1) |
150 vInit2= movingObject2.getVelocityAtInstant(instant) | 174 vInit2= movingObject2.getVelocityAtInstant(instant) |
151 v2= velocitySteering (vInit2,steering2) | 175 v2= velocitySteering (vInit2,steering2) |
152 distance= (p1-p2).norm2() | 176 distance= (p1-p2).norm2() |
153 distanceThreshold= 1.8 | 177 distanceThreshold= 1.8 |
154 t=1 | 178 t=1 |
155 while distance > distanceThreshold and t <= deltaT: | 179 while distance > distanceThreshold and t <= deltaT: |
156 # Extrapolation position | 180 # Extrapolation position |
157 (p1,v1) = motion(p1,v1,acc1) | 181 (p1,v1) = motion(p1,v1,acc1) |
158 (p2,v2) = motion(p2,v2,acc2) | 182 (p2,v2) = motion(p2,v2,acc2) |
159 distance= (p1-p2).norm2() | 183 distance= (p1-p2).norm2() |
160 if distance <=distanceThreshold: | 184 if distance <=distanceThreshold: |
161 ProbOfCollision= ProbOfCollision+1 | 185 ProbOfCollision= ProbOfCollision+1 |
162 t+=1 | 186 t+=1 |
163 POC= float(ProbOfCollision)/N | 187 POC= float(ProbOfCollision)/N |
164 return POC | 188 return POC |
165 | 189 |
166 | 190 |