Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 264:a04a6af4b810
modified functions to generate extrapolated trajectories for different positions/velocities
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 26 Jul 2012 03:54:41 -0400 |
parents | 36cb40c51a5e |
children | 7a3bf04cf016 |
comparison
equal
deleted
inserted
replaced
263:c71540470057 | 264:a04a6af4b810 |
---|---|
60 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} | 60 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
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 | |
66 class ExtrapolationParameters: | 65 class ExtrapolationParameters: |
67 def __init__(self, name): | 66 def __init__(self, name): |
68 self.name = name | 67 self.name = name |
69 | 68 |
70 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity): | 69 def createExtrapolatedTrajectories(extrapolationParameters, obj, instant): |
71 '''extrapolationParameters specific to each method (in name field) ''' | 70 '''extrapolationParameters specific to each method (in name field) ''' |
72 if extrapolationParameters.name == 'constant velocity': | 71 if extrapolationParameters.name == 'constant velocity': |
73 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = extrapolationParameters.maxSpeed)] | 72 return [ExtrapolatedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), |
73 maxSpeed = extrapolationParameters.maxSpeed)] | |
74 elif extrapolationParameters.name == 'normal adaptation': | 74 elif extrapolationParameters.name == 'normal adaptation': |
75 # generate several and with the right distribution | |
76 extrapolatedTrajectories = [] | 75 extrapolatedTrajectories = [] |
77 for i in xrange(extrapolationParameters.nExtrapolatedTrajectories): | 76 for i in xrange(extrapolationParameters.nExtrapolatedTrajectories): |
78 extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(initialPosition, initialVelocity, | 77 extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(obj.getPositionAtInstant(instant), |
78 obj.getVelocityAtInstant(instant), | |
79 extrapolationParameters.accelerationDistribution, | 79 extrapolationParameters.accelerationDistribution, |
80 extrapolationParameters.steeringDistribution, | 80 extrapolationParameters.steeringDistribution, |
81 maxSpeed = extrapolationParameters.maxSpeed)) | 81 maxSpeed = extrapolationParameters.maxSpeed)) |
82 return extrapolatedTrajectories | |
83 elif extrapolationParameters.name == 'pointset': | |
84 extrapolatedTrajectories = [] | |
85 features = [f for f in obj.features if f.existsAtInstant(instant)] | |
86 positions = [f.getPositionAtInstant(instant) for f in features] | |
87 velocities = [f.getVelocityAtInstant(instant) for f in features] | |
88 for initialPosition,initialVelocity in zip(positions, velocities): | |
89 extrapolatedTrajectories.append(ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, | |
90 maxSpeed = extrapolationParameters.maxSpeed)) | |
82 return extrapolatedTrajectories | 91 return extrapolatedTrajectories |
83 else: | 92 else: |
84 print('Unknown extrapolation hypothesis') | 93 print('Unknown extrapolation hypothesis') |
85 return [] | 94 return [] |
86 | 95 |
133 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) | 142 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) |
134 t2 += 1 | 143 t2 += 1 |
135 t1 += 1 | 144 t1 += 1 |
136 return collisionPoints, crossingZones | 145 return collisionPoints, crossingZones |
137 | 146 |
138 def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False): | 147 def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None): |
139 collisionPoints={} | 148 collisionPoints={} |
140 crossingZones={} | 149 crossingZones={} |
141 #TTCs = {} | 150 #TTCs = {} |
142 #pPETs = {} | 151 #pPETs = {} |
143 commonTimeInterval = obj1.commonTimeInterval(obj2) | 152 if timeInterval: |
153 commonTimeInterval = timeInterval | |
154 else: | |
155 commonTimeInterval = obj1.commonTimeInterval(obj2) | |
144 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 156 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)) | 157 print(obj1.num, obj2.num, i) |
146 extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2.getPositionAtInstant(i), obj2.getVelocityAtInstant(i)) | 158 extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1, i) |
159 extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2, i) | |
147 | 160 |
148 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon) | 161 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon) |
149 # if len(collisionPoints[i]) > 0: | 162 # if len(collisionPoints[i]) > 0: |
150 # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i]) | 163 # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i]) |
151 # if len(crossingZones[i]) > 0: | 164 # if len(crossingZones[i]) > 0: |