Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 266:aba9711b3149

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