Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 940:d8ab183a7351
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 |