Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 264:a04a6af4b810

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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: