Mercurial Hosting > traffic-intelligence
diff python/prediction.py @ 943:b1e8453c207c
work on motion prediction using motion patterns
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 19 Jul 2017 18:02:38 -0400 |
parents | ab13aaf41432 |
children | 84ebe1b031f1 |
line wrap: on
line diff
--- a/python/prediction.py Tue Jul 18 18:01:16 2017 -0400 +++ b/python/prediction.py Wed Jul 19 18:02:38 2017 -0400 @@ -82,7 +82,6 @@ def predictPosition(self, nTimeSteps): if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): if self.constantSpeed: - # calculate cumulative distance traj = self.prototype.getPositions() trajLength = traj.length() traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) @@ -161,8 +160,9 @@ return collision, t, p1, p2 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): - from matplotlib.pyplot import figure, axis, title, close, savefig + from matplotlib.pyplot import figure, axis, title, clf, savefig figure() + #clf() for et in predictedTrajectories1: et.predictPosition(int(np.round(timeHorizon))) et.plot('rx') @@ -170,12 +170,11 @@ for et in predictedTrajectories2: et.predictPosition(int(np.round(timeHorizon))) et.plot('bx') - obj1.plot('r') - obj2.plot('b') + obj1.plot('r', withOrigin = True) + obj2.plot('b', withOrigin = True) title('instant {0}'.format(currentInstant)) axis('equal') savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) - close() def calculateProbability(nMatching,similarity,objects): sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) @@ -255,23 +254,22 @@ prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) return prototypeTrajectories -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): +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): '''returns the lists of collision points and crossing zones''' - if usePrototypes: - prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) - prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) - predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) - predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) - else: - predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) - predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) + # if usePrototypes: + # prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + # prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + # predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) + # predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) + # else: + predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) + predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) collisionPoints = [] crossingZones = [] for et1 in predictedTrajectories1: for et2 in predictedTrajectories2: collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) - if collision: collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) elif computeCZ: # check if there is a crossing zone @@ -309,10 +307,10 @@ def generatePredictedTrajectories(self, obj, instant): return [] - 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): - return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) +# 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): +# return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) - 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): + 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): #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): '''Computes all crossing and collision points at each common instant for two road users. ''' collisionPoints={} @@ -322,34 +320,34 @@ else: commonTimeInterval = obj1.commonTimeInterval(obj2) if nProcesses == 1: - if usePrototypes: - firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) - commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors - commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors - for i in commonTimeIntervalList2: - 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) - if len(cp) != 0: - collisionPoints[i] = cp - if len(cz) != 0: - crossingZones[i] = cz - if collisionPoints!={} or crossingZones!={}: - for i in commonTimeIntervalList1: - if i not in commonTimeIntervalList2: - 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) - if len(cp) != 0: - collisionPoints[i] = cp - if len(cz) != 0: - crossingZones[i] = cz - else: - for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors - 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) - if len(cp) != 0: - collisionPoints[i] = cp - if len(cz) != 0: - crossingZones[i] = cz + # if usePrototypes: + # firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) + # commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors + # commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors + # for i in commonTimeIntervalList2: + # 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) + # if len(cp) != 0: + # collisionPoints[i] = cp + # if len(cz) != 0: + # crossingZones[i] = cz + # if collisionPoints!={} or crossingZones!={}: + # for i in commonTimeIntervalList1: + # if i not in commonTimeIntervalList2: + # 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) + # if len(cp) != 0: + # collisionPoints[i] = cp + # if len(cz) != 0: + # crossingZones[i] = cz + # else: + for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors + 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) + if len(cp) != 0: + collisionPoints[i] = cp + if len(cz) != 0: + crossingZones[i] = cz else: pool = Pool(processes = nProcesses) - jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]] + 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 #results = [j.get() for j in jobs] #results.sort() for j in jobs: @@ -437,7 +435,6 @@ return predictedTrajectories class PointSetPredictionParameters(PredictionParameters): - # todo generate several trajectories with normal adaptatoins from each position (feature) def __init__(self, maxSpeed): PredictionParameters.__init__(self, 'point set', maxSpeed) #self.nPredictedTrajectories = nPredictedTrajectories @@ -456,6 +453,7 @@ print('Object {} has no features'.format(obj.getNum())) return None + class EvasiveActionPredictionParameters(PredictionParameters): def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False): '''Suggested acceleration distribution may not be symmetric, eg @@ -580,22 +578,36 @@ return currentInstant, collisionPoints, crossingZones -#### -# Other Methods -#### class PrototypePredictionParameters(PredictionParameters): - def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True): - name = 'prototype' - PredictionParameters.__init__(self, name, maxSpeed) + def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): # lcss parameters + PredictionParameters.__init__(self, 'prototypes', None) + self.prototypes = prototypes self.nPredictedTrajectories = nPredictedTrajectories + self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance) + #self.similarityFunc = lambda x,y : self.lcss.computeNormalized(x, y) + self.minSimilarity = minSimilarity + self.minFeatureTime = minFeatureTime self.constantSpeed = constantSpeed + self.useFeatures = useFeatures - def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories): + def generatePredictedTrajectories(self, obj, instant): predictedTrajectories = [] - initialPosition = obj.getPositionAtInstant(instant) - initialVelocity = obj.getVelocityAtInstant(instant) - for prototypeTraj in prototypeTrajectories.keys(): - predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj])) + if instant-obj.getFirstInstant()+1 >= self.minFeatureTime: + if self.useFeatures and obj.hasFeatures(): + # get current features existing for the most time, sort on first instant of feature and take n first + pass + else: + if not hasattr(obj, 'prototypeSimilarities'): + obj.prototypeSimilarities = [] + for proto in self.prototypes: + self.lcss.similarities(proto.getMovingObject().getPositions().asArray().T, obj.getPositions().asArray().T) + similarities = self.lcss.similarityTable[-1, :-1].astype(float) + obj.prototypeSimilarities.append(similarities/np.minimum(np.arange(1.,len(similarities)+1), proto.getMovingObject().length()*np.ones(len(similarities)))) + for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities): + if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity: + initialPosition = obj.getPositionAtInstant(instant) + initialVelocity = obj.getVelocityAtInstant(instant) + predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings())) return predictedTrajectories if __name__ == "__main__":