Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/prediction.py @ 1211:a095d4fbb2ea

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
work in progress
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 02 May 2023 07:12:26 -0400
parents 1bad5f9b60de
children af329f3330ba
comparison
equal deleted inserted replaced
1210:1bad5f9b60de 1211:a095d4fbb2ea
32 return moving.Trajectory.fromPointList(list(self.predictedPositions.values())) 32 return moving.Trajectory.fromPointList(list(self.predictedPositions.values()))
33 33
34 def getPredictedSpeeds(self): 34 def getPredictedSpeeds(self):
35 return [so.norm for so in self.predictedSpeedOrientations.values()] 35 return [so.norm for so in self.predictedSpeedOrientations.values()]
36 36
37 def hasCollisionWith(self, other, t, collisionDistanceThreshold):
38 p1 = self.predictPosition(t)
39 p2 = other.predictPosition(t)
40 return (p1-p2).norm2() <= collisionDistanceThreshold, p1, p2
41
37 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs): 42 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
38 self.getPredictedTrajectory().plot(options, withOrigin, timeStep, **kwargs) 43 self.getPredictedTrajectory().plot(options, withOrigin, timeStep, **kwargs)
39 44
40 class PredictedTrajectoryConstant(PredictedTrajectory): 45 class PredictedTrajectoryConstant(PredictedTrajectory):
41 '''Predicted trajectory at constant speed or acceleration 46 '''Predicted trajectory at constant speed or acceleration
45 self.control = control 50 self.control = control
46 self.maxSpeed = maxSpeed 51 self.maxSpeed = maxSpeed
47 self.probability = probability 52 self.probability = probability
48 self.predictedPositions = {0: initialPosition} 53 self.predictedPositions = {0: initialPosition}
49 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} 54 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
50 55
51 def getControl(self): 56 def getControl(self):
52 return self.control 57 return self.control
53 58
59 class PredictedTrajectoryConstantVelocity(PredictedTrajectory):
60 '''Predicted trajectory at constant speed or acceleration
61 TODO generalize by passing a series of velocities/accelerations'''
62
63 def __init__(self, initialPosition, initialVelocity, probability = 1.):
64 self.probability = probability
65 self.predictedPositions = {0: initialPosition}
66 self.initialVelocity = initialVelocity
67 #self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
68
69 def predictPosition(self, nTimeSteps):
70 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
71 self.predictPosition(nTimeSteps-1)
72 self.predictedPositions[nTimeSteps] = self.predictedPositions[nTimeSteps-1]+self.initialVelocity
73 return self.predictedPositions[nTimeSteps]
74
75 class PredictedPolyTrajectoryConstant(PredictedTrajectory):
76 '''Predicted trajectory of an object with an outline represented by a polygon
77 Simple method that just translates the polygon corners'''
78 def __init__(self, polyCorners, initialVelocity, probability = 1.):
79 self.probability = probability
80 self.predictedPositions = {0: polyCorners}
81 self.initialVelocity = initialVelocity
82 self.predictedTrajectories = [PredictedTrajectoryConstant(p, initialVelocity) for p in polyCorners]
83
54 class PredictedTrajectoryPrototype(PredictedTrajectory): 84 class PredictedTrajectoryPrototype(PredictedTrajectory):
55 '''Predicted trajectory that follows a prototype trajectory 85 '''Predicted trajectory that follows a prototype trajectory
56 The prototype is in the format of a moving.Trajectory: it could be 86 The prototype is in the format of a moving.Trajectory: it could be
57 1. an observed trajectory (extracted from video) 87 1. an observed trajectory (extracted from video)
58 2. a generic polyline (eg the road centerline) that a vehicle is supposed to follow 88 2. a generic polyline (eg the road centerline) that a vehicle is supposed to follow
153 def computeExpectedIndicator(points): 183 def computeExpectedIndicator(points):
154 return np.sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) 184 return np.sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
155 185
156 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): 186 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
157 '''Computes the first instant 187 '''Computes the first instant
158 at which two predicted trajectories are within some distance threshold 188 at which two predicted trajectories are colliding (depending on the trajectory format)
159 Computes all the times including timeHorizon 189 Computes all the times including timeHorizon
160 190
161 User has to check the first variable collision to know about a collision''' 191 User has to check the first variable collision to know about a collision'''
162 t = 1 192 t = 1
163 p1 = predictedTrajectory1.predictPosition(t) 193 collision, p1, p2 = predictedTrajectory1.hasCollisionWith(predictedTrajectory2, t, collisionDistanceThreshold)
164 p2 = predictedTrajectory2.predictPosition(t)
165 collision = (p1-p2).norm2() <= collisionDistanceThreshold
166 while t < timeHorizon and not collision: 194 while t < timeHorizon and not collision:
167 t += 1 195 t += 1
168 p1 = predictedTrajectory1.predictPosition(t) 196 collision, p1, p2 = predictedTrajectory1.hasCollisionWith(predictedTrajectory2, t, collisionDistanceThreshold)
169 p2 = predictedTrajectory2.predictPosition(t)
170 collision = (p1-p2).norm2() <= collisionDistanceThreshold
171 return collision, t, p1, p2 197 return collision, t, p1, p2
172 198
173 def computeCollisionTimePolygon(predictedTrajectories1, predictedTrajectories2, timeHorizon): 199 def computeCollisionTimePolygon(predictedTrajectories1, predictedTrajectories2, timeHorizon):
174 '''Computes the first instant 200 '''Computes the first instant
175 at which two objects represented by a series of points (eg box) 201 at which two objects represented by a series of points (eg box)
183 while t < timeHorizon and not collision: 209 while t < timeHorizon and not collision:
184 t += 1 210 t += 1
185 poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1]) 211 poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1])
186 poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2]) 212 poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2])
187 collision = poly1.overlaps(poly2) 213 collision = poly1.overlaps(poly2)
188 return collision, t, p1, p2 214 return collision, t
189 215
190 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True): 216 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True):
191 from matplotlib.pyplot import figure, axis, title, clf, savefig 217 from matplotlib.pyplot import figure, axis, title, clf, savefig
192 if printFigure: 218 if printFigure:
193 clf() 219 clf()
378 savePredictedTrajectoriesFigure(i, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) 404 savePredictedTrajectoriesFigure(i, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
379 405
380 return collisionProbabilities 406 return collisionProbabilities
381 407
382 class ConstantPredictionParameters(PredictionParameters): 408 class ConstantPredictionParameters(PredictionParameters):
383 def __init__(self, maxSpeed): 409 def __init__(self):
384 PredictionParameters.__init__(self, 'constant velocity', maxSpeed) 410 PredictionParameters.__init__(self, 'constant velocity')
385 411
386 def generatePredictedTrajectories(self, obj, instant): 412 def generatePredictedTrajectories(self, obj, instant):
387 return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), maxSpeed = self.maxSpeed)] 413 return [PredictedTrajectoryConstantVelocity(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant))]
388 414
389 class NormalAdaptationPredictionParameters(PredictionParameters): 415 class NormalAdaptationPredictionParameters(PredictionParameters):
390 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False): 416 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
391 '''An example of acceleration and steering distributions is 417 '''An example of acceleration and steering distributions is
392 lambda: random.triangular(-self.maxAcceleration, self.maxAcceleration, 0.) 418 lambda: random.triangular(-self.maxAcceleration, self.maxAcceleration, 0.)
436 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)] 462 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
437 positions = [f.getPositionAtInstant(instant) for f in features] 463 positions = [f.getPositionAtInstant(instant) for f in features]
438 velocities = [f.getVelocityAtInstant(instant) for f in features] 464 velocities = [f.getVelocityAtInstant(instant) for f in features]
439 probability = 1./float(len(positions)) 465 probability = 1./float(len(positions))
440 for initialPosition,initialVelocity in zip(positions, velocities): 466 for initialPosition,initialVelocity in zip(positions, velocities):
441 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, probability = probability, maxSpeed = self.maxSpeed)) 467 predictedTrajectories.append(PredictedTrajectoryConstantVelocity(initialPosition, initialVelocity, probability = probability))
442 return predictedTrajectories 468 return predictedTrajectories
443 else: 469 else:
444 print('Object {} has no features'.format(obj.getNum())) 470 print('Object {} has no features'.format(obj.getNum()))
445 return None 471 return None
446 472