Skip to content

Commit

Permalink
Fix error functions, actually use them in unit test.
Browse files Browse the repository at this point in the history
  • Loading branch information
myeatman-bdai committed Oct 17, 2024
1 parent 3d99327 commit 806726e
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 9 deletions.
10 changes: 5 additions & 5 deletions spatialmath/spline.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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)
]


Expand Down
6 changes: 2 additions & 4 deletions tests/test_spline.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 )

0 comments on commit 806726e

Please sign in to comment.