Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 940:d8ab183a7351

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
verified motion prediction with prototypes at constant speed (test needed)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 18 Jul 2017 13:46:17 -0400
parents a2f3f3ca241e
children ab13aaf41432
comparison
equal deleted inserted replaced
939:a2f3f3ca241e 940:d8ab183a7351
69 1. at constant speed (the instantaneous user speed) 69 1. at constant speed (the instantaneous user speed)
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, prototype, constantSpeed = False, probability = 1.):
75 ''' prototypeTrajectory is a MovingObject''' 75 ''' prototype is a MovingObject
76 self.prototypeTrajectory = prototypeTrajectory 76
77 Prediction at constant speed will not work for unrealistic trajectories
78 that do not follow a slowly changing velocity (eg moving object trajectories,
79 but is good for realistic motion (eg features)'''
80 self.prototype = prototype
77 self.constantSpeed = constantSpeed 81 self.constantSpeed = constantSpeed
78 self.probability = probability 82 self.probability = probability
79 self.predictedPositions = {0: initialPosition} 83 self.predictedPositions = {0: initialPosition}
80 self.closestPointIdx = prototypeTrajectory.getPositions().getClosestPoint(initialPosition) 84 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
81 self.deltaPosition = initialPosition-prototypeTrajectory.getPositionAt(self.closestPointIdx) 85 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx)
82 self.initialSpeed = initialVelocity.norm2() 86 self.initialSpeed = initialVelocity.norm2()
83 #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} 87 #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
84 88
85 def predictPosition(self, nTimeSteps): 89 def predictPosition(self, nTimeSteps):
86 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 90 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
87 if self.constantSpeed: 91 if self.constantSpeed:
88 # calculate cumulative distance 92 # calculate cumulative distance
89 traj = self.prototypeTrajectory.getPositions() 93 traj = self.prototype.getPositions()
90 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) 94 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
91 i = self.closestPointIdx 95 i = self.closestPointIdx
92 while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance: 96 while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance:
93 i += 1 97 i += 1
94 if i == traj.length(): 98 if i == traj.length():
95 v = self.prototypeTrajectory.getVelocityAt(-1) 99 v = self.prototype.getVelocityAt(-1)
96 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) 100 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
97 else: 101 else:
98 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) 102 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1))
99 else: # see c++ code, calculate ratio 103 else: # see c++ code, calculate ratio
100 speedNorm= self.predictedSpeedOrientations[0].norm 104 speedNorm= self.predictedSpeedOrientations[0].norm
101 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0] 105 instant=findNearestParams(self.predictedPositions[0],self.prototype)[0]
102 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:] 106 prototypeSpeeds= self.prototype.getSpeeds()[instant:]
103 ratio=float(speedNorm)/prototypeSpeeds[0] 107 ratio=float(speedNorm)/prototypeSpeeds[0]
104 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] 108 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
105 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] 109 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototype)[1]
106 if nTimeSteps<len(resampledSpeeds): 110 if nTimeSteps<len(resampledSpeeds):
107 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype) 111 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype)
108 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) 112 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
109 else: 113 else:
110 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype) 114 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype)
277 for et1 in predictedTrajectories1: 281 for et1 in predictedTrajectories1:
278 for et2 in predictedTrajectories2: 282 for et2 in predictedTrajectories2:
279 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) 283 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
280 284
281 if collision: 285 if collision:
282 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) 286 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
283 elif computeCZ: # check if there is a crossing zone 287 elif computeCZ: # check if there is a crossing zone
284 # TODO? zone should be around the points at which the traj are the closest 288 # TODO? zone should be around the points at which the traj are the closest
285 # look for CZ at different times, otherwise it would be a collision 289 # look for CZ at different times, otherwise it would be a collision
286 # an approximation would be to look for close points at different times, ie the complementary of collision points 290 # an approximation would be to look for close points at different times, ie the complementary of collision points
287 cz = None 291 cz = None
514 crossingZones = [] 518 crossingZones = []
515 519
516 p1 = obj1.getPositionAtInstant(currentInstant) 520 p1 = obj1.getPositionAtInstant(currentInstant)
517 p2 = obj2.getPositionAtInstant(currentInstant) 521 p2 = obj2.getPositionAtInstant(currentInstant)
518 if (p1-p2).norm2() <= collisionDistanceThreshold: 522 if (p1-p2).norm2() <= collisionDistanceThreshold:
519 collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)] 523 collisionPoints = [SafetyPoint((p1+p1)*0.5, 1., 0.)]
520 else: 524 else:
521 v1 = obj1.getVelocityAtInstant(currentInstant) 525 v1 = obj1.getVelocityAtInstant(currentInstant)
522 v2 = obj2.getVelocityAtInstant(currentInstant) 526 v2 = obj2.getVelocityAtInstant(currentInstant)
523 intersection = moving.intersection(p1, p1+v1, p2, p2+v2) 527 intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
524 528