Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 944:84ebe1b031f1
works with object trajectory, features todo
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 20 Jul 2017 12:12:28 -0400 |
parents | b1e8453c207c |
children | 05d4302bf67e |
comparison
equal
deleted
inserted
replaced
943:b1e8453c207c | 944:84ebe1b031f1 |
---|---|
157 p1 = predictedTrajectory1.predictPosition(t) | 157 p1 = predictedTrajectory1.predictPosition(t) |
158 p2 = predictedTrajectory2.predictPosition(t) | 158 p2 = predictedTrajectory2.predictPosition(t) |
159 collision = (p1-p2).norm2() <= collisionDistanceThreshold | 159 collision = (p1-p2).norm2() <= collisionDistanceThreshold |
160 return collision, t, p1, p2 | 160 return collision, t, p1, p2 |
161 | 161 |
162 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): | 162 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True): |
163 from matplotlib.pyplot import figure, axis, title, clf, savefig | 163 from matplotlib.pyplot import figure, axis, title, clf, savefig |
164 figure() | 164 if printFigure: |
165 #clf() | 165 clf() |
166 else: | |
167 figure() | |
166 for et in predictedTrajectories1: | 168 for et in predictedTrajectories1: |
167 et.predictPosition(int(np.round(timeHorizon))) | 169 for t in xrange(int(np.round(timeHorizon))): |
168 et.plot('rx') | 170 et.predictPosition(t) |
169 | 171 et.plot('rx') |
170 for et in predictedTrajectories2: | 172 for et in predictedTrajectories2: |
171 et.predictPosition(int(np.round(timeHorizon))) | 173 for t in xrange(int(np.round(timeHorizon))): |
172 et.plot('bx') | 174 et.predictPosition(t) |
175 et.plot('bx') | |
173 obj1.plot('r', withOrigin = True) | 176 obj1.plot('r', withOrigin = True) |
174 obj2.plot('b', withOrigin = True) | 177 obj2.plot('b', withOrigin = True) |
175 title('instant {0}'.format(currentInstant)) | 178 title('instant {0}'.format(currentInstant)) |
176 axis('equal') | 179 axis('equal') |
177 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) | 180 if printFigure: |
181 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) | |
178 | 182 |
179 def calculateProbability(nMatching,similarity,objects): | 183 def calculateProbability(nMatching,similarity,objects): |
180 sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) | 184 sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) |
181 prototypeProbability={} | 185 prototypeProbability={} |
182 for i in similarity.keys(): | 186 for i in similarity.keys(): |
252 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) | 256 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) |
253 else: | 257 else: |
254 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) | 258 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) |
255 return prototypeTrajectories | 259 return prototypeTrajectories |
256 | 260 |
257 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): | 261 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): |
258 '''returns the lists of collision points and crossing zones''' | 262 '''returns the lists of collision points and crossing zones''' |
259 # if usePrototypes: | |
260 # prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | |
261 # prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | |
262 # predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) | |
263 # predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) | |
264 # else: | |
265 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) | 263 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) |
266 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) | 264 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) |
267 | 265 |
268 collisionPoints = [] | 266 collisionPoints = [] |
269 crossingZones = [] | 267 crossingZones = [] |
270 for et1 in predictedTrajectories1: | 268 for et1 in predictedTrajectories1: |
271 for et2 in predictedTrajectories2: | 269 for et2 in predictedTrajectories2: |
272 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 270 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
273 if collision: | 271 if collision: |
274 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) | 272 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) |
275 elif computeCZ: # check if there is a crossing zone | 273 elif computeCZ: # check if there is a crossing zone |
276 # TODO? zone should be around the points at which the traj are the closest | 274 # TODO same computation as PET with metric + concatenate past trajectory with future trajectory |
277 # look for CZ at different times, otherwise it would be a collision | |
278 # an approximation would be to look for close points at different times, ie the complementary of collision points | |
279 cz = None | 275 cz = None |
280 t1 = 0 | 276 t1 = 0 |
281 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 | 277 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 |
282 t2 = 0 | 278 t2 = 0 |
283 while not cz and t2 < timeHorizon: | 279 while not cz and t2 < timeHorizon: |
284 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: | |
285 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) | |
286 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) | 280 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) |
287 if cz is not None: | 281 if cz is not None: |
288 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() | 282 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() |
289 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV))) | 283 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV))) |
290 t2 += 1 | 284 t2 += 1 |
305 return '{0} {1}'.format(self.name, self.maxSpeed) | 299 return '{0} {1}'.format(self.name, self.maxSpeed) |
306 | 300 |
307 def generatePredictedTrajectories(self, obj, instant): | 301 def generatePredictedTrajectories(self, obj, instant): |
308 return [] | 302 return [] |
309 | 303 |
310 # def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): | 304 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): |
311 # return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | |
312 | |
313 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):#,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): | |
314 #def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): | |
315 '''Computes all crossing and collision points at each common instant for two road users. ''' | 305 '''Computes all crossing and collision points at each common instant for two road users. ''' |
316 collisionPoints={} | 306 collisionPoints={} |
317 crossingZones={} | 307 crossingZones={} |
318 if timeInterval: | 308 if timeInterval: |
319 commonTimeInterval = timeInterval | 309 commonTimeInterval = timeInterval |
320 else: | 310 else: |
321 commonTimeInterval = obj1.commonTimeInterval(obj2) | 311 commonTimeInterval = obj1.commonTimeInterval(obj2) |
322 if nProcesses == 1: | 312 if nProcesses == 1: |
323 # if usePrototypes: | |
324 # firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) | |
325 # commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors | |
326 # commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors | |
327 # for i in commonTimeIntervalList2: | |
328 # i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | |
329 # if len(cp) != 0: | |
330 # collisionPoints[i] = cp | |
331 # if len(cz) != 0: | |
332 # crossingZones[i] = cz | |
333 # if collisionPoints!={} or crossingZones!={}: | |
334 # for i in commonTimeIntervalList1: | |
335 # if i not in commonTimeIntervalList2: | |
336 # i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | |
337 # if len(cp) != 0: | |
338 # collisionPoints[i] = cp | |
339 # if len(cz) != 0: | |
340 # crossingZones[i] = cz | |
341 # else: | |
342 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 313 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
343 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 314 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) |
344 if len(cp) != 0: | 315 if len(cp) != 0: |
345 collisionPoints[i] = cp | 316 collisionPoints[i] = cp |
346 if len(cz) != 0: | 317 if len(cz) != 0: |
347 crossingZones[i] = cz | 318 crossingZones[i] = cz |
348 else: | 319 else: |
349 pool = Pool(processes = nProcesses) | 320 pool = Pool(processes = nProcesses) |
350 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] #,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype | 321 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] |
351 #results = [j.get() for j in jobs] | |
352 #results.sort() | |
353 for j in jobs: | 322 for j in jobs: |
354 i, cp, cz = j.get() | 323 i, cp, cz = j.get() |
355 #if len(cp) != 0 or len(cz) != 0: | |
356 if len(cp) != 0: | 324 if len(cp) != 0: |
357 collisionPoints[i] = cp | 325 collisionPoints[i] = cp |
358 if len(cz) != 0: | 326 if len(cz) != 0: |
359 crossingZones[i] = cz | 327 crossingZones[i] = cz |
360 pool.close() | 328 pool.close() |
361 return collisionPoints, crossingZones | 329 return collisionPoints, crossingZones |
362 #return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step) | |
363 | 330 |
364 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): | 331 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
365 '''Computes only collision probabilities | 332 '''Computes only collision probabilities |
366 Returns for each instant the collision probability and number of samples drawn''' | 333 Returns for each instant the collision probability and number of samples drawn''' |
367 collisionProbabilities = {} | 334 collisionProbabilities = {} |
435 return predictedTrajectories | 402 return predictedTrajectories |
436 | 403 |
437 class PointSetPredictionParameters(PredictionParameters): | 404 class PointSetPredictionParameters(PredictionParameters): |
438 def __init__(self, maxSpeed): | 405 def __init__(self, maxSpeed): |
439 PredictionParameters.__init__(self, 'point set', maxSpeed) | 406 PredictionParameters.__init__(self, 'point set', maxSpeed) |
440 #self.nPredictedTrajectories = nPredictedTrajectories | |
441 | 407 |
442 def generatePredictedTrajectories(self, obj, instant): | 408 def generatePredictedTrajectories(self, obj, instant): |
443 predictedTrajectories = [] | 409 predictedTrajectories = [] |
444 if obj.hasFeatures(): | 410 if obj.hasFeatures(): |
445 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)] | 411 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)] |
517 if intersection is not None: | 483 if intersection is not None: |
518 dp1 = intersection-p1 | 484 dp1 = intersection-p1 |
519 dp2 = intersection-p2 | 485 dp2 = intersection-p2 |
520 dot1 = moving.Point.dot(dp1, v1) | 486 dot1 = moving.Point.dot(dp1, v1) |
521 dot2 = moving.Point.dot(dp2, v2) | 487 dot2 = moving.Point.dot(dp2, v2) |
522 #print dot1, dot2 | |
523 # (computeCZ and (dot1 > 0 or dot2 > 0)) or ( | |
524 if (computeCZ and (dot1 > 0 or dot2 > 0)) or (dot1 > 0 and dot2 > 0): # if the road users are moving towards the intersection or if computing pPET | 488 if (computeCZ and (dot1 > 0 or dot2 > 0)) or (dot1 > 0 and dot2 > 0): # if the road users are moving towards the intersection or if computing pPET |
525 dist1 = dp1.norm2() | 489 dist1 = dp1.norm2() |
526 dist2 = dp2.norm2() | 490 dist2 = dp2.norm2() |
527 s1 = math.copysign(v1.norm2(), dot1) | 491 s1 = math.copysign(v1.norm2(), dot1) |
528 s2 = math.copysign(v2.norm2(), dot2) | 492 s2 = math.copysign(v2.norm2(), dot2) |
577 pass # compute pPET | 541 pass # compute pPET |
578 | 542 |
579 return currentInstant, collisionPoints, crossingZones | 543 return currentInstant, collisionPoints, crossingZones |
580 | 544 |
581 class PrototypePredictionParameters(PredictionParameters): | 545 class PrototypePredictionParameters(PredictionParameters): |
582 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): # lcss parameters | 546 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): |
583 PredictionParameters.__init__(self, 'prototypes', None) | 547 PredictionParameters.__init__(self, 'prototypes', None) |
584 self.prototypes = prototypes | 548 self.prototypes = prototypes |
585 self.nPredictedTrajectories = nPredictedTrajectories | 549 self.nPredictedTrajectories = nPredictedTrajectories |
586 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance) | 550 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance) |
587 #self.similarityFunc = lambda x,y : self.lcss.computeNormalized(x, y) | |
588 self.minSimilarity = minSimilarity | 551 self.minSimilarity = minSimilarity |
589 self.minFeatureTime = minFeatureTime | 552 self.minFeatureTime = minFeatureTime |
590 self.constantSpeed = constantSpeed | 553 self.constantSpeed = constantSpeed |
591 self.useFeatures = useFeatures | 554 self.useFeatures = useFeatures |
592 | 555 |