Fix error functions, actually use them in unit test.

myeatman-bdai · Oct 24, 2024 · Oct 16, 2024 · Oct 17, 2024 · Oct 17, 2024 · Oct 17, 2024
commit 806726ee59a981dd40a107350365f483b7b71c40
@@ -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)
         ]
 
 

@@ -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 )
+    assert( fit.max_angular_error() < np.deg2rad(5.0) )
+    assert( fit.max_angular_error() < 0.1 )