69 lines
2.4 KiB
Python
69 lines
2.4 KiB
Python
 
|
"""Regression tests for the RTK–LiDAR coordinate and initialization contract."""
|
|||
|
|
|
|||
|
|
from __future__ import annotations
|
|||
|
|
|
|||
|
|
import math
|
|||
|
|
import sys
|
|||
|
|
from pathlib import Path
|
|||
|
|
|
|||
|
|
import numpy as np
|
|||
|
|
|
|||
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|||
|
|
sys.path.insert(0, str(ROOT / "tools"))
|
|||
|
|
sys.path.insert(0, str(ROOT / "code"))
|
|||
|
|
|
|||
|
|
from finalize_direct_rtk_lidar import coordinate_contract_audit # noqa: E402
|
|||
|
|
from prepare_multisensor_station_dataset import heading_to_enu_yaw # noqa: E402
|
|||
|
|
from rigorous_calibration import ( # noqa: E402
|
|||
|
|
build_parser,
|
|||
|
|
load_extrinsic_matrix,
|
|||
|
|
params_transform,
|
|||
|
|
transform_params,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def test_left_baseline_heading_plus_90_points_vehicle_forward() -> None:
|
|||
|
|
corrected, yaw = heading_to_enu_yaw(270.0, 90.0)
|
|||
|
|
assert corrected == 0.0
|
|||
|
|
assert math.degrees(yaw) == 90.0
|
|||
|
|
|
|||
|
|
|
|||
|
|
def test_east_vehicle_heading_maps_to_zero_enu_yaw() -> None:
|
|||
|
|
corrected, yaw = heading_to_enu_yaw(0.0, 90.0)
|
|||
|
|
assert corrected == 90.0
|
|||
|
|
assert math.degrees(yaw) == 0.0
|
|||
|
|
|
|||
|
|
|
|||
|
|
def test_pair_registration_has_no_extrinsic_argument() -> None:
|
|||
|
|
parser = build_parser()
|
|||
|
|
pair_options = {
|
|||
|
|
option
|
|||
|
|
for action in parser._subparsers._group_actions[0].choices["pairs"]._actions
|
|||
|
|
for option in action.option_strings
|
|||
|
|
}
|
|||
|
|
assert "--initial-extrinsic" not in pair_options
|
|||
|
|
assert "--global-voxel" in pair_options
|
|||
|
|
|
|||
|
|
|
|||
|
|
def test_mechanical_initial_round_trip() -> None:
|
|||
|
|
path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
|
|||
|
|
transform = load_extrinsic_matrix(path)
|
|||
|
|
np.testing.assert_allclose(transform[:3, 3], [0.414179474, 0.210859360, 0.004000001])
|
|||
|
|
np.testing.assert_allclose(transform[:3, :3], [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
|
|||
|
|
np.testing.assert_allclose(params_transform(transform_params(transform)), transform, atol=1e-12)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def test_near_180_degree_solution_is_flagged_for_physical_axis_check() -> None:
|
|||
|
|
initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
|
|||
|
|
initial = load_extrinsic_matrix(initial_path)
|
|||
|
|
# Flip the declared mechanical forward axis by ~180 deg about Z.
|
|||
|
|
solution = np.eye(4)
|
|||
|
|
solution[:3, :3] = initial[:3, :3] @ np.diag([-1.0, -1.0, 1.0])
|
|||
|
|
solution[:3, 3] = initial[:3, 3]
|
|||
|
|
audit = coordinate_contract_audit({
|
|||
|
|
"solver_initial_extrinsic": str(initial_path),
|
|||
|
|
"matrix_4x4": solution.tolist(),
|
|||
|
|
})
|
|||
|
|
assert audit["status"] == "near_180_degree_axis_conflict"
|
|||
|
|
assert audit["requires_physical_axis_confirmation"] is True
|