修正基线系标定默认：机械初值、地面ROI与航向偏移可配，并补充G90窗导出与契约测试

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-10 22:28:25 +08:00
parent 69bb44bccd
commit 46d2fa1d69
14 changed files with 932 additions and 38 deletions
@@ -0,0 +1,116 @@
+"""Regression tests for G90 GNHPR parsing and host-time LiDAR association."""
+
+from __future__ import annotations
+
+import json
+import sys
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+TOOLS = ROOT / "tools"
+CODE = ROOT / "code"
+sys.path.insert(0, str(TOOLS))
+sys.path.insert(0, str(TOOLS / "rscap_v2"))
+sys.path.insert(0, str(CODE))
+
+from build_multisensor_npz import build_combined  # noqa: E402
+from pipeline_common import parse_gnhpr  # noqa: E402
+from rigorous_calibration import load_npz_xyz  # noqa: E402
+
+
+def test_parse_gnhpr_fixed_heading():
+    row = parse_gnhpr("$GNHPR,070411.40,354.7437,000.2518,000.0000,4,26,0.00,0999*58")
+    assert row["type"] == "GNHPR"
+    assert row["raw_heading_deg"] == 354.7437
+    assert row["pitch_deg"] == 0.2518
+    assert row["roll_deg"] == 0.0
+    assert row["heading_quality"] == 4
+    assert row["satellites"] == 26
+    assert row["heading_valid"] is True
+
+
+def test_host_time_uses_lidar_receive_time_and_preserves_device_time(tmp_path: Path):
+    host_ns = 1_786_240_000_000_000_000
+    device_ns = 1_500_000_000_000_000_000
+    frame_dir = tmp_path / "lidar"
+    frame_dir.mkdir()
+    np.savez_compressed(
+        frame_dir / "frame.npz",
+        points=np.zeros((4, 4), dtype=np.float32),
+        unix_time_ns=np.asarray([device_ns], dtype=np.int64),
+        host_receive_utc_ns=np.asarray([host_ns], dtype=np.int64),
+    )
+
+    rtk = tmp_path / "rtk.jsonl"
+    rows = [
+        {
+            "type": "GGA",
+            "checksum_valid": True,
+            "host_receive_utc_ns": host_ns + 20_000_000,
+            "lat_deg": 31.0,
+            "lon_deg": 121.0,
+            "altitude_m": 10.0,
+            "fix_quality": 4,
+            "satellites": 20,
+            "raw_line": "$GNGGA,...",
+        },
+        {
+            "type": "GNHPR",
+            "checksum_valid": True,
+            "host_receive_utc_ns": host_ns - 10_000_000,
+            "raw_heading_deg": 90.0,
+            "pitch_deg": 1.0,
+            "roll_deg": 0.0,
+            "heading_quality": 4,
+            "heading_solution": "GNHPR_QUALITY_4",
+            "heading_valid": True,
+            "satellites": 22,
+            "raw_line": "$GNHPR,...",
+        },
+    ]
+    rtk.write_text("".join(json.dumps(row) + "\n" for row in rows), encoding="utf-8")
+    imu = tmp_path / "imu.jsonl"
+    imu.write_text("", encoding="utf-8")
+
+    out = tmp_path / "combined"
+    summary = build_combined(
+        [("STATION-01", frame_dir)],
+        [rtk],
+        [imu],
+        out,
+        time_basis="host",
+        rtk_max_dt_ms=100.0,
+    )
+
+    assert summary["frames"] == 1
+    assert summary["rtk_valid"] == 1
+    assert summary["heading_valid"] == 1
+    assert summary["rtk_fixed"] == 1
+    with np.load(next((out / "frames").glob("*.npz")), allow_pickle=False) as frame:
+        assert int(frame["lidar_association_time_ns"][0]) == host_ns
+        assert int(frame["unix_time_ns"][0]) == device_ns
+        assert int(frame["rtk_gga_dt_ns"][0]) == 20_000_000
+        assert int(frame["rtk_heading_dt_ns"][0]) == -10_000_000
+
+
+def test_registration_prefers_lidar_association_time(tmp_path: Path):
+    host_ns = 1_786_240_000_000_000_000
+    device_ns = 1_500_000_000_000_000_000
+    source = tmp_path / "frame.npz"
+    np.savez_compressed(
+        source,
+        points_raw=np.asarray(
+            [[1000.0, 0.0, 0.0, 1.0], [2000.0, 90.0, 0.0, 1.0]],
+            dtype=np.float32,
+        ),
+        unix_time_ns=np.asarray([device_ns], dtype=np.int64),
+        lidar_association_time_ns=np.asarray([host_ns], dtype=np.int64),
+        frame_counter=np.asarray([7], dtype=np.int64),
+    )
+
+    timestamp, counter, xyz = load_npz_xyz(source)
+    assert timestamp == host_ns / 1e9
+    assert counter == 7
+    assert xyz.shape == (2, 3)
@@ -0,0 +1,68 @@
+"""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