Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 939:a2f3f3ca241e

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 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]