迁移RTK-IMU标定到独立顶层包

rtk_imu/rtk_io.py

@@ -0,0 +1,139 @@
+"""RTK CSV loading for the independent RTK--IMU calibration path."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+
+import numpy as np
+
+from imu_lidar.geodesy import geodetic_to_enu
+
+
+@dataclass(frozen=True)
+class RtkSeries:
+    """Normalized RTK observations on the IMU device clock."""
+
+    t_s: np.ndarray
+    attitude_t_s: np.ndarray
+    position_enu_m: np.ndarray
+    heading_deg: np.ndarray
+    pitch_deg: np.ndarray
+    roll_deg: np.ndarray
+    fix_quality: np.ndarray
+    heading_quality: np.ndarray
+    heading_satellites: np.ndarray
+    heading_age_s: np.ndarray
+    hdop: np.ndarray
+    checksum_valid: np.ndarray
+    origin_geodetic: tuple[float, float, float]
+    source: Path
+
+    @property
+    def attitude_valid(self) -> np.ndarray:
+        """Strict fixed dual-antenna solutions suitable for calibration."""
+
+        return (
+            np.isfinite(self.heading_deg)
+            & np.isfinite(self.pitch_deg)
+            & np.isfinite(self.roll_deg)
+            & (self.heading_quality == 4.0)
+            & self.checksum_valid
+        )
+
+    @property
+    def attitude_float(self) -> np.ndarray:
+        """Float solutions retained for diagnostics but never calibration."""
+
+        return (
+            np.isfinite(self.heading_deg)
+            & np.isfinite(self.pitch_deg)
+            & np.isfinite(self.roll_deg)
+            & (self.heading_quality == 5.0)
+            & self.checksum_valid
+        )
+
+    @property
+    def position_valid(self) -> np.ndarray:
+        return (
+            np.all(np.isfinite(self.position_enu_m), axis=1)
+            & (self.fix_quality == 4.0)
+            & self.checksum_valid
+        )
+
+
+def _column(data: np.ndarray, name: str, *, default: float = np.nan) -> np.ndarray:
+    names = set(data.dtype.names or ())
+    if name not in names:
+        return np.full(data.shape[0], default, dtype=float)
+    return np.asarray(data[name], dtype=float).reshape(-1)
+
+
+def load_rtk_csv(path: Path | str) -> RtkSeries:
+    """Load an exported G90 RTK CSV and convert its positions to local ENU.
+
+    The required ``t`` column must already be NMEA measurement UTC mapped onto
+    the IMU device clock. Host receive time is deliberately never accepted as
+    a fallback because it is delayed by several seconds in the recorded data.
+    """
+
+    source = Path(path)
+    if not source.is_file():
+        raise FileNotFoundError(source)
+    data = np.genfromtxt(source, delimiter=",", names=True, dtype=float, encoding="utf-8")
+    if data.ndim == 0:
+        data = np.array([data], dtype=data.dtype)
+    names = set(data.dtype.names or ())
+    required = {"t", "lat_deg", "lon_deg", "altitude_m", "fix_quality"}
+    if not required.issubset(names):
+        raise ValueError(f"RTK CSV must contain {sorted(required)}, got {sorted(names)}")
+    t_s = _column(data, "t")
+    measurement_utc = _column(data, "t_measurement_utc_s")
+    hpr_measurement_utc = _column(data, "hpr_measurement_utc_s")
+    attitude_t = t_s.copy()
+    has_hpr_time = np.isfinite(measurement_utc) & np.isfinite(hpr_measurement_utc)
+    attitude_t[has_hpr_time] += hpr_measurement_utc[has_hpr_time] - measurement_utc[has_hpr_time]
+    order = np.argsort(t_s)
+    position, origin = geodetic_to_enu(
+        _column(data, "lat_deg")[order],
+        _column(data, "lon_deg")[order],
+        _column(data, "altitude_m")[order],
+    )
+    return RtkSeries(
+        t_s=t_s[order],
+        attitude_t_s=attitude_t[order],
+        position_enu_m=position,
+        heading_deg=_column(data, "heading_deg")[order],
+        pitch_deg=_column(data, "pitch_deg")[order],
+        roll_deg=_column(data, "roll_deg")[order],
+        fix_quality=_column(data, "fix_quality", default=0.0)[order],
+        heading_quality=_column(data, "heading_quality", default=0.0)[order],
+        heading_satellites=_column(data, "heading_satellites")[order],
+        heading_age_s=_column(data, "heading_age_s")[order],
+        hdop=_column(data, "hdop")[order],
+        checksum_valid=_column(data, "checksum_valid", default=1.0)[order] == 1.0,
+        origin_geodetic=origin,
+        source=source,
+    )
+
+
+def longest_valid_interval(t_s: np.ndarray, valid: np.ndarray, *, max_gap_s: float = 0.2) -> tuple[float, float]:
+    """Return the longest contiguous valid time interval."""
+
+    times = np.asarray(t_s, dtype=float).reshape(-1)
+    mask = np.asarray(valid, dtype=bool).reshape(-1)
+    indices = np.flatnonzero(mask)
+    if indices.size == 0:
+        raise ValueError("no valid RTK samples")
+    best_start = best_end = int(indices[0])
+    start = previous = int(indices[0])
+    for index in indices[1:]:
+        index = int(index)
+        if index != previous + 1 or times[index] - times[previous] > max_gap_s:
+            if times[previous] - times[start] > times[best_end] - times[best_start]:
+                best_start, best_end = start, previous
+            start = index
+        previous = index
+    if times[previous] - times[start] > times[best_end] - times[best_start]:
+        best_start, best_end = start, previous
+    return float(times[best_start]), float(times[best_end])