140 lines
5.0 KiB
Python
140 lines
5.0 KiB
Python
"""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])
|