"""Rotation and residual time-offset calibration between G90 RTK and HI13 IMU.""" from __future__ import annotations from dataclasses import dataclass, replace import numpy as np from scipy.optimize import least_squares from scipy.sparse import lil_matrix from imu_lidar.contracts import ImuSeries, MotionPair from imu_lidar.geometry import orthonormalize_rotation, rpy_deg_xyz, so3_exp, so3_log from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro from imu_lidar.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 method: str peak_width_s: tuple[float, float] per_session_offset_s: dict[str, float] per_session_peak_correlation: dict[str, float] @dataclass(frozen=True) class BaselineConsistencyAudit: """Two-DOF audit using only the physically observed ANT1-to-ANT2 axis.""" baseline_axis_imu: np.ndarray pair_count: int residual_rms_deg: float residual_median_deg: float residual_p95_deg: float per_session_rms_deg: dict[str, float] per_session_p95_deg: dict[str, float] per_session_axis_rms_deg: dict[str, np.ndarray] gyro_bias_by_session_rad_s: dict[str, np.ndarray] worst_pairs: tuple[dict[str, object], ...] ok: bool notes: tuple[str, ...] @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] baseline_consistency: BaselineConsistencyAudit observable_rotation_dof: int full_attitude_observable: bool legacy_full_attitude_numeric_ok: bool 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 t0_s: float t1_s: 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_heading_rate(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]: """Return signed vehicle yaw rate from the ANT1-to-ANT2 azimuth. ANT1-to-ANT2 points vehicle-right, so its clockwise heading increases when mathematical body yaw decreases. Only this signed heading channel is compared with IMU gyro_z; rotation about the baseline is unobservable. """ dt = np.diff(t) baseline = rotations[:, :, 0] heading = np.unwrap(np.arctan2(baseline[:, 0], baseline[:, 1])) rate = -np.diff(heading) / np.maximum(dt, 1e-6) valid = ( (dt >= 0.03) & (dt <= 0.25) & (np.abs(rate) >= np.deg2rad(0.5)) & (np.abs(rate) <= np.deg2rad(30.0)) ) return 0.5 * (t[:-1] + t[1:])[valid], rate[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: """Audit residual t_IMU - t_RTK from signed heading rate. A broad correlation peak remains diagnostic. It is never applied unless both peak separation and peak width pass. """ offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s) session_series: list[tuple[str, 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_rate = _rtk_heading_rate(t_rtk, rotations) imu_rate = session.imu.gyro_rad_s[:, 2] if midpoint.size >= 20: session_series.append( (session.session_id, midpoint, rtk_rate, session.imu.t_s, imu_rate) ) total_samples += int(midpoint.size) if not session_series: return TimeOffsetAudit( 0.0, np.nan, np.nan, 0, False, "signed_heading_rate_vs_imu_gyro_z", (np.nan, np.nan), {}, {}, ) scores = [] per_session_scores: dict[str, list[float]] = { session_id: [] for session_id, *_ in session_series } for offset in offsets: per_session = [] for session_id, midpoint, rtk_rate, imu_t, imu_rate in session_series: query = midpoint + offset inside = (query >= imu_t[0]) & (query <= imu_t[-1]) if np.count_nonzero(inside) < 20: per_session_scores[session_id].append(np.nan) continue interpolated = np.interp(query[inside], imu_t, imu_rate) value = _correlation(rtk_rate[inside], interpolated) per_session_scores[session_id].append(value) 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, "signed_heading_rate_vs_imu_gyro_z", (np.nan, np.nan), {}, {}, ) 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]) near_peak = np.flatnonzero(values >= peak - 0.005) peak_width = ( (float(offsets[near_peak[0]]), float(offsets[near_peak[-1]])) if near_peak.size else (np.nan, np.nan) ) per_session_offset = {} per_session_peak = {} for session_id, session_values in per_session_scores.items(): array = np.asarray(session_values, dtype=float) if np.any(np.isfinite(array)): index = int(np.nanargmax(array)) per_session_offset[session_id] = float(offsets[index]) per_session_peak[session_id] = float(array[index]) reliable = bool( peak >= 0.5 and (not np.isfinite(second) or peak - second >= 0.015) and np.isfinite(peak_width[0]) and peak_width[1] - peak_width[0] <= 0.03 ) return TimeOffsetAudit( float(offsets[best_index]), peak, second, total_samples, reliable, "signed_heading_rate_vs_imu_gyro_z", peak_width, per_session_offset, per_session_peak, ) 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) dt = np.diff(t) baseline = rotations[:, :, 0] baseline_step = np.arccos( np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0) ) broken_edge = ( (dt < 0.03) | (dt > 0.25) | (baseline_step / np.maximum(dt, 1e-6) > np.deg2rad(45.0)) ) broken_prefix = np.concatenate([[0], np.cumsum(broken_edge.astype(int))]) 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 if broken_prefix[j] - broken_prefix[i] != 0: 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, t0_s=float(t[i]), t1_s=float(t[j]), ) ) # Equalize total influence per session. Pair count and excitation otherwise # let long/high-motion sessions dominate the shared rotation. totals = { session.session_id: sum( pair.weight for pair in pairs if pair.session_id == session.session_id ) for session in sessions } nonzero = [value for value in totals.values() if value > 0.0] target = float(np.mean(nonzero)) if nonzero else 1.0 return [ replace(pair, weight=pair.weight * target / totals[pair.session_id]) for pair in pairs if totals[pair.session_id] > 0.0 ] 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_baseline_consistency( sessions: list[RotationSession], pairs: list[_Pair], ) -> BaselineConsistencyAudit: """Audit the two physically observable dual-antenna rotation DOFs. The confirmed ANT1-to-ANT2 axis is IMU +X. For every interval, the angle swept by the GNSS baseline must equal the angle swept by IMU +X under gyro preintegration. Rotation about +X cancels from this invariant and is not falsely scored as an RTK attitude residual. """ baseline_axis = np.array([1.0, 0.0, 0.0]) session_count = len(sessions) def raw_errors(parameters: np.ndarray) -> np.ndarray: biases = parameters.reshape(session_count, 3) values = [] for pair in pairs: corrected = apply_bias_jacobian_correction( pair.delta_R_zero_bias, pair.J_bg, biases[pair.session_index], ) observed = np.arccos(np.clip(pair.R_A[0, 0], -1.0, 1.0)) predicted = np.arccos( np.clip(baseline_axis @ corrected @ baseline_axis, -1.0, 1.0) ) values.append(predicted - observed) return np.asarray(values) def residual(parameters: np.ndarray) -> np.ndarray: errors = raw_errors(parameters) weighted = errors * np.sqrt(np.asarray([pair.weight for pair in pairs])) return np.concatenate([weighted, parameters / 0.003]) initial = np.zeros(3 * session_count) opt = least_squares( residual, initial, loss="huber", f_scale=np.deg2rad(0.25), max_nfev=60, ) biases = opt.x.reshape(session_count, 3) errors_deg = np.degrees(raw_errors(opt.x)) per_session_rms = {} per_session_p95 = {} per_session_axis_rms = {} for session in sessions: selection = np.asarray( [pair.session_id == session.session_id for pair in pairs], dtype=bool ) values = errors_deg[selection] per_session_rms[session.session_id] = ( float(np.sqrt(np.mean(values**2))) if values.size else np.nan ) per_session_p95[session.session_id] = ( float(np.percentile(np.abs(values), 95.0)) if values.size else np.nan ) axis_errors = [] for pair in np.asarray(pairs, dtype=object)[selection]: corrected = apply_bias_jacobian_correction( pair.delta_R_zero_bias, pair.J_bg, biases[pair.session_index], ) axis_errors.append(np.degrees(so3_log(pair.R_A @ corrected.T))) per_session_axis_rms[session.session_id] = ( np.sqrt(np.mean(np.asarray(axis_errors) ** 2, axis=0)) if axis_errors else np.full(3, np.nan) ) worst_indices = np.argsort(np.abs(errors_deg))[-20:][::-1] worst_pairs = tuple( { "session_id": pairs[index].session_id, "t0_s": pairs[index].t0_s, "t1_s": pairs[index].t1_s, "duration_s": pairs[index].t1_s - pairs[index].t0_s, "baseline_angle_residual_deg": float(errors_deg[index]), } for index in worst_indices ) rms = float(np.sqrt(np.mean(errors_deg**2))) median = float(np.median(np.abs(errors_deg))) p95 = float(np.percentile(np.abs(errors_deg), 95.0)) finite_session_rms = [ value for value in per_session_rms.values() if np.isfinite(value) ] finite_session_p95 = [ value for value in per_session_p95.values() if np.isfinite(value) ] ok = bool( len(pairs) >= 20 and rms <= 1.0 and p95 <= 2.0 and (not finite_session_rms or max(finite_session_rms) <= 1.5) and (not finite_session_p95 or max(finite_session_p95) <= 3.0) ) return BaselineConsistencyAudit( baseline_axis_imu=baseline_axis, pair_count=len(pairs), residual_rms_deg=rms, residual_median_deg=median, residual_p95_deg=p95, per_session_rms_deg=per_session_rms, per_session_p95_deg=per_session_p95, per_session_axis_rms_deg=per_session_axis_rms, gyro_bias_by_session_rad_s={ session.session_id: biases[index].copy() for index, session in enumerate(sessions) }, worst_pairs=worst_pairs, ok=ok, notes=( "ANT1(main,left)->ANT2(secondary,right) is fixed to IMU +X", "axis residual XYZ labels are baseline-spin(unobservable), baseline-elevation, heading", "full rotation about the baseline is not identifiable from two antennas", ), ) 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 ) baseline_audit = _solve_baseline_consistency(items, pairs) loo = {} if compute_loo and len(items) >= 3: for omitted in items: kept_items = [item for item in items if item.session_id != omitted.session_id] kept_index = {item.session_id: index for index, item in enumerate(kept_items)} kept_pairs = [ replace(pair, session_index=kept_index[pair.session_id]) for pair in pairs if pair.session_id != omitted.session_id ] if len(kept_pairs) < 6: loo[omitted.session_id] = np.nan continue try: loo_rotation, _, _, _ = _solve_core(kept_items, kept_pairs) except ValueError: loo[omitted.session_id] = np.nan continue 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 legacy_numeric_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 ) # GNHPR supplies the ANT1-to-ANT2 direction but no independent rotation # about that direction. A completed 3-D attitude is useful diagnostically, # but cannot pass the full extrinsic-rotation gate from this dataset alone. full_attitude_observable = False ok = False 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 re-optimizes the remaining per-session gyro biases", "legacy full-HPR rotation uses a zero-roll gauge completion and is diagnostic only", "dual antennas do not observe rotation about the ANT1-to-ANT2 baseline", ] 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 baseline_audit.ok: notes.append("the physically observable baseline consistency failed strict gates") if not legacy_numeric_ok: notes.append("the legacy gauge-completed rotation failed one or more numeric gates") notes.append("full rotation is not accepted; translation must remain frozen") 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, baseline_consistency=baseline_audit, observable_rotation_dof=2, full_attitude_observable=full_attitude_observable, legacy_full_attitude_numeric_ok=legacy_numeric_ok, ok=ok, notes=tuple(notes), )