Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 266:aba9711b3149
small modificatons and reorganization
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 27 Jul 2012 10:29:24 -0400 |
parents | 7a3bf04cf016 |
children | 32e88b513f5c |
comparison
equal
deleted
inserted
replaced
265:7a3bf04cf016 | 266:aba9711b3149 |
---|---|
61 | 61 |
62 def getControl(self): | 62 def getControl(self): |
63 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) | 63 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) |
64 | 64 |
65 class ExtrapolationParameters: | 65 class ExtrapolationParameters: |
66 def __init__(self, name): | 66 def __init__(self, name, maxSpeed): |
67 self.name = name | 67 self.name = name |
68 self.maxSpeed = maxSpeed | |
69 | |
70 def __str__(self): | |
71 return '{0} {1}'.format(self.name, self.maxSpeed) | |
68 | 72 |
69 def createExtrapolatedTrajectories(extrapolationParameters, obj, instant): | 73 def createExtrapolatedTrajectories(extrapolationParameters, obj, instant): |
70 '''extrapolationParameters specific to each method (in name field) ''' | 74 '''extrapolationParameters specific to each method (in name field) ''' |
71 if extrapolationParameters.name == 'constant velocity': | 75 if extrapolationParameters.name == 'constant velocity': |
72 return [ExtrapolatedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), | 76 return [ExtrapolatedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), |
100 self.x = p.x | 104 self.x = p.x |
101 self.y = p.y | 105 self.y = p.y |
102 self.probability = probability | 106 self.probability = probability |
103 self.indicator = indicator | 107 self.indicator = indicator |
104 | 108 |
105 def saveSafetyPoints(out, objNum1, objNum2, instant, points): | 109 @staticmethod |
106 for p in points: | 110 def save(out, points, objNum1, objNum2, instant): |
107 out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator)) | 111 for p in points: |
112 out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator)) | |
108 | 113 |
109 def computeExpectedIndicator(points): | 114 def computeExpectedIndicator(points): |
110 from numpy import sum | 115 from numpy import sum |
111 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) | 116 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) |
112 | 117 |
113 def computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): | 118 def computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon): |
114 '''returns the lists of collision points and crossing zones ''' | 119 '''returns the lists of collision points and crossing zones ''' |
120 extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1, i) | |
121 extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2, i) | |
122 | |
115 collisionPoints = [] | 123 collisionPoints = [] |
116 crossingZones = [] | 124 crossingZones = [] |
117 for et1 in extrapolatedTrajectories1: | 125 for et1 in extrapolatedTrajectories1: |
118 for et2 in extrapolatedTrajectories2: | 126 for et2 in extrapolatedTrajectories2: |
119 t = 1 | 127 t = 1 |
153 commonTimeInterval = timeInterval | 161 commonTimeInterval = timeInterval |
154 else: | 162 else: |
155 commonTimeInterval = obj1.commonTimeInterval(obj2) | 163 commonTimeInterval = obj1.commonTimeInterval(obj2) |
156 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 164 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
157 print(obj1.num, obj2.num, i) | 165 print(obj1.num, obj2.num, i) |
158 extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1, i) | 166 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon) |
159 extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2, i) | |
160 | |
161 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon) | |
162 # if len(collisionPoints[i]) > 0: | 167 # if len(collisionPoints[i]) > 0: |
163 # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i]) | 168 # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i]) |
164 # if len(crossingZones[i]) > 0: | 169 # if len(crossingZones[i]) > 0: |
165 # pPETs[i] = extrapolation.computeExpectedIndicator(crossingZones[i]) | 170 # pPETs[i] = extrapolation.computeExpectedIndicator(crossingZones[i]) |
166 saveSafetyPoints(outCP, obj1.num, obj2.num, i, collisionPoints[i]) | 171 SafetyPoint.save(outCP, collisionPoints[i], obj1.num, obj2.num, i) |
167 saveSafetyPoints(outCZ, obj1.num, obj2.num, i, crossingZones[i]) | 172 SafetyPoint.save(outCZ, crossingZones[i], obj1.num, obj2.num, i) |
168 | 173 |
169 if debug: | 174 if debug: |
170 from matplotlib.pyplot import figure, axis, title | 175 from matplotlib.pyplot import figure, axis, title |
171 figure() | 176 figure() |
172 obj1.draw('r') | 177 obj1.draw('r') |