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

imu_lidar/geodesy.py

@@ -0,0 +1,74 @@
+"""Small WGS84 geodesy helpers used by the RTK--IMU calibration path."""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+WGS84_A_M = 6378137.0
+WGS84_F = 1.0 / 298.257223563
+WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
+
+
+def geodetic_to_ecef(
+    latitude_deg: np.ndarray,
+    longitude_deg: np.ndarray,
+    altitude_m: np.ndarray,
+) -> np.ndarray:
+    """Convert WGS84 latitude/longitude/ellipsoidal height to ECEF metres."""
+
+    latitude = np.deg2rad(np.asarray(latitude_deg, dtype=float))
+    longitude = np.deg2rad(np.asarray(longitude_deg, dtype=float))
+    altitude = np.asarray(altitude_m, dtype=float)
+    latitude, longitude, altitude = np.broadcast_arrays(latitude, longitude, altitude)
+    sin_lat = np.sin(latitude)
+    cos_lat = np.cos(latitude)
+    radius = WGS84_A_M / np.sqrt(1.0 - WGS84_E2 * sin_lat**2)
+    x = (radius + altitude) * cos_lat * np.cos(longitude)
+    y = (radius + altitude) * cos_lat * np.sin(longitude)
+    z = (radius * (1.0 - WGS84_E2) + altitude) * sin_lat
+    return np.stack([x, y, z], axis=-1)
+
+
+def geodetic_to_enu(
+    latitude_deg: np.ndarray,
+    longitude_deg: np.ndarray,
+    altitude_m: np.ndarray,
+    *,
+    origin_latitude_deg: float | None = None,
+    origin_longitude_deg: float | None = None,
+    origin_altitude_m: float | None = None,
+) -> tuple[np.ndarray, tuple[float, float, float]]:
+    """Convert WGS84 samples to a local east/north/up frame.
+
+    When no origin is supplied, the first finite sample is used. The returned
+    origin tuple is ``(latitude_deg, longitude_deg, altitude_m)``.
+    """
+
+    lat = np.asarray(latitude_deg, dtype=float).reshape(-1)
+    lon = np.asarray(longitude_deg, dtype=float).reshape(-1)
+    alt = np.asarray(altitude_m, dtype=float).reshape(-1)
+    if not (lat.size == lon.size == alt.size):
+        raise ValueError("latitude, longitude and altitude must have equal length")
+    finite = np.isfinite(lat) & np.isfinite(lon) & np.isfinite(alt)
+    if not np.any(finite):
+        raise ValueError("no finite geodetic sample")
+    first = int(np.flatnonzero(finite)[0])
+    lat0 = float(lat[first] if origin_latitude_deg is None else origin_latitude_deg)
+    lon0 = float(lon[first] if origin_longitude_deg is None else origin_longitude_deg)
+    alt0 = float(alt[first] if origin_altitude_m is None else origin_altitude_m)
+
+    ecef = geodetic_to_ecef(lat, lon, alt)
+    ecef0 = geodetic_to_ecef(np.array(lat0), np.array(lon0), np.array(alt0)).reshape(3)
+    delta = ecef - ecef0
+    phi = np.deg2rad(lat0)
+    lam = np.deg2rad(lon0)
+    rotation = np.array(
+        [
+            [-np.sin(lam), np.cos(lam), 0.0],
+            [-np.sin(phi) * np.cos(lam), -np.sin(phi) * np.sin(lam), np.cos(phi)],
+            [np.cos(phi) * np.cos(lam), np.cos(phi) * np.sin(lam), np.sin(phi)],
+        ],
+        dtype=float,
+    )
+    return delta @ rotation.T, (lat0, lon0, alt0)

imu_lidar/imu_io.py

@@ -15,6 +15,7 @@ Accepted inputs
 
 from __future__ import annotations
 
+import csv
 from pathlib import Path
 
 import numpy as np
@@ -36,16 +37,17 @@ def load_imu_samples(path: Path | str) -> ImuSeries:
 
 
 def _load_imu_csv(path: Path) -> ImuSeries:
-    data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
-    if data.ndim == 0:
-        data = np.array([data])
-    names = set(data.dtype.names or ())
-    required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
-    if not required.issubset(names):
-        raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
-    t = np.asarray(data["t"], dtype=float).reshape(-1)
-    gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
-    acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
+    required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
+    with path.open("r", encoding="utf-8-sig", newline="") as handle:
+        header = next(csv.reader(handle), [])
+    names = set(header)
+    if not set(required_order).issubset(names):
+        raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
+    usecols = [header.index(name) for name in required_order]
+    data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
+    t = np.asarray(data[:, 0], dtype=float).reshape(-1)
+    gyro = np.asarray(data[:, 1:4], dtype=float)
+    acc = np.asarray(data[:, 4:7], dtype=float)
     order = np.argsort(t)
     return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
 

imu_lidar/imu_preintegration.py

@@ -158,9 +158,14 @@ def preintegrate_gyro(
         if dt <= 0:
             continue
 
-        # Exact endpoint gyro via linear interpolation inside the sample interval.
-        g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
-        g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
+        # Exact local endpoint interpolation. ``seg0`` and ``seg1`` are inside
+        # this adjacent sample interval, so scanning the full series with
+        # np.interp here would turn pair construction into quadratic work.
+        sample_dt = max(t_b - t_a, 1e-12)
+        u0 = (seg0 - t_a) / sample_dt
+        u1 = (seg1 - t_a) / sample_dt
+        g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
+        g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
         omega = 0.5 * (g_a + g_b) - bias
         gyro_norms.append(float(np.linalg.norm(omega)))
 
@@ -279,10 +284,13 @@ def preintegrate_imu(
         if dt <= 0:
             continue
 
-        g_a = _interp_vec(times_s, gyro_rad_s, seg0)
-        g_b = _interp_vec(times_s, gyro_rad_s, seg1)
-        a_a = _interp_vec(times_s, acc_m_s2, seg0)
-        a_b = _interp_vec(times_s, acc_m_s2, seg1)
+        sample_dt = max(t_b - t_a, 1e-12)
+        u0 = (seg0 - t_a) / sample_dt
+        u1 = (seg1 - t_a) / sample_dt
+        g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
+        g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
+        a_a = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
+        a_b = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
         omega = 0.5 * (g_a + g_b) - bg
         acc = 0.5 * (a_a + a_b) - ba
         gyro_norms.append(float(np.linalg.norm(omega)))

imu_lidar/rtk_attitude.py

@@ -0,0 +1,73 @@
+"""GNHPR attitude conventions and SO(3) interpolation."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.spatial.transform import Rotation, Slerp
+
+@dataclass(frozen=True)
+class GnhprConvention:
+    """Interpretation of GNHPR angles as ``R_ENU_RTK``.
+
+    Heading is normally clockwise from north. With ENU and an x-forward RTK
+    frame this becomes yaw ``90 deg - heading``. Aircraft-positive pitch is
+    nose-up, which is the negative mathematical Y rotation in an FLU frame.
+    Alternative signs are retained for empirical protocol validation.
+    """
+
+    name: str
+    heading_sign: float = -1.0
+    pitch_sign: float = -1.0
+    roll_sign: float = 1.0
+
+
+EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
+GNHPR_CANDIDATES = (
+    EXPECTED_GNHPR,
+    GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
+    GnhprConvention("heading_opposite__pitch_nose_up", 1.0, -1.0, 1.0),
+    GnhprConvention("heading_opposite__pitch_opposite", 1.0, 1.0, 1.0),
+)
+
+
+def gnhpr_to_rotation_enu_rtk(
+    heading_deg: np.ndarray,
+    pitch_deg: np.ndarray,
+    roll_deg: np.ndarray,
+    convention: GnhprConvention = EXPECTED_GNHPR,
+) -> np.ndarray:
+    """Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
+
+    heading = np.asarray(heading_deg, dtype=float).reshape(-1)
+    pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
+    roll = np.asarray(roll_deg, dtype=float).reshape(-1)
+    if not (heading.size == pitch.size == roll.size):
+        raise ValueError("heading, pitch and roll must have equal length")
+    yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
+    pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
+    roll_rad = np.deg2rad(convention.roll_sign * roll)
+    angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
+    return Rotation.from_euler('ZYX', angles).as_matrix()
+
+
+def interpolate_rotations(
+    source_t_s: np.ndarray,
+    rotations: np.ndarray,
+    query_t_s: np.ndarray,
+) -> np.ndarray:
+    """Slerp a monotonic SO(3) series without extrapolation."""
+
+    source_t = np.asarray(source_t_s, dtype=float).reshape(-1)
+    query_t = np.asarray(query_t_s, dtype=float).reshape(-1)
+    matrices = np.asarray(rotations, dtype=float).reshape(-1, 3, 3)
+    if source_t.size < 2 or matrices.shape[0] != source_t.size:
+        raise ValueError("need at least two timestamped rotations")
+    if np.any(np.diff(source_t) <= 0):
+        unique_t, unique_indices = np.unique(source_t, return_index=True)
+        source_t = unique_t
+        matrices = matrices[unique_indices]
+    if np.any(query_t < source_t[0]) or np.any(query_t > source_t[-1]):
+        raise ValueError("rotation interpolation does not extrapolate")
+    return Slerp(source_t, Rotation.from_matrix(matrices))(query_t).as_matrix()

imu_lidar/rtk_imu_replay.py

@@ -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

imu_lidar/rtk_imu_rotation.py

@@ -0,0 +1,439 @@
+"""Rotation and residual time-offset calibration between G90 RTK and HI13 IMU."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.optimize import least_squares
+from scipy.sparse import lil_matrix
+
+from .contracts import ImuSeries, MotionPair
+from .geometry import orthonormalize_rotation, rpy_deg_xyz, so3_exp, so3_log
+from .imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
+from .rotation_handeye import estimate_rotation_handeye_initial
+from .rtk_attitude import GNHPR_CANDIDATES, GnhprConvention, gnhpr_to_rotation_enu_rtk
+from .rtk_io import RtkSeries
+
+
+@dataclass(frozen=True)
+class RotationSession:
+    session_id: str
+    batch_id: str
+    imu: ImuSeries
+    rtk: RtkSeries
+
+
+@dataclass(frozen=True)
+class TimeOffsetAudit:
+    offset_s: float
+    peak_correlation: float
+    second_best_correlation: float
+    evaluated_samples: int
+    reliable: bool
+
+
+@dataclass(frozen=True)
+class RotationCalibrationResult:
+    R_RTK_IMU: np.ndarray
+    rpy_deg: np.ndarray
+    gyro_bias_by_session_rad_s: dict[str, np.ndarray]
+    time_offset: TimeOffsetAudit
+    applied_time_offset_s: float
+    convention: GnhprConvention
+    convention_scores_deg: dict[str, float]
+    pair_count: int
+    residual_rms_deg: float
+    residual_median_deg: float
+    residual_p95_deg: float
+    rotation_std_deg: np.ndarray
+    information_singular_values: np.ndarray
+    per_session_rms_deg: dict[str, float]
+    loo_delta_deg: dict[str, float]
+    ok: bool
+    notes: tuple[str, ...]
+
+
+@dataclass(frozen=True)
+class _Pair:
+    session_index: int
+    session_id: str
+    R_A: np.ndarray
+    delta_R_zero_bias: np.ndarray
+    J_bg: np.ndarray
+    weight: float
+
+
+def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
+    valid = rtk.attitude_valid & rtk.position_valid
+    t = rtk.attitude_t_s[valid]
+    angles = np.column_stack(
+        [rtk.heading_deg[valid], rtk.pitch_deg[valid], rtk.roll_deg[valid]]
+    )
+    if t.size < 2:
+        raise ValueError(f"not enough valid RTK attitude rows: {rtk.source}")
+    # GGA is faster than HPR, so nearest-neighbour export repeats attitude rows.
+    # Keep only changes and place them at the first associated GGA measurement.
+    changed = np.ones(t.size, dtype=bool)
+    changed[1:] = np.any(np.abs(np.diff(angles, axis=0)) > 1e-10, axis=1)
+    t = t[changed]
+    angles = angles[changed]
+    order = np.argsort(t)
+    t = t[order]
+    angles = angles[order]
+    unique_t, unique_indices = np.unique(t, return_index=True)
+    rotations = gnhpr_to_rotation_enu_rtk(
+        angles[unique_indices, 0],
+        angles[unique_indices, 1],
+        angles[unique_indices, 2],
+        convention,
+    )
+    return unique_t, rotations
+
+
+def _rtk_angular_speed(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    dt = np.diff(t)
+    valid = (dt >= 0.03) & (dt <= 0.5)
+    midpoint = 0.5 * (t[:-1] + t[1:])
+    speed = np.array(
+        [np.linalg.norm(so3_log(rotations[i].T @ rotations[i + 1])) for i in range(t.size - 1)]
+    ) / np.maximum(dt, 1e-6)
+    return midpoint[valid], speed[valid]
+
+
+def _correlation(a: np.ndarray, b: np.ndarray) -> float:
+    a = np.asarray(a, dtype=float)
+    b = np.asarray(b, dtype=float)
+    if a.size < 20 or np.std(a) < 1e-5 or np.std(b) < 1e-5:
+        return np.nan
+    return float(np.corrcoef(a, b)[0, 1])
+
+
+def audit_time_offset(
+    sessions: list[RotationSession] | tuple[RotationSession, ...],
+    *,
+    search_half_width_s: float = 0.30,
+    step_s: float = 0.005,
+) -> TimeOffsetAudit:
+    """Estimate residual ``t_IMU - t_RTK`` from invariant angular-speed norms."""
+
+    offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
+    session_series: list[tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
+    total_samples = 0
+    for session in sessions:
+        t_rtk, rotations = _attitude_rows(session.rtk, GNHPR_CANDIDATES[0])
+        midpoint, rtk_speed = _rtk_angular_speed(t_rtk, rotations)
+        imu_speed = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
+        motion = rtk_speed > np.deg2rad(0.5)
+        midpoint = midpoint[motion]
+        rtk_speed = rtk_speed[motion]
+        if midpoint.size >= 20:
+            session_series.append((midpoint, rtk_speed, session.imu.t_s, imu_speed))
+            total_samples += int(midpoint.size)
+    if not session_series:
+        return TimeOffsetAudit(0.0, np.nan, np.nan, 0, False)
+    scores = []
+    for offset in offsets:
+        per_session = []
+        for midpoint, rtk_speed, imu_t, imu_speed in session_series:
+            query = midpoint + offset
+            inside = (query >= imu_t[0]) & (query <= imu_t[-1])
+            if np.count_nonzero(inside) < 20:
+                continue
+            interpolated = np.interp(query[inside], imu_t, imu_speed)
+            value = _correlation(rtk_speed[inside], interpolated)
+            if np.isfinite(value):
+                per_session.append(value)
+        scores.append(float(np.median(per_session)) if per_session else np.nan)
+    values = np.asarray(scores, dtype=float)
+    if not np.any(np.isfinite(values)):
+        return TimeOffsetAudit(0.0, np.nan, np.nan, total_samples, False)
+    best_index = int(np.nanargmax(values))
+    exclusion = np.abs(offsets - offsets[best_index]) >= 0.03
+    second = float(np.nanmax(values[exclusion])) if np.any(np.isfinite(values[exclusion])) else np.nan
+    peak = float(values[best_index])
+    reliable = bool(peak >= 0.35 and (not np.isfinite(second) or peak - second >= 0.015))
+    return TimeOffsetAudit(float(offsets[best_index]), peak, second, total_samples, reliable)
+
+
+def _nearest_index(times: np.ndarray, target: float) -> int:
+    index = int(np.searchsorted(times, target))
+    candidates = [max(0, index - 1), min(times.size - 1, index)]
+    return min(candidates, key=lambda item: abs(float(times[item]) - target))
+
+
+def _make_pairs(
+    sessions: list[RotationSession],
+    convention: GnhprConvention,
+    time_offset_s: float,
+    *,
+    anchor_step_s: float = 5.0,
+    intervals_s: tuple[float, ...] = (0.75, 1.5, 3.0),
+    preintegration_cache: dict[tuple[str, float, float], object] | None = None,
+) -> list[_Pair]:
+    pairs: list[_Pair] = []
+    cache = {} if preintegration_cache is None else preintegration_cache
+    for session_index, session in enumerate(sessions):
+        t, rotations = _attitude_rows(session.rtk, convention)
+        next_anchor = float(t[0])
+        for i in range(t.size - 1):
+            if t[i] + 1e-9 < next_anchor:
+                continue
+            next_anchor = float(t[i] + anchor_step_s)
+            for duration in intervals_s:
+                j = _nearest_index(t, float(t[i] + duration))
+                if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
+                    continue
+                imu_t0 = float(t[i] + time_offset_s)
+                imu_t1 = float(t[j] + time_offset_s)
+                if imu_t0 < session.imu.t_s[0] or imu_t1 > session.imu.t_s[-1]:
+                    continue
+                r_a = orthonormalize_rotation(rotations[i].T @ rotations[j])
+                cache_key = (session.session_id, round(imu_t0, 6), round(imu_t1, 6))
+                preint = cache.get(cache_key)
+                if preint is None:
+                    preint = preintegrate_gyro(
+                        session.imu.t_s,
+                        session.imu.gyro_rad_s,
+                        imu_t0,
+                        imu_t1,
+                    )
+                    cache[cache_key] = preint
+                angle_a = np.linalg.norm(so3_log(r_a))
+                angle_b = np.linalg.norm(so3_log(preint.delta_R))
+                if min(angle_a, angle_b) < np.deg2rad(0.8):
+                    continue
+                weight = float(np.clip(min(angle_a, angle_b) / np.deg2rad(5.0), 0.2, 3.0))
+                pairs.append(
+                    _Pair(
+                        session_index=session_index,
+                        session_id=session.session_id,
+                        R_A=r_a,
+                        delta_R_zero_bias=preint.delta_R,
+                        J_bg=preint.J_bg,
+                        weight=weight,
+                    )
+                )
+    return pairs
+
+
+def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
+    if len(pairs) < 6:
+        raise ValueError("need at least 6 excited RTK--IMU rotation pairs")
+    generic = [
+        MotionPair(
+            session_id=pair.session_id,
+            i=index,
+            j=index + 1,
+            t_i_s=0.0,
+            t_j_s=1.0,
+            R_A=pair.R_A,
+            R_B=pair.delta_R_zero_bias,
+            metadata={"weight": pair.weight},
+        )
+        for index, pair in enumerate(pairs)
+    ]
+    r0 = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
+    session_count = len(sessions)
+
+    def unpack(parameters: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+        return orthonormalize_rotation(so3_exp(parameters[:3])), parameters[3:].reshape(session_count, 3)
+
+    def residual(parameters: np.ndarray) -> np.ndarray:
+        r_x, biases = unpack(parameters)
+        rows = []
+        for pair in pairs:
+            corrected = apply_bias_jacobian_correction(
+                pair.delta_R_zero_bias,
+                pair.J_bg,
+                biases[pair.session_index],
+            )
+            error = so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T)
+            rows.append(np.sqrt(pair.weight) * error)
+        # HI13 bias is session-specific; this weak prior only removes degenerate
+        # bias/extrinsic trades and is much looser than observed static bias.
+        rows.append((biases / 0.03).reshape(-1))
+        return np.concatenate(rows)
+
+    initial = np.concatenate([so3_log(r0), np.zeros(3 * session_count)])
+    jacobian_pattern = lil_matrix((3 * len(pairs) + 3 * session_count, initial.size), dtype=int)
+    for pair_index, pair in enumerate(pairs):
+        row = 3 * pair_index
+        jacobian_pattern[row : row + 3, 0:3] = 1
+        bias_col = 3 + 3 * pair.session_index
+        jacobian_pattern[row : row + 3, bias_col : bias_col + 3] = 1
+    prior_row = 3 * len(pairs)
+    jacobian_pattern[prior_row:, 3:] = 1
+    opt = least_squares(
+        residual,
+        initial,
+        loss="huber",
+        f_scale=np.deg2rad(0.5),
+        jac_sparsity=jacobian_pattern.tocsr(),
+        tr_solver='lsmr',
+        max_nfev=40,
+    )
+    r_x, biases = unpack(opt.x)
+    errors = []
+    for pair in pairs:
+        corrected = apply_bias_jacobian_correction(
+            pair.delta_R_zero_bias,
+            pair.J_bg,
+            biases[pair.session_index],
+        )
+        errors.append(np.degrees(np.linalg.norm(so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T))))
+    jacobian = opt.jac.toarray() if hasattr(opt.jac, 'toarray') else np.asarray(opt.jac, dtype=float)
+    information = jacobian.T @ jacobian
+    dof = max(residual(opt.x).size - opt.x.size, 1)
+    variance = float(np.sum(residual(opt.x) ** 2) / dof)
+    covariance = np.linalg.pinv(information, rcond=1e-10) * variance
+    return r_x, biases, np.asarray(errors), covariance
+
+
+def solve_rtk_imu_rotation(
+    sessions: list[RotationSession] | tuple[RotationSession, ...],
+    *,
+    compute_loo: bool = True,
+) -> RotationCalibrationResult:
+    """Solve shared ``R_RTK_IMU`` and per-session gyro biases."""
+
+    items = list(sessions)
+    if not items:
+        raise ValueError("at least one RTK--IMU session is required")
+    time_audit = audit_time_offset(items)
+    offset = time_audit.offset_s if time_audit.reliable else 0.0
+    candidates: list[tuple[GnhprConvention, list[_Pair]]] = []
+    scores: dict[str, float] = {}
+    preintegration_cache: dict[tuple[str, float, float], object] = {}
+    for convention in GNHPR_CANDIDATES:
+        pairs = _make_pairs(items, convention, offset, preintegration_cache=preintegration_cache)
+        if len(pairs) < 6:
+            scores[convention.name] = 1e9
+            continue
+        generic = [
+            MotionPair(
+                session_id=pair.session_id,
+                i=index,
+                j=index + 1,
+                t_i_s=0.0,
+                t_j_s=1.0,
+                R_A=pair.R_A,
+                R_B=pair.delta_R_zero_bias,
+                metadata={'weight': pair.weight},
+            )
+            for index, pair in enumerate(pairs)
+        ]
+        initial_rotation = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
+        preliminary_errors = np.asarray(
+            [
+                np.degrees(
+                    np.linalg.norm(
+                        so3_log(
+                            initial_rotation.T
+                            @ pair.R_A
+                            @ initial_rotation
+                            @ pair.delta_R_zero_bias.T
+                        )
+                    )
+                )
+                for pair in pairs
+            ]
+        )
+        scores[convention.name] = float(np.sqrt(np.mean(preliminary_errors**2)))
+        candidates.append((convention, pairs))
+    if not candidates:
+        raise ValueError("no GNHPR convention produced enough rotation pairs")
+    convention, pairs = min(candidates, key=lambda item: scores[item[0].name])
+    rotation, biases, errors, covariance = _solve_core(items, pairs)
+    per_session = {}
+    for session in items:
+        values = [error for pair, error in zip(pairs, errors) if pair.session_id == session.session_id]
+        per_session[session.session_id] = (
+            float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
+        )
+    loo = {}
+    if compute_loo and len(items) >= 3:
+        for omitted in items:
+            kept_pairs = [
+                pair
+                for pair in pairs
+                if pair.session_id != omitted.session_id
+            ]
+            if len(kept_pairs) < 6:
+                loo[omitted.session_id] = np.nan
+                continue
+            def loo_residual(rotvec: np.ndarray) -> np.ndarray:
+                candidate = orthonormalize_rotation(so3_exp(rotvec))
+                rows = []
+                for pair in kept_pairs:
+                    corrected = apply_bias_jacobian_correction(
+                        pair.delta_R_zero_bias,
+                        pair.J_bg,
+                        biases[pair.session_index],
+                    )
+                    rows.append(
+                        np.sqrt(pair.weight)
+                        * so3_log(candidate.T @ pair.R_A @ candidate @ corrected.T)
+                    )
+                return np.concatenate(rows)
+
+            loo_opt = least_squares(
+                loo_residual,
+                so3_log(rotation),
+                loss='huber',
+                f_scale=np.deg2rad(0.5),
+                max_nfev=30,
+            )
+            loo_rotation = orthonormalize_rotation(so3_exp(loo_opt.x))
+            loo[omitted.session_id] = float(np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation))))
+    rotation_cov = covariance[:3, :3]
+    std_deg = np.degrees(np.sqrt(np.maximum(np.diag(rotation_cov), 0.0)))
+    singular_values = np.linalg.svd(np.linalg.pinv(rotation_cov, rcond=1e-12), compute_uv=False)
+    rms = float(np.sqrt(np.mean(errors**2)))
+    median = float(np.median(errors))
+    p95 = float(np.percentile(errors, 95.0))
+    finite_loo = [value for value in loo.values() if np.isfinite(value)]
+    sorted_scores = sorted(scores.values())
+    convention_gap = sorted_scores[1] - sorted_scores[0] if len(sorted_scores) > 1 else np.inf
+    ok = bool(
+        len(pairs) >= 20
+        and rms <= 1.0
+        and p95 <= 2.0
+        and float(np.max(std_deg)) <= 0.5
+        and (not finite_loo or max(finite_loo) <= 1.0)
+        and convention_gap >= 0.05
+    )
+    notes = [
+        "transform convention: p_RTK = R_RTK_IMU p_IMU",
+        f"residual time convention: t_IMU = t_RTK + {offset:+.6f} s",
+        f"GNHPR convention score gap={convention_gap:.4f} deg",
+        "GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
+        "LOO is conditional: per-session gyro biases are held at their all-session estimates",
+    ]
+    if not time_audit.reliable:
+        notes.append("time-offset correlation was ambiguous; held residual offset at zero")
+    if convention is not GNHPR_CANDIDATES[0]:
+        notes.append("empirical best GNHPR convention differs from protocol expectation; manual verification required")
+    if not ok:
+        notes.append("rotation failed one or more strict acceptance gates")
+    return RotationCalibrationResult(
+        R_RTK_IMU=rotation,
+        rpy_deg=rpy_deg_xyz(rotation),
+        gyro_bias_by_session_rad_s={
+            session.session_id: biases[index].copy() for index, session in enumerate(items)
+        },
+        time_offset=time_audit,
+        applied_time_offset_s=offset,
+        convention=convention,
+        convention_scores_deg=scores,
+        pair_count=len(pairs),
+        residual_rms_deg=rms,
+        residual_median_deg=median,
+        residual_p95_deg=p95,
+        rotation_std_deg=std_deg,
+        information_singular_values=singular_values,
+        per_session_rms_deg=per_session,
+        loo_delta_deg=loo,
+        ok=ok,
+        notes=tuple(notes),
+    )

imu_lidar/rtk_imu_translation.py

@@ -0,0 +1,389 @@
+"""Lever-arm calibration from RTK positions and full IMU preintegration."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.sparse import coo_matrix, csr_matrix, eye
+from scipy.sparse.linalg import lsqr, splu
+from scipy.spatial.transform import Rotation, Slerp
+
+from .geometry import make_transform, orthonormalize_rotation, so3_exp
+from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, _attitude_rows
+
+
+@dataclass(frozen=True)
+class TranslationCalibrationResult:
+    lever_IMU_to_RTK_in_IMU_m: np.ndarray
+    t_RTK_IMU_m: np.ndarray
+    T_RTK_IMU: np.ndarray
+    translation_std_m: np.ndarray
+    lever_information_singular_values: np.ndarray
+    lever_precision_rank: int
+    position_residual_rms_xyz_m: np.ndarray
+    velocity_residual_rms_xyz_m_s: np.ndarray
+    accel_bias_by_session_m_s2: dict[str, np.ndarray]
+    knot_count_by_session: dict[str, int]
+    loo_delta_m: dict[str, np.ndarray]
+    ok: bool
+    notes: tuple[str, ...]
+
+
+@dataclass(frozen=True)
+class _SessionFactors:
+    session: RotationSession
+    knot_t_s: np.ndarray
+    position_enu_m: np.ndarray
+    R_ENU_IMU: np.ndarray
+    delta_p: tuple[np.ndarray, ...]
+    delta_v: tuple[np.ndarray, ...]
+    J_p_ba: tuple[np.ndarray, ...]
+    J_v_ba: tuple[np.ndarray, ...]
+    duration_s: np.ndarray
+
+
+def _preintegrate_translation_interval(
+    times_s: np.ndarray,
+    gyro_rad_s: np.ndarray,
+    acc_m_s2: np.ndarray,
+    t0: float,
+    t1: float,
+    gyro_bias_rad_s: np.ndarray,
+) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, float]:
+    """Fast nominal ``delta_p/delta_v`` and accel-bias Jacobians.
+
+    Rotation covariance and gyro-bias Jacobians are deliberately omitted here:
+    rotation and gyro bias have already been fixed by Phase R1, while the
+    translation linear system only consumes the accelerometer-bias Jacobians.
+    """
+
+    left = max(int(np.searchsorted(times_s, t0, side='left') - 1), 0)
+    right = min(int(np.searchsorted(times_s, t1, side='right')), times_s.size - 1)
+    delta_r = np.eye(3)
+    delta_v = np.zeros(3)
+    delta_p = np.zeros(3)
+    j_v_ba = np.zeros((3, 3))
+    j_p_ba = np.zeros((3, 3))
+    for index in range(left, right):
+        sample_t0 = float(times_s[index])
+        sample_t1 = float(times_s[index + 1])
+        if sample_t1 <= t0 or sample_t0 >= t1:
+            continue
+        segment_t0 = max(sample_t0, t0)
+        segment_t1 = min(sample_t1, t1)
+        dt = segment_t1 - segment_t0
+        if dt <= 0.0:
+            continue
+        sample_dt = max(sample_t1 - sample_t0, 1e-12)
+        u0 = (segment_t0 - sample_t0) / sample_dt
+        u1 = (segment_t1 - sample_t0) / sample_dt
+        gyro0 = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
+        gyro1 = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
+        acc0 = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
+        acc1 = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
+        omega = 0.5 * (gyro0 + gyro1) - gyro_bias_rad_s
+        acc = 0.5 * (acc0 + acc1)
+        r_i = delta_r
+        delta_p = delta_p + delta_v * dt + 0.5 * r_i @ acc * dt**2
+        delta_v = delta_v + r_i @ acc * dt
+        j_p_ba = j_p_ba + j_v_ba * dt - 0.5 * r_i * dt**2
+        j_v_ba = j_v_ba - r_i * dt
+        delta_r = orthonormalize_rotation(delta_r @ so3_exp(omega * dt))
+    return delta_p, delta_v, j_p_ba, j_v_ba, float(max(t1 - t0, 0.0))
+
+
+def _make_session_factors(
+    session: RotationSession,
+    rotation: RotationCalibrationResult,
+    *,
+    knot_step_s: float,
+) -> _SessionFactors:
+    t_attitude, r_enu_rtk = _attitude_rows(session.rtk, rotation.convention)
+    position_valid = session.rtk.position_valid
+    t_position = session.rtk.t_s[position_valid]
+    position = session.rtk.position_enu_m[position_valid]
+    time_offset_s = rotation.applied_time_offset_s
+    start = max(float(t_attitude[0]), float(t_position[0]), float(session.imu.t_s[0] - time_offset_s))
+    end = min(float(t_attitude[-1]), float(t_position[-1]), float(session.imu.t_s[-1] - time_offset_s))
+    if end - start < 5.0:
+        raise ValueError(f"{session.session_id}: less than 5 s common RTK/IMU support")
+    knot_t = np.arange(start + 0.25, end - 0.25, knot_step_s)
+    if knot_t.size < 4:
+        raise ValueError(f"{session.session_id}: not enough translation knots")
+    position_knots = np.column_stack(
+        [np.interp(knot_t, t_position, position[:, axis]) for axis in range(3)]
+    )
+    r_enu_rtk_knots = Slerp(t_attitude, Rotation.from_matrix(r_enu_rtk))(knot_t).as_matrix()
+    r_enu_imu = r_enu_rtk_knots @ rotation.R_RTK_IMU
+    bg = rotation.gyro_bias_by_session_rad_s[session.session_id]
+    delta_p: list[np.ndarray] = []
+    delta_v: list[np.ndarray] = []
+    j_p_ba: list[np.ndarray] = []
+    j_v_ba: list[np.ndarray] = []
+    durations = []
+    for t0, t1 in zip(knot_t[:-1], knot_t[1:]):
+        dp, dv, jp, jv, duration = _preintegrate_translation_interval(
+            session.imu.t_s,
+            session.imu.gyro_rad_s,
+            session.imu.acc_m_s2,
+            float(t0 + time_offset_s),
+            float(t1 + time_offset_s),
+            bg,
+        )
+        delta_p.append(dp)
+        delta_v.append(dv)
+        j_p_ba.append(jp)
+        j_v_ba.append(jv)
+        durations.append(duration)
+    return _SessionFactors(
+        session=session,
+        knot_t_s=knot_t,
+        position_enu_m=position_knots,
+        R_ENU_IMU=r_enu_imu,
+        delta_p=tuple(delta_p),
+        delta_v=tuple(delta_v),
+        J_p_ba=tuple(j_p_ba),
+        J_v_ba=tuple(j_v_ba),
+        duration_s=np.asarray(durations),
+    )
+
+
+def _append_block(
+    rows: list[int],
+    cols: list[int],
+    values: list[float],
+    rhs: list[float],
+    groups: list[int],
+    matrix_blocks: list[tuple[int, np.ndarray]],
+    vector: np.ndarray,
+    sigma: np.ndarray,
+    group: int,
+) -> None:
+    row0 = len(rhs)
+    for axis in range(3):
+        rhs.append(float(vector[axis] / sigma[axis]))
+        groups.append(group)
+        for col0, block in matrix_blocks:
+            for local_col in range(block.shape[1]):
+                value = float(block[axis, local_col] / sigma[axis])
+                if value != 0.0:
+                    rows.append(row0 + axis)
+                    cols.append(col0 + local_col)
+                    values.append(value)
+
+
+def _build_system(
+    factors: list[_SessionFactors],
+    *,
+    position_sigma_xyz_m: np.ndarray,
+    velocity_sigma_xyz_m_s: np.ndarray,
+) -> tuple[csr_matrix, np.ndarray, np.ndarray, dict[str, tuple[int, int]], list[tuple[str, int, str]]]:
+    # x = [shared lever(3), per-session ba(3), per-knot velocities(3*K)]
+    offsets: dict[str, tuple[int, int]] = {}
+    variable_count = 3
+    for item in factors:
+        ba_offset = variable_count
+        velocity_offset = ba_offset + 3
+        offsets[item.session.session_id] = (ba_offset, velocity_offset)
+        variable_count = velocity_offset + 3 * item.knot_t_s.size
+    rows: list[int] = []
+    cols: list[int] = []
+    values: list[float] = []
+    rhs: list[float] = []
+    groups: list[int] = []
+    factor_labels: list[tuple[str, int, str]] = []
+    gravity = np.array([0.0, 0.0, -9.80665])
+    group = 0
+    for item in factors:
+        ba_offset, velocity_offset = offsets[item.session.session_id]
+        for index, dt in enumerate(item.duration_s):
+            r_i = item.R_ENU_IMU[index]
+            r_j = item.R_ENU_IMU[index + 1]
+            dp_rtk = item.position_enu_m[index + 1] - item.position_enu_m[index]
+            constant_p = dp_rtk - 0.5 * gravity * dt**2 - r_i @ item.delta_p[index]
+            _append_block(
+                rows,
+                cols,
+                values,
+                rhs,
+                groups,
+                [
+                    (0, r_i - r_j),
+                    (ba_offset, -r_i @ item.J_p_ba[index]),
+                    (velocity_offset + 3 * index, -dt * np.eye(3)),
+                ],
+                -constant_p,
+                position_sigma_xyz_m,
+                group,
+            )
+            factor_labels.append((item.session.session_id, group, "position"))
+            group += 1
+            constant_v = -gravity * dt - r_i @ item.delta_v[index]
+            _append_block(
+                rows,
+                cols,
+                values,
+                rhs,
+                groups,
+                [
+                    (ba_offset, -r_i @ item.J_v_ba[index]),
+                    (velocity_offset + 3 * index, -np.eye(3)),
+                    (velocity_offset + 3 * (index + 1), np.eye(3)),
+                ],
+                -constant_v,
+                velocity_sigma_xyz_m_s,
+                group,
+            )
+            factor_labels.append((item.session.session_id, group, "velocity"))
+            group += 1
+        # Loose physical bias prior. It prevents an unobservable constant
+        # acceleration from masquerading as gravity while remaining data-led.
+        _append_block(
+            rows,
+            cols,
+            values,
+            rhs,
+            groups,
+            [(ba_offset, np.eye(3))],
+            np.zeros(3),
+            np.full(3, 0.5),
+            group,
+        )
+        factor_labels.append((item.session.session_id, group, "bias_prior"))
+        group += 1
+    matrix = coo_matrix((values, (rows, cols)), shape=(len(rhs), variable_count)).tocsr()
+    return matrix, np.asarray(rhs), np.asarray(groups), offsets, factor_labels
+
+
+def _irls(matrix: csr_matrix, rhs: np.ndarray, groups: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    row_weights = np.ones(rhs.size)
+    solution = np.zeros(matrix.shape[1])
+    for _ in range(5):
+        weighted = matrix.multiply(row_weights[:, None])
+        solution = lsqr(weighted, rhs * row_weights, atol=1e-10, btol=1e-10, iter_lim=3000)[0]
+        residual = matrix @ solution - rhs
+        new_weights = np.ones_like(row_weights)
+        for group in np.unique(groups):
+            selection = groups == group
+            norm = float(np.linalg.norm(residual[selection]))
+            if norm > 3.0:
+                new_weights[selection] = np.sqrt(3.0 / norm)
+        if np.max(np.abs(new_weights - row_weights)) < 1e-3:
+            row_weights = new_weights
+            break
+        row_weights = new_weights
+    return solution, row_weights
+
+
+def _solve_factors(
+    factors: list[_SessionFactors],
+    position_sigma: np.ndarray,
+    velocity_sigma: np.ndarray,
+) -> tuple[np.ndarray, np.ndarray, csr_matrix, np.ndarray, dict[str, tuple[int, int]], np.ndarray, np.ndarray]:
+    matrix, rhs, groups, offsets, labels = _build_system(
+        factors,
+        position_sigma_xyz_m=position_sigma,
+        velocity_sigma_xyz_m_s=velocity_sigma,
+    )
+    solution, row_weights = _irls(matrix, rhs, groups)
+    weighted = matrix.multiply(row_weights[:, None]).tocsr()
+    residual = matrix @ solution - rhs
+    data_groups = {group for _, group, kind in labels if kind != "bias_prior"}
+    data_rows = np.isin(groups, list(data_groups))
+    variance = float(np.sum((residual[data_rows] * row_weights[data_rows]) ** 2) / max(np.count_nonzero(data_rows) - solution.size, 1))
+    information = (weighted.T @ weighted).tocsc() + eye(weighted.shape[1], format="csc") * 1e-10
+    h_ll = information[:3, :3].toarray()
+    h_ln = information[:3, 3:]
+    h_nn = information[3:, 3:]
+    nuisance_solve = splu(h_nn).solve(h_ln.T.toarray())
+    schur = h_ll - h_ln.toarray() @ nuisance_solve
+    covariance_lever = np.linalg.pinv(schur, rcond=1e-10) * variance
+    return solution, covariance_lever, matrix, rhs, offsets, groups, residual
+
+
+def solve_rtk_imu_translation(
+    sessions: list[RotationSession] | tuple[RotationSession, ...],
+    rotation: RotationCalibrationResult,
+    *,
+    knot_step_s: float = 2.0,
+    compute_loo: bool = True,
+) -> TranslationCalibrationResult:
+    """Estimate the shared IMU-to-RTK lever arm and return ``T_RTK_IMU``."""
+
+    items = list(sessions)
+    factors = [_make_session_factors(session, rotation, knot_step_s=knot_step_s) for session in items]
+    position_sigma = np.array([0.025, 0.025, 0.060])
+    velocity_sigma = np.array([0.08, 0.08, 0.12])
+    solution, covariance_l, matrix, rhs, offsets, groups, residual = _solve_factors(
+        factors, position_sigma, velocity_sigma
+    )
+    lever = solution[:3]
+    t_rtk_imu = -rotation.R_RTK_IMU @ lever
+    covariance_t = rotation.R_RTK_IMU @ covariance_l @ rotation.R_RTK_IMU.T
+    std_t = np.sqrt(np.maximum(np.diag(covariance_t), 0.0))
+    schur_information = np.linalg.pinv(covariance_l, rcond=1e-12)
+    singular_values = np.linalg.svd(schur_information, compute_uv=False)
+    threshold = max(float(singular_values[0]) * 1e-4, 1e-9)
+    rank = int(np.count_nonzero(singular_values > threshold))
+
+    # Recover physical residuals: system rows are grouped in XYZ triples and
+    # alternate position/velocity, followed by one bias prior per session.
+    position_errors: list[np.ndarray] = []
+    velocity_errors: list[np.ndarray] = []
+    cursor = 0
+    for item in factors:
+        for _ in range(item.knot_t_s.size - 1):
+            position_errors.append(residual[cursor : cursor + 3] * position_sigma)
+            cursor += 3
+            velocity_errors.append(residual[cursor : cursor + 3] * velocity_sigma)
+            cursor += 3
+        cursor += 3
+    pos_rms = np.sqrt(np.mean(np.asarray(position_errors) ** 2, axis=0))
+    vel_rms = np.sqrt(np.mean(np.asarray(velocity_errors) ** 2, axis=0))
+    biases = {
+        item.session.session_id: solution[offsets[item.session.session_id][0] : offsets[item.session.session_id][0] + 3].copy()
+        for item in factors
+    }
+    loo: dict[str, np.ndarray] = {}
+    if compute_loo and len(factors) >= 3:
+        for omitted in factors:
+            kept = [item for item in factors if item.session.session_id != omitted.session.session_id]
+            loo_solution, *_ = _solve_factors(kept, position_sigma, velocity_sigma)
+            loo[omitted.session.session_id] = (-rotation.R_RTK_IMU @ loo_solution[:3]) - t_rtk_imu
+    max_loo_xy = max((float(np.linalg.norm(value[:2])) for value in loo.values()), default=0.0)
+    max_loo_z = max((abs(float(value[2])) for value in loo.values()), default=0.0)
+    ok = bool(
+        rank == 3
+        and float(np.max(std_t[:2])) <= 0.05
+        and float(std_t[2]) <= 0.10
+        and float(np.max(pos_rms[:2])) <= 0.10
+        and float(pos_rms[2]) <= 0.20
+        and max_loo_xy <= 0.10
+        and max_loo_z <= 0.20
+    )
+    notes = [
+        "lever l is vector IMU-origin -> RTK-origin expressed in IMU",
+        "transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
+        "RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
+    ]
+    if not rotation.ok:
+        notes.append("upstream rotation is not accepted, so translation is diagnostic only")
+        ok = False
+    if not ok:
+        notes.append("translation failed one or more strict acceptance gates")
+    return TranslationCalibrationResult(
+        lever_IMU_to_RTK_in_IMU_m=lever,
+        t_RTK_IMU_m=t_rtk_imu,
+        T_RTK_IMU=make_transform(t_rtk_imu, rotation.R_RTK_IMU),
+        translation_std_m=std_t,
+        lever_information_singular_values=singular_values,
+        lever_precision_rank=rank,
+        position_residual_rms_xyz_m=pos_rms,
+        velocity_residual_rms_xyz_m_s=vel_rms,
+        accel_bias_by_session_m_s2=biases,
+        knot_count_by_session={item.session.session_id: int(item.knot_t_s.size) for item in factors},
+        loo_delta_m=loo,
+        ok=ok,
+        notes=tuple(notes),
+    )

imu_lidar/rtk_io.py

@@ -0,0 +1,116 @@
+"""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 .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
+    hdop: np.ndarray
+    origin_geodetic: tuple[float, float, float]
+    source: Path
+
+    @property
+    def attitude_valid(self) -> np.ndarray:
+        return (
+            np.isfinite(self.heading_deg)
+            & np.isfinite(self.pitch_deg)
+            & np.isfinite(self.roll_deg)
+            & np.isin(self.heading_quality, (4.0, 5.0))
+        )
+
+    @property
+    def position_valid(self) -> np.ndarray:
+        return np.all(np.isfinite(self.position_enu_m), axis=1) & np.isin(
+            self.fix_quality, (4.0, 5.0)
+        )
+
+
+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],
+        hdop=_column(data, "hdop")[order],
+        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])