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) path.write_text(json.dumps(document, ensure_ascii=False, indent=2), 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 coordinate_contract_audit(raw: dict) -> dict: """Compare the data-driven solution with the declared mechanical initial. 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. """ 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 return { "status": "near_180_degree_axis_conflict" if near_180 else "no_near_180_degree_axis_conflict", "requires_physical_axis_confirmation": near_180, "mechanical_initial_path": str(path.resolve()), "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." ), } 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", "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", }, "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"] ) 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" ), "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()