nsaunier/traffic-intelligence: trafficintelligence/prediction.py comparison

comparison trafficintelligence/prediction.py @ 1212:af329f3330ba

work in progress on 3D safety analysis

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 02 May 2023 17:11:24 -0400
parents	a095d4fbb2ea
children	01c24c1cdb70

comparison

equal deleted inserted replaced

-:a095d4fbb2ea
+:af329f3330ba
 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
 self.predictPosition(nTimeSteps-1)
 self.predictedPositions[nTimeSteps] = self.predictedPositions[nTimeSteps-1]+self.initialVelocity
 return self.predictedPositions[nTimeSteps]
-class PredictedPolyTrajectoryConstant(PredictedTrajectory):
+class PredictedPolyTrajectoryConstantVelocity(PredictedTrajectory):
 '''Predicted trajectory of an object with an outline represented by a polygon
-Simple method that just translates the polygon corners'''
+Simple method that just translates the polygon corners (set of moving.Point)'''
 def __init__(self, polyCorners, initialVelocity, probability = 1.):
 self.probability = probability
-self.predictedPositions = {0: polyCorners}
+self.predictedPositions = {0: moving.pointsToShapely(polyCorners)}
 self.initialVelocity = initialVelocity
-self.predictedTrajectories = [PredictedTrajectoryConstant(p, initialVelocity) for p in polyCorners]
+self.predictedTrajectories = [PredictedTrajectoryConstantVelocity(p, initialVelocity) for p in polyCorners]
+def predictPosition(self, nTimeSteps):
+if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
+self.predictedPositions[nTimeSteps] = moving.pointsToShapely([t.predictPosition(nTimeSteps) for t in self.predictedTrajectories])
+return self.predictedPositions[nTimeSteps]
+def hasCollisionWith(self, other, t, collisionDistanceThreshold):
+poly1 = self.predictPosition(t)
+poly2 = other.predictPosition(t)
+return poly1.overlaps(poly2), poly1, poly2
 class PredictedTrajectoryPrototype(PredictedTrajectory):
 '''Predicted trajectory that follows a prototype trajectory
 The prototype is in the format of a moving.Trajectory: it could be
 1. an observed trajectory (extracted from video)
 2. a generic polyline (eg the road centerline) that a vehicle is supposed to follow
 while t < timeHorizon and not collision:
 t += 1
 collision, p1, p2 = predictedTrajectory1.hasCollisionWith(predictedTrajectory2, t, collisionDistanceThreshold)
 return collision, t, p1, p2
-def computeCollisionTimePolygon(predictedTrajectories1, predictedTrajectories2, timeHorizon):
-'''Computes the first instant
-at which two objects represented by a series of points (eg box)
-Computes all the times including timeHorizon
-User has to check the first variable collision to know about a collision'''
-t = 1
-poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1])
-poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2])
-collision = poly1.overlaps(poly2)
-while t < timeHorizon and not collision:
-t += 1
-poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1])
-poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2])
-collision = poly1.overlaps(poly2)
-return collision, t
 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True):
 from matplotlib.pyplot import figure, axis, title, clf, savefig
 if printFigure:
 clf()
 else:
 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
 return prototypeTrajectories
 class PredictionParameters(object):
-def __init__(self, name, maxSpeed, useCurvilinear = False):
+def __init__(self, name, maxSpeed = None, useCurvilinear = False):
 self.name = name
 self.maxSpeed = maxSpeed
 self.useCurvilinear = useCurvilinear
 def __str__(self):
 return '{0} {1}'.format(self.name, self.maxSpeed)
 def generatePredictedTrajectories(self, obj, instant):
 return None
+def computeCollisionPoint(self, p1, p2, probability, t):
+return SafetyPoint((p1+p2)*0.5, probability, t)
 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
 '''returns the lists of collision points and crossing zones'''
 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant)
 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant)
 crossingZones = None
 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))
+collisionPoints.append(self.computeCollisionPoint(p1, p2, et1.probability*et2.probability, t))
 elif computeCZ: # check if there is a crossing zone
 # 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
 PredictionParameters.__init__(self, 'constant velocity')
 def generatePredictedTrajectories(self, obj, instant):
 return [PredictedTrajectoryConstantVelocity(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant))]
+class ConstantPolyPredictionParameters(PredictionParameters):
+def __init__(self):
+PredictionParameters.__init__(self, 'constant velocity for polygon representation')
+def generatePredictedTrajectories(self, obj, instant):
+return [PredictedPolyTrajectoryConstantVelocity(obj.getBoundingPolygon(instant), obj.getVelocityAtInstant(instant))]
+def computeCollisionPoint(self, p1, p2, probability, t):
+polyRepr1 = p1.representative_point()
+polyRepr2 = p2.representative_point()
+p = moving.Point(polyRepr1.x+polyRepr2.x, polyRepr1.y+polyRepr2.y)
+return SafetyPoint(p, probability, t)
 class NormalAdaptationPredictionParameters(PredictionParameters):
 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
 '''An example of acceleration and steering distributions is
 lambda: random.triangular(-self.maxAcceleration, self.maxAcceleration, 0.)
 '''
 predictedTrajectories.append(PredictedTrajectoryConstantVelocity(initialPosition, initialVelocity, probability = probability))
 return predictedTrajectories
 else:
 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
 lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)'''
 moving.NormAngle(self.accelerationDistribution(),
 self.steeringDistribution()),
 probability,
 self.maxSpeed))
 return predictedTrajectories
 class CVDirectPredictionParameters(PredictionParameters):
 '''Prediction parameters of prediction at constant velocity
 using direct computation of the intersecting point
 Warning: the computed time to collision may be higher than timeHorizon (not used)'''

Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/prediction.py @ 1212:af329f3330ba