添加 LiDAR-IMU 外参标定流水线与说明文档

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-01 12:02:37 +08:00
commit cf1fad7594
40 changed files with 5502 additions and 0 deletions
@@ -0,0 +1,133 @@
+"""Pretty-print calibration summary.json and optionally write a small preview figure."""
+
+from __future__ import annotations
+
+import argparse
+import json
+from pathlib import Path
+
+
+def _load(path: Path) -> dict:
+    return json.loads(path.read_text(encoding="utf-8"))
+
+
+def _session0(summary: dict) -> dict:
+    sessions = summary.get("details", {}).get("sessions", [])
+    return sessions[0] if sessions else {}
+
+
+def print_report(summary: dict, truth: dict | None = None) -> None:
+    print("---------- calibration report ----------")
+    print(f"status : {summary.get('status')}")
+    print(f"message: {summary.get('message')}")
+    dt = summary.get("time_offset_s")
+    if dt is not None:
+        print(f"δt     : {dt:.6f} s   (t_imu = t_lidar + δt)")
+        if truth and "delta_t_s" in truth:
+            print(f"         truth={truth['delta_t_s']:.6f} s  err={dt - truth['delta_t_s']:+.6f} s")
+
+    t_block = summary.get("T_IMU_lidar")
+    if isinstance(t_block, dict):
+        rpy = t_block.get("rpy_deg_xyz")
+        trans = t_block.get("translation_m")
+        if rpy is not None:
+            print(f"RPY xyz: [{rpy[0]:.3f}, {rpy[1]:.3f}, {rpy[2]:.3f}] deg")
+            if truth and "yaw_extrinsic_deg" in truth:
+                print(f"         yaw truth≈{truth['yaw_extrinsic_deg']:.3f} deg")
+        if trans is not None:
+            print(f"t      : [{trans[0]:.4f}, {trans[1]:.4f}, {trans[2]:.4f}] m")
+
+    session = _session0(summary)
+    handeye = session.get("handeye") or {}
+    joint = session.get("joint") or {}
+    if handeye:
+        print(
+            "handeye: "
+            f"pairs={handeye.get('pair_count')}  "
+            f"rms={handeye.get('residual_rms_deg')}°  "
+            f"median={handeye.get('residual_median_deg')}°  "
+            f"ok={handeye.get('ok')}"
+        )
+    if joint:
+        obs = joint.get("observability") or {}
+        print(
+            "joint  : "
+            f"rot_rms={joint.get('residual_rms_rot_deg')}°  "
+            f"trans_rms={joint.get('residual_rms_trans_m')} m  "
+            f"trans_accepted={joint.get('translation_accepted')}"
+        )
+        print(
+            "observ : "
+            f"rotation={obs.get('rotation_observable')}  "
+            f"translation={obs.get('translation_observable')}  "
+            f"cond_R={obs.get('condition_rotation')}"
+        )
+        if joint.get("gravity_m_s2") is not None:
+            print(f"gravity: {joint.get('gravity_m_s2')}")
+        if joint.get("gyro_bias_rad_s") is not None:
+            print(f"b_g    : {joint.get('gyro_bias_rad_s')}")
+    print("----------------------------------------")
+
+
+def maybe_plot(summary: dict, plot_path: Path, truth: dict | None = None) -> None:
+    try:
+        import matplotlib.pyplot as plt
+    except ImportError:
+        print("(matplotlib not installed; skip plot)")
+        return
+
+    session = _session0(summary)
+    handeye = session.get("handeye") or {}
+    joint = session.get("joint") or {}
+    labels = []
+    values = []
+    if handeye.get("residual_rms_deg") is not None:
+        labels.append("handeye\nRMS (°)")
+        values.append(float(handeye["residual_rms_deg"]))
+    if joint.get("residual_rms_rot_deg") is not None:
+        labels.append("joint rot\nRMS (°)")
+        values.append(float(joint["residual_rms_rot_deg"]))
+    if joint.get("residual_rms_trans_m") is not None:
+        labels.append("joint trans\nRMS (m)")
+        values.append(float(joint["residual_rms_trans_m"]))
+    dt = summary.get("time_offset_s")
+    if dt is not None:
+        labels.append("|δt| (s)")
+        values.append(abs(float(dt)))
+        if truth and "delta_t_s" in truth:
+            labels.append("|δt err| (s)")
+            values.append(abs(float(dt) - float(truth["delta_t_s"])))
+
+    if not labels:
+        print("(no numeric fields to plot)")
+        return
+
+    fig, ax = plt.subplots(figsize=(7.5, 3.8))
+    ax.bar(labels, values, color="#3b6ea5")
+    ax.set_title(f"Calibration preview — {summary.get('status')}")
+    ax.set_ylabel("value")
+    ax.grid(axis="y", alpha=0.3)
+    fig.tight_layout()
+    plot_path.parent.mkdir(parents=True, exist_ok=True)
+    fig.savefig(plot_path, dpi=120)
+    plt.close(fig)
+    print(f"wrote plot: {plot_path}")
+
+
+def main(argv: list[str] | None = None) -> int:
+    parser = argparse.ArgumentParser(description="Show LiDAR–IMU calibration summary")
+    parser.add_argument("--summary", type=Path, required=True, help="Path to summary.json")
+    parser.add_argument("--truth-meta", type=Path, default=None, help="Optional synthetic meta.json")
+    parser.add_argument("--plot", type=Path, default=None, help="Optional PNG path for a bar chart")
+    args = parser.parse_args(argv)
+
+    summary = _load(args.summary)
+    truth = _load(args.truth_meta) if args.truth_meta and args.truth_meta.exists() else None
+    print_report(summary, truth)
+    if args.plot is not None:
+        maybe_plot(summary, args.plot, truth)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())