Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 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 |
comparison
equal
deleted
inserted
replaced
256:dc1faa7287bd | 257:9281878ff19e |
---|---|
53 self.predictedPositions = {0: initialPosition} | 53 self.predictedPositions = {0: initialPosition} |
54 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} | 54 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
55 | 55 |
56 def getControl(self): | 56 def getControl(self): |
57 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) | 57 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) |
58 | |
59 | |
60 class ExtrapolationParameters: | |
61 def __init__(self, name): | |
62 self.name = name | |
63 | |
64 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): | |
65 '''extrapolationParameters specific to each method (in name field) ''' | |
66 if extrapolationHypothesis.name == 'constant velocity': | |
67 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] | |
68 else: | |
69 print('Unknown extrapolation hypothesis') | |
70 return [] | |
71 | |
72 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): | |
73 '''returns the lists of collision points and crossing zones ''' | |
74 collisionPoints = [] | |
75 TTCs = [] | |
76 crossingZones = [] | |
77 pPETs = [] | |
78 for et1 in extrapolatedTrajectories1: | |
79 for et2 in extrapolatedTrajectories2: | |
80 | |
81 t = 1 | |
82 p1 = et1.predictPosition(t) | |
83 p2 = et2.predictPosition(t) | |
84 while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold: | |
85 p1 = et1.predictPosition(t) | |
86 p2 = et2.predictPosition(t) | |
87 t += 1 | |
88 | |
89 if t <= timeHorizon: | |
90 TTCs.append(t) # todo store probability | |
91 collisionPoints.append((p1+p2).multiply(0.5)) | |
92 else: # check if there is a crossing zone | |
93 for t1 in xrange(timeHorizon-1): | |
94 for t2 in xrange(timeHorizon-1): | |
95 cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) | |
96 if cz: | |
97 crossingZones.append(cz) | |
98 pPETs.append(abs(t1-t2)) | |
99 break | |
100 return collisionPoints, crossingZones, TTCs, pPETs | |
101 | |
58 | 102 |
59 | 103 |
60 # Default values: to remove because we cannot tweak that from a script where the value may be different | 104 # Default values: to remove because we cannot tweak that from a script where the value may be different |
61 FPS= 25 # No. of frame per second (FPS) | 105 FPS= 25 # No. of frame per second (FPS) |
62 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec | 106 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec |