新增独立RTK与IMU外参标定流程及质量验证

2026-08-21 10:04:41 +08:00
parent 1233f8aafd
commit c2da6dd192
38 changed files with 3937 additions and 17 deletions
@@ -0,0 +1,172 @@
+"""End-to-end orchestration and JSON reporting for RTK--IMU calibration."""
+
+from __future__ import annotations
+
+import csv
+import json
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from .imu_io import load_imu_samples
+from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, solve_rtk_imu_rotation
+from .rtk_imu_translation import TranslationCalibrationResult, solve_rtk_imu_translation
+from .rtk_io import load_rtk_csv
+
+
+@dataclass(frozen=True)
+class InventoryEntry:
+    session_id: str
+    batch_id: str
+    imu_csv: Path
+    rtk_csv: Path
+
+
+def load_inventory(path: Path | str) -> list[InventoryEntry]:
+    """Load the project RTK inventory and derive each paired IMU path."""
+
+    source = Path(path)
+    entries: list[InventoryEntry] = []
+    with source.open("r", encoding="utf-8-sig", newline="") as handle:
+        for row in csv.DictReader(handle):
+            rtk_csv = Path(row["current_rtk_csv"])
+            imu_csv = rtk_csv.with_name("imu.csv")
+            entries.append(
+                InventoryEntry(
+                    session_id=row["session"],
+                    batch_id=row["batch"],
+                    imu_csv=imu_csv,
+                    rtk_csv=rtk_csv,
+                )
+            )
+    if not entries:
+        raise ValueError(f"empty RTK inventory: {source}")
+    return entries
+
+
+def load_sessions(entries: list[InventoryEntry] | tuple[InventoryEntry, ...]) -> list[RotationSession]:
+    sessions = []
+    for entry in entries:
+        sessions.append(
+            RotationSession(
+                session_id=entry.session_id,
+                batch_id=entry.batch_id,
+                imu=load_imu_samples(entry.imu_csv),
+                rtk=load_rtk_csv(entry.rtk_csv),
+            )
+        )
+    return sessions
+
+
+def _jsonable(value: Any) -> Any:
+    if isinstance(value, np.ndarray):
+        return value.tolist()
+    if isinstance(value, np.generic):
+        return value.item()
+    if isinstance(value, Path):
+        return str(value)
+    if hasattr(value, "__dataclass_fields__"):
+        return {key: _jsonable(item) for key, item in asdict(value).items()}
+    if isinstance(value, dict):
+        return {str(key): _jsonable(item) for key, item in value.items()}
+    if isinstance(value, (list, tuple)):
+        return [_jsonable(item) for item in value]
+    return value
+
+
+def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
+    rows = []
+    for session in sessions:
+        rtk = session.rtk
+        valid_position = rtk.position_valid
+        valid_attitude = rtk.attitude_valid & valid_position
+        rows.append(
+            {
+                "session_id": session.session_id,
+                "batch_id": session.batch_id,
+                "imu_samples": int(session.imu.t_s.size),
+                "rtk_samples": int(rtk.t_s.size),
+                "fixed_position_ratio": float(np.mean(valid_position)),
+                "fixed_attitude_ratio": float(np.mean(valid_attitude)),
+                "common_time_span_s": [
+                    float(max(session.imu.t_s[0], rtk.t_s[0])),
+                    float(min(session.imu.t_s[-1], rtk.t_s[-1])),
+                ],
+                "origin_geodetic": list(rtk.origin_geodetic),
+                "imu_source": str(session.imu.t_s.size) + " normalized samples",
+                "rtk_source": str(rtk.source),
+            }
+        )
+    return {"session_count": len(sessions), "sessions": rows}
+
+
+def run_calibration(
+    sessions: list[RotationSession],
+    output_directory: Path | str,
+    *,
+    rotation_only: bool = False,
+    compute_loo: bool = True,
+    knot_step_s: float = 2.0,
+    rtk_frame_definition: str = '',
+    rtk_reference_point: str = '',
+) -> tuple[RotationCalibrationResult, TranslationCalibrationResult | None]:
+    """Run calibration and publish human-readable JSON artifacts."""
+
+    output = Path(output_directory)
+    output.mkdir(parents=True, exist_ok=True)
+    rotation = solve_rtk_imu_rotation(sessions, compute_loo=compute_loo)
+    translation = None
+    if not rotation_only:
+        translation = solve_rtk_imu_translation(
+            sessions,
+            rotation,
+            knot_step_s=knot_step_s,
+            compute_loo=compute_loo,
+        )
+    audit_payload = dataset_audit(sessions)
+    rotation_payload = _jsonable(rotation)
+    translation_payload = None if translation is None else _jsonable(translation)
+    (output / "dataset_audit.json").write_text(
+        json.dumps(audit_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
+    )
+    (output / "rotation_result.json").write_text(
+        json.dumps(rotation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
+    )
+    if translation_payload is not None:
+        (output / "translation_result.json").write_text(
+            json.dumps(translation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
+        )
+    interpretation_complete = bool(rtk_frame_definition.strip() and rtk_reference_point.strip())
+    accepted = bool(
+        rotation.ok and translation is not None and translation.ok and interpretation_complete
+    )
+    blockers = []
+    if not rotation.ok:
+        blockers.append('rotation quality gates failed')
+    if translation is None or not translation.ok:
+        blockers.append('translation quality gates failed or were not run')
+    if not rtk_frame_definition.strip():
+        blockers.append('RTK frame_definition is empty')
+    if not rtk_reference_point.strip():
+        blockers.append('RTK reference_point is empty')
+    summary = {
+        "status": "accepted" if accepted else "diagnostic_not_accepted",
+        "transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
+        "rtk_frame_definition": rtk_frame_definition,
+        "rtk_reference_point": rtk_reference_point,
+        "interpretation_blockers": blockers,
+        "R_RTK_IMU": rotation.R_RTK_IMU.tolist(),
+        "t_RTK_IMU_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
+        "T_RTK_IMU": None if translation is None else translation.T_RTK_IMU.tolist(),
+        "rotation_ok": rotation.ok,
+        "translation_ok": None if translation is None else translation.ok,
+        "rotation_result": "rotation_result.json",
+        "translation_result": None if translation is None else "translation_result.json",
+        "dataset_audit": "dataset_audit.json",
+    }
+    (output / "summary.json").write_text(
+        json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
+    )
+    return rotation, translation