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

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

code/finalize_direct_rtk_lidar.py

@@ -15,7 +15,17 @@ def load(path: Path) -> dict:
 
 def write(path: Path, document: dict) -> None:
     path.parent.mkdir(parents=True, exist_ok=True)
-    path.write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
+
+    def default(obj):
+        if isinstance(obj, (np.bool_, np.integer)):
+            return obj.item()
+        if isinstance(obj, np.floating):
+            return float(obj)
+        if isinstance(obj, np.ndarray):
+            return obj.tolist()
+        raise TypeError(f"Object of type {type(obj).__name__} is not JSON serializable")
+
+    path.write_text(json.dumps(document, ensure_ascii=False, indent=2, default=default), encoding="utf-8")
 
 
 def inverse(t: np.ndarray) -> np.ndarray:
@@ -34,12 +44,127 @@ def delta(a: np.ndarray, b: np.ndarray) -> dict:
     }
 
 
+def wrap180(deg: float) -> float:
+    return (deg + 180.0) % 360.0 - 180.0
+
+
+def yaw_deg_of(transform: np.ndarray) -> float:
+    return float(Rotation.from_matrix(transform[:3, :3]).as_euler("xyz", degrees=True)[2])
+
+
+def mechanical_self_consistency(document: dict) -> dict:
+    """Reject mechanical JSON that mixes incompatible baseline / body definitions."""
+    translation = np.asarray(document["translation_m"], float)
+    yaw = float(document["rotation_rpy_deg_xyz"][2])
+    side = str(document.get("baseline_points", "")).strip().lower()
+    frame_mode = str(document.get("frame_mode", "")).strip().lower()
+    heading_offset = float(document.get("heading_offset_deg", 0.0) or 0.0)
+    vehicle_forward = (
+        frame_mode == "vehicle_forward_heading_offset"
+        or abs(heading_offset) > 1e-6
+    )
+    issues: list[str] = []
+
+    if vehicle_forward:
+        if abs(wrap180(yaw)) > 15.0:
+            issues.append(
+                f"vehicle-forward mechanical initial requires yaw≈0°, got {yaw:g}°"
+            )
+        lever = document.get("vehicle_flu_lever_master_to_lidar_m")
+        if lever is not None:
+            if float(np.linalg.norm(translation - np.asarray(lever, float))) > 0.05:
+                issues.append(
+                    "vehicle-forward translation_m must match vehicle_flu_lever_master_to_lidar_m"
+                )
+        if abs(heading_offset + 90.0) > 1e-6 and abs(heading_offset - 90.0) > 1e-6:
+            issues.append(
+                f"vehicle-forward heading_offset_deg should be ±90 for left/right baseline, got {heading_offset:g}"
+            )
+    elif side in {"vehicle_left", "left"}:
+        if abs(wrap180(yaw - (-90.0))) > 15.0:
+            issues.append(
+                f"baseline_points=vehicle_left requires yaw≈-90°, got {yaw:g}°"
+            )
+        if translation[0] <= 0.0 or translation[1] <= 0.0:
+            issues.append(
+                "baseline_points=vehicle_left expects +X/+Y lever in RTK baseline frame "
+                f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
+            )
+    elif side in {"vehicle_right", "right"}:
+        if abs(wrap180(yaw - 90.0)) > 15.0:
+            issues.append(
+                f"baseline_points=vehicle_right requires yaw≈+90°, got {yaw:g}°"
+            )
+        # Swapped but centerline-symmetric master (vehicle left): +X / -Y in baseline frame.
+        if translation[0] <= 0.0 or translation[1] >= 0.0:
+            issues.append(
+                "baseline_points=vehicle_right (master on vehicle left, baseline to the right) "
+                "expects +X/-Y lever in RTK baseline frame "
+                f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
+            )
+    else:
+        left_xy = translation[0] > 0.05 and translation[1] > 0.05
+        right_xy = translation[0] < -0.05 and translation[1] < -0.05
+        swapped_right_xy = translation[0] > 0.05 and translation[1] < -0.05
+        if left_xy and abs(wrap180(yaw - 90.0)) <= 15.0:
+            issues.append(
+                "mixed baseline definition: +X/+Y translation (left-baseline) combined with yaw≈+90° (right-baseline)"
+            )
+        if right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
+            issues.append(
+                "mixed baseline definition: -X/-Y translation combined with yaw≈-90°"
+            )
+        if swapped_right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
+            issues.append(
+                "mixed baseline definition: +X/-Y translation (swapped-master right-baseline) "
+                "combined with yaw≈-90° (left-baseline)"
+            )
+
+    return {
+        "baseline_points": side or None,
+        "frame_mode": frame_mode or None,
+        "heading_offset_deg": heading_offset,
+        "consistent": not issues,
+        "issues": issues,
+    }
+
+
+def solution_matches_declared_side(solution: np.ndarray, document: dict) -> dict:
+    """Check whether the solved extrinsic agrees with the mechanical baseline side."""
+    side = str(document.get("baseline_points", "")).strip().lower()
+    yaw = yaw_deg_of(solution)
+    t = solution[:3, 3]
+    expected_yaw = float(document["rotation_rpy_deg_xyz"][2])
+    yaw_err = abs(wrap180(yaw - expected_yaw))
+    xy_err = float(np.linalg.norm(t[:2] - np.asarray(document["translation_m"][:2], float)))
+    z_err = float(abs(t[2] - float(document["translation_m"][2])))
+
+    opposite_yaw = abs(wrap180(yaw - expected_yaw) - 180.0) <= 15.0 or abs(
+        wrap180(yaw - expected_yaw) + 180.0
+    ) <= 15.0
+    # Same XY sign as mechanical but yaw flipped ~180° (classic mixed inheritance).
+    same_xy_sign = (t[0] * float(document["translation_m"][0]) > 0.0) and (
+        t[1] * float(document["translation_m"][1]) > 0.0
+    )
+    mixed_inheritance = same_xy_sign and opposite_yaw
+
+    return {
+        "baseline_points": side or None,
+        "solution_yaw_deg": yaw,
+        "expected_yaw_deg": expected_yaw,
+        "yaw_error_deg": yaw_err,
+        "xy_error_m": xy_err,
+        "z_error_m": z_err,
+        "mixed_translation_rotation_inheritance": bool(mixed_inheritance),
+        "near_expected_pose": bool(yaw_err <= 15.0 and xy_err <= 0.25),
+    }
+
+
 def coordinate_contract_audit(raw: dict) -> dict:
-    """Compare the data-driven solution with the declared mechanical initial.
+    """Audit mechanical self-consistency and solution agreement.
 
     A near-180-degree disagreement is not auto-corrected: it normally means
-    that one physical forward-axis statement is reversed. Silently rotating
-    the point cloud would preserve residuals while changing the frame contract.
+    that one physical forward-axis / baseline-direction statement is reversed.
     """
     path_text = raw.get("solver_initial_extrinsic")
     if not path_text:
@@ -59,14 +184,29 @@ def coordinate_contract_audit(raw: dict) -> dict:
     solution = np.asarray(raw["matrix_4x4"], float)
     comparison = delta(initial, solution)
     near_180 = abs(comparison["rotation_deg"] - 180.0) <= 15.0
+    mech_check = mechanical_self_consistency(initial_document)
+    match = solution_matches_declared_side(solution, initial_document)
+
+    if not mech_check["consistent"]:
+        status = "mechanical_initial_inconsistent"
+    elif match["mixed_translation_rotation_inheritance"] or near_180:
+        status = "near_180_degree_axis_conflict"
+    elif not match["near_expected_pose"]:
+        status = "solution_disagrees_with_mechanical_baseline_side"
+    else:
+        status = "no_near_180_degree_axis_conflict"
+
+    requires = status != "no_near_180_degree_axis_conflict"
     return {
-        "status": "near_180_degree_axis_conflict" if near_180 else "no_near_180_degree_axis_conflict",
-        "requires_physical_axis_confirmation": near_180,
+        "status": status,
+        "requires_physical_axis_confirmation": requires,
         "mechanical_initial_path": str(path.resolve()),
+        "mechanical_self_consistency": mech_check,
+        "solution_vs_declared_baseline_side": match,
         "solution_relative_to_mechanical_initial": comparison,
         "note": (
-            "No automatic 180-degree point-cloud flip was applied. Confirm the Helios "
-            "aviation-connector side and the G90 vehicle-forward definition before deployment."
+            "No automatic 180-degree correction was applied. Confirm static GNHPR "
+            "left/right vs vehicle heading and Helios +X vs vehicle forward before deployment."
         ),
     }
 
@@ -87,12 +227,18 @@ def corrected(raw: dict, backend: str, reference_height: float, heading_offset_d
             "RTK": {
                 "origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
                 "x_axis": x_axis,
-                "y_axis": "left",
+                "y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
                 "z_axis": "up",
                 "yaw_enu_deg": f"90 - (rawHeadingDeg + {heading_offset_deg:g})",
                 "frame_mode": "baseline_raw_heading" if baseline_frame else "vehicle_forward_heading_offset",
             },
-            "LiDAR": "raw LiDAR sensor frame",
+            "LiDAR": {
+                "description": "raw Helios sensor frame from points_raw polar decode",
+                "x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
+                "y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
+                "z_axis": "up",
+                "origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ",
+            },
         },
         "backend": backend,
         "measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")),
@@ -154,18 +300,33 @@ def main() -> None:
     needs_axis_confirmation = bool(
         final["coordinate_contract_audit"]["requires_physical_axis_confirmation"]
     )
+    status = final["coordinate_contract_audit"]["status"]
+    reason_map = {
+        "mechanical_initial_inconsistent": (
+            "Mechanical initial mixes incompatible baseline-left/right translation and yaw; "
+            "fix run/rtk_lidar_mechanical_initial.json before trusting deployment"
+        ),
+        "near_180_degree_axis_conflict": (
+            "Physical axis confirmation is required because the data-driven solution differs "
+            "from the declared mechanical initial by approximately 180 degrees "
+            "(or inherits mixed translation/rotation signs)"
+        ),
+        "solution_disagrees_with_mechanical_baseline_side": (
+            "Solution yaw/XY disagree with the declared mechanical baseline side; "
+            "confirm static GNHPR direction before deployment"
+        ),
+    }
     final["selection"] = {
         "recommended": not needs_axis_confirmation,
         "reason": (
-            "Physical axis confirmation is required because the data-driven solution differs "
-            "from the declared mechanical initial by approximately 180 degrees"
-            if needs_axis_confirmation else
-            "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp"
+            reason_map.get(
+                status,
+                "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
+            )
         ),
         "open3d_vs_small_gicp": delta(open_t, small_t),
     }
 
-
     write(args.result_root / "final_T_RTK_lidar.json", final)
     summary = {
         "final": {