Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 269:a9988971aac8
removed legacy code + tweaks
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Sun, 29 Jul 2012 04:09:43 -0400 |
parents | 0c0b92f621f6 |
children | 05c9b0cb8202 |
comparison
equal
deleted
inserted
replaced
268:0c0b92f621f6 | 269:a9988971aac8 |
---|---|
104 self.steeringDistribution, | 104 self.steeringDistribution, |
105 maxSpeed = self.maxSpeed)) | 105 maxSpeed = self.maxSpeed)) |
106 return extrapolatedTrajectories | 106 return extrapolatedTrajectories |
107 | 107 |
108 class PointSetExtrapolationParameters(ExtrapolationParameters): | 108 class PointSetExtrapolationParameters(ExtrapolationParameters): |
109 # todo generate several trajectories with normal adaptatoins from each position (feature) | |
109 def __init__(self, maxSpeed): | 110 def __init__(self, maxSpeed): |
110 ExtrapolationParameters.__init__(self, 'point set', maxSpeed) | 111 ExtrapolationParameters.__init__(self, 'point set', maxSpeed) |
111 | 112 |
112 def generateExtrapolatedTrajectories(self, obj, instant): | 113 def generateExtrapolatedTrajectories(self, obj, instant): |
113 extrapolatedTrajectories = [] | 114 extrapolatedTrajectories = [] |
183 p1 = extrapolatedTrajectory1.predictPosition(t) | 184 p1 = extrapolatedTrajectory1.predictPosition(t) |
184 p2 = extrapolatedTrajectory2.predictPosition(t) | 185 p2 = extrapolatedTrajectory2.predictPosition(t) |
185 t += 1 | 186 t += 1 |
186 return t, p1, p2 | 187 return t, p1, p2 |
187 | 188 |
188 def computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon): | 189 def computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, debug = False): |
189 '''returns the lists of collision points and crossing zones ''' | 190 '''returns the lists of collision points and crossing zones |
191 | |
192 Check: Extrapolating all the points together, as if they represent the whole vehicle''' | |
190 extrapolatedTrajectories1 = extrapolationParameters.generateExtrapolatedTrajectories(obj1, i) | 193 extrapolatedTrajectories1 = extrapolationParameters.generateExtrapolatedTrajectories(obj1, i) |
191 extrapolatedTrajectories2 = extrapolationParameters.generateExtrapolatedTrajectories(obj2, i) | 194 extrapolatedTrajectories2 = extrapolationParameters.generateExtrapolatedTrajectories(obj2, i) |
192 | 195 |
193 collisionPoints = [] | 196 collisionPoints = [] |
194 crossingZones = [] | 197 crossingZones = [] |
212 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) | 215 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) |
213 if cz: | 216 if cz: |
214 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) | 217 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) |
215 t2 += 1 | 218 t2 += 1 |
216 t1 += 1 | 219 t1 += 1 |
220 | |
221 if debug: | |
222 from matplotlib.pyplot import figure, axis, title | |
223 figure() | |
224 for et in extrapolatedTrajectories1: | |
225 et.predictPosition(timeHorizon) | |
226 et.draw('rx') | |
227 | |
228 for et in extrapolatedTrajectories2: | |
229 et.predictPosition(timeHorizon) | |
230 et.draw('bx') | |
231 obj1.draw('r') | |
232 obj2.draw('b') | |
233 title('instant {0}'.format(i)) | |
234 axis('equal') | |
235 | |
217 return collisionPoints, crossingZones | 236 return collisionPoints, crossingZones |
218 | 237 |
219 def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None): | 238 def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None): |
220 collisionPoints={} | 239 collisionPoints={} |
221 crossingZones={} | 240 crossingZones={} |
223 commonTimeInterval = timeInterval | 242 commonTimeInterval = timeInterval |
224 else: | 243 else: |
225 commonTimeInterval = obj1.commonTimeInterval(obj2) | 244 commonTimeInterval = obj1.commonTimeInterval(obj2) |
226 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 245 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
227 print(obj1.num, obj2.num, i) | 246 print(obj1.num, obj2.num, i) |
228 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon) | 247 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, debug) |
229 SafetyPoint.save(outCP, collisionPoints[i], obj1.num, obj2.num, i) | 248 SafetyPoint.save(outCP, collisionPoints[i], obj1.num, obj2.num, i) |
230 SafetyPoint.save(outCZ, crossingZones[i], obj1.num, obj2.num, i) | 249 SafetyPoint.save(outCZ, crossingZones[i], obj1.num, obj2.num, i) |
231 | |
232 if debug: | |
233 from matplotlib.pyplot import figure, axis, title | |
234 figure() | |
235 obj1.draw('r') | |
236 obj2.draw('b') | |
237 for et in extrapolatedTrajectories1: | |
238 et.predictPosition(timeHorizon) | |
239 et.draw('rx') | |
240 | |
241 for et in extrapolatedTrajectories2: | |
242 et.predictPosition(timeHorizon) | |
243 et.draw('bx') | |
244 title('instant {0}'.format(i)) | |
245 axis('equal') | |
246 | 250 |
247 return collisionPoints, crossingZones | 251 return collisionPoints, crossingZones |
248 | 252 |
249 def computeCollisionProbability(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, out, debug = False, timeInterval = None): | 253 def computeCollisionProbability(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, out, debug = False, timeInterval = None): |
250 collisionProbabilities = {} | 254 collisionProbabilities = {} |
268 out.write('{0} {1} {2} {3} {4}\n'.format(obj1.num, obj2.num, nSamples, i, collisionProbabilities[i])) | 272 out.write('{0} {1} {2} {3} {4}\n'.format(obj1.num, obj2.num, nSamples, i, collisionProbabilities[i])) |
269 | 273 |
270 if debug: | 274 if debug: |
271 from matplotlib.pyplot import figure, axis, title | 275 from matplotlib.pyplot import figure, axis, title |
272 figure() | 276 figure() |
273 obj1.draw('r') | |
274 obj2.draw('b') | |
275 for et in extrapolatedTrajectories1: | 277 for et in extrapolatedTrajectories1: |
276 et.predictPosition(timeHorizon) | 278 et.predictPosition(timeHorizon) |
277 et.draw('rx') | 279 et.draw('rx') |
278 | 280 |
279 for et in extrapolatedTrajectories2: | 281 for et in extrapolatedTrajectories2: |
280 et.predictPosition(timeHorizon) | 282 et.predictPosition(timeHorizon) |
281 et.draw('bx') | 283 et.draw('bx') |
284 obj1.draw('r') | |
285 obj2.draw('b') | |
282 title('instant {0}'.format(i)) | 286 title('instant {0}'.format(i)) |
283 axis('equal') | 287 axis('equal') |
284 | 288 |
285 return collisionProbabilities | 289 return collisionProbabilities |
286 | |
287 ############### | |
288 | |
289 # Default values: to remove because we cannot tweak that from a script where the value may be different | |
290 FPS= 25 # No. of frame per second (FPS) | |
291 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec | |
292 deltaT= FPS*5 # extrapolatation time Horizon = 5 second | |
293 | |
294 def motion (position, velocity, acceleration): | |
295 ''' extrapolation hypothesis: constant acceleration''' | |
296 from math import atan2,cos,sin | |
297 vInit= velocity | |
298 vInitial= velocity.norm2() | |
299 theta= atan2(velocity.y,velocity.x) | |
300 vFinal= vInitial+acceleration | |
301 | |
302 if acceleration<= 0: | |
303 v= max(0,vFinal) | |
304 velocity= moving.Point(v* cos(theta),v* sin(theta)) | |
305 position= position+ (velocity+vInit). multiply(0.5) | |
306 else: | |
307 v= min(vLimit,vFinal) | |
308 velocity= moving.Point(v* cos(theta),v* sin(theta)) | |
309 position= position+ (velocity+vInit). multiply(0.5) | |
310 return(position,velocity) | |
311 | |
312 def motionPET (position, velocity, acceleration, deltaT): | |
313 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' | |
314 from math import atan2,cos,sin,fabs | |
315 vInit= velocity | |
316 vInitial= velocity.norm2() | |
317 theta= atan2(velocity.y,velocity.x) | |
318 vFinal= vInitial+acceleration * deltaT | |
319 if acceleration< 0: | |
320 if vFinal> 0: | |
321 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | |
322 position= position+ (vInit+ velocity). multiply(0.5*deltaT) | |
323 else: | |
324 T= fabs(vInitial/acceleration) | |
325 position= position + vInit. multiply(0.5*T) | |
326 elif acceleration> 0 : | |
327 if vFinal<= vLimit: | |
328 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | |
329 position= position+ (vInit+ velocity). multiply(0.5*deltaT) | |
330 else: | |
331 time1= fabs((vLimit-vInitial)/acceleration) | |
332 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) | |
333 position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1)) | |
334 elif acceleration == 0: | |
335 position= position + velocity. multiply(deltaT) | |
336 | |
337 return position | |
338 | |
339 def timePET (position, velocity, acceleration, intersectedPoint ): | |
340 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' | |
341 from math import atan2,cos,sin,fabs | |
342 vInit= velocity | |
343 vInitial= velocity.norm2() | |
344 theta= atan2(velocity.y,velocity.x) | |
345 vFinal= vInitial+acceleration * deltaT | |
346 if acceleration< 0: | |
347 if vFinal> 0: | |
348 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | |
349 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) | |
350 else: | |
351 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x))) | |
352 elif acceleration> 0 : | |
353 if vFinal<= vLimit: | |
354 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) | |
355 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) | |
356 else: | |
357 time1= fabs((vLimit-vInitial)/acceleration) | |
358 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) | |
359 time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) | |
360 if time2<=time1: | |
361 time= time2 | |
362 else: | |
363 position2= (position+ (velocity+vInit). multiply(0.5*time1)) | |
364 time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x)) | |
365 elif acceleration == 0: | |
366 time= fabs((intersectedPoint.x-position.x)/(velocity.x)) | |
367 | |
368 return time | |
369 | |
370 def motionSteering (position, velocity, deltaTheta, deltaT ): | |
371 ''' extrapolation hypothesis: steering with deltaTheta''' | |
372 from math import atan2,cos,sin | |
373 vInitial= velocity.norm2() | |
374 theta= atan2(velocity.y,velocity.x) | |
375 newTheta= theta + deltaTheta | |
376 velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) | |
377 position= position+ (velocity). multiply(deltaT) | |
378 return position | |
379 | |
380 def MonteCarlo(movingObject1,movingObject2, instant): | |
381 ''' Monte Carlo Simulation : estimate the probability of collision''' | |
382 from random import uniform | |
383 from math import pow, sqrt, sin, cos,atan2 | |
384 N=1000 | |
385 ProbOfCollision = 0 | |
386 for n in range (1, N): | |
387 # acceleration limit | |
388 acc1 = uniform(-0.040444,0) | |
389 acc2 = uniform(-0.040444,0) | |
390 p1= movingObject1.getPositionAtInstant(instant) | |
391 p2= movingObject2.getPositionAtInstant(instant) | |
392 v1= movingObject1.getVelocityAtInstant(instant) | |
393 v2= movingObject2.getVelocityAtInstant(instant) | |
394 distance= (p1-p2).norm2() | |
395 distanceThreshold= 1.8 | |
396 t=1 | |
397 while distance > distanceThreshold and t <= deltaT: | |
398 # Extrapolation position | |
399 (p1,v1) = motion(p1,v1,acc1) | |
400 (p2,v2) = motion(p2,v2,acc2) | |
401 distance= (p1-p2).norm2() | |
402 if distance <=distanceThreshold: | |
403 ProbOfCollision= ProbOfCollision+1 | |
404 t+=1 | |
405 POC= float(ProbOfCollision)/N | |
406 return POC | |
407 | |
408 def velocitySteering(velocity,steering): | |
409 from math import atan2,cos,sin | |
410 vInitial= velocity.norm2() | |
411 theta= atan2(velocity.y,velocity.x) | |
412 newTheta= theta + steering | |
413 v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) | |
414 return v | |
415 | |
416 def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2): | |
417 ''' Monte Carlo Simulation : estimate the probability of collision in case of steering''' | |
418 from random import uniform | |
419 from math import pow, sqrt, sin, cos,atan2 | |
420 N=1000 | |
421 L= 2.4 | |
422 ProbOfCollision = 0 | |
423 for n in range (1, N): | |
424 # acceleration limit | |
425 acc1 = uniform(-0.040444,0) | |
426 acc2 = uniform(-0.040444,0) | |
427 p1= movingObject1.getPositionAtInstant(instant) | |
428 p2= movingObject2.getPositionAtInstant(instant) | |
429 vInit1= movingObject1.getVelocityAtInstant(instant) | |
430 v1= velocitySteering (vInit1,steering1) | |
431 vInit2= movingObject2.getVelocityAtInstant(instant) | |
432 v2= velocitySteering (vInit2,steering2) | |
433 distance= (p1-p2).norm2() | |
434 distanceThreshold= 1.8 | |
435 t=1 | |
436 while distance > distanceThreshold and t <= deltaT: | |
437 # Extrapolation position | |
438 (p1,v1) = motion(p1,v1,acc1) | |
439 (p2,v2) = motion(p2,v2,acc2) | |
440 distance= (p1-p2).norm2() | |
441 if distance <=distanceThreshold: | |
442 ProbOfCollision= ProbOfCollision+1 | |
443 t+=1 | |
444 POC= float(ProbOfCollision)/N | |
445 return POC | |
446 | 290 |
447 | 291 |
448 if __name__ == "__main__": | 292 if __name__ == "__main__": |
449 import doctest | 293 import doctest |
450 import unittest | 294 import unittest |