350 lines
15 KiB
Python
350 lines
15 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)
|
|
|
|
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()
|