Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 600:414b2e7cd873
merge feature-minDistance-based collisionPoints calculation in prediction file
author | Mohamed Gomaa |
---|---|
date | Thu, 18 Apr 2013 17:12:53 -0400 |
parents | f65b828e5521 |
children | 480c8edf177e |
comparison
equal
deleted
inserted
replaced
599:4b5fe2de1e8d | 600:414b2e7cd873 |
---|---|
177 | 177 |
178 def computeExpectedIndicator(points): | 178 def computeExpectedIndicator(points): |
179 from numpy import sum | 179 from numpy import sum |
180 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]) |
181 | 181 |
182 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): | 182 def getPredictedSetPositions (predictedTrajectory,instant): |
183 positions=[] | |
184 for p in predictedTrajectory: | |
185 positions.append(p.predictPosition(instant)) | |
186 return positions | |
187 | |
188 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon,asWholeVehicle=False ): | |
183 '''Computes the first instant at which two predicted trajectories are within some distance threshold''' | 189 '''Computes the first instant at which two predicted trajectories are within some distance threshold''' |
184 t = 1 | 190 if asWholeVehicle: |
185 p1 = predictedTrajectory1.predictPosition(t) | 191 from scipy.spatial.distance import cdist |
186 p2 = predictedTrajectory2.predictPosition(t) | 192 import numpy as np |
187 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: | 193 t = 1 |
194 p1= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory1,t)] | |
195 p2= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory2,t)] | |
196 distance= cdist(p1,p2, metric='euclidean') | |
197 minDist= distance.min() | |
198 while t <= timeHorizon and minDist > collisionDistanceThreshold: | |
199 p1= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory1,t)] | |
200 p2= [p.astuple() for p in getPredictedSetPositions (predictedTrajectory2,t)] | |
201 distance= cdist(p1,p2, metric='euclidean') | |
202 minDist= distance.min() | |
203 t += 1 | |
204 Index=np.unravel_index(distance.argmin(), distance.shape) | |
205 involvedPosition1= p1[Index[0]] | |
206 involvedPosition2= p2[Index[1]] | |
207 return t, involvedPosition1, involvedPosition2 | |
208 else: | |
209 t = 1 | |
188 p1 = predictedTrajectory1.predictPosition(t) | 210 p1 = predictedTrajectory1.predictPosition(t) |
189 p2 = predictedTrajectory2.predictPosition(t) | 211 p2 = predictedTrajectory2.predictPosition(t) |
190 t += 1 | 212 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: |
191 return t, p1, p2 | 213 p1 = predictedTrajectory1.predictPosition(t) |
192 | 214 p2 = predictedTrajectory2.predictPosition(t) |
193 def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): | 215 t += 1 |
194 '''returns the lists of collision points and crossing zones | 216 return t, p1, p2 |
195 | 217 |
196 Check: Predicting all the points together, as if they represent the whole vehicle''' | 218 def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, asWholeVehicle=False): |
219 '''returns the lists of collision points and crossing zones | |
220 Check: Predicting all the points together, as if they represent the whole vehicle''' # Done | |
221 | |
197 predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant) | 222 predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant) |
198 predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant) | 223 predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant) |
199 | 224 |
200 collisionPoints = [] | 225 collisionPoints = [] |
201 crossingZones = [] | 226 crossingZones = [] |
202 for et1 in predictedTrajectories1: | 227 if asWholeVehicle: |
203 for et2 in predictedTrajectories2: | 228 t, p1, p2 = computeCollisionTime(predictedTrajectories1, predictedTrajectories2, collisionDistanceThreshold, timeHorizon,asWholeVehicle) |
204 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 229 if t <= timeHorizon: |
230 collisionPoints.append(SafetyPoint((moving.Point(p1[0]+p2[0],p1[1]+p2[1])).multiply(0.5), 1, t)) | |
231 else: | |
232 for et1 in predictedTrajectories1: | |
233 for et2 in predictedTrajectories2: | |
234 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon,asWholeVehicle) | |
205 | 235 |
206 if t <= timeHorizon: | 236 if t <= timeHorizon: |
207 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) | 237 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) |
208 elif computeCZ: # check if there is a crossing zone | 238 elif computeCZ: # check if there is a crossing zone |
209 # TODO? zone should be around the points at which the traj are the closest | 239 # TODO? zone should be around the points at which the traj are the closest |
210 # look for CZ at different times, otherwise it would be a collision | 240 # look for CZ at different times, otherwise it would be a collision |
211 # an approximation would be to look for close points at different times, ie the complementary of collision points | 241 # an approximation would be to look for close points at different times, ie the complementary of collision points |
212 cz = None | 242 cz = None |
213 t1 = 0 | 243 t1 = 0 |
214 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 | 244 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 |
215 t2 = 0 | 245 t2 = 0 |
216 while not cz and t2 < timeHorizon: | 246 while not cz and t2 < timeHorizon: |
217 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: | 247 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: |
218 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) | 248 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) |
219 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) | 249 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) |
220 if cz: | 250 if cz: |
221 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) | 251 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) |
222 t2 += 1 | 252 t2 += 1 |
223 t1 += 1 | 253 t1 += 1 |
224 | 254 |
225 if debug: | 255 if debug: |
226 from matplotlib.pyplot import figure, axis, title | 256 from matplotlib.pyplot import figure, axis, title |
227 figure() | 257 figure() |
228 for et in predictedTrajectories1: | 258 for et in predictedTrajectories1: |
237 title('instant {0}'.format(i)) | 267 title('instant {0}'.format(i)) |
238 axis('equal') | 268 axis('equal') |
239 | 269 |
240 return collisionPoints, crossingZones | 270 return collisionPoints, crossingZones |
241 | 271 |
242 def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None): | 272 def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,asWholeVehicle=False): |
243 '''Computes all crossing and collision points at each common instant for two road users. ''' | 273 '''Computes all crossing and collision points at each common instant for two road users. ''' |
244 collisionPoints={} | 274 collisionPoints={} |
245 crossingZones={} | 275 crossingZones={} |
246 if timeInterval: | 276 if timeInterval: |
247 commonTimeInterval = timeInterval | 277 commonTimeInterval = timeInterval |
248 else: | 278 else: |
249 commonTimeInterval = obj1.commonTimeInterval(obj2) | 279 commonTimeInterval = obj1.commonTimeInterval(obj2) |
250 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 280 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
251 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug) | 281 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug,asWholeVehicle) |
252 | 282 |
253 return collisionPoints, crossingZones | 283 return collisionPoints, crossingZones |
254 | 284 |
255 def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): | 285 def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
256 '''Computes only collision probabilities | 286 '''Computes only collision probabilities |
289 title('instant {0}'.format(i)) | 319 title('instant {0}'.format(i)) |
290 axis('equal') | 320 axis('equal') |
291 | 321 |
292 return collisionProbabilities | 322 return collisionProbabilities |
293 | 323 |
294 | |
295 if __name__ == "__main__": | 324 if __name__ == "__main__": |
296 import doctest | 325 import doctest |
297 import unittest | 326 import unittest |
298 suite = doctest.DocFileSuite('tests/prediction.txt') | 327 suite = doctest.DocFileSuite('tests/prediction.txt') |
299 #suite = doctest.DocTestSuite() | 328 #suite = doctest.DocTestSuite() |