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

Co-authored-by: Cursor <cursoragent@cursor.com>

imu_lidar/finalize.py

@@ -0,0 +1,76 @@
+"""Package calibration outputs as JSON-friendly artifacts."""
+
+from __future__ import annotations
+
+import json
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from .contracts import CalibrationResult, CalibrationStatus
+from .geometry import rotation_matrix_to_quaternion_xyzw, rpy_deg_xyz
+
+
+def _to_serializable(value: Any) -> Any:
+    if isinstance(value, np.ndarray):
+        return value.tolist()
+    if isinstance(value, (np.floating, np.integer, np.bool_)):
+        return value.item()
+    if isinstance(value, Path):
+        return str(value)
+    if isinstance(value, dict):
+        return {str(k): _to_serializable(v) for k, v in value.items()}
+    if isinstance(value, (list, tuple)):
+        return [_to_serializable(v) for v in value]
+    return value
+
+
+def finalize_result(
+    *,
+    status: CalibrationStatus,
+    message: str,
+    details: dict[str, Any],
+    T_IMU_lidar: np.ndarray | None = None,
+    time_offset_s: float | None = None,
+    output_directory: Path | None = None,
+) -> CalibrationResult:
+    """Build the result envelope and optionally write report files."""
+
+    result = CalibrationResult(
+        status=status,
+        message=message,
+        details=_to_serializable(details),
+        T_IMU_lidar=None if T_IMU_lidar is None else np.asarray(T_IMU_lidar, dtype=float),
+        time_offset_s=time_offset_s,
+    )
+
+    if output_directory is not None:
+        output_directory = Path(output_directory)
+        output_directory.mkdir(parents=True, exist_ok=True)
+        summary = {
+            "status": status.value,
+            "message": message,
+            "time_offset_s": time_offset_s,
+            "details": result.details,
+        }
+        if result.T_IMU_lidar is not None:
+            t = result.T_IMU_lidar
+            summary["T_IMU_lidar"] = {
+                "matrix": t.tolist(),
+                "translation_m": t[:3, 3].tolist(),
+                "rotation_quaternion_xyzw": rotation_matrix_to_quaternion_xyzw(t[:3, :3]).tolist(),
+                "rpy_deg_xyz": rpy_deg_xyz(t[:3, :3]).tolist(),
+                "convention": "p_IMU = T_IMU_lidar * p_lidar",
+            }
+            (output_directory / "T_IMU_lidar.json").write_text(
+                json.dumps(summary["T_IMU_lidar"], indent=2),
+                encoding="utf-8",
+            )
+        if time_offset_s is not None:
+            (output_directory / "time_offset.json").write_text(
+                json.dumps({"delta_t_s": time_offset_s, "definition": "t_imu = t_lidar + delta_t"}, indent=2),
+                encoding="utf-8",
+            )
+        (output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
+    return result