Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 556:dc58ad777a72
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 = {} |