Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 556:dc58ad777a72

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 14 Jul 2014 01:56:51 -0400
parents 727e3c529519
children b91f33e098ee
comparison
equal deleted inserted replaced
555:f13220f765e0 556:dc58ad777a72
122 p1 = predictedTrajectory1.predictPosition(t) 122 p1 = predictedTrajectory1.predictPosition(t)
123 p2 = predictedTrajectory2.predictPosition(t) 123 p2 = predictedTrajectory2.predictPosition(t)
124 t += 1 124 t += 1
125 return t, p1, p2 125 return t, p1, p2
126 126
127 def computeCrossingsCollisionsAtInstant(predictionParams, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
128 '''returns the lists of collision points and crossing zones'''
129 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
130 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
131
132 collisionPoints = []
133 crossingZones = []
134 for et1 in predictedTrajectories1:
135 for et2 in predictedTrajectories2:
136 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
137
138 if t <= timeHorizon:
139 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
140 elif computeCZ: # check if there is a crossing zone
141 # TODO? zone should be around the points at which the traj are the closest
142 # look for CZ at different times, otherwise it would be a collision
143 # an approximation would be to look for close points at different times, ie the complementary of collision points
144 cz = None
145 t1 = 0
146 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
147 t2 = 0
148 while not cz and t2 < timeHorizon:
149 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
150 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
151 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
152 if cz:
153 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
154 t2 += 1
155 t1 += 1
156
157 if debug:
158 from matplotlib.pyplot import figure, axis, title
159 figure()
160 for et in predictedTrajectories1:
161 et.predictPosition(timeHorizon)
162 et.plot('rx')
163
164 for et in predictedTrajectories2:
165 et.predictPosition(timeHorizon)
166 et.plot('bx')
167 obj1.plot('r')
168 obj2.plot('b')
169 title('instant {0}'.format(currentInstant))
170 axis('equal')
171
172 return currentInstant, collisionPoints, crossingZones
173
174 def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
175 '''Computes all crossing and collision points at each common instant for two road users. '''
176 collisionPoints={}
177 crossingZones={}
178 if timeInterval:
179 commonTimeInterval = timeInterval
180 else:
181 commonTimeInterval = obj1.commonTimeInterval(obj2)
182 if nProcesses == 1:
183 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
184 i, collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
185 else:
186 from multiprocessing import Pool
187 pool = Pool(processes = nProcesses)
188 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ)) for i in list(commonTimeInterval)[:-1]]
189 #results = [j.get() for j in jobs]
190 #results.sort()
191 for j in jobs:
192 i, cp, cz = j.get()
193 #if len(cp) != 0 or len(cz) != 0:
194 collisionPoints[i] = cp
195 crossingZones[i] = cz
196 pool.close()
197 return collisionPoints, crossingZones
198
127 class PredictionParameters: 199 class PredictionParameters:
128 def __init__(self, name, maxSpeed): 200 def __init__(self, name, maxSpeed):
129 self.name = name 201 self.name = name
130 self.maxSpeed = maxSpeed 202 self.maxSpeed = maxSpeed
131 203
134 206
135 def generatePredictedTrajectories(self, obj, instant): 207 def generatePredictedTrajectories(self, obj, instant):
136 return [] 208 return []
137 209
138 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): 210 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
139 '''returns the lists of collision points and crossing zones''' 211 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False)
140 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant) 212
141 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant) 213 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
142 214 return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1)
143 collisionPoints = []
144 crossingZones = []
145 for et1 in predictedTrajectories1:
146 for et2 in predictedTrajectories2:
147 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
148
149 if t <= timeHorizon:
150 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
151 elif computeCZ: # check if there is a crossing zone
152 # TODO? zone should be around the points at which the traj are the closest
153 # look for CZ at different times, otherwise it would be a collision
154 # an approximation would be to look for close points at different times, ie the complementary of collision points
155 cz = None
156 t1 = 0
157 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
158 t2 = 0
159 while not cz and t2 < timeHorizon:
160 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
161 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
162 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
163 if cz:
164 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
165 t2 += 1
166 t1 += 1
167
168 if debug:
169 from matplotlib.pyplot import figure, axis, title
170 figure()
171 for et in predictedTrajectories1:
172 et.predictPosition(timeHorizon)
173 et.plot('rx')
174
175 for et in predictedTrajectories2:
176 et.predictPosition(timeHorizon)
177 et.plot('bx')
178 obj1.plot('r')
179 obj2.plot('b')
180 title('instant {0}'.format(currentInstant))
181 axis('equal')
182
183 return collisionPoints, crossingZones
184
185 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):
186 '''Computes all crossing and collision points at each common instant for two road users. '''
187 collisionPoints={}
188 crossingZones={}
189 if timeInterval:
190 commonTimeInterval = timeInterval
191 else:
192 commonTimeInterval = obj1.commonTimeInterval(obj2)
193 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
194 collisionPoints[i], crossingZones[i] = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
195
196 return collisionPoints, crossingZones
197 215
198 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): 216 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
199 '''Computes only collision probabilities 217 '''Computes only collision probabilities
200 Returns for each instant the collision probability and number of samples drawn''' 218 Returns for each instant the collision probability and number of samples drawn'''
201 collisionProbabilities = {} 219 collisionProbabilities = {}