189 lines
7.7 KiB
Python
189 lines
7.7 KiB
Python
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()
|