Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 258:d90be3c02267
reasonably efficient computation of collision points and crossing zones
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 24 Jul 2012 12:37:47 -0400 |
parents | 9281878ff19e |
children | 8ab76b95ee72 |
comparison
equal
deleted
inserted
replaced
257:9281878ff19e | 258:d90be3c02267 |
---|---|
7 class ExtrapolatedTrajectory: | 7 class ExtrapolatedTrajectory: |
8 '''Class for extrapolated trajectories with lazy evaluation | 8 '''Class for extrapolated trajectories with lazy evaluation |
9 if the predicted position has not been already computed, compute it | 9 if the predicted position has not been already computed, compute it |
10 | 10 |
11 it should also have a probability''' | 11 it should also have a probability''' |
12 | |
13 def __init__(self): | |
14 self.probability = 0. | |
15 self.predictedPositions = {} | |
16 self.predictedSpeedOrientations = {} | |
17 self.collisionPoints = {} | |
18 self.crossingZones = {} | |
12 | 19 |
13 def predictPosition(self, nTimeSteps): | 20 def predictPosition(self, nTimeSteps): |
14 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): | 21 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
15 self.predictPosition(nTimeSteps-1) | 22 self.predictPosition(nTimeSteps-1) |
16 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed) | 23 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed) |
61 def __init__(self, name): | 68 def __init__(self, name): |
62 self.name = name | 69 self.name = name |
63 | 70 |
64 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): | 71 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): |
65 '''extrapolationParameters specific to each method (in name field) ''' | 72 '''extrapolationParameters specific to each method (in name field) ''' |
66 if extrapolationHypothesis.name == 'constant velocity': | 73 if extrapolationParameters.name == 'constant velocity': |
67 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] | 74 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] |
68 else: | 75 else: |
69 print('Unknown extrapolation hypothesis') | 76 print('Unknown extrapolation hypothesis') |
70 return [] | 77 return [] |
71 | 78 |
79 class CollisionPoint(moving.Point): | |
80 def __init__(self, p, probability = 1., TTC = -1): | |
81 self.x = p.x | |
82 self.y = p.y | |
83 self.probability = probability | |
84 self.TTC = TTC | |
85 | |
86 class CrossingZone(moving.Point): | |
87 def __init__(self, p, probability = 1., pPET = -1): | |
88 self.x = p.x | |
89 self.y = p.y | |
90 self.probability = probability | |
91 self.pPET = pPET | |
92 | |
72 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): | 93 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): |
73 '''returns the lists of collision points and crossing zones ''' | 94 '''returns the lists of collision points and crossing zones ''' |
74 collisionPoints = [] | 95 collisionPoints = [] |
75 TTCs = [] | |
76 crossingZones = [] | 96 crossingZones = [] |
77 pPETs = [] | |
78 for et1 in extrapolatedTrajectories1: | 97 for et1 in extrapolatedTrajectories1: |
79 for et2 in extrapolatedTrajectories2: | 98 for et2 in extrapolatedTrajectories2: |
80 | 99 |
81 t = 1 | 100 t = 1 |
82 p1 = et1.predictPosition(t) | 101 p1 = et1.predictPosition(t) |
83 p2 = et2.predictPosition(t) | 102 p2 = et2.predictPosition(t) |
84 while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold: | 103 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: |
85 p1 = et1.predictPosition(t) | 104 p1 = et1.predictPosition(t) |
86 p2 = et2.predictPosition(t) | 105 p2 = et2.predictPosition(t) |
87 t += 1 | 106 t += 1 |
88 | 107 |
89 if t <= timeHorizon: | 108 if t <= timeHorizon: |
90 TTCs.append(t) # todo store probability | 109 collisionPoints.append(CollisionPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) |
91 collisionPoints.append((p1+p2).multiply(0.5)) | |
92 else: # check if there is a crossing zone | 110 else: # check if there is a crossing zone |
93 for t1 in xrange(timeHorizon-1): | 111 # TODO? zone should be around the points at which the traj are the closest |
94 for t2 in xrange(timeHorizon-1): | 112 # look for CZ at different times, otherwise it would be a collision |
95 cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) | 113 # an approximation would be to look for close points at different times, ie the complementary of collision points |
114 cz = None | |
115 t1 = 0 | |
116 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 | |
117 t2 = 0 | |
118 while not cz and t2 < timeHorizon: | |
119 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: | |
120 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) | |
121 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) | |
96 if cz: | 122 if cz: |
97 crossingZones.append(cz) | 123 crossingZones.append(CrossingZone(cz, et1.probability*et2.probability, abs(t1-t2))) |
98 pPETs.append(abs(t1-t2)) | 124 t2 += 1 |
99 break | 125 t1 += 1 |
100 return collisionPoints, crossingZones, TTCs, pPETs | 126 return collisionPoints, crossingZones |
101 | 127 |
102 | 128 |
103 | 129 |
104 # Default values: to remove because we cannot tweak that from a script where the value may be different | 130 # Default values: to remove because we cannot tweak that from a script where the value may be different |
105 FPS= 25 # No. of frame per second (FPS) | 131 FPS= 25 # No. of frame per second (FPS) |