Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 244:5027c174ab90

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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