Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 289:e56c34c1ebac
refactored and commented functions (saving data is now outside of the computation functions)
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 29 Jan 2013 11:21:42 -0500 |
parents | bbd9c09e6869 |
children | df58d361f19e |
comparison
equal
deleted
inserted
replaced
288:e0d41c7f53d4 | 289:e56c34c1ebac |
---|---|
165 self.x = p.x | 165 self.x = p.x |
166 self.y = p.y | 166 self.y = p.y |
167 self.probability = probability | 167 self.probability = probability |
168 self.indicator = indicator | 168 self.indicator = indicator |
169 | 169 |
170 def __str__(self): | |
171 return '{0} {1} {2} {3}'.format(self.x, self.y, self.probability, self.indicator) | |
172 | |
170 @staticmethod | 173 @staticmethod |
171 def save(out, points, objNum1, objNum2, instant): | 174 def save(out, points, predictionInstant, objNum1, objNum2): |
172 for p in points: | 175 for p in points: |
173 out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator)) | 176 out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p)) |
174 | 177 |
175 def computeExpectedIndicator(points): | 178 def computeExpectedIndicator(points): |
176 from numpy import sum | 179 from numpy import sum |
177 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) | 180 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) |
178 | 181 |
179 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): | 182 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): |
183 '''Computes the first instant at which two predicted trajectories are within some distance threshold''' | |
180 t = 1 | 184 t = 1 |
181 p1 = predictedTrajectory1.predictPosition(t) | 185 p1 = predictedTrajectory1.predictPosition(t) |
182 p2 = predictedTrajectory2.predictPosition(t) | 186 p2 = predictedTrajectory2.predictPosition(t) |
183 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: | 187 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: |
184 p1 = predictedTrajectory1.predictPosition(t) | 188 p1 = predictedTrajectory1.predictPosition(t) |
185 p2 = predictedTrajectory2.predictPosition(t) | 189 p2 = predictedTrajectory2.predictPosition(t) |
186 t += 1 | 190 t += 1 |
187 return t, p1, p2 | 191 return t, p1, p2 |
188 | 192 |
189 def computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False): | 193 def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False): |
190 '''returns the lists of collision points and crossing zones | 194 '''returns the lists of collision points and crossing zones |
191 | 195 |
192 Check: Predicting all the points together, as if they represent the whole vehicle''' | 196 Check: Predicting all the points together, as if they represent the whole vehicle''' |
193 predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, i) | 197 predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant) |
194 predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, i) | 198 predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant) |
195 | 199 |
196 collisionPoints = [] | 200 collisionPoints = [] |
197 crossingZones = [] | 201 crossingZones = [] |
198 for et1 in predictedTrajectories1: | 202 for et1 in predictedTrajectories1: |
199 for et2 in predictedTrajectories2: | 203 for et2 in predictedTrajectories2: |
200 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 204 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
201 | 205 |
202 if t <= timeHorizon: | 206 if t <= timeHorizon: |
203 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) | 207 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), currentInstant, et1.probability*et2.probability, t)) |
204 else: # check if there is a crossing zone | 208 else: # check if there is a crossing zone |
205 # TODO? zone should be around the points at which the traj are the closest | 209 # TODO? zone should be around the points at which the traj are the closest |
206 # look for CZ at different times, otherwise it would be a collision | 210 # look for CZ at different times, otherwise it would be a collision |
207 # an approximation would be to look for close points at different times, ie the complementary of collision points | 211 # an approximation would be to look for close points at different times, ie the complementary of collision points |
208 cz = None | 212 cz = None |
233 title('instant {0}'.format(i)) | 237 title('instant {0}'.format(i)) |
234 axis('equal') | 238 axis('equal') |
235 | 239 |
236 return collisionPoints, crossingZones | 240 return collisionPoints, crossingZones |
237 | 241 |
238 def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None): | 242 def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
243 '''Computes all crossing and collision points at each common instant for two road users. ''' | |
239 collisionPoints={} | 244 collisionPoints={} |
240 crossingZones={} | 245 crossingZones={} |
241 if timeInterval: | 246 if timeInterval: |
242 commonTimeInterval = timeInterval | 247 commonTimeInterval = timeInterval |
243 else: | 248 else: |
244 commonTimeInterval = obj1.commonTimeInterval(obj2) | 249 commonTimeInterval = obj1.commonTimeInterval(obj2) |
245 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 250 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
246 print(obj1.num, obj2.num, i) | 251 print(obj1.num, obj2.num, i) |
247 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug) | 252 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug) |
248 SafetyPoint.save(outCP, collisionPoints[i], obj1.num, obj2.num, i) | |
249 SafetyPoint.save(outCZ, crossingZones[i], obj1.num, obj2.num, i) | |
250 | 253 |
251 return collisionPoints, crossingZones | 254 return collisionPoints, crossingZones |
252 | 255 |
253 def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, out, debug = False, timeInterval = None): | 256 def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
257 '''Computes only collision probabilities | |
258 Returns for each instant the collision probability and number of samples drawn''' | |
254 collisionProbabilities = {} | 259 collisionProbabilities = {} |
255 if timeInterval: | 260 if timeInterval: |
256 commonTimeInterval = timeInterval | 261 commonTimeInterval = timeInterval |
257 else: | 262 else: |
258 commonTimeInterval = obj1.commonTimeInterval(obj2) | 263 commonTimeInterval = obj1.commonTimeInterval(obj2) |
266 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 271 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
267 if t <= timeHorizon: | 272 if t <= timeHorizon: |
268 nCollisions += 1 | 273 nCollisions += 1 |
269 # take into account probabilities ?? | 274 # take into account probabilities ?? |
270 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) | 275 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) |
271 collisionProbabilities[i] = float(nCollisions)/nSamples | 276 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] |
272 out.write('{0} {1} {2} {3} {4}\n'.format(obj1.num, obj2.num, nSamples, i, collisionProbabilities[i])) | |
273 | 277 |
274 if debug: | 278 if debug: |
275 from matplotlib.pyplot import figure, axis, title | 279 from matplotlib.pyplot import figure, axis, title |
276 figure() | 280 figure() |
277 for et in predictedTrajectories1: | 281 for et in predictedTrajectories1: |