Combined version of SplineSmoother and ConvexHullSmoother added

trajectorySmoothing/Spline_Convex.py

+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