重构RTK-IMU标定链路并完成机械先验工程验证

tests/test_rtk_imu_calibration.py

@@ -7,7 +7,7 @@ import numpy as np
 from imu_lidar.geodesy import geodetic_to_enu
 from imu_lidar.geometry import so3_exp
 from imu_lidar.imu_preintegration import preintegrate_gyro, preintegrate_imu
-from imu_lidar.rtk_attitude import gnhpr_to_rotation_enu_rtk
+from imu_lidar.rtk_attitude import gnhpr_to_baseline_enu, gnhpr_to_rotation_enu_rtk
 from imu_lidar.rtk_imu_translation import _preintegrate_translation_interval
 from imu_lidar.rtk_io import load_rtk_csv
 
@@ -34,6 +34,22 @@ def test_gnhpr_heading_maps_north_clockwise_into_enu() -> None:
     assert np.allclose(rotations[1][:, 0], [1.0, 0.0, 0.0], atol=1e-12)
 
 
+def test_gnhpr_baseline_is_main_to_secondary_and_ignores_roll() -> None:
+    baseline = gnhpr_to_baseline_enu(
+        np.array([0.0, 90.0]),
+        np.array([30.0, 0.0]),
+    )
+    assert np.allclose(baseline[0], [0.0, np.sqrt(0.75), 0.5], atol=1e-12)
+    assert np.allclose(baseline[1], [1.0, 0.0, 0.0], atol=1e-12)
+
+    rotations = gnhpr_to_rotation_enu_rtk(
+        np.array([90.0, 90.0]),
+        np.zeros(2),
+        np.array([0.0, 45.0]),
+    )
+    assert np.allclose(rotations[0], rotations[1], atol=1e-12)
+
+
 def test_rtk_loader_uses_hpr_measurement_time_for_attitude(tmp_path: Path) -> None:
     path = tmp_path / "rtk.csv"
     path.write_text(
@@ -49,6 +65,21 @@ def test_rtk_loader_uses_hpr_measurement_time_for_attitude(tmp_path: Path) -> No
     assert np.all(loaded.attitude_valid)
 
 
+def test_rtk_loader_accepts_only_checksum_valid_fixed_hpr(tmp_path: Path) -> None:
+    path = tmp_path / "rtk.csv"
+    path.write_text(
+        "t,lat_deg,lon_deg,altitude_m,fix_quality,heading_deg,pitch_deg,roll_deg,"
+        "heading_quality,checksum_valid\n"
+        "0.0,30.0,114.0,20.0,4,10.0,0.0,0.0,4,1\n"
+        "0.1,30.0,114.0,20.0,4,11.0,0.0,0.0,5,1\n"
+        "0.2,30.0,114.0,20.0,4,12.0,0.0,0.0,4,0\n",
+        encoding="utf-8",
+    )
+    loaded = load_rtk_csv(path)
+    assert loaded.attitude_valid.tolist() == [True, False, False]
+    assert loaded.attitude_float.tolist() == [False, True, False]
+
+
 def test_fast_local_preintegration_matches_reference() -> None:
     times = np.linspace(0.0, 1.0, 101)
     gyro = np.tile(np.array([0.03, -0.02, 0.15]), (times.size, 1))