Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 269:a9988971aac8

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