Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 939:a2f3f3ca241e
work in progress
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 17 Jul 2017 17:56:52 -0400 |
parents | b67a784beb69 |
children | d8ab183a7351 |
comparison
equal
deleted
inserted
replaced
938:fbf12382f3f8 | 939:a2f3f3ca241e |
---|---|
70 2. following the trajectory path, at the speed of the user | 70 2. following the trajectory path, at the speed of the user |
71 (applying a constant ratio equal | 71 (applying a constant ratio equal |
72 to the ratio of the user instantaneous speed and the trajectory closest speed)''' | 72 to the ratio of the user instantaneous speed and the trajectory closest speed)''' |
73 | 73 |
74 def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = False, probability = 1.): | 74 def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = False, probability = 1.): |
75 ''' prototypeTrajectory is a MovingObject''' | |
75 self.prototypeTrajectory = prototypeTrajectory | 76 self.prototypeTrajectory = prototypeTrajectory |
76 self.constantSpeed = constantSpeed | 77 self.constantSpeed = constantSpeed |
77 self.probability = probability | 78 self.probability = probability |
78 self.predictedPositions = {0: initialPosition} | 79 self.predictedPositions = {0: initialPosition} |
79 self.closestPointIdx = prototypeTrajectory.getClosestPoint(initialPosition) | 80 self.closestPointIdx = prototypeTrajectory.getPositions().getClosestPoint(initialPosition) |
80 self.deltaPosition = prototypeTrajectory[self.closestPointIdx]-initialPosition | 81 self.deltaPosition = initialPosition-prototypeTrajectory.getPositionAt(self.closestPointIdx) |
81 self.initialSpeed = initialVelocity.norm() | 82 self.initialSpeed = initialVelocity.norm2() |
82 #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} | 83 #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} |
83 | 84 |
84 def predictPosition(self, nTimeSteps): | 85 def predictPosition(self, nTimeSteps): |
85 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): | 86 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
86 if self.constantSpeed: | 87 if self.constantSpeed: |
87 # calculate cumulative distance | 88 # calculate cumulative distance |
88 pass | 89 traj = self.prototypeTrajectory.getPositions() |
90 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) | |
91 i = self.closestPointIdx | |
92 while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance: | |
93 i += 1 | |
94 if i == traj.length(): | |
95 v = self.prototypeTrajectory.getVelocityAt(-1) | |
96 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) | |
97 else: | |
98 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) | |
89 else: # see c++ code, calculate ratio | 99 else: # see c++ code, calculate ratio |
90 speedNorm= self.predictedSpeedOrientations[0].norm | 100 speedNorm= self.predictedSpeedOrientations[0].norm |
91 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0] | 101 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0] |
92 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:] | 102 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:] |
93 ratio=float(speedNorm)/prototypeSpeeds[0] | 103 ratio=float(speedNorm)/prototypeSpeeds[0] |