"""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), )