Mercurial Hosting > traffic-intelligence
changeset 257:9281878ff19e
untested collision/crossing computation
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 24 Jul 2012 03:27:29 -0400 |
parents | dc1faa7287bd |
children | d90be3c02267 |
files | python/extrapolation.py |
diffstat | 1 files changed, 44 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- a/python/extrapolation.py Tue Jul 24 01:37:21 2012 -0400 +++ b/python/extrapolation.py Tue Jul 24 03:27:29 2012 -0400 @@ -57,6 +57,50 @@ return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) +class ExtrapolationParameters: + def __init__(self, name): + self.name = name + +def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): + '''extrapolationParameters specific to each method (in name field) ''' + if extrapolationHypothesis.name == 'constant velocity': + return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] + else: + print('Unknown extrapolation hypothesis') + return [] + +def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): + '''returns the lists of collision points and crossing zones ''' + collisionPoints = [] + TTCs = [] + crossingZones = [] + pPETs = [] + for et1 in extrapolatedTrajectories1: + for et2 in extrapolatedTrajectories2: + + t = 1 + p1 = et1.predictPosition(t) + p2 = et2.predictPosition(t) + while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold: + p1 = et1.predictPosition(t) + p2 = et2.predictPosition(t) + t += 1 + + if t <= timeHorizon: + TTCs.append(t) # todo store probability + collisionPoints.append((p1+p2).multiply(0.5)) + else: # check if there is a crossing zone + for t1 in xrange(timeHorizon-1): + for t2 in xrange(timeHorizon-1): + cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) + if cz: + crossingZones.append(cz) + pPETs.append(abs(t1-t2)) + break + return collisionPoints, crossingZones, TTCs, pPETs + + + # Default values: to remove because we cannot tweak that from a script where the value may be different FPS= 25 # No. of frame per second (FPS) vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec