mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 22:06:37 +00:00
Yuzviak
46 lines
1.4 KiB
Python
46 lines
1.4 KiB
Python
"""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)
|