From 183c85493c72bf44e19886e946daa2168ba61c60 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Wed, 16 Oct 2024 10:41:58 -0400
Subject: [PATCH 01/21] Bump version number for release.

---
 pyproject.toml              |   2 +-
 spatialmath/base/animate.py |  10 +--
 spatialmath/spline.py       | 160 +++++++++++++++++++++++++++++++++++-
 3 files changed, 163 insertions(+), 9 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index 97674f19..3c49f713 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -1,6 +1,6 @@
 [project]
 name = "spatialmath-python"
-version = "1.1.11"
+version = "1.1.12"
 authors = [
   { name="Peter Corke", email="rvc@petercorke.com" },
 ]
diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py
index 7654a5a0..337b223f 100755
--- a/spatialmath/base/animate.py
+++ b/spatialmath/base/animate.py
@@ -320,7 +320,7 @@ def __init__(self, anim: Animate, h, xs, ys, zs):
             self.anim = anim
 
         def draw(self, T):
-            p = T @ self.p
+            p = T * self.p
             self.h.set_data(p[0, :], p[1, :])
             self.h.set_3d_properties(p[2, :])
 
@@ -378,7 +378,7 @@ def __init__(self, anim, h):
 
         def draw(self, T):
             # import ipdb; ipdb.set_trace()
-            p = T @ self.p
+            p = T * self.p
 
             # reshape it
             p = p[0:3, :].T.reshape(3, 2, 3)
@@ -432,7 +432,7 @@ def __init__(self, anim, h, x, y, z):
             self.anim = anim
 
         def draw(self, T):
-            p = T @ self.p
+            p = T * self.p
             # x2, y2, _ = proj3d.proj_transform(
             #   p[0], p[1], p[2], self.anim.ax.get_proj())
             # self.h.set_position((x2, y2))
@@ -759,7 +759,7 @@ def __init__(self, anim, h, xs, ys):
             self.anim = anim
 
         def draw(self, T):
-            p = T @ self.p
+            p = T * self.p
             self.h.set_data(p[0, :], p[1, :])
 
     def plot(self, x, y, *args, **kwargs):
@@ -837,7 +837,7 @@ def __init__(self, anim, h, x, y):
             self.anim = anim
 
         def draw(self, T):
-            p = T @ self.p
+            p = T * self.p
             # x2, y2, _ = proj3d.proj_transform(
             #   p[0], p[1], p[2], self.anim.ax.get_proj())
             # self.h.set_position((x2, y2))
diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index f81dcec3..42bf903b 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -2,9 +2,7 @@
 # MIT Licence, see details in top-level file: LICENCE
 
 """
-Classes for parameterizing a trajectory in SE3 with B-splines. 
-
-Copies parts of the API from scipy's B-spline class.
+Classes for parameterizing a trajectory in SE3 with splines. 
 """
 
 from typing import Any, Dict, List, Optional
@@ -14,6 +12,162 @@
 import matplotlib.pyplot as plt
 from spatialmath.base.transforms3d import tranimate, trplot
 
+from typing import Any, List
+
+import matplotlib.pyplot as plt
+import numpy as np
+import numpy.typing as npt
+from scipy.interpolate import CubicSpline
+from scipy.spatial.transform import Rotation, RotationSpline
+from spatialmath import SE3, SO3, Twist3
+from spatialmath.base.transforms3d import tranimate
+
+
+class InterpSplineSE3:
+    """Class for an interpolated trajectory in SE3 through waypoints with a cubic spline.
+
+    A combination of scipy.interpolate.CubicSpline and scipy.spatial.transform.RotationSpline (itself also cubic)
+    under the hood.
+    """
+
+    def __init__(
+        self,
+        timestamps: list[float] | npt.NDArray,
+        waypoints: list[SE3],
+        *,
+        normalize_time: bool = True,
+        bc_type: str | tuple = "not-a-knot",  # not-a-knot is scipy default; None is invalid
+    ) -> None:
+        """Construct a InterpSplineSE3 object
+
+        Extends the scipy CubicSpline object
+        https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.CubicSpline.html#cubicspline
+
+        Args :
+            timestamps : list of times corresponding to provided poses
+            waypoints : list of SE3 objects that govern the shape of the spline.
+            normalize_time : flag to map times into the range [0, 1]
+            bc_type : boundary condition provided to scipy CubicSpline backend.
+                      string options: ["not-a-knot" (default), "clamped", "natural", "periodic"].
+                      For tuple options and details see the scipy docs link above.
+        """
+
+        self.waypoints = waypoints
+        self.timestamps = np.array(timestamps)
+
+        if normalize_time:
+            self.timestamps = self.timestamps - self.timestamps[0]
+            self.timestamps = self.timestamps / self.timestamps[-1]
+
+        self.xyz_data = np.array([pose.t for pose in waypoints])
+        self.so3_data = Rotation.from_matrix(np.array([(pose.R) for pose in waypoints]))
+
+        self.spline_xyz = CubicSpline(self.timestamps, self.xyz_data, bc_type=bc_type)
+        self.spline_so3 = RotationSpline(self.timestamps, self.so3_data)
+
+        self.interpolation_indices = list(range(len(waypoints)))
+
+    def __call__(self, t: float) -> Any:
+
+        return SE3.Rt(t=self.spline_xyz(t), R=self.spline_so3(t).as_matrix())
+
+    def derivative(self, t: float) -> Twist3:
+        linear_vel = self.spline_xyz.derivative()(t)
+        angular_vel = self.spline_so3(t, 1)
+        return Twist3(linear_vel, angular_vel)
+
+    def max_angular_error(self) -> float:
+        return np.max(self.angular_errors())
+
+    def angular_errors(self) -> list[float]:
+        return [
+            SO3(pose).angdist(SO3(self.spline_so3(timestamp).as_matrix()))
+            for pose, timestamp in zip(self.waypoints, self.timestamps, strict=True)
+        ]
+
+    def max_euclidean_error(self) -> float:
+        return np.max(self.euclidean_errors())
+
+    def euclidean_errors(self) -> List[float]:
+        return [
+            np.linalg.norm(pose.t - self.spline_xyz(timestamp))
+            for pose, timestamp in zip(self.waypoints, self.timestamps, strict=True)
+        ]
+
+    def downsample(self, epsilon_xyz: float = 1e-3, epsilon_angle: float = 1e-1) -> int:
+        chosen_indices: set[int] = set()
+        interpolation_indices = self.interpolation_indices.copy()
+
+        for _ in range(len(self.timestamps) - 2):  # you must have at least 2 indices
+            choices = list(set(interpolation_indices).difference(chosen_indices))
+
+            index = np.random.choice(choices)
+
+            chosen_indices.add(index)
+            interpolation_indices.remove(index)
+
+            self.spline_xyz = CubicSpline(self.timestamps[interpolation_indices], self.xyz_data[interpolation_indices])
+            self.spline_so3 = RotationSpline(
+                self.timestamps[interpolation_indices], self.so3_data[interpolation_indices]
+            )
+
+            time = self.timestamps[index]
+            angular_error = SO3(self.waypoints[index]).angdist(SO3(self.spline_so3(time).as_matrix()))
+            euclidean_error = np.linalg.norm(self.waypoints[index].t - self.spline_xyz(time))
+            if angular_error > epsilon_angle or euclidean_error > epsilon_xyz:
+                interpolation_indices.insert(int(np.searchsorted(interpolation_indices, index, side="right")), index)
+
+        self.interpolation_indices = interpolation_indices
+        return len(self.waypoints) - len(interpolation_indices)
+
+    def visualize(
+        self,
+        num_samples: int,
+        pose_marker_length: float = 0.2,
+        animate: bool = False,
+        ax: plt.Axes | None = None,
+    ) -> None:
+        """Displays an animation of the trajectory with the control poses."""
+        if ax is None:
+            fig = plt.figure(figsize=(10, 10))
+            ax = fig.add_subplot(projection="3d")
+
+        samples = [self(t) for t in np.linspace(0, 1, num_samples)]
+        if not animate:
+            x = [pose.x for pose in samples]
+            y = [pose.y for pose in samples]
+            z = [pose.z for pose in samples]
+            ax.plot(x, y, z, "c", linewidth=1.0)  # plot spline fit
+
+        x = [pose.x for pose in self.waypoints]
+        y = [pose.y for pose in self.waypoints]
+        z = [pose.z for pose in self.waypoints]
+        ax.plot(x, y, z, "r*")  # plot source data
+
+        x = [self.waypoints[i].x for i in self.interpolation_indices]
+        y = [self.waypoints[i].y for i in self.interpolation_indices]
+        z = [self.waypoints[i].z for i in self.interpolation_indices]
+        ax.plot(x, y, z, "go", fillstyle="none")  # plot interpolation indices
+
+        if animate:
+            tranimate(samples, repeat=True, length=pose_marker_length, wait=True)  # animate pose along trajectory
+        else:
+            plt.show()
+
+    def to_numpy(self) -> dict[str, npt.NDArray]:
+        """Export spline parameters as dictionary of numpy arrays."""
+        return {"timestamps": self.timestamps, "twists": np.vstack([1.0 * pose.twist().A for pose in self.waypoints])}
+
+    def from_numpy(self, data: dict[str, npt.NDArray]) -> None:
+        """Reconstruct spline from 'to_numpy' parameters."""
+        self.timestamps = data["timestamps"]
+        self.waypoints = [SE3.Exp(twist) for twist in data["twists"]]
+
+
+class SplineFit:
+
+    pass
+
 
 class BSplineSE3:
     """A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.

From af175d37f6725721110d7a185b801955426db3bd Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 09:45:27 -0400
Subject: [PATCH 02/21] Towards a clean interpolating spline.

---
 spatialmath/spline.py | 202 ++++++++++++++++++++++++------------------
 1 file changed, 118 insertions(+), 84 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 42bf903b..9260b1ae 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -12,7 +12,7 @@
 import matplotlib.pyplot as plt
 from spatialmath.base.transforms3d import tranimate, trplot
 
-from typing import Any, List
+from typing import Any, List, Tuple
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -24,7 +24,7 @@
 
 
 class InterpSplineSE3:
-    """Class for an interpolated trajectory in SE3 through waypoints with a cubic spline.
+    """Class for an interpolated trajectory in SE3, as a function of time, through waypoints with a cubic spline.
 
     A combination of scipy.interpolate.CubicSpline and scipy.spatial.transform.RotationSpline (itself also cubic)
     under the hood.
@@ -32,7 +32,7 @@ class InterpSplineSE3:
 
     def __init__(
         self,
-        timestamps: list[float] | npt.NDArray,
+        timepoints: list[float] | npt.NDArray,
         waypoints: list[SE3],
         *,
         normalize_time: bool = True,
@@ -44,7 +44,7 @@ def __init__(
         https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.CubicSpline.html#cubicspline
 
         Args :
-            timestamps : list of times corresponding to provided poses
+            timepoints : list of times corresponding to provided poses
             waypoints : list of SE3 objects that govern the shape of the spline.
             normalize_time : flag to map times into the range [0, 1]
             bc_type : boundary condition provided to scipy CubicSpline backend.
@@ -53,19 +53,18 @@ def __init__(
         """
 
         self.waypoints = waypoints
-        self.timestamps = np.array(timestamps)
+        self.timepoints = np.array(timepoints)
 
         if normalize_time:
-            self.timestamps = self.timestamps - self.timestamps[0]
-            self.timestamps = self.timestamps / self.timestamps[-1]
+            self.timepoints = self.timepoints - self.timepoints[0]
+            self.timepoints = self.timepoints / self.timepoints[-1]
 
-        self.xyz_data = np.array([pose.t for pose in waypoints])
-        self.so3_data = Rotation.from_matrix(np.array([(pose.R) for pose in waypoints]))
-
-        self.spline_xyz = CubicSpline(self.timestamps, self.xyz_data, bc_type=bc_type)
-        self.spline_so3 = RotationSpline(self.timestamps, self.so3_data)
-
-        self.interpolation_indices = list(range(len(waypoints)))
+        self.spline_xyz = CubicSpline(
+            self.timepoints, 
+            np.array([pose.t for pose in self.waypoints]), 
+            bc_type=bc_type
+        )
+        self.spline_so3 = RotationSpline(self.timepoints, self.so3_data)
 
     def __call__(self, t: float) -> Any:
 
@@ -76,56 +75,13 @@ def derivative(self, t: float) -> Twist3:
         angular_vel = self.spline_so3(t, 1)
         return Twist3(linear_vel, angular_vel)
 
-    def max_angular_error(self) -> float:
-        return np.max(self.angular_errors())
-
-    def angular_errors(self) -> list[float]:
-        return [
-            SO3(pose).angdist(SO3(self.spline_so3(timestamp).as_matrix()))
-            for pose, timestamp in zip(self.waypoints, self.timestamps, strict=True)
-        ]
-
-    def max_euclidean_error(self) -> float:
-        return np.max(self.euclidean_errors())
-
-    def euclidean_errors(self) -> List[float]:
-        return [
-            np.linalg.norm(pose.t - self.spline_xyz(timestamp))
-            for pose, timestamp in zip(self.waypoints, self.timestamps, strict=True)
-        ]
-
-    def downsample(self, epsilon_xyz: float = 1e-3, epsilon_angle: float = 1e-1) -> int:
-        chosen_indices: set[int] = set()
-        interpolation_indices = self.interpolation_indices.copy()
-
-        for _ in range(len(self.timestamps) - 2):  # you must have at least 2 indices
-            choices = list(set(interpolation_indices).difference(chosen_indices))
-
-            index = np.random.choice(choices)
-
-            chosen_indices.add(index)
-            interpolation_indices.remove(index)
-
-            self.spline_xyz = CubicSpline(self.timestamps[interpolation_indices], self.xyz_data[interpolation_indices])
-            self.spline_so3 = RotationSpline(
-                self.timestamps[interpolation_indices], self.so3_data[interpolation_indices]
-            )
-
-            time = self.timestamps[index]
-            angular_error = SO3(self.waypoints[index]).angdist(SO3(self.spline_so3(time).as_matrix()))
-            euclidean_error = np.linalg.norm(self.waypoints[index].t - self.spline_xyz(time))
-            if angular_error > epsilon_angle or euclidean_error > epsilon_xyz:
-                interpolation_indices.insert(int(np.searchsorted(interpolation_indices, index, side="right")), index)
-
-        self.interpolation_indices = interpolation_indices
-        return len(self.waypoints) - len(interpolation_indices)
-
     def visualize(
         self,
         num_samples: int,
         pose_marker_length: float = 0.2,
         animate: bool = False,
         ax: plt.Axes | None = None,
+        input_poses: List[SE3] | None = None
     ) -> None:
         """Displays an animation of the trajectory with the control poses."""
         if ax is None:
@@ -142,12 +98,13 @@ def visualize(
         x = [pose.x for pose in self.waypoints]
         y = [pose.y for pose in self.waypoints]
         z = [pose.z for pose in self.waypoints]
-        ax.plot(x, y, z, "r*")  # plot source data
+        ax.plot(x, y, z, "r*")  # plot waypoints
 
-        x = [self.waypoints[i].x for i in self.interpolation_indices]
-        y = [self.waypoints[i].y for i in self.interpolation_indices]
-        z = [self.waypoints[i].z for i in self.interpolation_indices]
-        ax.plot(x, y, z, "go", fillstyle="none")  # plot interpolation indices
+        if input_poses is not None:
+            x = [pose.x for pose in input_poses]
+            y = [pose.y for pose in input_poses]
+            z = [pose.z for pose in input_poses]
+            ax.plot(x, y, z, "go", fillstyle="none")  # plot compare to input poses
 
         if animate:
             tranimate(samples, repeat=True, length=pose_marker_length, wait=True)  # animate pose along trajectory
@@ -156,18 +113,88 @@ def visualize(
 
     def to_numpy(self) -> dict[str, npt.NDArray]:
         """Export spline parameters as dictionary of numpy arrays."""
-        return {"timestamps": self.timestamps, "twists": np.vstack([1.0 * pose.twist().A for pose in self.waypoints])}
+        return {"timepoints": self.timepoints, "twists": np.vstack([1.0 * pose.twist().A for pose in self.waypoints])}
 
     def from_numpy(self, data: dict[str, npt.NDArray]) -> None:
         """Reconstruct spline from 'to_numpy' parameters."""
-        self.timestamps = data["timestamps"]
+        self.timepoints = data["timepoints"]
         self.waypoints = [SE3.Exp(twist) for twist in data["twists"]]
 
 
 class SplineFit:
 
-    pass
+    def __init__(
+        self, 
+        time_data: npt.NDArray, 
+        pose_data: npt.NDArray,
+    ) -> None:
+        self.time_data = time_data
+        self.pose_data = pose_data
+
+        self.xyz_data = np.array([pose.t for pose in pose_data])
+        self.so3_data = Rotation.from_matrix(np.array([(pose.R) for pose in pose_data]))
 
+        self.spline: InterpSplineSE3 | BSplineSE3 | None = None
+
+    def downsampled_interpolation(
+        self, 
+        epsilon_xyz: float = 1e-3, 
+        epsilon_angle: float = 1e-1,
+        normalize_time: bool = True,
+        bc_type: str | tuple = "not-a-knot",
+    ) -> Tuple[InterpSplineSE3, List[int]]:
+        """
+            Return:
+                downsampled interpolating spline, 
+                list of removed indices from input data
+        """
+        spline = InterpSplineSE3(
+            self.time_data, 
+            self.pose_data, 
+            normalize_time = normalize_time, 
+            bc_type=bc_type
+        )
+        chosen_indices: set[int] = set()
+        interpolation_indices = list(range(len(self.pose_data)))
+
+        for _ in range(len(self.time_data) - 2):  # you must have at least 2 indices
+            choices = list(set(interpolation_indices).difference(chosen_indices))
+
+            index = np.random.choice(choices)
+
+            chosen_indices.add(index)
+            interpolation_indices.remove(index)
+
+            spline.spline_xyz = CubicSpline(self.time_data[interpolation_indices], self.xyz_data[interpolation_indices])
+            spline.spline_so3 = RotationSpline(
+                self.time_data[interpolation_indices], self.so3_data[interpolation_indices]
+            )
+
+            time = self.time_data[index]
+            angular_error = SO3(self.pose_data[index]).angdist(SO3(spline.spline_so3(time).as_matrix()))
+            euclidean_error = np.linalg.norm(self.pose_data[index].t - spline.spline_xyz(time))
+            if angular_error > epsilon_angle or euclidean_error > epsilon_xyz:
+                interpolation_indices.insert(int(np.searchsorted(interpolation_indices, index, side="right")), index)
+
+        return spline, interpolation_indices
+    
+    def max_angular_error(self) -> float:
+        return np.max(self.angular_errors())
+
+    def angular_errors(self) -> list[float]:
+        return [
+            SO3(pose).angdist(SO3(self.spline_so3(timestamp).as_matrix()))
+            for pose, timestamp in zip(self.waypoints, self.timepoints, strict=True)
+        ]
+
+    def max_euclidean_error(self) -> float:
+        return np.max(self.euclidean_errors())
+
+    def euclidean_errors(self) -> List[float]:
+        return [
+            np.linalg.norm(pose.t - self.spline_xyz(timestamp))
+            for pose, timestamp in zip(self.waypoints, self.timepoints, strict=True)
+        ]
 
 class BSplineSE3:
     """A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.
@@ -232,28 +259,35 @@ def __call__(self, t: float) -> SE3:
     def visualize(
         self,
         num_samples: int,
-        length: float = 1.0,
-        repeat: bool = False,
-        ax: Optional[plt.Axes] = None,
-        kwargs_trplot: Dict[str, Any] = {"color": "green"},
-        kwargs_tranimate: Dict[str, Any] = {"wait": True},
-        kwargs_plot: Dict[str, Any] = {},
+        pose_marker_length: float = 0.2,
+        animate: bool = False,
+        ax: plt.Axes | None = None,
+        input_poses: List[SE3] | None = None
     ) -> None:
         """Displays an animation of the trajectory with the control poses."""
-        out_poses = [self(t) for t in np.linspace(0, 1, num_samples)]
-        x = [pose.x for pose in out_poses]
-        y = [pose.y for pose in out_poses]
-        z = [pose.z for pose in out_poses]
-
         if ax is None:
             fig = plt.figure(figsize=(10, 10))
             ax = fig.add_subplot(projection="3d")
 
-        trplot(
-            [np.array(self.control_poses)], ax=ax, length=length, **kwargs_trplot
-        )  # plot control points
-        ax.plot(x, y, z, **kwargs_plot)  # plot x,y,z trajectory
+        samples = [self(t) for t in np.linspace(0, 1, num_samples)]
+        if not animate:
+            x = [pose.x for pose in samples]
+            y = [pose.y for pose in samples]
+            z = [pose.z for pose in samples]
+            ax.plot(x, y, z, "c", linewidth=1.0)  # plot spline fit
+
+        x = [pose.x for pose in self.control_poses]
+        y = [pose.y for pose in self.control_poses]
+        z = [pose.z for pose in self.control_poses]
+        ax.plot(x, y, z, "r*")  # plot waypoints
 
-        tranimate(
-            out_poses, repeat=repeat, length=length, **kwargs_tranimate
-        )  # animate pose along trajectory
+        if input_poses is not None:
+            x = [pose.x for pose in input_poses]
+            y = [pose.y for pose in input_poses]
+            z = [pose.z for pose in input_poses]
+            ax.plot(x, y, z, "go", fillstyle="none")  # plot compare to input poses
+
+        if animate:
+            tranimate(samples, repeat=True, length=pose_marker_length, wait=True)  # animate pose along trajectory
+        else:
+            plt.show()

From 1e0f7ed7c4e0d746af8e23fb39e5eb21955f113b Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 14:42:49 +0000
Subject: [PATCH 03/21] Spline fit class.

---
 spatialmath/spline.py | 19 ++++++-------------
 1 file changed, 6 insertions(+), 13 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 9260b1ae..21f57fe6 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -111,15 +111,6 @@ def visualize(
         else:
             plt.show()
 
-    def to_numpy(self) -> dict[str, npt.NDArray]:
-        """Export spline parameters as dictionary of numpy arrays."""
-        return {"timepoints": self.timepoints, "twists": np.vstack([1.0 * pose.twist().A for pose in self.waypoints])}
-
-    def from_numpy(self, data: dict[str, npt.NDArray]) -> None:
-        """Reconstruct spline from 'to_numpy' parameters."""
-        self.timepoints = data["timepoints"]
-        self.waypoints = [SE3.Exp(twist) for twist in data["twists"]]
-
 
 class SplineFit:
 
@@ -176,6 +167,7 @@ def downsampled_interpolation(
             if angular_error > epsilon_angle or euclidean_error > epsilon_xyz:
                 interpolation_indices.insert(int(np.searchsorted(interpolation_indices, index, side="right")), index)
 
+        self.spline = spline
         return spline, interpolation_indices
     
     def max_angular_error(self) -> float:
@@ -183,8 +175,8 @@ def max_angular_error(self) -> float:
 
     def angular_errors(self) -> list[float]:
         return [
-            SO3(pose).angdist(SO3(self.spline_so3(timestamp).as_matrix()))
-            for pose, timestamp in zip(self.waypoints, self.timepoints, strict=True)
+            pose.angdist(self.spline(t))
+            for pose, t in zip(self.waypoints, self.timepoints, strict=True)
         ]
 
     def max_euclidean_error(self) -> float:
@@ -192,10 +184,11 @@ def max_euclidean_error(self) -> float:
 
     def euclidean_errors(self) -> List[float]:
         return [
-            np.linalg.norm(pose.t - self.spline_xyz(timestamp))
-            for pose, timestamp in zip(self.waypoints, self.timepoints, strict=True)
+            np.linalg.norm(pose.t - self.spline(t).t)
+            for pose, t in zip(self.waypoints, self.timepoints, strict=True)
         ]
 
+
 class BSplineSE3:
     """A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.
 

From 578f4d3b58c8d245c38974ad252ae30b51a52920 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 17:09:31 +0000
Subject: [PATCH 04/21] Working unit test I hope.

---
 spatialmath/__init__.py |  4 +++-
 tests/test_spline.py    | 52 ++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 54 insertions(+), 2 deletions(-)

diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py
index e6ef1f77..18cb74b4 100644
--- a/spatialmath/__init__.py
+++ b/spatialmath/__init__.py
@@ -15,7 +15,7 @@
 )
 from spatialmath.quaternion import Quaternion, UnitQuaternion
 from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion
-from spatialmath.spline import BSplineSE3
+from spatialmath.spline import BSplineSE3, InterpSplineSE3, SplineFit
 
 # from spatialmath.Plucker import *
 # from spatialmath import base as smb
@@ -45,6 +45,8 @@
     "Polygon2",
     "Ellipse",
     "BSplineSE3",
+    "InterpSplineSE3",
+    "SplineFit"
 ]
 
 try:
diff --git a/tests/test_spline.py b/tests/test_spline.py
index f518fcfb..5d0bf2fe 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -5,7 +5,7 @@
 import sys
 import pytest
 
-from spatialmath import BSplineSE3, SE3
+from spatialmath import BSplineSE3, SE3, InterpSplineSE3, SplineFit, SO3
 
 
 class TestBSplineSE3(unittest.TestCase):
@@ -30,3 +30,53 @@ def test_evaluation(self):
     def test_visualize(self):
         spline = BSplineSE3(self.control_poses)
         spline.visualize(num_samples=100, repeat=False)
+
+class TestInterpSplineSE3:
+    waypoints = [
+        SE3.Trans([e, 2 * np.cos(e / 2 * np.pi), 2 * np.sin(e / 2 * np.pi)])
+        * SE3.Ry(e / 8 * np.pi)
+        for e in range(0, 8)
+    ]
+    times = np.linspace(0, 10, len(waypoints))
+
+    @classmethod
+    def tearDownClass(cls):
+        plt.close("all")
+
+    def test_constructor(self):
+        InterpSplineSE3(self.times, self.waypoints)
+
+    def test_evaluation(self):
+        spline = InterpSplineSE3(self.times, self.waypoints)
+        nt.assert_almost_equal(spline(0).A, self.waypoints[0].A)
+        nt.assert_almost_equal(spline(1).A, self.waypoints[-1].A)
+
+    def test_visualize(self):
+        spline = InterpSplineSE3(self.times, self.waypoints)
+        spline.visualize(num_samples=100, repeat=False)
+
+class TestSpineFit:
+    num_data_points = 300
+    time_horizon = 5
+    num_viz_points = 100
+
+    # make a helix
+    timestamps = np.linspace(0, 1, num_data_points)
+    trajectory = [
+        SE3.Rt(
+            t=[t * 0.4, 0.4 * np.sin(t * 2 * np.pi * 0.5), 0.4 * np.cos(t * 2 * np.pi * 0.5)],
+            R=SO3.Rx(t * 2 * np.pi * 0.5),
+        )
+        for t in timestamps * time_horizon
+    ]
+
+    fit = SplineFit(timestamps, trajectory)
+    spline, kept_indices = fit.downsampled_interpolation()
+
+    fraction_points_removed = 1.0 - len(kept_indices) / num_data_points 
+    assert(fraction_points_removed > 0.2)
+    
+    sample = spline(timestamps[50])
+    true_pose = trajectory[50]
+    assert( sample.angdist(true_pose) <np.deg2rad(5.0) )
+    assert( np.linalg.norm(sample.t - true_pose.t) < 0.1 )
\ No newline at end of file

From bce7f589eac3e215cd96bf82deb033042a579b1e Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 13:15:12 -0400
Subject: [PATCH 05/21] Unit tests.

---
 spatialmath/spline.py | 2 +-
 tests/test_spline.py  | 5 +++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 21f57fe6..5762a1af 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -64,7 +64,7 @@ def __init__(
             np.array([pose.t for pose in self.waypoints]), 
             bc_type=bc_type
         )
-        self.spline_so3 = RotationSpline(self.timepoints, self.so3_data)
+        self.spline_so3 = RotationSpline(self.timepoints, Rotation.from_matrix(np.array([(pose.R) for pose in self.waypoints])))
 
     def __call__(self, t: float) -> Any:
 
diff --git a/tests/test_spline.py b/tests/test_spline.py
index 5d0bf2fe..2ef2e252 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -29,7 +29,7 @@ def test_evaluation(self):
 
     def test_visualize(self):
         spline = BSplineSE3(self.control_poses)
-        spline.visualize(num_samples=100, repeat=False)
+        spline.visualize(num_samples=100, animate=True)
 
 class TestInterpSplineSE3:
     waypoints = [
@@ -53,7 +53,8 @@ def test_evaluation(self):
 
     def test_visualize(self):
         spline = InterpSplineSE3(self.times, self.waypoints)
-        spline.visualize(num_samples=100, repeat=False)
+        spline.visualize(num_samples=100, animate=True)
+        
 
 class TestSpineFit:
     num_data_points = 300

From 806726ee59a981dd40a107350365f483b7b71c40 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 13:49:46 -0400
Subject: [PATCH 06/21] Fix error functions, actually use them in unit test.

---
 spatialmath/spline.py | 10 +++++-----
 tests/test_spline.py  |  6 ++----
 2 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 5762a1af..d6302ea7 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -32,8 +32,8 @@ class InterpSplineSE3:
 
     def __init__(
         self,
-        timepoints: list[float] | npt.NDArray,
-        waypoints: list[SE3],
+        timepoints: List[float] | npt.NDArray,
+        waypoints: List[SE3],
         *,
         normalize_time: bool = True,
         bc_type: str | tuple = "not-a-knot",  # not-a-knot is scipy default; None is invalid
@@ -173,10 +173,10 @@ def downsampled_interpolation(
     def max_angular_error(self) -> float:
         return np.max(self.angular_errors())
 
-    def angular_errors(self) -> list[float]:
+    def angular_errors(self) -> List[float]:
         return [
             pose.angdist(self.spline(t))
-            for pose, t in zip(self.waypoints, self.timepoints, strict=True)
+            for pose, t in zip(self.pose_data, self.time_data, strict=True)
         ]
 
     def max_euclidean_error(self) -> float:
@@ -185,7 +185,7 @@ def max_euclidean_error(self) -> float:
     def euclidean_errors(self) -> List[float]:
         return [
             np.linalg.norm(pose.t - self.spline(t).t)
-            for pose, t in zip(self.waypoints, self.timepoints, strict=True)
+            for pose, t in zip(self.pose_data, self.time_data, strict=True)
         ]
 
 
diff --git a/tests/test_spline.py b/tests/test_spline.py
index 2ef2e252..feb853b2 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -77,7 +77,5 @@ class TestSpineFit:
     fraction_points_removed = 1.0 - len(kept_indices) / num_data_points 
     assert(fraction_points_removed > 0.2)
     
-    sample = spline(timestamps[50])
-    true_pose = trajectory[50]
-    assert( sample.angdist(true_pose) <np.deg2rad(5.0) )
-    assert( np.linalg.norm(sample.t - true_pose.t) < 0.1 )
\ No newline at end of file
+    assert( fit.max_angular_error() < np.deg2rad(5.0) )
+    assert( fit.max_angular_error() < 0.1 )
\ No newline at end of file

From aa8148a2d3d90dbffaa12d362ca1ae0b4bdbb6f5 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 13:53:09 -0400
Subject: [PATCH 07/21] Make mac typ hinting happy?

---
 spatialmath/spline.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index d6302ea7..653b329e 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -32,7 +32,7 @@ class InterpSplineSE3:
 
     def __init__(
         self,
-        timepoints: List[float] | npt.NDArray,
+        timepoints: List[float],
         waypoints: List[SE3],
         *,
         normalize_time: bool = True,
@@ -116,8 +116,8 @@ class SplineFit:
 
     def __init__(
         self, 
-        time_data: npt.NDArray, 
-        pose_data: npt.NDArray,
+        time_data: List[float], 
+        pose_data: List[SE3],
     ) -> None:
         self.time_data = time_data
         self.pose_data = pose_data

From d5f2c4243c1260b47ddb013916eeac554f6889a9 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 13:58:07 -0400
Subject: [PATCH 08/21] ...

---
 spatialmath/spline.py | 28 +++++++++++++++++-----------
 1 file changed, 17 insertions(+), 11 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 653b329e..36f1b743 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -21,9 +21,15 @@
 from scipy.spatial.transform import Rotation, RotationSpline
 from spatialmath import SE3, SO3, Twist3
 from spatialmath.base.transforms3d import tranimate
+from abc import ABC, abstractmethod
 
+class SplineSE3(ABC):
 
-class InterpSplineSE3:
+    @abstractmethod
+    def __call__(self, t: float) -> SE3:
+        pass
+
+class InterpSplineSE3(SplineSE3):
     """Class for an interpolated trajectory in SE3, as a function of time, through waypoints with a cubic spline.
 
     A combination of scipy.interpolate.CubicSpline and scipy.spatial.transform.RotationSpline (itself also cubic)
@@ -32,11 +38,11 @@ class InterpSplineSE3:
 
     def __init__(
         self,
-        timepoints: List[float],
+        timepoints: List[float], 
         waypoints: List[SE3],
         *,
         normalize_time: bool = True,
-        bc_type: str | tuple = "not-a-knot",  # not-a-knot is scipy default; None is invalid
+        bc_type: str = "not-a-knot",  # not-a-knot is scipy default; None is invalid
     ) -> None:
         """Construct a InterpSplineSE3 object
 
@@ -66,7 +72,7 @@ def __init__(
         )
         self.spline_so3 = RotationSpline(self.timepoints, Rotation.from_matrix(np.array([(pose.R) for pose in self.waypoints])))
 
-    def __call__(self, t: float) -> Any:
+    def __call__(self, t: float) -> SE3:
 
         return SE3.Rt(t=self.spline_xyz(t), R=self.spline_so3(t).as_matrix())
 
@@ -80,8 +86,8 @@ def visualize(
         num_samples: int,
         pose_marker_length: float = 0.2,
         animate: bool = False,
-        ax: plt.Axes | None = None,
-        input_poses: List[SE3] | None = None
+        ax: Optional[plt.Axes] = None,
+        input_poses: Optional[List[SE3]] = None
     ) -> None:
         """Displays an animation of the trajectory with the control poses."""
         if ax is None:
@@ -125,14 +131,14 @@ def __init__(
         self.xyz_data = np.array([pose.t for pose in pose_data])
         self.so3_data = Rotation.from_matrix(np.array([(pose.R) for pose in pose_data]))
 
-        self.spline: InterpSplineSE3 | BSplineSE3 | None = None
+        self.spline: Optional[SplineSE3] = None
 
     def downsampled_interpolation(
         self, 
         epsilon_xyz: float = 1e-3, 
         epsilon_angle: float = 1e-1,
         normalize_time: bool = True,
-        bc_type: str | tuple = "not-a-knot",
+        bc_type: str = "not-a-knot",
     ) -> Tuple[InterpSplineSE3, List[int]]:
         """
             Return:
@@ -189,7 +195,7 @@ def euclidean_errors(self) -> List[float]:
         ]
 
 
-class BSplineSE3:
+class BSplineSE3(SplineSE3):
     """A class to parameterize a trajectory in SE3 with a 6-dimensional B-spline.
 
     The SE3 control poses are converted to se3 twists (the lie algebra) and a B-spline
@@ -254,8 +260,8 @@ def visualize(
         num_samples: int,
         pose_marker_length: float = 0.2,
         animate: bool = False,
-        ax: plt.Axes | None = None,
-        input_poses: List[SE3] | None = None
+        ax: Optional[plt.Axes]  = None,
+        input_poses: Optional[List[SE3]] = None
     ) -> None:
         """Displays an animation of the trajectory with the control poses."""
         if ax is None:

From ecc9b594df3fad0b7bfb1d03f9a3b74072302522 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 13:59:47 -0400
Subject: [PATCH 09/21] Old python compatilbity is hard.

---
 spatialmath/spline.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 36f1b743..d9f5c57e 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -182,7 +182,7 @@ def max_angular_error(self) -> float:
     def angular_errors(self) -> List[float]:
         return [
             pose.angdist(self.spline(t))
-            for pose, t in zip(self.pose_data, self.time_data, strict=True)
+            for pose, t in zip(self.pose_data, self.time_data)
         ]
 
     def max_euclidean_error(self) -> float:
@@ -191,7 +191,7 @@ def max_euclidean_error(self) -> float:
     def euclidean_errors(self) -> List[float]:
         return [
             np.linalg.norm(pose.t - self.spline(t).t)
-            for pose, t in zip(self.pose_data, self.time_data, strict=True)
+            for pose, t in zip(self.pose_data, self.time_data)
         ]
 
 

From 2c5190f505f62312d30d7a3edaa2772a5bd8f3d6 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 14:17:20 -0400
Subject: [PATCH 10/21] Clean up imports.

---
 spatialmath/spline.py | 16 +++++-----------
 1 file changed, 5 insertions(+), 11 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index d9f5c57e..b4c6262f 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -5,23 +5,17 @@
 Classes for parameterizing a trajectory in SE3 with splines. 
 """
 
-from typing import Any, Dict, List, Optional
-from scipy.interpolate import BSpline
-from spatialmath import SE3
-import numpy as np
-import matplotlib.pyplot as plt
-from spatialmath.base.transforms3d import tranimate, trplot
-
-from typing import Any, List, Tuple
+from abc import ABC, abstractmethod
+from typing import List, Optional
 
 import matplotlib.pyplot as plt
 import numpy as np
-import numpy.typing as npt
-from scipy.interpolate import CubicSpline
+from scipy.interpolate import BSpline, CubicSpline
 from scipy.spatial.transform import Rotation, RotationSpline
+
 from spatialmath import SE3, SO3, Twist3
 from spatialmath.base.transforms3d import tranimate
-from abc import ABC, abstractmethod
+
 
 class SplineSE3(ABC):
 

From 1b8fc78646c478b153583876057cf07408d490a4 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Thu, 17 Oct 2024 14:20:30 -0400
Subject: [PATCH 11/21] isort failed me

---
 spatialmath/spline.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index b4c6262f..827e7ca9 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -6,7 +6,7 @@
 """
 
 from abc import ABC, abstractmethod
-from typing import List, Optional
+from typing import List, Optional, Tuple
 
 import matplotlib.pyplot as plt
 import numpy as np

From c7b7be6eadd9e7522ed7863af238e2b881a5c472 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Fri, 18 Oct 2024 16:17:22 -0400
Subject: [PATCH 12/21] Address review comments.

---
 spatialmath/base/animate.py |  10 +--
 spatialmath/spline.py       | 156 ++++++++++++++++--------------------
 tests/test_spline.py        |  21 +++--
 3 files changed, 89 insertions(+), 98 deletions(-)

diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py
index 337b223f..7654a5a0 100755
--- a/spatialmath/base/animate.py
+++ b/spatialmath/base/animate.py
@@ -320,7 +320,7 @@ def __init__(self, anim: Animate, h, xs, ys, zs):
             self.anim = anim
 
         def draw(self, T):
-            p = T * self.p
+            p = T @ self.p
             self.h.set_data(p[0, :], p[1, :])
             self.h.set_3d_properties(p[2, :])
 
@@ -378,7 +378,7 @@ def __init__(self, anim, h):
 
         def draw(self, T):
             # import ipdb; ipdb.set_trace()
-            p = T * self.p
+            p = T @ self.p
 
             # reshape it
             p = p[0:3, :].T.reshape(3, 2, 3)
@@ -432,7 +432,7 @@ def __init__(self, anim, h, x, y, z):
             self.anim = anim
 
         def draw(self, T):
-            p = T * self.p
+            p = T @ self.p
             # x2, y2, _ = proj3d.proj_transform(
             #   p[0], p[1], p[2], self.anim.ax.get_proj())
             # self.h.set_position((x2, y2))
@@ -759,7 +759,7 @@ def __init__(self, anim, h, xs, ys):
             self.anim = anim
 
         def draw(self, T):
-            p = T * self.p
+            p = T @ self.p
             self.h.set_data(p[0, :], p[1, :])
 
     def plot(self, x, y, *args, **kwargs):
@@ -837,7 +837,7 @@ def __init__(self, anim, h, x, y):
             self.anim = anim
 
         def draw(self, T):
-            p = T * self.p
+            p = T @ self.p
             # x2, y2, _ = proj3d.proj_transform(
             #   p[0], p[1], p[2], self.anim.ax.get_proj())
             # self.h.set_position((x2, y2))
diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 827e7ca9..f3be2380 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -6,6 +6,7 @@
 """
 
 from abc import ABC, abstractmethod
+from functools import cached_property
 from typing import List, Optional, Tuple
 
 import matplotlib.pyplot as plt
@@ -19,21 +20,60 @@
 
 class SplineSE3(ABC):
 
+    def __init__(self) -> None:
+        self.control_poses: SE3
+
     @abstractmethod
     def __call__(self, t: float) -> SE3:
         pass
 
+    def visualize(
+        self,
+        num_samples: int,
+        pose_marker_length: float = 0.2,
+        animate: bool = False,
+        ax: Optional[plt.Axes]  = None,
+        input_poses: Optional[List[SE3]] = None
+    ) -> None:
+        """Displays an animation of the trajectory with the control poses."""
+        if ax is None:
+            fig = plt.figure(figsize=(10, 10))
+            ax = fig.add_subplot(projection="3d")
+
+        samples = [self(t) for t in np.linspace(0, 1, num_samples)]
+        if not animate:
+            x = [pose.x for pose in samples]
+            y = [pose.y for pose in samples]
+            z = [pose.z for pose in samples]
+            ax.plot(x, y, z, "c", linewidth=1.0)  # plot spline fit
+
+        x = [pose.x for pose in self.control_poses]
+        y = [pose.y for pose in self.control_poses]
+        z = [pose.z for pose in self.control_poses]
+        ax.plot(x, y, z, "r*")  # plot control_poses
+
+        if input_poses is not None:
+            x = [pose.x for pose in input_poses]
+            y = [pose.y for pose in input_poses]
+            z = [pose.z for pose in input_poses]
+            ax.plot(x, y, z, "go", fillstyle="none")  # plot compare to input poses
+
+        if animate:
+            tranimate(samples, repeat=True, length=pose_marker_length, wait=True)  # animate pose along trajectory
+        else:
+            plt.show()
+
 class InterpSplineSE3(SplineSE3):
-    """Class for an interpolated trajectory in SE3, as a function of time, through waypoints with a cubic spline.
+    """Class for an interpolated trajectory in SE3, as a function of time, through control_poses with a cubic spline.
 
     A combination of scipy.interpolate.CubicSpline and scipy.spatial.transform.RotationSpline (itself also cubic)
     under the hood.
     """
-
+    _e = 1e-12
     def __init__(
         self,
         timepoints: List[float], 
-        waypoints: List[SE3],
+        control_poses: List[SE3],
         *,
         normalize_time: bool = True,
         bc_type: str = "not-a-knot",  # not-a-knot is scipy default; None is invalid
@@ -45,72 +85,48 @@ def __init__(
 
         Args :
             timepoints : list of times corresponding to provided poses
-            waypoints : list of SE3 objects that govern the shape of the spline.
+            control_poses : list of SE3 objects that govern the shape of the spline.
             normalize_time : flag to map times into the range [0, 1]
             bc_type : boundary condition provided to scipy CubicSpline backend.
                       string options: ["not-a-knot" (default), "clamped", "natural", "periodic"].
                       For tuple options and details see the scipy docs link above.
         """
-
-        self.waypoints = waypoints
+        super().__init__()
+        self.control_poses = control_poses
         self.timepoints = np.array(timepoints)
 
+        if self.timepoints[-1] < self._e:
+            raise ValueError("Difference between start and end timepoints is less than {self._e}")
+        
+        if len(self.control_poses) != len(self.timepoints):
+            raise ValueError("Length of control_poses and timepoints must be equal.")
+        
+        if len(self.timepoints) < 2:
+            raise ValueError("Need at least 2 data points to make a trajectory.")
+
         if normalize_time:
             self.timepoints = self.timepoints - self.timepoints[0]
             self.timepoints = self.timepoints / self.timepoints[-1]
 
         self.spline_xyz = CubicSpline(
             self.timepoints, 
-            np.array([pose.t for pose in self.waypoints]), 
+            np.array([pose.t for pose in self.control_poses]), 
             bc_type=bc_type
         )
-        self.spline_so3 = RotationSpline(self.timepoints, Rotation.from_matrix(np.array([(pose.R) for pose in self.waypoints])))
+        self.spline_so3 = RotationSpline(self.timepoints, Rotation.from_matrix(np.array([(pose.R) for pose in self.control_poses])))
 
     def __call__(self, t: float) -> SE3:
-
+        """Compute function value at t. 
+        Return:
+            pose: SE3
+        """
         return SE3.Rt(t=self.spline_xyz(t), R=self.spline_so3(t).as_matrix())
 
     def derivative(self, t: float) -> Twist3:
         linear_vel = self.spline_xyz.derivative()(t)
-        angular_vel = self.spline_so3(t, 1)
+        angular_vel = self.spline_so3(t, 1) #1 is angular rate, 2 is angular acceleration
         return Twist3(linear_vel, angular_vel)
 
-    def visualize(
-        self,
-        num_samples: int,
-        pose_marker_length: float = 0.2,
-        animate: bool = False,
-        ax: Optional[plt.Axes] = None,
-        input_poses: Optional[List[SE3]] = None
-    ) -> None:
-        """Displays an animation of the trajectory with the control poses."""
-        if ax is None:
-            fig = plt.figure(figsize=(10, 10))
-            ax = fig.add_subplot(projection="3d")
-
-        samples = [self(t) for t in np.linspace(0, 1, num_samples)]
-        if not animate:
-            x = [pose.x for pose in samples]
-            y = [pose.y for pose in samples]
-            z = [pose.z for pose in samples]
-            ax.plot(x, y, z, "c", linewidth=1.0)  # plot spline fit
-
-        x = [pose.x for pose in self.waypoints]
-        y = [pose.y for pose in self.waypoints]
-        z = [pose.z for pose in self.waypoints]
-        ax.plot(x, y, z, "r*")  # plot waypoints
-
-        if input_poses is not None:
-            x = [pose.x for pose in input_poses]
-            y = [pose.y for pose in input_poses]
-            z = [pose.z for pose in input_poses]
-            ax.plot(x, y, z, "go", fillstyle="none")  # plot compare to input poses
-
-        if animate:
-            tranimate(samples, repeat=True, length=pose_marker_length, wait=True)  # animate pose along trajectory
-        else:
-            plt.show()
-
 
 class SplineFit:
 
@@ -127,7 +143,7 @@ def __init__(
 
         self.spline: Optional[SplineSE3] = None
 
-    def downsampled_interpolation(
+    def stochastic_downsample_interpolation(
         self, 
         epsilon_xyz: float = 1e-3, 
         epsilon_angle: float = 1e-1,
@@ -147,6 +163,8 @@ def downsampled_interpolation(
         )
         chosen_indices: set[int] = set()
         interpolation_indices = list(range(len(self.pose_data)))
+        interpolation_indices.remove(0)
+        interpolation_indices.remove(len(self.pose_data) - 1)
 
         for _ in range(len(self.time_data) - 2):  # you must have at least 2 indices
             choices = list(set(interpolation_indices).difference(chosen_indices))
@@ -164,15 +182,16 @@ def downsampled_interpolation(
             time = self.time_data[index]
             angular_error = SO3(self.pose_data[index]).angdist(SO3(spline.spline_so3(time).as_matrix()))
             euclidean_error = np.linalg.norm(self.pose_data[index].t - spline.spline_xyz(time))
-            if angular_error > epsilon_angle or euclidean_error > epsilon_xyz:
+            if (angular_error > epsilon_angle) or (euclidean_error > epsilon_xyz):
                 interpolation_indices.insert(int(np.searchsorted(interpolation_indices, index, side="right")), index)
 
         self.spline = spline
         return spline, interpolation_indices
     
     def max_angular_error(self) -> float:
-        return np.max(self.angular_errors())
+        return np.max(self.angular_errors)
 
+    @cached_property
     def angular_errors(self) -> List[float]:
         return [
             pose.angdist(self.spline(t))
@@ -180,8 +199,9 @@ def angular_errors(self) -> List[float]:
         ]
 
     def max_euclidean_error(self) -> float:
-        return np.max(self.euclidean_errors())
+        return np.max(self.euclidean_errors)
 
+    @cached_property
     def euclidean_errors(self) -> List[float]:
         return [
             np.linalg.norm(pose.t - self.spline(t).t)
@@ -215,7 +235,7 @@ def __init__(
         at a given t input. If none, they are automatically, uniformly generated based on number of control poses and
         degree of spline.
         """
-
+        super().__init__()
         self.control_poses = control_poses
 
         # a matrix where each row is a control pose as a twist
@@ -249,38 +269,4 @@ def __call__(self, t: float) -> SE3:
         twist = np.hstack([spline(t) for spline in self.splines])
         return SE3.Exp(twist)
 
-    def visualize(
-        self,
-        num_samples: int,
-        pose_marker_length: float = 0.2,
-        animate: bool = False,
-        ax: Optional[plt.Axes]  = None,
-        input_poses: Optional[List[SE3]] = None
-    ) -> None:
-        """Displays an animation of the trajectory with the control poses."""
-        if ax is None:
-            fig = plt.figure(figsize=(10, 10))
-            ax = fig.add_subplot(projection="3d")
-
-        samples = [self(t) for t in np.linspace(0, 1, num_samples)]
-        if not animate:
-            x = [pose.x for pose in samples]
-            y = [pose.y for pose in samples]
-            z = [pose.z for pose in samples]
-            ax.plot(x, y, z, "c", linewidth=1.0)  # plot spline fit
 
-        x = [pose.x for pose in self.control_poses]
-        y = [pose.y for pose in self.control_poses]
-        z = [pose.z for pose in self.control_poses]
-        ax.plot(x, y, z, "r*")  # plot waypoints
-
-        if input_poses is not None:
-            x = [pose.x for pose in input_poses]
-            y = [pose.y for pose in input_poses]
-            z = [pose.z for pose in input_poses]
-            ax.plot(x, y, z, "go", fillstyle="none")  # plot compare to input poses
-
-        if animate:
-            tranimate(samples, repeat=True, length=pose_marker_length, wait=True)  # animate pose along trajectory
-        else:
-            plt.show()
diff --git a/tests/test_spline.py b/tests/test_spline.py
index feb853b2..41dfc6cb 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -51,12 +51,15 @@ def test_evaluation(self):
         nt.assert_almost_equal(spline(0).A, self.waypoints[0].A)
         nt.assert_almost_equal(spline(1).A, self.waypoints[-1].A)
 
+    def test_small_delta_t(self):
+        InterpSplineSE3(np.linspace(0, InterpSplineSE3._e, len(self.waypoints)), self.waypoints)
+
     def test_visualize(self):
         spline = InterpSplineSE3(self.times, self.waypoints)
         spline.visualize(num_samples=100, animate=True)
         
 
-class TestSpineFit:
+class TestSplineFit:
     num_data_points = 300
     time_horizon = 5
     num_viz_points = 100
@@ -71,11 +74,13 @@ class TestSpineFit:
         for t in timestamps * time_horizon
     ]
 
-    fit = SplineFit(timestamps, trajectory)
-    spline, kept_indices = fit.downsampled_interpolation()
+    def test_spline_fit(self):
+        fit = SplineFit(self.timestamps, self.trajectory)
+        spline, kept_indices = fit.stochastic_downsample_interpolation()
 
-    fraction_points_removed = 1.0 - len(kept_indices) / num_data_points 
-    assert(fraction_points_removed > 0.2)
-    
-    assert( fit.max_angular_error() < np.deg2rad(5.0) )
-    assert( fit.max_angular_error() < 0.1 )
\ No newline at end of file
+        fraction_points_removed = 1.0 - len(kept_indices) / self.num_data_points 
+        assert(fraction_points_removed > 0.2)
+        
+        assert( fit.max_angular_error() < np.deg2rad(5.0) )
+        assert( fit.max_angular_error() < 0.1 )
+        spline.visualize(num_samples=100, animate=True)
\ No newline at end of file

From 51f9204479d9f4f0b3be111005ff9b2ae5806b11 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Fri, 18 Oct 2024 16:25:41 -0400
Subject: [PATCH 13/21] DAVID SUROVIK IS A GENIUS ALL HAIL HIM

---
 spatialmath/base/animate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py
index 7654a5a0..a2e31f72 100755
--- a/spatialmath/base/animate.py
+++ b/spatialmath/base/animate.py
@@ -217,7 +217,7 @@ def update(frame, animation):
             if isinstance(frame, float):
                 # passed a single transform, interpolate it
                 T = smb.trinterp(start=self.start, end=self.end, s=frame)
-            elif isinstance(frame, NDArray):
+            elif isinstance(frame, np.ndarray):
                 # type is SO3Array or SE3Array when Animate.trajectory is not None
                 T = frame
             else:

From 9634c24258080e388088767b692302eb51158851 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Mon, 21 Oct 2024 09:24:43 -0400
Subject: [PATCH 14/21] Clean up visualization interface. Take review comments.
 Improve unit test for interpolated spline.

---
 spatialmath/spline.py | 41 ++++++++++++++++++++---------------------
 tests/test_spline.py  | 24 +++++++++++++++---------
 2 files changed, 35 insertions(+), 30 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index f3be2380..36de8c74 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -29,37 +29,32 @@ def __call__(self, t: float) -> SE3:
 
     def visualize(
         self,
-        num_samples: int,
+        times: list[float],
         pose_marker_length: float = 0.2,
         animate: bool = False,
+        repeat: bool = True,
         ax: Optional[plt.Axes]  = None,
-        input_poses: Optional[List[SE3]] = None
+        input_trajectory: Optional[List[SE3]] = None,
     ) -> None:
-        """Displays an animation of the trajectory with the control poses."""
+        """Displays an animation of the trajectory with the control poses against an optional input trajectory."""
         if ax is None:
             fig = plt.figure(figsize=(10, 10))
             ax = fig.add_subplot(projection="3d")
 
-        samples = [self(t) for t in np.linspace(0, 1, num_samples)]
+        samples = [self(t) for t in times]
         if not animate:
-            x = [pose.x for pose in samples]
-            y = [pose.y for pose in samples]
-            z = [pose.z for pose in samples]
-            ax.plot(x, y, z, "c", linewidth=1.0)  # plot spline fit
-
-        x = [pose.x for pose in self.control_poses]
-        y = [pose.y for pose in self.control_poses]
-        z = [pose.z for pose in self.control_poses]
-        ax.plot(x, y, z, "r*")  # plot control_poses
-
-        if input_poses is not None:
-            x = [pose.x for pose in input_poses]
-            y = [pose.y for pose in input_poses]
-            z = [pose.z for pose in input_poses]
-            ax.plot(x, y, z, "go", fillstyle="none")  # plot compare to input poses
+            pos = np.array([pose.t for pose in samples])
+            ax.plot(pos[:,0], pos[:,1], pos[:,2], "c", linewidth=1.0)  # plot spline fit
+
+        pos = np.array([pose.t for pose in self.control_poses])
+        ax.plot(pos[:,0], pos[:,1], pos[:,2], "r*")  # plot control_poses
+
+        if input_trajectory is not None:
+            pos = np.array([pose.t for pose in input_trajectory])
+            ax.plot(pos[:,0], pos[:,1], pos[:,2], "go", fillstyle="none")  # plot compare to input poses
 
         if animate:
-            tranimate(samples, repeat=True, length=pose_marker_length, wait=True)  # animate pose along trajectory
+            tranimate(samples, length=pose_marker_length, wait=True, repeat = repeat)  # animate pose along trajectory
         else:
             plt.show()
 
@@ -75,7 +70,7 @@ def __init__(
         timepoints: List[float], 
         control_poses: List[SE3],
         *,
-        normalize_time: bool = True,
+        normalize_time: bool = False,
         bc_type: str = "not-a-knot",  # not-a-knot is scipy default; None is invalid
     ) -> None:
         """Construct a InterpSplineSE3 object
@@ -151,6 +146,10 @@ def stochastic_downsample_interpolation(
         bc_type: str = "not-a-knot",
     ) -> Tuple[InterpSplineSE3, List[int]]:
         """
+        Uses a random dropout heuristic to downsample a trajectory with an interpolated spline. 
+
+        This code does not ensure the global fit is within epsilon_xyz and epsilon_angle. 
+
             Return:
                 downsampled interpolating spline, 
                 list of removed indices from input data
diff --git a/tests/test_spline.py b/tests/test_spline.py
index 41dfc6cb..1af623f6 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -2,8 +2,6 @@
 import numpy as np
 import matplotlib.pyplot as plt
 import unittest
-import sys
-import pytest
 
 from spatialmath import BSplineSE3, SE3, InterpSplineSE3, SplineFit, SO3
 
@@ -29,7 +27,7 @@ def test_evaluation(self):
 
     def test_visualize(self):
         spline = BSplineSE3(self.control_poses)
-        spline.visualize(num_samples=100, animate=True)
+        spline.visualize(np.linspace(0, 1.0, 100), animate=True, repeat=False)
 
 class TestInterpSplineSE3:
     waypoints = [
@@ -37,7 +35,8 @@ class TestInterpSplineSE3:
         * SE3.Ry(e / 8 * np.pi)
         for e in range(0, 8)
     ]
-    times = np.linspace(0, 10, len(waypoints))
+    time_horizon = 10
+    times = np.linspace(0, time_horizon, len(waypoints))
 
     @classmethod
     def tearDownClass(cls):
@@ -48,15 +47,22 @@ def test_constructor(self):
 
     def test_evaluation(self):
         spline = InterpSplineSE3(self.times, self.waypoints)
-        nt.assert_almost_equal(spline(0).A, self.waypoints[0].A)
-        nt.assert_almost_equal(spline(1).A, self.waypoints[-1].A)
-
+        for time, pose in zip(self.times, self.waypoints):
+            nt.assert_almost_equal(spline(time).angdist(pose), 0.0)
+            nt.assert_almost_equal(np.linalg.norm(spline(time).t - pose.t), 0.0)
+
+        spline = InterpSplineSE3(self.times, self.waypoints, normalize_time=True)
+        norm_time = spline.timepoints
+        for time, pose in zip(norm_time, self.waypoints):
+            nt.assert_almost_equal(spline(time).angdist(pose), 0.0)
+            nt.assert_almost_equal(np.linalg.norm(spline(time).t - pose.t), 0.0)
+  
     def test_small_delta_t(self):
         InterpSplineSE3(np.linspace(0, InterpSplineSE3._e, len(self.waypoints)), self.waypoints)
 
     def test_visualize(self):
         spline = InterpSplineSE3(self.times, self.waypoints)
-        spline.visualize(num_samples=100, animate=True)
+        spline.visualize(np.linspace(0, self.time_horizon, 100), animate=True, repeat=False)
         
 
 class TestSplineFit:
@@ -83,4 +89,4 @@ def test_spline_fit(self):
         
         assert( fit.max_angular_error() < np.deg2rad(5.0) )
         assert( fit.max_angular_error() < 0.1 )
-        spline.visualize(num_samples=100, animate=True)
\ No newline at end of file
+        spline.visualize(np.linspace(0, self.time_horizon, 100), animate=True, repeat=False)
\ No newline at end of file

From a2294edaec5ff589e5ddd21fc126be1d7ca08055 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Mon, 21 Oct 2024 09:42:08 -0400
Subject: [PATCH 15/21] Add ruff formatter.

---
 .pre-commit-config.yaml | 12 ++++++------
 spatialmath/spline.py   |  2 +-
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 1a3b93ca..0a1781b5 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -1,10 +1,10 @@
 repos:
-# - repo: https://github.com/charliermarsh/ruff-pre-commit
-#   # Ruff version.
-#   rev: 'v0.1.0'
-#   hooks:
-#     - id: ruff
-#       args: ['--fix', '--config', 'pyproject.toml']
+- repo: https://github.com/charliermarsh/ruff-pre-commit
+  # Ruff version.
+  rev: 'v0.1.0'
+  hooks:
+    - id: ruff
+      args: ['--fix', '--config', 'pyproject.toml']
 
 - repo: https://github.com/psf/black
   rev: 23.10.0
diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 36de8c74..256fe1b9 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -29,7 +29,7 @@ def __call__(self, t: float) -> SE3:
 
     def visualize(
         self,
-        times: list[float],
+        times: List[float],
         pose_marker_length: float = 0.2,
         animate: bool = False,
         repeat: bool = True,

From 4e7aa35c7279700a11a851d1c58e7b2dc41772ff Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Mon, 21 Oct 2024 09:43:05 -0400
Subject: [PATCH 16/21] Trigger file formatting.

---
 spatialmath/spline.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 256fe1b9..7bd4132e 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -124,7 +124,8 @@ def derivative(self, t: float) -> Twist3:
 
 
 class SplineFit:
-
+    """ A general class to fit various SE3 splines to data.
+    """
     def __init__(
         self, 
         time_data: List[float], 

From 551e47366836084e9c14b7bd4c5d3c3efd131153 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Mon, 21 Oct 2024 09:46:16 -0400
Subject: [PATCH 17/21] Add some more pre commit.

---
 .pre-commit-config.yaml | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 0a1781b5..7c28f075 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -14,6 +14,21 @@ repos:
       args: ['--config', 'pyproject.toml']
       verbose: true
 
+- repo: https://github.com/pre-commit/pre-commit-hooks
+  rev: v4.5.0
+  hooks:
+  -   id: end-of-file-fixer
+  -   id: debug-statements # Ensure we don't commit `import pdb; pdb.set_trace()`
+      exclude: |
+        (?x)^(
+          docker/ros/web/static/.*|
+        )$
+  -   id: trailing-whitespace
+      exclude: |
+        (?x)^(
+          docker/ros/web/static/.*|
+          (.*/).*\.patch|
+        )$
 # - repo: https://github.com/pre-commit/mirrors-mypy
 #   rev: v1.6.1
 #   hooks:

From 37181dd8f32dc2c5b90d9aca3a503d64557c2ef3 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Mon, 21 Oct 2024 09:49:42 -0400
Subject: [PATCH 18/21] ...

---
 spatialmath/spline.py | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 7bd4132e..66181e45 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -29,19 +29,23 @@ def __call__(self, t: float) -> SE3:
 
     def visualize(
         self,
-        times: List[float],
+        sample_times: List[float],
         pose_marker_length: float = 0.2,
         animate: bool = False,
         repeat: bool = True,
         ax: Optional[plt.Axes]  = None,
         input_trajectory: Optional[List[SE3]] = None,
     ) -> None:
-        """Displays an animation of the trajectory with the control poses against an optional input trajectory."""
+        """Displays an animation of the trajectory with the control poses against an optional input trajectory.
+        
+        Args:
+            times: which times to sample the spline at and plot
+        """
         if ax is None:
             fig = plt.figure(figsize=(10, 10))
             ax = fig.add_subplot(projection="3d")
 
-        samples = [self(t) for t in times]
+        samples = [self(t) for t in sample_times]
         if not animate:
             pos = np.array([pose.t for pose in samples])
             ax.plot(pos[:,0], pos[:,1], pos[:,2], "c", linewidth=1.0)  # plot spline fit

From 8dbc0595a2fc2e0acb3b79c25d4a9dc18634c3b0 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Mon, 21 Oct 2024 09:52:17 -0400
Subject: [PATCH 19/21] Run the black formatter.

---
 spatialmath/spline.py | 100 ++++++++++++++++++++++++++----------------
 1 file changed, 62 insertions(+), 38 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 66181e45..bccdcf81 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -19,7 +19,6 @@
 
 
 class SplineSE3(ABC):
-
     def __init__(self) -> None:
         self.control_poses: SE3
 
@@ -33,11 +32,11 @@ def visualize(
         pose_marker_length: float = 0.2,
         animate: bool = False,
         repeat: bool = True,
-        ax: Optional[plt.Axes]  = None,
+        ax: Optional[plt.Axes] = None,
         input_trajectory: Optional[List[SE3]] = None,
     ) -> None:
         """Displays an animation of the trajectory with the control poses against an optional input trajectory.
-        
+
         Args:
             times: which times to sample the spline at and plot
         """
@@ -48,30 +47,39 @@ def visualize(
         samples = [self(t) for t in sample_times]
         if not animate:
             pos = np.array([pose.t for pose in samples])
-            ax.plot(pos[:,0], pos[:,1], pos[:,2], "c", linewidth=1.0)  # plot spline fit
+            ax.plot(
+                pos[:, 0], pos[:, 1], pos[:, 2], "c", linewidth=1.0
+            )  # plot spline fit
 
         pos = np.array([pose.t for pose in self.control_poses])
-        ax.plot(pos[:,0], pos[:,1], pos[:,2], "r*")  # plot control_poses
+        ax.plot(pos[:, 0], pos[:, 1], pos[:, 2], "r*")  # plot control_poses
 
         if input_trajectory is not None:
             pos = np.array([pose.t for pose in input_trajectory])
-            ax.plot(pos[:,0], pos[:,1], pos[:,2], "go", fillstyle="none")  # plot compare to input poses
+            ax.plot(
+                pos[:, 0], pos[:, 1], pos[:, 2], "go", fillstyle="none"
+            )  # plot compare to input poses
 
         if animate:
-            tranimate(samples, length=pose_marker_length, wait=True, repeat = repeat)  # animate pose along trajectory
+            tranimate(
+                samples, length=pose_marker_length, wait=True, repeat=repeat
+            )  # animate pose along trajectory
         else:
             plt.show()
 
+
 class InterpSplineSE3(SplineSE3):
     """Class for an interpolated trajectory in SE3, as a function of time, through control_poses with a cubic spline.
 
     A combination of scipy.interpolate.CubicSpline and scipy.spatial.transform.RotationSpline (itself also cubic)
     under the hood.
     """
+
     _e = 1e-12
+
     def __init__(
         self,
-        timepoints: List[float], 
+        timepoints: List[float],
         control_poses: List[SE3],
         *,
         normalize_time: bool = False,
@@ -95,11 +103,13 @@ def __init__(
         self.timepoints = np.array(timepoints)
 
         if self.timepoints[-1] < self._e:
-            raise ValueError("Difference between start and end timepoints is less than {self._e}")
-        
+            raise ValueError(
+                "Difference between start and end timepoints is less than {self._e}"
+            )
+
         if len(self.control_poses) != len(self.timepoints):
             raise ValueError("Length of control_poses and timepoints must be equal.")
-        
+
         if len(self.timepoints) < 2:
             raise ValueError("Need at least 2 data points to make a trajectory.")
 
@@ -108,14 +118,17 @@ def __init__(
             self.timepoints = self.timepoints / self.timepoints[-1]
 
         self.spline_xyz = CubicSpline(
-            self.timepoints, 
-            np.array([pose.t for pose in self.control_poses]), 
-            bc_type=bc_type
+            self.timepoints,
+            np.array([pose.t for pose in self.control_poses]),
+            bc_type=bc_type,
+        )
+        self.spline_so3 = RotationSpline(
+            self.timepoints,
+            Rotation.from_matrix(np.array([(pose.R) for pose in self.control_poses])),
         )
-        self.spline_so3 = RotationSpline(self.timepoints, Rotation.from_matrix(np.array([(pose.R) for pose in self.control_poses])))
 
     def __call__(self, t: float) -> SE3:
-        """Compute function value at t. 
+        """Compute function value at t.
         Return:
             pose: SE3
         """
@@ -123,16 +136,18 @@ def __call__(self, t: float) -> SE3:
 
     def derivative(self, t: float) -> Twist3:
         linear_vel = self.spline_xyz.derivative()(t)
-        angular_vel = self.spline_so3(t, 1) #1 is angular rate, 2 is angular acceleration
+        angular_vel = self.spline_so3(
+            t, 1
+        )  # 1 is angular rate, 2 is angular acceleration
         return Twist3(linear_vel, angular_vel)
 
 
 class SplineFit:
-    """ A general class to fit various SE3 splines to data.
-    """
+    """A general class to fit various SE3 splines to data."""
+
     def __init__(
-        self, 
-        time_data: List[float], 
+        self,
+        time_data: List[float],
         pose_data: List[SE3],
     ) -> None:
         self.time_data = time_data
@@ -144,26 +159,26 @@ def __init__(
         self.spline: Optional[SplineSE3] = None
 
     def stochastic_downsample_interpolation(
-        self, 
-        epsilon_xyz: float = 1e-3, 
+        self,
+        epsilon_xyz: float = 1e-3,
         epsilon_angle: float = 1e-1,
         normalize_time: bool = True,
         bc_type: str = "not-a-knot",
     ) -> Tuple[InterpSplineSE3, List[int]]:
         """
-        Uses a random dropout heuristic to downsample a trajectory with an interpolated spline. 
+        Uses a random dropout heuristic to downsample a trajectory with an interpolated spline.
 
-        This code does not ensure the global fit is within epsilon_xyz and epsilon_angle. 
+        This code does not ensure the global fit is within epsilon_xyz and epsilon_angle.
 
             Return:
-                downsampled interpolating spline, 
+                downsampled interpolating spline,
                 list of removed indices from input data
         """
         spline = InterpSplineSE3(
-            self.time_data, 
-            self.pose_data, 
-            normalize_time = normalize_time, 
-            bc_type=bc_type
+            self.time_data,
+            self.pose_data,
+            normalize_time=normalize_time,
+            bc_type=bc_type,
         )
         chosen_indices: set[int] = set()
         interpolation_indices = list(range(len(self.pose_data)))
@@ -178,20 +193,31 @@ def stochastic_downsample_interpolation(
             chosen_indices.add(index)
             interpolation_indices.remove(index)
 
-            spline.spline_xyz = CubicSpline(self.time_data[interpolation_indices], self.xyz_data[interpolation_indices])
+            spline.spline_xyz = CubicSpline(
+                self.time_data[interpolation_indices],
+                self.xyz_data[interpolation_indices],
+            )
             spline.spline_so3 = RotationSpline(
-                self.time_data[interpolation_indices], self.so3_data[interpolation_indices]
+                self.time_data[interpolation_indices],
+                self.so3_data[interpolation_indices],
             )
 
             time = self.time_data[index]
-            angular_error = SO3(self.pose_data[index]).angdist(SO3(spline.spline_so3(time).as_matrix()))
-            euclidean_error = np.linalg.norm(self.pose_data[index].t - spline.spline_xyz(time))
+            angular_error = SO3(self.pose_data[index]).angdist(
+                SO3(spline.spline_so3(time).as_matrix())
+            )
+            euclidean_error = np.linalg.norm(
+                self.pose_data[index].t - spline.spline_xyz(time)
+            )
             if (angular_error > epsilon_angle) or (euclidean_error > epsilon_xyz):
-                interpolation_indices.insert(int(np.searchsorted(interpolation_indices, index, side="right")), index)
+                interpolation_indices.insert(
+                    int(np.searchsorted(interpolation_indices, index, side="right")),
+                    index,
+                )
 
         self.spline = spline
         return spline, interpolation_indices
-    
+
     def max_angular_error(self) -> float:
         return np.max(self.angular_errors)
 
@@ -272,5 +298,3 @@ def __call__(self, t: float) -> SE3:
         """
         twist = np.hstack([spline(t) for spline in self.splines])
         return SE3.Exp(twist)
-
-

From 02b1f524c846d258dafcda677e34199b26ce5a66 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Tue, 22 Oct 2024 14:33:20 -0400
Subject: [PATCH 20/21] Small updates.

---
 spatialmath/spline.py | 14 +++++++-------
 tests/test_spline.py  |  6 +++---
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index bccdcf81..71da572d 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -7,7 +7,7 @@
 
 from abc import ABC, abstractmethod
 from functools import cached_property
-from typing import List, Optional, Tuple
+from typing import List, Optional, Tuple, Set
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -29,16 +29,16 @@ def __call__(self, t: float) -> SE3:
     def visualize(
         self,
         sample_times: List[float],
+        input_trajectory: Optional[List[SE3]] = None,
         pose_marker_length: float = 0.2,
         animate: bool = False,
         repeat: bool = True,
         ax: Optional[plt.Axes] = None,
-        input_trajectory: Optional[List[SE3]] = None,
     ) -> None:
         """Displays an animation of the trajectory with the control poses against an optional input trajectory.
 
         Args:
-            times: which times to sample the spline at and plot
+            sample_times: which times to sample the spline at and plot
         """
         if ax is None:
             fig = plt.figure(figsize=(10, 10))
@@ -180,10 +180,10 @@ def stochastic_downsample_interpolation(
             normalize_time=normalize_time,
             bc_type=bc_type,
         )
-        chosen_indices: set[int] = set()
+        chosen_indices: Set[int] = set()
         interpolation_indices = list(range(len(self.pose_data)))
-        interpolation_indices.remove(0)
-        interpolation_indices.remove(len(self.pose_data) - 1)
+        chosen_indices.add(0)
+        chosen_indices.add(len(self.pose_data) - 1)
 
         for _ in range(len(self.time_data) - 2):  # you must have at least 2 indices
             choices = list(set(interpolation_indices).difference(chosen_indices))
@@ -263,7 +263,7 @@ def __init__(
         - degree: int that controls degree of the polynomial that governs any given point on the spline.
         - knots: list of floats that govern which control points are active during evaluating the spline
         at a given t input. If none, they are automatically, uniformly generated based on number of control poses and
-        degree of spline.
+        degree of spline on the range [0,1].
         """
         super().__init__()
         self.control_poses = control_poses
diff --git a/tests/test_spline.py b/tests/test_spline.py
index 1af623f6..4da63501 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -27,7 +27,7 @@ def test_evaluation(self):
 
     def test_visualize(self):
         spline = BSplineSE3(self.control_poses)
-        spline.visualize(np.linspace(0, 1.0, 100), animate=True, repeat=False)
+        spline.visualize(sample_times= np.linspace(0, 1.0, 100), animate=True, repeat=False)
 
 class TestInterpSplineSE3:
     waypoints = [
@@ -62,7 +62,7 @@ def test_small_delta_t(self):
 
     def test_visualize(self):
         spline = InterpSplineSE3(self.times, self.waypoints)
-        spline.visualize(np.linspace(0, self.time_horizon, 100), animate=True, repeat=False)
+        spline.visualize(sample_times= np.linspace(0, self.time_horizon, 100), animate=True, repeat=False)
         
 
 class TestSplineFit:
@@ -89,4 +89,4 @@ def test_spline_fit(self):
         
         assert( fit.max_angular_error() < np.deg2rad(5.0) )
         assert( fit.max_angular_error() < 0.1 )
-        spline.visualize(np.linspace(0, self.time_horizon, 100), animate=True, repeat=False)
\ No newline at end of file
+        spline.visualize(sample_times= np.linspace(0, self.time_horizon, 100), animate=True, repeat=False)
\ No newline at end of file

From bf284792dbf0e470bc9c4af6d94b65900be70af1 Mon Sep 17 00:00:00 2001
From: Mark Yeatman <myeatman@theaiinstitute.com>
Date: Wed, 23 Oct 2024 11:54:32 -0400
Subject: [PATCH 21/21] Found bug in spline fit code.

---
 spatialmath/spline.py | 94 +++++++++++++++++++++----------------------
 tests/test_spline.py  |  5 ++-
 2 files changed, 50 insertions(+), 49 deletions(-)

diff --git a/spatialmath/spline.py b/spatialmath/spline.py
index 71da572d..0a472ecc 100644
--- a/spatialmath/spline.py
+++ b/spatialmath/spline.py
@@ -152,10 +152,6 @@ def __init__(
     ) -> None:
         self.time_data = time_data
         self.pose_data = pose_data
-
-        self.xyz_data = np.array([pose.t for pose in pose_data])
-        self.so3_data = Rotation.from_matrix(np.array([(pose.R) for pose in pose_data]))
-
         self.spline: Optional[SplineSE3] = None
 
     def stochastic_downsample_interpolation(
@@ -164,64 +160,67 @@ def stochastic_downsample_interpolation(
         epsilon_angle: float = 1e-1,
         normalize_time: bool = True,
         bc_type: str = "not-a-knot",
+        check_type: str = "local"
     ) -> Tuple[InterpSplineSE3, List[int]]:
         """
-        Uses a random dropout heuristic to downsample a trajectory with an interpolated spline.
-
-        This code does not ensure the global fit is within epsilon_xyz and epsilon_angle.
+        Uses a random dropout to downsample a trajectory with an interpolated spline. Keeps the start and 
+        end points of the trajectory. Takes a random order of the remaining indices, and then checks the error bound 
+        of just that point if check_type=="local", checks the error of the whole trajectory is check_type=="global".
+        Local is **much** faster.
 
             Return:
                 downsampled interpolating spline,
                 list of removed indices from input data
         """
-        spline = InterpSplineSE3(
-            self.time_data,
-            self.pose_data,
-            normalize_time=normalize_time,
-            bc_type=bc_type,
-        )
-        chosen_indices: Set[int] = set()
-        interpolation_indices = list(range(len(self.pose_data)))
-        chosen_indices.add(0)
-        chosen_indices.add(len(self.pose_data) - 1)
-
-        for _ in range(len(self.time_data) - 2):  # you must have at least 2 indices
-            choices = list(set(interpolation_indices).difference(chosen_indices))
 
-            index = np.random.choice(choices)
-
-            chosen_indices.add(index)
-            interpolation_indices.remove(index)
+        interpolation_indices = list(range(len(self.pose_data)))
 
-            spline.spline_xyz = CubicSpline(
-                self.time_data[interpolation_indices],
-                self.xyz_data[interpolation_indices],
-            )
-            spline.spline_so3 = RotationSpline(
-                self.time_data[interpolation_indices],
-                self.so3_data[interpolation_indices],
+        # randomly attempt to remove poses from the trajectory 
+        # always keep the start and end
+        removal_choices = interpolation_indices.copy()
+        removal_choices.remove(0)
+        removal_choices.remove(len(self.pose_data) - 1)
+        np.random.shuffle(removal_choices)
+        for candidate_removal_index in removal_choices:
+            interpolation_indices.remove(candidate_removal_index)
+
+            self.spline = InterpSplineSE3(
+                [self.time_data[i] for i in interpolation_indices],
+                [self.pose_data[i] for i in interpolation_indices],
+                normalize_time=normalize_time,
+                bc_type=bc_type,
             )
 
-            time = self.time_data[index]
-            angular_error = SO3(self.pose_data[index]).angdist(
-                SO3(spline.spline_so3(time).as_matrix())
-            )
-            euclidean_error = np.linalg.norm(
-                self.pose_data[index].t - spline.spline_xyz(time)
-            )
-            if (angular_error > epsilon_angle) or (euclidean_error > epsilon_xyz):
-                interpolation_indices.insert(
-                    int(np.searchsorted(interpolation_indices, index, side="right")),
-                    index,
+            sample_time = self.time_data[candidate_removal_index]
+            if check_type is "local":
+                angular_error = SO3(self.pose_data[candidate_removal_index]).angdist(
+                    SO3(self.spline.spline_so3(sample_time).as_matrix())
+                )
+                euclidean_error = np.linalg.norm(
+                    self.pose_data[candidate_removal_index].t - self.spline.spline_xyz(sample_time)
                 )
+            elif check_type is "global":
+                angular_error = self.max_angular_error()
+                euclidean_error = self.max_euclidean_error()
+            else:
+                raise ValueError(f"check_type must be 'local' of 'global', is {check_type}.")
+            
+            if (angular_error > epsilon_angle) or (euclidean_error > epsilon_xyz):
+                interpolation_indices.append(candidate_removal_index)
+                interpolation_indices.sort()
+
+        self.spline = InterpSplineSE3(
+            [self.time_data[i] for i in interpolation_indices],
+            [self.pose_data[i] for i in interpolation_indices],
+            normalize_time=normalize_time,
+            bc_type=bc_type,
+        )
 
-        self.spline = spline
-        return spline, interpolation_indices
+        return self.spline, interpolation_indices
 
     def max_angular_error(self) -> float:
-        return np.max(self.angular_errors)
+        return np.max(self.angular_errors())
 
-    @cached_property
     def angular_errors(self) -> List[float]:
         return [
             pose.angdist(self.spline(t))
@@ -229,9 +228,8 @@ def angular_errors(self) -> List[float]:
         ]
 
     def max_euclidean_error(self) -> float:
-        return np.max(self.euclidean_errors)
+        return np.max(self.euclidean_errors())
 
-    @cached_property
     def euclidean_errors(self) -> List[float]:
         return [
             np.linalg.norm(pose.t - self.spline(t).t)
diff --git a/tests/test_spline.py b/tests/test_spline.py
index 4da63501..361bc28f 100644
--- a/tests/test_spline.py
+++ b/tests/test_spline.py
@@ -84,8 +84,11 @@ def test_spline_fit(self):
         fit = SplineFit(self.timestamps, self.trajectory)
         spline, kept_indices = fit.stochastic_downsample_interpolation()
 
-        fraction_points_removed = 1.0 - len(kept_indices) / self.num_data_points 
+        fraction_points_removed = 1.0 - len(kept_indices) / self.num_data_points
+        
         assert(fraction_points_removed > 0.2)
+        assert(len(spline.control_poses)==len(kept_indices))
+        assert(len(spline.timepoints)==len(kept_indices))
         
         assert( fit.max_angular_error() < np.deg2rad(5.0) )
         assert( fit.max_angular_error() < 0.1 )