185 lines
7.1 KiB
Python
185 lines
7.1 KiB
Python
"""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_lidar.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
|
|
|
|
|
|
DEFAULT_RTK_FRAME_DEFINITION = (
|
|
"right-handed vehicle-fixed frame: +X ANT1(main,left)->ANT2(secondary,right), "
|
|
"+Y vehicle forward/IMU +Y, +Z vehicle up/IMU +Z"
|
|
)
|
|
DEFAULT_RTK_REFERENCE_POINT = (
|
|
"GGA ANT1/main-antenna phase center, 1.916499878 m above ground"
|
|
)
|
|
|
|
|
|
@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
|
|
float_attitude = rtk.attitude_float & 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)),
|
|
"float_attitude_ratio": float(np.mean(float_attitude)),
|
|
"checksum_valid_ratio": float(np.mean(rtk.checksum_valid)),
|
|
"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 = DEFAULT_RTK_FRAME_DEFINITION,
|
|
rtk_reference_point: str = DEFAULT_RTK_REFERENCE_POINT,
|
|
) -> 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 and rotation.ok:
|
|
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('full RTK-to-IMU rotation is not observable from the lateral dual-antenna baseline')
|
|
if translation is None or not translation.ok:
|
|
blockers.append('translation is frozen until a full rotation is observable and accepted')
|
|
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
|