Mercurial Hosting > traffic-intelligence
changeset 937:b67a784beb69
work started on prototype prediction
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 17 Jul 2017 01:38:06 -0400 |
parents | 56cc8a1f7082 |
children | fbf12382f3f8 |
files | python/events.py python/moving.py python/prediction.py |
diffstat | 3 files changed, 27 insertions(+), 8 deletions(-) [+] |
line wrap: on
line diff
--- a/python/events.py Fri Jul 14 16:48:42 2017 -0400 +++ b/python/events.py Mon Jul 17 01:38:06 2017 -0400 @@ -219,6 +219,7 @@ def computeCrossingsCollisions(self, predictionParameters, 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. ''' TTCs = {} + collisionProbabilities = {} if usePrototypes: route1= getRoute(self.roadUser1,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination) route2= getRoute(self.roadUser2,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination) @@ -228,10 +229,12 @@ else: commonTimeInterval = self.timeInterval self.collisionPoints, crossingZones = predictionParameters.computeCrossingsCollisions(self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step) - for i, cp in self.collisionPoints.iteritems(): - TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cp) + for i, cps in self.collisionPoints.iteritems(): + TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cps) + collisionProbabilities[i] = sum([p.probability for p in cps]) if len(TTCs) > 0: self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs, mostSevereIsMax=False)) + self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[6], collisionProbabilities)) # crossing zones and pPET if computeCZ:
--- a/python/moving.py Fri Jul 14 16:48:42 2017 -0400 +++ b/python/moving.py Mon Jul 17 01:38:06 2017 -0400 @@ -865,6 +865,23 @@ positions = self.getPositions().asArray().T return cdist(positions, positions, metric = metric).max() + def getClosestPoint(self, p1, maxDist2 = None): + '''Returns the instant of the closest position in trajectory to p1 (and the point) + if maxDist is not None, will check the distance is smaller + TODO: could use cdist for different metrics''' + distances2 = [] + minDist2 = float('inf') + i = -1 + for p2 in self: + distances2.append(Point.distanceNorm2(p1, p2)) + if distances2[-1] < minDist2: + minDist2 = distances2[-1] + i = len(distances)-1 + if maxDist2 is None or (maxDist2 is not None and minDist2 < maxDist2): + return i + else: + return None + def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) have a cosine with refDirection is smaller than cosineThreshold'''
--- a/python/prediction.py Fri Jul 14 16:48:42 2017 -0400 +++ b/python/prediction.py Mon Jul 17 01:38:06 2017 -0400 @@ -76,17 +76,16 @@ self.constantSpeed = constantSpeed self.probability = probability self.predictedPositions = {0: initialPosition} - self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} + self.closestPointIdx = prototypeTrajectory.getClosestPoint(initialPosition) + self.deltaPosition = prototypeTrajectory[self.closestPointIdx]-initialPosition + self.initialSpeed = initialVelocity.norm() + #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} def predictPosition(self, nTimeSteps): if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): if self.constantSpeed: # calculate cumulative distance - speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm - anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] - self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype) - self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) - + pass else: # see c++ code, calculate ratio speedNorm= self.predictedSpeedOrientations[0].norm instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]