from __future__ import annotations import argparse import json import math from pathlib import Path import numpy as np from scipy.spatial.transform import Rotation def load(path: Path) -> dict: return json.loads(path.read_text(encoding="utf-8-sig")) def write(path: Path, document: dict) -> None: path.parent.mkdir(parents=True, exist_ok=True) 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: result = np.eye(4) result[:3, :3] = t[:3, :3].T result[:3, 3] = -result[:3, :3] @ t[:3, 3] return result def delta(a: np.ndarray, b: np.ndarray) -> dict: d = inverse(a) @ b return { "translation_m": float(np.linalg.norm(d[:3, 3])), "rotation_deg": float(np.linalg.norm(Rotation.from_matrix(d[:3, :3]).as_rotvec()) * 180.0 / math.pi), "delta_matrix_4x4": d.tolist(), } 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: """Audit mechanical self-consistency and solution agreement. A near-180-degree disagreement is not auto-corrected: it normally means that one physical forward-axis / baseline-direction statement is reversed. """ path_text = raw.get("solver_initial_extrinsic") if not path_text: return { "status": "mechanical_initial_not_available", "requires_physical_axis_confirmation": False, } path = Path(path_text) if not path.exists(): return { "status": "mechanical_initial_file_missing", "requires_physical_axis_confirmation": False, "mechanical_initial_path": str(path), } initial_document = load(path) initial = np.asarray(initial_document["matrix_4x4"], float) 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": 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 correction was applied. Confirm static GNHPR " "left/right vs vehicle heading and Helios +X vs vehicle forward before deployment." ), } def corrected(raw: dict, backend: str, reference_height: float, heading_offset_deg: float) -> dict: baseline_frame = abs(heading_offset_deg) <= 1e-12 x_axis = ( "horizontal projection of the rawHeading baseline direction reported by the receiver" if baseline_frame else "vehicle forward after applying the configured G90 heading offset" ) return { "schema_version": 1, "success": bool(raw["success"]), "convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame", "equation": "A_RTK_ij X = X B_LiDAR_ij", "frames": { "RTK": { "origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration", "x_axis": x_axis, "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": { "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")), "solver_initial_extrinsic": raw.get("solver_initial_extrinsic"), "body_heading_offset_deg": heading_offset_deg, "body_heading_offset_used": abs(heading_offset_deg) > 1e-12, "body_antenna_lever_xy_used": False, "translation_m": raw["translation_m"], "rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"], "quaternion_xyzw": raw["quaternion_xyzw"], "coordinate_contract_audit": coordinate_contract_audit(raw), "matrix_4x4": raw["matrix_4x4"], "quality": { "stations": raw["estimation"]["stations"], "pairs": raw["estimation"]["pairs"], "residuals": raw["estimation"]["residuals"], "weighted_jacobian_condition_number": raw["weighted_jacobian_condition_number"], "linearized_one_sigma": raw["linearized_one_sigma"], "bootstrap": raw["bootstrap"], }, "z_constraint": { "observable_from_planar_AX_XB": False, "method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground", "rtk_reference_height_above_ground_m": reference_height, "warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion", }, "important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification", } def main() -> None: parser = argparse.ArgumentParser() parser.add_argument("--result-root", type=Path, required=True) parser.add_argument("--reference-height", type=float, required=True) parser.add_argument("--heading-offset-deg", type=float, required=True) args = parser.parse_args() def solver_output(directory: str) -> Path: raw = args.result_root / directory / "extrinsic_raw.json" standard = args.result_root / directory / "extrinsic.json" return raw if raw.exists() else standard paths = { "open3d_gicp": solver_output("open3d_gicp"), "small_gicp": solver_output("small_gicp"), "consensus": solver_output("consensus"), } docs = {} for backend, path in paths.items(): document = corrected( load(path), backend, args.reference_height, args.heading_offset_deg ) write(path.with_name("extrinsic_rtk_lidar.json"), document) docs[backend] = document open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float) small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float) final = dict(docs["consensus"]) 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": ( 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": { "translation_m": final["translation_m"], "rotation_rpy_deg_xyz": final["rotation_rpy_deg_xyz"], "pairs": final["quality"]["pairs"], "translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"], "rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"], "condition_number": final["quality"]["weighted_jacobian_condition_number"], "coordinate_contract_status": final["coordinate_contract_audit"]["status"], "recommended_for_deployment": final["selection"]["recommended"], }, "backend_difference": delta(open_t, small_t), } write(args.result_root / "summary.json", summary) print(json.dumps(summary, ensure_ascii=False, indent=2)) if __name__ == "__main__": main()