Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 260:36cb40c51a5e

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