Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 944:84ebe1b031f1

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