test(e2e): add trajectory RMSE/ATE/RPE metrics

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

tests/e2e/test_metrics.py

@@ -0,0 +1,45 @@
+"""Trajectory comparison metrics."""
+
+import numpy as np
+import pytest
+
+from gps_denied.testing.metrics import (
+    trajectory_rmse,
+    absolute_trajectory_error,
+    relative_pose_error,
+)
+
+
+def test_rmse_identical_trajectories_is_zero():
+    est = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
+    gt = est.copy()
+    assert trajectory_rmse(est, gt) == pytest.approx(0.0)
+
+
+def test_rmse_constant_offset():
+    # Constant 1m offset in x
+    est = np.array([[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
+    gt = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]])
+    assert trajectory_rmse(est, gt) == pytest.approx(1.0)
+
+
+def test_rmse_rejects_length_mismatch():
+    est = np.zeros((3, 3))
+    gt = np.zeros((4, 3))
+    with pytest.raises(ValueError):
+        trajectory_rmse(est, gt)
+
+
+def test_ate_returns_summary_stats():
+    est = np.array([[1.0, 0.0, 0.0], [2.0, 0.0, 0.0], [3.0, 0.0, 0.0]])
+    gt = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
+    result = absolute_trajectory_error(est, gt)
+    assert result["rmse"] == pytest.approx(1.0)
+    assert result["mean"] == pytest.approx(1.0)
+    assert result["max"] == pytest.approx(1.0)
+
+
+def test_rpe_identical_trajectories_is_zero():
+    traj = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
+    result = relative_pose_error(traj, traj, delta=1)
+    assert result["rmse"] == pytest.approx(0.0)