Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 942:ab13aaf41432
implemented motion prediction with prototypes at constant ratio, with tests
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 18 Jul 2017 18:01:16 -0400 |
parents | d8ab183a7351 |
children | b1e8453c207c |
comparison
equal
deleted
inserted
replaced
941:c5191acb025f | 942:ab13aaf41432 |
---|---|
48 self.predictedPositions = {0: initialPosition} | 48 self.predictedPositions = {0: initialPosition} |
49 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} | 49 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
50 | 50 |
51 def getControl(self): | 51 def getControl(self): |
52 return self.control | 52 return self.control |
53 | |
54 def findNearestParams(initialPosition, prototypeTrajectory): | |
55 ''' nearest parameters are the index of minDistance and the orientation ''' | |
56 distances=[] | |
57 for position in prototypeTrajectory.positions: | |
58 distances.append(moving.Point.distanceNorm2(initialPosition, position)) | |
59 minDistanceIndex= np.argmin(distances) | |
60 return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle | |
61 | 53 |
62 class PredictedTrajectoryPrototype(PredictedTrajectory): | 54 class PredictedTrajectoryPrototype(PredictedTrajectory): |
63 '''Predicted trajectory that follows a prototype trajectory | 55 '''Predicted trajectory that follows a prototype trajectory |
64 The prototype is in the format of a moving.Trajectory: it could be | 56 The prototype is in the format of a moving.Trajectory: it could be |
65 1. an observed trajectory (extracted from video) | 57 1. an observed trajectory (extracted from video) |
82 self.probability = probability | 74 self.probability = probability |
83 self.predictedPositions = {0: initialPosition} | 75 self.predictedPositions = {0: initialPosition} |
84 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition) | 76 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition) |
85 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) | 77 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) |
86 self.initialSpeed = initialVelocity.norm2() | 78 self.initialSpeed = initialVelocity.norm2() |
87 #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[1])}#moving.NormAngle.fromPoint(initialVelocity)} | 79 if not constantSpeed: |
80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2() | |
88 | 81 |
89 def predictPosition(self, nTimeSteps): | 82 def predictPosition(self, nTimeSteps): |
90 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): | 83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
91 if self.constantSpeed: | 84 if self.constantSpeed: |
92 # calculate cumulative distance | 85 # calculate cumulative distance |
93 traj = self.prototype.getPositions() | 86 traj = self.prototype.getPositions() |
87 trajLength = traj.length() | |
94 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) | 88 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) |
95 i = self.closestPointIdx | 89 i = self.closestPointIdx |
96 while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance: | 90 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance: |
97 i += 1 | 91 i += 1 |
98 if i == traj.length(): | 92 if i == trajLength: |
99 v = self.prototype.getVelocityAt(-1) | 93 v = self.prototype.getVelocityAt(-1) |
100 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) | 94 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) |
101 else: | 95 else: |
102 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) | 96 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) |
103 else: # see c++ code, calculate ratio | 97 else: |
104 speedNorm= self.predictedSpeedOrientations[0].norm | 98 traj = self.prototype.getPositions() |
105 instant=findNearestParams(self.predictedPositions[0],self.prototype)[0] | 99 trajLength = traj.length() |
106 prototypeSpeeds= self.prototype.getSpeeds()[instant:] | 100 nSteps = self.ratio*nTimeSteps+self.closestPointIdx |
107 ratio=float(speedNorm)/prototypeSpeeds[0] | 101 i = int(np.floor(nSteps)) |
108 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] | 102 if nSteps < trajLength-1: |
109 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototype)[1] | 103 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i]+(traj[i+1]-traj[i])*(nSteps-i) |
110 if nTimeSteps<len(resampledSpeeds): | |
111 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype) | |
112 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) | |
113 else: | 104 else: |
114 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype) | 105 v = self.prototype.getVelocityAt(-1) |
115 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) | 106 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[-1]+v*(nSteps-trajLength+1) |
116 | |
117 return self.predictedPositions[nTimeSteps] | 107 return self.predictedPositions[nTimeSteps] |
118 | 108 |
119 class PredictedTrajectoryRandomControl(PredictedTrajectory): | 109 class PredictedTrajectoryRandomControl(PredictedTrajectory): |
120 '''Random vehicle control: suitable for normal adaptation''' | 110 '''Random vehicle control: suitable for normal adaptation''' |
121 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None): | 111 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None): |