Mercurial Hosting > traffic-intelligence
changeset 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 |
files | python/prediction.py scripts/safety-analysis.py |
diffstat | 2 files changed, 21 insertions(+), 58 deletions(-) [+] |
line wrap: on
line diff
--- a/python/prediction.py Wed Jul 19 18:02:38 2017 -0400 +++ b/python/prediction.py Thu Jul 20 12:12:28 2017 -0400 @@ -159,22 +159,26 @@ collision = (p1-p2).norm2() <= collisionDistanceThreshold return collision, t, p1, p2 -def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): +def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True): from matplotlib.pyplot import figure, axis, title, clf, savefig - figure() - #clf() + if printFigure: + clf() + else: + figure() for et in predictedTrajectories1: - et.predictPosition(int(np.round(timeHorizon))) - et.plot('rx') - + for t in xrange(int(np.round(timeHorizon))): + et.predictPosition(t) + et.plot('rx') for et in predictedTrajectories2: - et.predictPosition(int(np.round(timeHorizon))) - et.plot('bx') + for t in xrange(int(np.round(timeHorizon))): + et.predictPosition(t) + et.plot('bx') 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)) + if printFigure: + savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) def calculateProbability(nMatching,similarity,objects): sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) @@ -254,16 +258,10 @@ 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): '''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) + predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) collisionPoints = [] crossingZones = [] @@ -273,16 +271,12 @@ if collision: collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) elif computeCZ: # check if there is a crossing zone - # TODO? zone should be around the points at which the traj are the closest - # look for CZ at different times, otherwise it would be a collision - # an approximation would be to look for close points at different times, ie the complementary of collision points + # TODO same computation as PET with metric + concatenate past trajectory with future trajectory cz = None t1 = 0 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 t2 = 0 while not cz and t2 < timeHorizon: - #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: - # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) if cz is not None: deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() @@ -307,11 +301,7 @@ 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 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): + def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): '''Computes all crossing and collision points at each common instant for two road users. ''' collisionPoints={} crossingZones={} @@ -320,46 +310,23 @@ 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 = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) 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)) 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() + jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] for j in jobs: i, cp, cz = j.get() - #if len(cp) != 0 or len(cz) != 0: if len(cp) != 0: collisionPoints[i] = cp if len(cz) != 0: crossingZones[i] = cz pool.close() return collisionPoints, crossingZones -#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) def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): '''Computes only collision probabilities @@ -437,7 +404,6 @@ class PointSetPredictionParameters(PredictionParameters): def __init__(self, maxSpeed): PredictionParameters.__init__(self, 'point set', maxSpeed) - #self.nPredictedTrajectories = nPredictedTrajectories def generatePredictedTrajectories(self, obj, instant): predictedTrajectories = [] @@ -519,8 +485,6 @@ dp2 = intersection-p2 dot1 = moving.Point.dot(dp1, v1) dot2 = moving.Point.dot(dp2, v2) - #print dot1, dot2 - # (computeCZ and (dot1 > 0 or dot2 > 0)) or ( 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 dist1 = dp1.norm2() dist2 = dp2.norm2() @@ -579,12 +543,11 @@ return currentInstant, collisionPoints, crossingZones class PrototypePredictionParameters(PredictionParameters): - def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): # lcss parameters + def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): 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
--- a/scripts/safety-analysis.py Wed Jul 19 18:02:38 2017 -0400 +++ b/scripts/safety-analysis.py Thu Jul 20 12:12:28 2017 -0400 @@ -55,7 +55,7 @@ prototypes = storage.loadPrototypesFromSqlite(args.prototypeDatabaseFilename) for p in prototypes: p.getMovingObject().computeCumulativeDistances() - predictionParameters = prediction.PrototypePredictionParameters(prototypes, params.nPredictedTrajectories, 2., 0.5, 'cityblock', 10, params.constantSpeedPrototypePrediction, params.useFeaturesForPrediction) + predictionParameters = prediction.PrototypePredictionParameters(prototypes, params.nPredictedTrajectories, 2., 0.4, 'cityblock', 10, params.constantSpeedPrototypePrediction, params.useFeaturesForPrediction) # else: # no else required, since parameters is required as argument