改为车头向前整链：主从装反机械初值、双天线 pitch/roll 姿态与默认 HeadingOffsetDeg=-90

Co-authored-by: Cursor <cursoragent@cursor.com>

tests/test_rtk_lidar_coordinate_contract.py

@@ -12,8 +12,12 @@ 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 finalize_direct_rtk_lidar import (  # noqa: E402
+    coordinate_contract_audit,
+    mechanical_self_consistency,
+)
 from prepare_multisensor_station_dataset import heading_to_enu_yaw  # noqa: E402
+from rtk_attitude import attitude_rotation, rtk_body_rotation  # noqa: E402
 from rigorous_calibration import (  # noqa: E402
     build_parser,
     load_extrinsic_matrix,
@@ -34,6 +38,32 @@ def test_east_vehicle_heading_maps_to_zero_enu_yaw() -> None:
     assert math.degrees(yaw) == 0.0
 
 
+def test_attitude_rotation_applies_baseline_pitch_elevation() -> None:
+    _, yaw = heading_to_enu_yaw(0.0, 0.0)  # heading north → body X = +North
+    rotation = attitude_rotation(yaw, pitch_deg=10.0, roll_deg=0.0)
+    body_x = rotation @ np.array([1.0, 0.0, 0.0])
+    np.testing.assert_allclose(
+        body_x,
+        [0.0, math.cos(math.radians(10.0)), math.sin(math.radians(10.0))],
+        atol=1e-12,
+    )
+
+
+def test_vehicle_forward_offset_keeps_pitch_about_baseline() -> None:
+    # Baseline points east (vehicle right if nose north); pitch elevates baseline X.
+    # Vehicle-forward offset -90 must not simply Ry after vehicle yaw.
+    raw_heading = 90.0
+    pitch = 10.0
+    r_correct = rtk_body_rotation(raw_heading, -90.0, pitch_deg=pitch, roll_deg=0.0)
+    _, yaw_raw = heading_to_enu_yaw(raw_heading, 0.0)
+    r_baseline = attitude_rotation(yaw_raw, pitch_deg=pitch, roll_deg=0.0)
+    rz90 = np.array([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
+    np.testing.assert_allclose(r_correct, r_baseline @ rz90, atol=1e-12)
+    # Level vehicle-forward X should point north.
+    r_level = rtk_body_rotation(raw_heading, -90.0, pitch_deg=0.0, roll_deg=0.0)
+    np.testing.assert_allclose(r_level @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-12)
+
+
 def test_pair_registration_has_no_extrinsic_argument() -> None:
     parser = build_parser()
     pair_options = {
@@ -45,18 +75,50 @@ def test_pair_registration_has_no_extrinsic_argument() -> None:
     assert "--global-voxel" in pair_options
 
 
-def test_mechanical_initial_round_trip() -> None:
+def test_mechanical_initial_is_vehicle_forward_swapped_master() -> None:
     path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
+    document = __import__("json").loads(path.read_text(encoding="utf-8-sig"))
     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(transform[:3, 3], [0.210859360, -0.414179474, 0.078500001])
+    np.testing.assert_allclose(transform[:3, :3], np.eye(3))
     np.testing.assert_allclose(params_transform(transform_params(transform)), transform, atol=1e-12)
+    assert document["baseline_points"] == "vehicle_right"
+    assert document["frame_mode"] == "vehicle_forward_heading_offset"
+    assert document["heading_offset_deg"] == -90.0
+    assert document["rotation_rpy_deg_xyz"][2] == 0.0
+    check = mechanical_self_consistency(document)
+    assert check["consistent"] is True
+
+
+def test_mixed_left_xy_plus_right_yaw_mechanical_is_rejected() -> None:
+    mixed = {
+        "baseline_points": "vehicle_left",
+        "translation_m": [0.414179474, 0.210859360, 0.078500001],
+        "rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
+        "matrix_4x4": [
+            [0.0, -1.0, 0.0, 0.414179474],
+            [1.0, 0.0, 0.0, 0.210859360],
+            [0.0, 0.0, 1.0, 0.078500001],
+            [0.0, 0.0, 0.0, 1.0],
+        ],
+    }
+    check = mechanical_self_consistency(mixed)
+    assert check["consistent"] is False
+
+
+def test_deprecated_minus_xy_right_baseline_is_rejected_for_swapped_master() -> None:
+    deprecated = {
+        "baseline_points": "vehicle_right",
+        "translation_m": [-0.414179474, -0.210859360, 0.078500001],
+        "rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
+    }
+    check = mechanical_self_consistency(deprecated)
+    assert check["consistent"] is False
 
 
 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]
@@ -66,3 +128,26 @@ def test_near_180_degree_solution_is_flagged_for_physical_axis_check() -> None:
     })
     assert audit["status"] == "near_180_degree_axis_conflict"
     assert audit["requires_physical_axis_confirmation"] is True
+
+
+def test_previous_mixed_result_branch_is_not_recommended() -> None:
+    """Old baseline-frame mixed solution disagrees with vehicle-forward mechanical initial."""
+    initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
+    solution = np.array(
+        [
+            [0.00942353438668686, -0.9999215926659111, 0.00824654595155475, 0.4123055815579212],
+            [0.9998714355322929, 0.009529416150714898, 0.012895837871912157, 0.2173092104098051],
+            [-0.012973411511822136, 0.008123961367132958, 0.9998828390593821, 0.10405760639434848],
+            [0.0, 0.0, 0.0, 1.0],
+        ],
+        float,
+    )
+    audit = coordinate_contract_audit({
+        "solver_initial_extrinsic": str(initial_path),
+        "matrix_4x4": solution.tolist(),
+    })
+    assert audit["requires_physical_axis_confirmation"] is True
+    assert audit["status"] in {
+        "near_180_degree_axis_conflict",
+        "solution_disagrees_with_mechanical_baseline_side",
+    }