Add LiDAR RTK hand-eye calibration workflow and results

2026-07-22 09:31:44 +08:00
commit 62c7ab2e98
61 changed files with 156232 additions and 0 deletions
@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+"""Build a concise backend comparison and select the recommended result."""
+import argparse
+import json
+from pathlib import Path
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--open3d", required=True)
+    parser.add_argument("--small", required=True)
+    parser.add_argument("--open3d-quality", required=True)
+    parser.add_argument("--small-quality", required=True)
+    parser.add_argument("--open3d-check", required=True)
+    parser.add_argument("--small-check", required=True)
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--recommended-output", required=True)
+    args = parser.parse_args()
+
+    open_result = json.loads(Path(args.open3d).read_text(encoding="utf-8-sig"))
+    small_result = json.loads(Path(args.small).read_text(encoding="utf-8-sig"))
+    open_quality = json.loads(Path(args.open3d_quality).read_text(encoding="utf-8-sig"))
+    small_quality = json.loads(Path(args.small_quality).read_text(encoding="utf-8-sig"))
+    open_check = json.loads(Path(args.open3d_check).read_text(encoding="utf-8-sig"))
+    small_check = json.loads(Path(args.small_check).read_text(encoding="utf-8-sig"))
+    x_open = np.asarray(open_result["matrix_4x4"], float)
+    x_small = np.asarray(small_result["matrix_4x4"], float)
+    delta = np.linalg.inv(x_open) @ x_small
+
+    def compact(result, quality, check):
+        estimate = result["estimation"]["residuals"]
+        auxiliary = check["metrics"]
+        return {
+            "translation_m": result["translation_m"],
+            "rotation_rpy_deg_xyz": result["rotation_rpy_deg_xyz"],
+            "estimation_pairs": estimate["pairs"],
+            "estimation_translation_rms_m": estimate["translation_m"]["rms"],
+            "estimation_rotation_rms_deg": estimate["rotation_deg"]["rms"],
+            "bootstrap_std": result["bootstrap"]["std"],
+            "initial_B_loop_closure": quality["accepted_loop_closure"],
+            "batch1_auxiliary_pairs": auxiliary["pairs"],
+            "batch1_auxiliary_translation_rms_m": auxiliary["translation_m"]["rms"],
+            "batch1_auxiliary_rotation_rms_deg": auxiliary["rotation_deg"]["rms"],
+        }
+
+    summary = {
+        "recommended_backend": "open3d_gicp",
+        "selection_reason": (
+            "The two X estimates agree closely; Open3D has lower second-batch AX residual, "
+            "better B loop closure, and lower first-batch auxiliary residual."
+        ),
+        "coordinate_convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
+        "measured_extrinsic_used_as_initial": False,
+        "second_batch_role": "estimation (dense RTK)",
+        "first_batch_role": "auxiliary check only (sparse RTK)",
+        "backend_difference": {
+            "translation_m": float(np.linalg.norm(delta[:3, 3])),
+            "rotation_deg": float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude())),
+        },
+        "open3d_gicp": compact(open_result, open_quality, open_check),
+        "small_gicp": compact(small_result, small_quality, small_check),
+        "important_limit": (
+            "Backend agreement is strong, but AX rotation RMS remains about one degree. "
+            "This is not a centimetre-grade absolute certification."
+        ),
+    }
+    output = Path(args.output)
+    output.parent.mkdir(parents=True, exist_ok=True)
+    output.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
+    recommended = dict(open_result)
+    recommended["selection"] = {
+        "recommended_backend": "open3d_gicp",
+        "comparison_summary": str(output.name),
+        "backend_difference": summary["backend_difference"],
+        "warning": summary["important_limit"],
+    }
+    Path(args.recommended_output).write_text(
+        json.dumps(recommended, ensure_ascii=False, indent=2), encoding="utf-8"
+    )
+    print(json.dumps(summary, ensure_ascii=False, indent=2))
+
+
+if __name__ == "__main__":
+    main()