83 lines
2.9 KiB
Python
83 lines
2.9 KiB
Python
"""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,
|
|
motion_pairs_payload: dict[str, Any] | 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",
|
|
)
|
|
if motion_pairs_payload is not None:
|
|
from .motion_pairs_io import save_motion_pairs
|
|
|
|
save_motion_pairs(output_directory / "motion_pairs.json", motion_pairs_payload)
|
|
summary["motion_pairs_file"] = "motion_pairs.json"
|
|
(output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
|
|
return result
|