Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 557:b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 14 Jul 2014 17:33:43 -0400 |
parents | dc58ad777a72 |
children | 806df5f61c03 |
comparison
equal
deleted
inserted
replaced
556:dc58ad777a72 | 557:b91f33e098ee |
---|---|
121 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: | 121 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: |
122 p1 = predictedTrajectory1.predictPosition(t) | 122 p1 = predictedTrajectory1.predictPosition(t) |
123 p2 = predictedTrajectory2.predictPosition(t) | 123 p2 = predictedTrajectory2.predictPosition(t) |
124 t += 1 | 124 t += 1 |
125 return t, p1, p2 | 125 return t, p1, p2 |
126 | |
127 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): | |
128 from matplotlib.pyplot import figure, axis, title, close, savefig | |
129 figure() | |
130 for et in predictedTrajectories1: | |
131 et.predictPosition(timeHorizon) | |
132 et.plot('rx') | |
133 | |
134 for et in predictedTrajectories2: | |
135 et.predictPosition(timeHorizon) | |
136 et.plot('bx') | |
137 obj1.plot('r') | |
138 obj2.plot('b') | |
139 title('instant {0}'.format(currentInstant)) | |
140 axis('equal') | |
141 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) | |
142 close() | |
126 | 143 |
127 def computeCrossingsCollisionsAtInstant(predictionParams, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): | 144 def computeCrossingsCollisionsAtInstant(predictionParams, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): |
128 '''returns the lists of collision points and crossing zones''' | 145 '''returns the lists of collision points and crossing zones''' |
129 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) | 146 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) |
130 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) | 147 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) |
153 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) | 170 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) |
154 t2 += 1 | 171 t2 += 1 |
155 t1 += 1 | 172 t1 += 1 |
156 | 173 |
157 if debug: | 174 if debug: |
158 from matplotlib.pyplot import figure, axis, title | 175 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) |
159 figure() | |
160 for et in predictedTrajectories1: | |
161 et.predictPosition(timeHorizon) | |
162 et.plot('rx') | |
163 | |
164 for et in predictedTrajectories2: | |
165 et.predictPosition(timeHorizon) | |
166 et.plot('bx') | |
167 obj1.plot('r') | |
168 obj2.plot('b') | |
169 title('instant {0}'.format(currentInstant)) | |
170 axis('equal') | |
171 | 176 |
172 return currentInstant, collisionPoints, crossingZones | 177 return currentInstant, collisionPoints, crossingZones |
173 | 178 |
174 def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): | 179 def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): |
175 '''Computes all crossing and collision points at each common instant for two road users. ''' | 180 '''Computes all crossing and collision points at each common instant for two road users. ''' |
183 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 188 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
184 i, collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) | 189 i, collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) |
185 else: | 190 else: |
186 from multiprocessing import Pool | 191 from multiprocessing import Pool |
187 pool = Pool(processes = nProcesses) | 192 pool = Pool(processes = nProcesses) |
188 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ)) for i in list(commonTimeInterval)[:-1]] | 193 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] |
189 #results = [j.get() for j in jobs] | 194 #results = [j.get() for j in jobs] |
190 #results.sort() | 195 #results.sort() |
191 for j in jobs: | 196 for j in jobs: |
192 i, cp, cz = j.get() | 197 i, cp, cz = j.get() |
193 #if len(cp) != 0 or len(cz) != 0: | 198 #if len(cp) != 0 or len(cz) != 0: |
206 | 211 |
207 def generatePredictedTrajectories(self, obj, instant): | 212 def generatePredictedTrajectories(self, obj, instant): |
208 return [] | 213 return [] |
209 | 214 |
210 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): | 215 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): |
211 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False) | 216 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) |
212 | 217 |
213 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): | 218 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): |
214 return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1) | 219 return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses) |
215 | 220 |
216 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): | 221 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
217 '''Computes only collision probabilities | 222 '''Computes only collision probabilities |
218 Returns for each instant the collision probability and number of samples drawn''' | 223 Returns for each instant the collision probability and number of samples drawn''' |
219 collisionProbabilities = {} | 224 collisionProbabilities = {} |
221 commonTimeInterval = timeInterval | 226 commonTimeInterval = timeInterval |
222 else: | 227 else: |
223 commonTimeInterval = obj1.commonTimeInterval(obj2) | 228 commonTimeInterval = obj1.commonTimeInterval(obj2) |
224 for i in list(commonTimeInterval)[:-1]: | 229 for i in list(commonTimeInterval)[:-1]: |
225 nCollisions = 0 | 230 nCollisions = 0 |
226 print(obj1.num, obj2.num, i) | |
227 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i) | 231 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i) |
228 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i) | 232 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i) |
229 for et1 in predictedTrajectories1: | 233 for et1 in predictedTrajectories1: |
230 for et2 in predictedTrajectories2: | 234 for et2 in predictedTrajectories2: |
231 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 235 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
234 # take into account probabilities ?? | 238 # take into account probabilities ?? |
235 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) | 239 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) |
236 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] | 240 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] |
237 | 241 |
238 if debug: | 242 if debug: |
239 from matplotlib.pyplot import figure, axis, title | 243 savePredictedTrajectoriesFigure(i, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) |
240 figure() | |
241 for et in predictedTrajectories1: | |
242 et.predictPosition(timeHorizon) | |
243 et.plot('rx') | |
244 | |
245 for et in predictedTrajectories2: | |
246 et.predictPosition(timeHorizon) | |
247 et.plot('bx') | |
248 obj1.plot('r') | |
249 obj2.plot('b') | |
250 title('instant {0}'.format(i)) | |
251 axis('equal') | |
252 | 244 |
253 return collisionProbabilities | 245 return collisionProbabilities |
254 | 246 |
255 class ConstantPredictionParameters(PredictionParameters): | 247 class ConstantPredictionParameters(PredictionParameters): |
256 def __init__(self, maxSpeed): | 248 def __init__(self, maxSpeed): |