diff --git a/trajectorySmoothing/Spline_Convex.py b/trajectorySmoothing/Spline_Convex.py new file mode 100644 index 0000000000000000000000000000000000000000..b6597a15918858aab706a7ebb21d20fe3cb3117a --- /dev/null +++ b/trajectorySmoothing/Spline_Convex.py @@ -0,0 +1,119 @@ +from ConvexHullSmoother import * +from SplineSmoother import * +import math + +class Spline_Convex: + + def __init__(self): + self._shortname = "Spline_Convex" + + def smooth(self, trajectory,hullfitter=SimpleHullFitter(), n=1, s=0.3, eps=0.1): + """ + smoothes a trajectory using the spline and the method of the convex hull. + It switches between this two methods by interpolation. + :param trajectory: trajectory to be smoothed + :param hullfitter: typ of hullfitter for the convex hull + :param n: number of frames that should be between the frames where the speed is determined + :param s: speed at which the methods should change + :param eps: area around the speed in which the methods should also not change. Prevents too much changes. + :return: trajectorynew: smoothed trajectory + """ + ssmoother = SplineSmoother() + hsmoother = ConvexHullSmoother(LimitedMinimumLengthHC(), hullfitter) + trajectoryspline = ssmoother.smooth(trajectory) + trajectorykonvex = hsmoother.smooth(trajectory) + trajectorynew = trajectoryspline + + i = 0 + speed_upper = s + eps/2 + speed_lower = s - eps/2 + + + while i < len(trajectory.frames): + interpolation = False + speed = trajectorykonvex.get_speed(trajectory.frames[i]) + print(speed) + if speed > speed_upper: + + if i != 0: + k = i + if(trajectorynew.x_values[i-1] == trajectorykonvex.x_values[i-1]) and (trajectorynew.y_values[i-1] == trajectorykonvex.y_values[i-1]): + + if k < len(trajectory.frames) - 10: + interpolation = True + for j in range(0, 11, 1): # interpolation + m = j/10 + trajectorynew.x_values[k] = (1 - m) * trajectorykonvex.x_values[k] + (m * trajectoryspline.x_values[k]) + trajectorynew.y_values[k] = (1 - m) * trajectorykonvex.y_values[k] + (m * trajectoryspline.y_values[k]) + k += 1 + + else: + if i < len(trajectory.frames) - n: + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, i, i + n) + + else: + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, i) + + elif speed < speed_lower: + + if i != 0: + k = i + if(trajectorynew.x_values[i-1] != trajectorykonvex.x_values[i-1]) and (trajectorynew.y_values[i-1] != trajectorykonvex.y_values[i-1]): + + if k < len(trajectory.frames)-10: + interpolation = True + for j in range(0, 11, 1): # interpolation + m = j/10 + trajectorynew.x_values[k] = (1-m)*trajectoryspline.x_values[k] + (m*trajectorykonvex.x_values[k]) + trajectorynew.y_values[k] = (1-m)*trajectoryspline.y_values[k] + (m*trajectorykonvex.y_values[k]) + k += 1 + + if n > 10: + if k < len(trajectory.frames)-(n-10): + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, k, k + (n-10)) + + else: + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, k) + + elif i < len(trajectory.frames) - n: + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, i, i + n) + else: + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, i) + else: + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, i, i + n) + + else: + if (trajectorynew.x_values[i - 1] == trajectorykonvex.x_values[i - 1]) and (trajectorynew.y_values[i - 1] == trajectorykonvex.y_values[i - 1]): + if i < len(trajectory.frames)-n: + trajectorynew=self.trajectory_setvalues(trajectorynew,trajectorykonvex,i,i+n) + else: + trajectorynew = self.trajectory_setvalues(trajectorynew, trajectorykonvex, i) + if not interpolation or (interpolation and n > 10): + i += n + else: + i +=11 + + return trajectorynew + + def trajectory_setvalues(self, trajectory1, trajectory2, start, end=None): + """ + sets the values of trajectory1 by using the values of trajectory2 from start to end. + :param trajectory1: trajectory, which values are set + :param trajectory2: trajectory, which values are used + :param start: beginning of the section where the values of trajectory1 are changed + :param end: ending of the section where the values of trajectory1 are changed + :return: trajectory1 with the new values + """ + + if end: + trajectory1.frames[start:end] = trajectory2.frames[start:end] + trajectory1.x_values[start:end] = trajectory2.x_values[start:end] + trajectory1.y_values[start:end] = trajectory2.y_values[start:end] + trajectory1.z_values[start:end] = trajectory2.z_values[start:end] + else: + trajectory1.frames[start:] = trajectory2.frames[start:] + trajectory1.x_values[start:] = trajectory2.x_values[start:] + trajectory1.y_values[start:] = trajectory2.y_values[start:] + trajectory1.z_values[start:] = trajectory2.z_values[start:] + + return trajectory1