Mercurial Hosting > traffic-intelligence
changeset 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 | c5191acb025f |
| files | python/prediction.py python/tests/moving.txt python/utils.py |
| diffstat | 3 files changed, 21 insertions(+), 20 deletions(-) [+] |
line wrap: on
line diff
--- a/python/prediction.py Mon Jul 17 17:56:52 2017 -0400 +++ b/python/prediction.py Tue Jul 18 13:46:17 2017 -0400 @@ -71,38 +71,42 @@ (applying a constant ratio equal to the ratio of the user instantaneous speed and the trajectory closest speed)''' - def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = False, probability = 1.): - ''' prototypeTrajectory is a MovingObject''' - self.prototypeTrajectory = prototypeTrajectory + def __init__(self, initialPosition, initialVelocity, prototype, constantSpeed = False, probability = 1.): + ''' prototype is a MovingObject + + Prediction at constant speed will not work for unrealistic trajectories + that do not follow a slowly changing velocity (eg moving object trajectories, + but is good for realistic motion (eg features)''' + self.prototype = prototype self.constantSpeed = constantSpeed self.probability = probability self.predictedPositions = {0: initialPosition} - self.closestPointIdx = prototypeTrajectory.getPositions().getClosestPoint(initialPosition) - self.deltaPosition = initialPosition-prototypeTrajectory.getPositionAt(self.closestPointIdx) + self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition) + self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) self.initialSpeed = initialVelocity.norm2() - #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} + #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[1])}#moving.NormAngle.fromPoint(initialVelocity)} def predictPosition(self, nTimeSteps): if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): if self.constantSpeed: # calculate cumulative distance - traj = self.prototypeTrajectory.getPositions() + traj = self.prototype.getPositions() traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) i = self.closestPointIdx while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance: i += 1 if i == traj.length(): - v = self.prototypeTrajectory.getVelocityAt(-1) + v = self.prototype.getVelocityAt(-1) self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2()) else: self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1)) else: # see c++ code, calculate ratio speedNorm= self.predictedSpeedOrientations[0].norm - instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0] - prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:] + instant=findNearestParams(self.predictedPositions[0],self.prototype)[0] + prototypeSpeeds= self.prototype.getSpeeds()[instant:] ratio=float(speedNorm)/prototypeSpeeds[0] resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] - anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] + anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototype)[1] if nTimeSteps<len(resampledSpeeds): self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype) self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) @@ -279,7 +283,7 @@ collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) if collision: - collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) + collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) elif computeCZ: # check if there is a crossing zone # TODO? zone should be around the points at which the traj are the closest # look for CZ at different times, otherwise it would be a collision @@ -516,7 +520,7 @@ p1 = obj1.getPositionAtInstant(currentInstant) p2 = obj2.getPositionAtInstant(currentInstant) if (p1-p2).norm2() <= collisionDistanceThreshold: - collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)] + collisionPoints = [SafetyPoint((p1+p1)*0.5, 1., 0.)] else: v1 = obj1.getVelocityAtInstant(currentInstant) v2 = obj2.getVelocityAtInstant(currentInstant)
--- a/python/tests/moving.txt Mon Jul 17 17:56:52 2017 -0400 +++ b/python/tests/moving.txt Tue Jul 18 13:46:17 2017 -0400 @@ -55,7 +55,7 @@ (2.000000,-3.000000) >>> -Point(1,2) (-1.000000,-2.000000) ->>> Point(1,2).multiply(0.5) +>>> Point(1,2)*0.5 (0.500000,1.000000) >>> Point(3,2).norm2Squared()
--- a/python/utils.py Mon Jul 17 17:56:52 2017 -0400 +++ b/python/utils.py Tue Jul 18 13:46:17 2017 -0400 @@ -836,14 +836,11 @@ # plotting section ######################### -def plotPolygon(poly, options = ''): +def plotPolygon(poly, options = '', **kwargs): 'Plots shapely polygon poly' - from numpy.core.multiarray import array from matplotlib.pyplot import plot - from shapely.geometry import Polygon - - tmp = array(poly.exterior) - plot(tmp[:,0], tmp[:,1], options) + x,y = poly.exterior.xy + plot(x, y, options, **kwargs) def stepPlot(X, firstX, lastX, initialCount = 0, increment = 1): '''for each value in X, increment by increment the initial count