Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 557:b91f33e098ee

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