Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 260:36cb40c51a5e
modified the organization of the code
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 24 Jul 2012 18:07:23 -0400 |
parents | 8ab76b95ee72 |
children | a04a6af4b810 |
comparison
equal
deleted
inserted
replaced
259:8ab76b95ee72 | 260:36cb40c51a5e |
---|---|
32 def draw(self, options = '', withOrigin = False, **kwargs): | 32 def draw(self, options = '', withOrigin = False, **kwargs): |
33 self.getPredictedTrajectory().draw(options, withOrigin, **kwargs) | 33 self.getPredictedTrajectory().draw(options, withOrigin, **kwargs) |
34 | 34 |
35 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): | 35 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): |
36 '''Extrapolated trajectory at constant speed or acceleration | 36 '''Extrapolated trajectory at constant speed or acceleration |
37 TODO add limits if acceleration | |
38 TODO generalize by passing a series of velocities/accelerations''' | 37 TODO generalize by passing a series of velocities/accelerations''' |
39 | 38 |
40 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None): | 39 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None): |
41 self.control = control | 40 self.control = control |
42 self.maxSpeed = maxSpeed | 41 self.maxSpeed = maxSpeed |
66 | 65 |
67 class ExtrapolationParameters: | 66 class ExtrapolationParameters: |
68 def __init__(self, name): | 67 def __init__(self, name): |
69 self.name = name | 68 self.name = name |
70 | 69 |
71 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): | 70 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity): |
72 '''extrapolationParameters specific to each method (in name field) ''' | 71 '''extrapolationParameters specific to each method (in name field) ''' |
73 if extrapolationParameters.name == 'constant velocity': | 72 if extrapolationParameters.name == 'constant velocity': |
74 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] | 73 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = extrapolationParameters.maxSpeed)] |
74 elif extrapolationParameters.name == 'normal adaptation': | |
75 # generate several and with the right distribution | |
76 extrapolatedTrajectories = [] | |
77 for i in xrange(extrapolationParameters.nExtrapolatedTrajectories): | |
78 extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(initialPosition, initialVelocity, | |
79 extrapolationParameters.accelerationDistribution, | |
80 extrapolationParameters.steeringDistribution, | |
81 maxSpeed = extrapolationParameters.maxSpeed)) | |
82 return extrapolatedTrajectories | |
75 else: | 83 else: |
76 print('Unknown extrapolation hypothesis') | 84 print('Unknown extrapolation hypothesis') |
77 return [] | 85 return [] |
78 | 86 |
79 class SafetyPoint(moving.Point): | 87 class SafetyPoint(moving.Point): |
83 self.x = p.x | 91 self.x = p.x |
84 self.y = p.y | 92 self.y = p.y |
85 self.probability = probability | 93 self.probability = probability |
86 self.indicator = indicator | 94 self.indicator = indicator |
87 | 95 |
96 def saveSafetyPoints(out, objNum1, objNum2, instant, points): | |
97 for p in points: | |
98 out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator)) | |
99 | |
88 def computeExpectedIndicator(points): | 100 def computeExpectedIndicator(points): |
89 from numpy import sum | 101 from numpy import sum |
90 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) | 102 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) |
91 | 103 |
92 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): | 104 def computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): |
93 '''returns the lists of collision points and crossing zones ''' | 105 '''returns the lists of collision points and crossing zones ''' |
94 collisionPoints = [] | 106 collisionPoints = [] |
95 crossingZones = [] | 107 crossingZones = [] |
96 for et1 in extrapolatedTrajectories1: | 108 for et1 in extrapolatedTrajectories1: |
97 for et2 in extrapolatedTrajectories2: | 109 for et2 in extrapolatedTrajectories2: |
98 | |
99 t = 1 | 110 t = 1 |
100 p1 = et1.predictPosition(t) | 111 p1 = et1.predictPosition(t) |
101 p2 = et2.predictPosition(t) | 112 p2 = et2.predictPosition(t) |
102 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: | 113 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: |
103 p1 = et1.predictPosition(t) | 114 p1 = et1.predictPosition(t) |
122 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) | 133 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) |
123 t2 += 1 | 134 t2 += 1 |
124 t1 += 1 | 135 t1 += 1 |
125 return collisionPoints, crossingZones | 136 return collisionPoints, crossingZones |
126 | 137 |
127 | 138 def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False): |
139 collisionPoints={} | |
140 crossingZones={} | |
141 #TTCs = {} | |
142 #pPETs = {} | |
143 commonTimeInterval = obj1.commonTimeInterval(obj2) | |
144 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | |
145 extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1.getPositionAtInstant(i), obj1.getVelocityAtInstant(i)) | |
146 extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2.getPositionAtInstant(i), obj2.getVelocityAtInstant(i)) | |
147 | |
148 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon) | |
149 # if len(collisionPoints[i]) > 0: | |
150 # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i]) | |
151 # if len(crossingZones[i]) > 0: | |
152 # pPETs[i] = extrapolation.computeExpectedIndicator(crossingZones[i]) | |
153 saveSafetyPoints(outCP, obj1.num, obj2.num, i, collisionPoints[i]) | |
154 saveSafetyPoints(outCZ, obj1.num, obj2.num, i, crossingZones[i]) | |
155 | |
156 if debug: | |
157 from matplotlib.pyplot import figure, axis | |
158 figure() | |
159 obj1.draw('r') | |
160 obj2.draw('b') | |
161 for et in extrapolatedTrajectories1: | |
162 et.predictPosition(timeHorizon) | |
163 et.draw('rx') | |
164 for et in extrapolatedTrajectories2: | |
165 et.predictPosition(timeHorizon) | |
166 et.draw('bx') | |
167 axis('equal') | |
168 | |
169 | |
170 # probability of successful evasive action = 1-P(Collision) using larger control values | |
171 | |
172 return collisionPoints, crossingZones | |
173 | |
174 ############### | |
128 | 175 |
129 # Default values: to remove because we cannot tweak that from a script where the value may be different | 176 # Default values: to remove because we cannot tweak that from a script where the value may be different |
130 FPS= 25 # No. of frame per second (FPS) | 177 FPS= 25 # No. of frame per second (FPS) |
131 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec | 178 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec |
132 deltaT= FPS*5 # extrapolatation time Horizon = 5 second | 179 deltaT= FPS*5 # extrapolatation time Horizon = 5 second |