"""Session-aware Phase-A rotation/bias comparison and observability analysis.""" from __future__ import annotations from collections import Counter from collections.abc import Callable, Mapping from dataclasses import dataclass, field, replace from typing import Any import numpy as np from scipy.optimize import least_squares from scipy.spatial.transform import Rotation from .contracts import ImuSeries, MotionPair from .geometry import ( orthonormalize_rotation, rotation_angle_deg, rpy_deg_xyz, so3_exp, so3_log, ) from .imu_preintegration import ( apply_bias_jacobian_correction, preintegrate_gyro, preintegration_rotation_residual, residual_whiten_matrix, ) from .rotation_handeye import ( estimate_rotation_handeye_initial, select_strong_rotation_pairs, ) @dataclass(frozen=True) class PhaseASessionDiagnostics: session_id: str pair_count: int gyro_bias0_rad_s: np.ndarray gyro_bias_rad_s: np.ndarray residual_rms_deg: float residual_median_deg: float residual_p95_deg: float outlier_fraction_gt_5deg: float accepted: bool included_in_final: bool = True @dataclass(frozen=True) class PhaseAVariantResult: name: str R_IMU_lidar: np.ndarray rpy_deg_xyz: np.ndarray gyro_bias_rad_s_per_session: dict[str, np.ndarray] sessions: tuple[PhaseASessionDiagnostics, ...] residual_rms_deg: float residual_median_deg: float residual_p95_deg: float outlier_fraction_gt_5deg: float accepted: bool rotation_prior_used: bool optimizer_success: bool optimizer_nfev: int cost: float residual_count: int parameter_count: int jacobian: np.ndarray = field(repr=False) @dataclass(frozen=True) class PhaseAMarginalObservability: rank: int condition: float eigenvalues: np.ndarray weakest_direction_left_tangent: np.ndarray covariance_rotvec_rad2: np.ndarray yaw_std_deg: float direction_std_deg: np.ndarray precision_rank: int residual_variance_scale: float hessian_marginal: np.ndarray ok: bool notes: tuple[str, ...] = () @dataclass(frozen=True) class PhaseALeaveOneOutResult: held_out_session: str data_only_rpy_deg_xyz: np.ndarray prior_rpy_deg_xyz: np.ndarray data_only_gyro_bias_rad_s_per_session: dict[str, np.ndarray] prior_gyro_bias_rad_s_per_session: dict[str, np.ndarray] data_only_training_rms_deg: float data_only_training_p95_deg: float prior_training_rms_deg: float prior_training_p95_deg: float held_out_static_bg_rms_deg: float held_out_static_bg_p95_deg: float held_out_fitted_bg_rms_deg: float held_out_fitted_bg_p95_deg: float held_out_fitted_bg_rad_s: np.ndarray @dataclass(frozen=True) class PhaseAPriorSensitivityResult: sigma_deg: float rpy_deg_xyz: np.ndarray rotation_difference_from_data_deg: float residual_rms_deg: float residual_p95_deg: float @dataclass(frozen=True) class PhaseAComparisonResult: fixed_bg_data_only: PhaseAVariantResult session_bg_data_only: PhaseAVariantResult session_bg_with_rotation_prior: PhaseAVariantResult observable_subspace_with_prior: PhaseAVariantResult | None marginal_observability: PhaseAMarginalObservability leave_one_out: tuple[PhaseALeaveOneOutResult, ...] strong_pair_counts_per_session: dict[str, int] excluded_sessions: tuple[str, ...] data_vs_prior_yaw_diff_deg: float data_vs_prior_geodesic_deg: float leave_one_out_yaw_range_deg: float leave_one_out_observable_max_deg: float prior_sensitivity: tuple[PhaseAPriorSensitivityResult, ...] strong_pair_candidate_count: int decorrelated_pair_count: int decorrelation_block_s: float solution_status: str recommended_result: str partial_accepted: bool accepted: bool acceptance_checks: dict[str, bool] notes: tuple[str, ...] = () ProgressCallback = Callable[[str, dict[str, Any]], None] def _emit(callback: ProgressCallback | None, event: str, **fields: Any) -> None: if callback is not None: callback(event, fields) def _pair_cov(pair: MotionPair) -> np.ndarray: raw = pair.metadata.get("cov") if raw is not None: return np.asarray(raw, dtype=float).reshape(3, 3) sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2)) return np.eye(3) * max(sigma, 1e-4) ** 2 def _pair_rotation_information(pair: MotionPair) -> float: """Approximate independent rotation information carried by one factor.""" angle_rad = np.deg2rad( min( float(pair.metadata.get("rotation_deg_A", rotation_angle_deg(pair.R_A))), float(pair.metadata.get("rotation_deg_B", rotation_angle_deg(pair.R_B))), ) ) variance = max(float(np.trace(_pair_cov(pair))) / 3.0, 1e-12) fitness = float(np.clip(pair.fitness, 0.1, 1.0)) return float((angle_rad * angle_rad / variance) * fitness) def select_decorrelated_phase_a_pairs( pairs: list[MotionPair] | tuple[MotionPair, ...], *, block_s: float = 3.0, max_pairs_per_block: int = 1, ) -> list[MotionPair]: """Keep the most informative factors in non-overlapping time blocks. Sliding-window motion pairs share most IMU and LiDAR samples. Treating all of them as independent makes covariance and leave-one-out tests far too optimistic. A per-session time block is therefore the statistical unit. """ values = list(pairs) if block_s <= 0.0 or max_pairs_per_block <= 0: return values grouped: dict[tuple[str, int], list[MotionPair]] = {} for pair in values: t0 = float(pair.metadata.get("t_i_imu_s", pair.t_i_s)) t1 = float(pair.metadata.get("t_j_imu_s", pair.t_j_s)) block = int(np.floor((0.5 * (t0 + t1)) / block_s)) grouped.setdefault((pair.session_id, block), []).append(pair) selected: list[MotionPair] = [] for key in sorted(grouped): ranked = sorted( grouped[key], key=lambda pair: ( -_pair_rotation_information(pair), float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s)), pair.i, pair.j, ), ) selected.extend(ranked[:max_pairs_per_block]) return selected def _pair_bias0(pair: MotionPair, fallback: np.ndarray) -> np.ndarray: raw = pair.metadata.get("gyro_bias0_rad_s") if raw is None: return np.asarray(fallback, dtype=float).reshape(3) return np.asarray(raw, dtype=float).reshape(3) def phase_a_metadata_complete(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> bool: """Return whether exact Phase-A bias correction metadata is present.""" return all("J_bg" in pair.metadata and "cov" in pair.metadata for pair in pairs) def rehydrate_phase_a_pairs( pairs: list[MotionPair] | tuple[MotionPair, ...], *, imu_by_session: Mapping[str, ImuSeries], bias0_by_session: Mapping[str, np.ndarray], progress_callback: ProgressCallback | None = None, ) -> tuple[list[MotionPair], dict[str, Any]]: """Recompute only gyro preintegration metadata; LiDAR registration is untouched.""" enriched: list[MotionPair] = [] errors_deg: list[float] = [] total = len(pairs) for index, pair in enumerate(pairs, start=1): if pair.session_id not in imu_by_session: raise KeyError(f"missing IMU for session {pair.session_id!r}") imu = imu_by_session[pair.session_id] fallback = np.asarray(bias0_by_session[pair.session_id], dtype=float).reshape(3) bias0 = _pair_bias0(pair, fallback) try: t0 = float(pair.metadata["t_i_imu_s"]) t1 = float(pair.metadata["t_j_imu_s"]) except KeyError as exc: raise ValueError( f"pair {pair.session_id}:{pair.i}->{pair.j} lacks cached IMU time bounds" ) from exc preint = preintegrate_gyro(imu.t_s, imu.gyro_rad_s, t0, t1, bias0) error_deg = rotation_angle_deg(preint.delta_R.T @ pair.R_A) errors_deg.append(error_deg) metadata = dict(pair.metadata) metadata.update( { "J_bg": preint.J_bg.tolist(), "cov": preint.cov.tolist(), "duration_s": float(preint.duration_s), "mean_gyro_norm": float(preint.mean_gyro_norm), "preint_sigma_rad": float(preint.sigma_rad), "weight": float(preint.weight), "gyro_bias0_rad_s": bias0.tolist(), "phase_a_metadata_rehydrated": True, "rehydrated_R_A_error_deg": float(error_deg), } ) enriched.append(replace(pair, metadata=metadata)) if index == 1 or index == total or index % 500 == 0: _emit( progress_callback, "rehydrate_progress", processed=index, total=total, progress_pct=100.0 * index / max(total, 1), session=pair.session_id, max_R_A_error_deg=max(errors_deg), ) report = { "pair_count": total, "max_R_A_error_deg": max(errors_deg) if errors_deg else 0.0, "rms_R_A_error_deg": ( float(np.sqrt(np.mean(np.square(errors_deg)))) if errors_deg else 0.0 ), } return enriched, report def _bias_bases( pairs: list[MotionPair], supplied: Mapping[str, np.ndarray] | None ) -> dict[str, np.ndarray]: supplied_map = {} if supplied is None else supplied result: dict[str, np.ndarray] = {} for session_id in sorted({pair.session_id for pair in pairs}): if session_id in supplied_map: result[session_id] = np.asarray(supplied_map[session_id], dtype=float).reshape(3) continue pair = next(item for item in pairs if item.session_id == session_id) result[session_id] = _pair_bias0(pair, np.zeros(3)) return result def _rotation_distribution( errors_deg: list[float], ) -> tuple[float, float, float, float, bool]: if not errors_deg: return 1e9, 1e9, 1e9, 1.0, False values = np.asarray(errors_deg, dtype=float) rms = float(np.sqrt(np.mean(values**2))) median = float(np.median(values)) p95 = float(np.percentile(values, 95.0)) outlier_fraction = float(np.mean(values > 5.0)) accepted = ( len(values) >= 3 and rms < 1.5 and median < 0.5 and p95 < 1.5 and outlier_fraction <= 0.005 ) return rms, median, p95, outlier_fraction, accepted def _corrected_delta_r( pair: MotionPair, bias: np.ndarray, bias0: np.ndarray ) -> np.ndarray: raw = pair.metadata.get("J_bg") if raw is None: raise ValueError( "Phase-A bias optimization requires J_bg; rehydrate the v1 cache from IMU first" ) return apply_bias_jacobian_correction( pair.R_A, np.asarray(raw, dtype=float).reshape(3, 3), bias - bias0, ) def _wrap_angle_deg(value: float) -> float: return float((value + 180.0) % 360.0 - 180.0) def _session_balance_scales(pairs: list[MotionPair]) -> dict[str, float]: information: dict[str, float] = {} for pair in pairs: information[pair.session_id] = ( information.get(pair.session_id, 0.0) + _pair_rotation_information(pair) ) target = float(np.mean(list(information.values()))) return { session_id: float( np.clip(np.sqrt(target / max(value, 1e-12)), 0.25, 4.0) ) for session_id, value in information.items() } class _PhaseAProblem: def __init__( self, pairs: list[MotionPair], bias_bases: Mapping[str, np.ndarray], *, bias_prior_sigma_rad_s: float, ) -> None: if not pairs: raise ValueError("Phase-A requires at least one strong-rotation pair") self.pairs = list(pairs) self.session_ids = sorted({pair.session_id for pair in pairs}) self.session_index = {sid: index for index, sid in enumerate(self.session_ids)} self.bias_bases = { sid: np.asarray(bias_bases[sid], dtype=float).reshape(3) for sid in self.session_ids } self.whiten = np.stack( [residual_whiten_matrix(_pair_cov(pair)) for pair in pairs] ) self.session_scales = _session_balance_scales(pairs) self.balance_values = np.asarray( [self.session_scales[pair.session_id] for pair in pairs], dtype=float, ) self.pair_session_indices = np.asarray( [self.session_index[pair.session_id] for pair in pairs], dtype=int, ) self.r_a = np.stack([pair.R_A for pair in pairs]) self.r_b = np.stack([pair.R_B for pair in pairs]) self.j_bg = np.stack( [ np.asarray( pair.metadata.get("J_bg", np.zeros((3, 3))), dtype=float, ).reshape(3, 3) for pair in pairs ] ) self.pair_bias0 = np.stack( [ _pair_bias0(pair, self.bias_bases[pair.session_id]) for pair in pairs ] ) self.bias_prior_sigma = max(float(bias_prior_sigma_rad_s), 1e-6) def _unpack( self, vector: np.ndarray, rotation_seed: np.ndarray, *, optimize_bias: bool, rotation_basis: np.ndarray, ) -> tuple[np.ndarray, dict[str, np.ndarray]]: rotation_dim = int(rotation_basis.shape[1]) rotation_delta = rotation_basis @ vector[:rotation_dim] rotation = orthonormalize_rotation(so3_exp(rotation_delta) @ rotation_seed) if not optimize_bias: return rotation, { sid: value.copy() for sid, value in self.bias_bases.items() } biases = { sid: vector[ rotation_dim + 3 * index : rotation_dim + 3 * (index + 1) ].copy() for sid, index in self.session_index.items() } return rotation, biases def _batch_error_vectors( self, rotation: np.ndarray, biases: Mapping[str, np.ndarray], *, optimize_bias: bool, ) -> np.ndarray: if optimize_bias: bias_matrix = np.stack( [biases[sid] for sid in self.session_ids] ) delta_bias = ( bias_matrix[self.pair_session_indices] - self.pair_bias0 ) correction_vectors = np.einsum( "nij,nj->ni", self.j_bg, delta_bias ) corrections = Rotation.from_rotvec( correction_vectors ).as_matrix() delta_r = np.matmul(self.r_a, corrections) else: delta_r = self.r_a predicted = np.matmul( np.matmul(rotation[None, :, :], self.r_b), rotation.T[None, :, :], ) error_matrices = np.matmul( np.swapaxes(delta_r, 1, 2), predicted, ) return Rotation.from_matrix(error_matrices).as_rotvec() def raw_errors( self, rotation: np.ndarray, biases: Mapping[str, np.ndarray], pairs: list[MotionPair] | None = None, ) -> list[float]: if pairs is None: vectors = self._batch_error_vectors( rotation, biases, optimize_bias=True, ) return np.degrees(np.linalg.norm(vectors, axis=1)).tolist() errors: list[float] = [] for pair in pairs: bias0 = _pair_bias0( pair, self.bias_bases[pair.session_id] ) delta_r = _corrected_delta_r( pair, biases[pair.session_id], bias0 ) error = preintegration_rotation_residual( delta_r, rotation, pair.R_B ) errors.append( float(np.degrees(np.linalg.norm(error))) ) return errors def solve( self, *, name: str, rotation_seed: np.ndarray, optimize_bias: bool, rotation_prior: np.ndarray | None = None, rotation_prior_sigma_deg: float = 15.0, bias_seed: Mapping[str, np.ndarray] | None = None, rotation_basis: np.ndarray | None = None, max_nfev: int = 200, ) -> PhaseAVariantResult: if optimize_bias and not phase_a_metadata_complete(self.pairs): raise ValueError( "Phase-A bias optimization requires J_bg/cov for every strong pair" ) basis = ( np.eye(3) if rotation_basis is None else np.asarray(rotation_basis, dtype=float).reshape(3, -1) ) if basis.shape[1] and not np.allclose( basis.T @ basis, np.eye(basis.shape[1]), atol=1e-6 ): raise ValueError("rotation_basis columns must be orthonormal") rotation_dim = int(basis.shape[1]) dimension = rotation_dim + ( 3 * len(self.session_ids) if optimize_bias else 0 ) x0 = np.zeros(dimension) if optimize_bias: source = self.bias_bases if bias_seed is None else bias_seed for sid, index in self.session_index.items(): x0[ rotation_dim + 3 * index : rotation_dim + 3 * (index + 1) ] = np.asarray( source[sid], dtype=float ).reshape(3) prior = ( None if rotation_prior is None else orthonormalize_rotation( np.asarray(rotation_prior, dtype=float).reshape(3, 3) ) ) prior_sigma_rad = max( np.deg2rad(float(rotation_prior_sigma_deg)), 1e-6 ) def residual(vector: np.ndarray) -> np.ndarray: rotation, biases = self._unpack( vector, rotation_seed, optimize_bias=optimize_bias, rotation_basis=basis, ) error_vectors = self._batch_error_vectors( rotation, biases, optimize_bias=optimize_bias, ) weighted = np.einsum( "nij,nj->ni", self.whiten, error_vectors ) rows: list[np.ndarray] = [ ( self.balance_values[:, None] * weighted ).reshape(-1) ] if optimize_bias: for sid in self.session_ids: rows.append( (biases[sid] - self.bias_bases[sid]) / self.bias_prior_sigma ) if prior is not None: rows.append(so3_log(prior.T @ rotation) / prior_sigma_rad) return np.concatenate(rows) optimum = least_squares( residual, x0, loss="huber", f_scale=1.0, max_nfev=max_nfev, ) rotation, biases = self._unpack( optimum.x, rotation_seed, optimize_bias=optimize_bias, rotation_basis=basis, ) global_errors = self.raw_errors(rotation, biases) rms, median, p95, outlier, global_accepted = _rotation_distribution( global_errors ) sessions: list[PhaseASessionDiagnostics] = [] for sid in self.session_ids: local_pairs = [ pair for pair in self.pairs if pair.session_id == sid ] local_errors = self.raw_errors(rotation, biases, local_pairs) srms, smedian, sp95, soutlier, saccepted = _rotation_distribution( local_errors ) sessions.append( PhaseASessionDiagnostics( session_id=sid, pair_count=len(local_pairs), gyro_bias0_rad_s=self.bias_bases[sid].copy(), gyro_bias_rad_s=biases[sid].copy(), residual_rms_deg=srms, residual_median_deg=smedian, residual_p95_deg=sp95, outlier_fraction_gt_5deg=soutlier, accepted=saccepted, ) ) accepted = bool( optimum.success and global_accepted and all(item.accepted for item in sessions) ) return PhaseAVariantResult( name=name, R_IMU_lidar=rotation, rpy_deg_xyz=rpy_deg_xyz(rotation), gyro_bias_rad_s_per_session=biases, sessions=tuple(sessions), residual_rms_deg=rms, residual_median_deg=median, residual_p95_deg=p95, outlier_fraction_gt_5deg=outlier, accepted=accepted, rotation_prior_used=prior is not None, optimizer_success=bool(optimum.success), optimizer_nfev=int(optimum.nfev), cost=float(optimum.cost), residual_count=int(optimum.fun.size), parameter_count=int(optimum.x.size), jacobian=np.asarray(optimum.jac, dtype=float), ) def _yaw_rad(rotation: np.ndarray) -> float: return float(np.arctan2(rotation[1, 0], rotation[0, 0])) def _marginal_observability( variant: PhaseAVariantResult, *, direction_std_max_deg: float, ) -> PhaseAMarginalObservability: jacobian = np.asarray(variant.jacobian, dtype=float) hessian = jacobian.T @ jacobian h_rr = hessian[:3, :3] notes: list[str] = [] if hessian.shape[0] > 3: h_rb = hessian[:3, 3:] h_bb = hessian[3:, 3:] singular_b = np.linalg.svd(h_bb, compute_uv=False) if singular_b[-1] <= max(singular_b[0] * 1e-12, 1e-12): notes.append( "H_bb is rank-deficient; Schur complement uses pseudoinverse" ) h_marginal = ( h_rr - h_rb @ np.linalg.pinv(h_bb, rcond=1e-12) @ h_rb.T ) else: h_marginal = h_rr h_marginal = 0.5 * (h_marginal + h_marginal.T) eigenvalues, eigenvectors = np.linalg.eigh(h_marginal) max_eigen = max(float(eigenvalues[-1]), 1e-18) rank_threshold = max(max_eigen * 1e-6, 1e-9) rank = int(np.sum(eigenvalues > rank_threshold)) positive = eigenvalues[eigenvalues > rank_threshold] condition = ( float(max_eigen / positive[0]) if positive.size == 3 else float("inf") ) dof = max(variant.residual_count - variant.parameter_count, 1) sigma2 = max(2.0 * variant.cost / dof, 1e-12) covariance = sigma2 * np.linalg.pinv(h_marginal, rcond=1e-12) direction_variances = np.full(3, np.inf) observable_eigen = eigenvalues > rank_threshold direction_variances[observable_eigen] = ( sigma2 / eigenvalues[observable_eigen] ) direction_std_deg = np.degrees(np.sqrt(direction_variances)) precision_rank = int( np.sum(direction_std_deg <= float(direction_std_max_deg)) ) epsilon = 1e-6 yaw0 = _yaw_rad(variant.R_IMU_lidar) yaw_gradient = np.zeros(3) for axis in range(3): delta = np.zeros(3) delta[axis] = epsilon perturbed = orthonormalize_rotation( so3_exp(delta) @ variant.R_IMU_lidar ) difference = np.arctan2( np.sin(_yaw_rad(perturbed) - yaw0), np.cos(_yaw_rad(perturbed) - yaw0), ) yaw_gradient[axis] = difference / epsilon yaw_coefficients = eigenvectors.T @ yaw_gradient if np.any( (~observable_eigen) & (np.abs(yaw_coefficients) > 1e-6) ): yaw_variance = float("inf") else: yaw_variance = float( np.sum( np.square(yaw_coefficients[observable_eigen]) * direction_variances[observable_eigen] ) ) yaw_std_deg = float(np.degrees(np.sqrt(max(yaw_variance, 0.0)))) if rank < 3: notes.append(f"marginalized rotation rank={rank}/3") if precision_rank < 3: notes.append( "only " f"{precision_rank}/3 rotation directions meet the " f"{direction_std_max_deg:.3f} deg precision threshold" ) notes.append( "formal yaw std uses the robust, session-balanced linearized Phase-A " "model; leave-one-session-out remains the primary stability check" ) return PhaseAMarginalObservability( rank=rank, condition=condition, eigenvalues=eigenvalues, weakest_direction_left_tangent=eigenvectors[:, 0], covariance_rotvec_rad2=covariance, yaw_std_deg=yaw_std_deg, direction_std_deg=direction_std_deg, precision_rank=precision_rank, residual_variance_scale=float(sigma2), hessian_marginal=h_marginal, ok=bool(precision_rank == 3 and np.isfinite(yaw_std_deg)), notes=tuple(notes), ) def _evaluate_static_bias( pairs: list[MotionPair], rotation: np.ndarray ) -> tuple[float, float]: errors = [ float( np.degrees( np.linalg.norm( preintegration_rotation_residual( pair.R_A, rotation, pair.R_B ) ) ) ) for pair in pairs ] rms, _, p95, _, _ = _rotation_distribution(errors) return rms, p95 def _solve_three_variants( pairs: list[MotionPair], bias_bases: Mapping[str, np.ndarray], *, rotation_prior: np.ndarray | None, rotation_prior_sigma_deg: float, bias_prior_sigma_rad_s: float, max_nfev: int, progress_callback: ProgressCallback | None = None, solve_label: str = "full", ) -> tuple[PhaseAVariantResult, PhaseAVariantResult, PhaseAVariantResult]: data_seed = estimate_rotation_handeye_initial(pairs) problem = _PhaseAProblem( pairs, bias_bases, bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, ) _emit( progress_callback, "variant_start", solve_label=solve_label, variant="A0_fixed_bg_data_only", pair_count=len(pairs), ) fixed = problem.solve( name="A0_fixed_bg_data_only", rotation_seed=data_seed, optimize_bias=False, max_nfev=max_nfev, ) _emit( progress_callback, "variant_complete", solve_label=solve_label, variant=fixed.name, rpy_deg=fixed.rpy_deg_xyz.tolist(), rms_deg=fixed.residual_rms_deg, p95_deg=fixed.residual_p95_deg, ) _emit( progress_callback, "variant_start", solve_label=solve_label, variant="A1_session_bg_data_only", pair_count=len(pairs), ) data_only = problem.solve( name="A1_session_bg_data_only", rotation_seed=fixed.R_IMU_lidar, optimize_bias=True, bias_seed=bias_bases, max_nfev=max_nfev, ) _emit( progress_callback, "variant_complete", solve_label=solve_label, variant=data_only.name, rpy_deg=data_only.rpy_deg_xyz.tolist(), rms_deg=data_only.residual_rms_deg, p95_deg=data_only.residual_p95_deg, ) _emit( progress_callback, "variant_start", solve_label=solve_label, variant="A2_session_bg_with_rotation_prior", pair_count=len(pairs), ) with_prior = problem.solve( name="A2_session_bg_with_rotation_prior", rotation_seed=data_only.R_IMU_lidar, optimize_bias=True, rotation_prior=rotation_prior, rotation_prior_sigma_deg=rotation_prior_sigma_deg, bias_seed=data_only.gyro_bias_rad_s_per_session, max_nfev=max_nfev, ) _emit( progress_callback, "variant_complete", solve_label=solve_label, variant=with_prior.name, rpy_deg=with_prior.rpy_deg_xyz.tolist(), rms_deg=with_prior.residual_rms_deg, p95_deg=with_prior.residual_p95_deg, ) return fixed, data_only, with_prior def solve_phase_a_comparison( pairs: list[MotionPair] | tuple[MotionPair, ...], *, gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None, rotation_prior: np.ndarray | None = None, rotation_prior_sigma_deg: float = 15.0, preexcluded_session_ids: set[str] | None = None, strong_rotation_min_deg: float = 1.0, decorrelation_block_s: float = 0.0, max_pairs_per_block: int = 1, bias_prior_sigma_rad_s: float = 0.002, yaw_std_max_deg: float = 0.5, leave_one_out_yaw_range_max_deg: float = 1.0, data_prior_difference_max_deg: float = 1.0, run_leave_one_out: bool = True, max_nfev: int = 200, progress_callback: ProgressCallback | None = None, ) -> PhaseAComparisonResult: """Solve and compare data-only and installation-prior Phase-A variants.""" all_pairs = list(pairs) strong_candidates = select_strong_rotation_pairs( all_pairs, min_rotation_deg=strong_rotation_min_deg, ) counts = Counter(pair.session_id for pair in strong_candidates) excluded = ( set() if preexcluded_session_ids is None else set(preexcluded_session_ids) ) eligible_pairs = [ pair for pair in strong_candidates if pair.session_id not in excluded ] active_pairs = select_decorrelated_phase_a_pairs( eligible_pairs, block_s=decorrelation_block_s, max_pairs_per_block=max_pairs_per_block, ) if not active_pairs: raise ValueError( "no active strong-rotation pairs remain after session exclusion" ) bases_all = _bias_bases(all_pairs, gyro_bias_rad_s_by_session) bases_active = { sid: bases_all[sid] for sid in sorted({pair.session_id for pair in active_pairs}) } if not phase_a_metadata_complete(active_pairs): raise ValueError( "Phase-A A1/A2 require J_bg/cov metadata; rehydrate cache from raw IMU" ) _emit( progress_callback, "phase_a_variants_start", all_pair_count=len(all_pairs), strong_pair_count=len(strong_candidates), active_pair_count=len(active_pairs), active_sessions=len(bases_active), excluded_sessions=sorted(excluded), ) fixed, data_only, with_prior = _solve_three_variants( active_pairs, bases_active, rotation_prior=rotation_prior, rotation_prior_sigma_deg=rotation_prior_sigma_deg, bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, max_nfev=max_nfev, progress_callback=progress_callback, solve_label="full", ) _emit( progress_callback, "phase_a_variants_complete", A0_rpy_deg=fixed.rpy_deg_xyz.tolist(), A1_rpy_deg=data_only.rpy_deg_xyz.tolist(), A2_rpy_deg=with_prior.rpy_deg_xyz.tolist(), ) marginal = _marginal_observability( data_only, direction_std_max_deg=yaw_std_max_deg, ) _, marginal_directions = np.linalg.eigh(marginal.hessian_marginal) observable_mask = marginal.direction_std_deg <= yaw_std_max_deg observable_basis = marginal_directions[:, observable_mask] partial_result: PhaseAVariantResult | None = None if rotation_prior is not None and 0 < observable_basis.shape[1] < 3: partial_problem = _PhaseAProblem( active_pairs, bases_active, bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, ) partial_result = partial_problem.solve( name="A3_observable_subspace_with_prior", rotation_seed=rotation_prior, optimize_bias=True, bias_seed=data_only.gyro_bias_rad_s_per_session, rotation_basis=observable_basis, max_nfev=max_nfev, ) prior_sensitivity: list[PhaseAPriorSensitivityResult] = [] if rotation_prior is not None: sensitivity_problem = _PhaseAProblem( active_pairs, bases_active, bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, ) for sigma_deg in (15.0, 3.0, 1.0, 0.5, 0.2): sensitivity = sensitivity_problem.solve( name=f"prior_sensitivity_{sigma_deg:g}deg", rotation_seed=data_only.R_IMU_lidar, optimize_bias=True, rotation_prior=rotation_prior, rotation_prior_sigma_deg=sigma_deg, bias_seed=data_only.gyro_bias_rad_s_per_session, max_nfev=max_nfev, ) difference = float( np.degrees( np.linalg.norm( so3_log( data_only.R_IMU_lidar.T @ sensitivity.R_IMU_lidar ) ) ) ) prior_sensitivity.append( PhaseAPriorSensitivityResult( sigma_deg=sigma_deg, rpy_deg_xyz=sensitivity.rpy_deg_xyz, rotation_difference_from_data_deg=difference, residual_rms_deg=sensitivity.residual_rms_deg, residual_p95_deg=sensitivity.residual_p95_deg, ) ) leave_one_out: list[PhaseALeaveOneOutResult] = [] leave_one_out_rotations: list[np.ndarray] = [] active_session_ids = sorted(bases_active) if run_leave_one_out and len(active_session_ids) >= 3: for fold_index, held_out in enumerate(active_session_ids, start=1): train_pairs = [ pair for pair in active_pairs if pair.session_id != held_out ] train_bases = { sid: value for sid, value in bases_active.items() if sid != held_out } _emit( progress_callback, "leave_one_out_start", fold=fold_index, folds=len(active_session_ids), held_out_session=held_out, training_pair_count=len(train_pairs), ) _, fold_data, fold_prior = _solve_three_variants( train_pairs, train_bases, rotation_prior=rotation_prior, rotation_prior_sigma_deg=rotation_prior_sigma_deg, bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, max_nfev=max_nfev, progress_callback=progress_callback, solve_label=f"loo_without_{held_out}", ) held_out_pairs = [ pair for pair in active_pairs if pair.session_id == held_out ] held_rms, held_p95 = _evaluate_static_bias( held_out_pairs, fold_data.R_IMU_lidar ) held_problem = _PhaseAProblem( held_out_pairs, {held_out: bases_active[held_out]}, bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, ) held_fit = held_problem.solve( name=f"held_out_bg_refit_{held_out}", rotation_seed=fold_data.R_IMU_lidar, optimize_bias=True, bias_seed={held_out: bases_active[held_out]}, rotation_basis=np.zeros((3, 0)), max_nfev=max_nfev, ) leave_one_out_rotations.append(fold_data.R_IMU_lidar) leave_one_out.append( PhaseALeaveOneOutResult( held_out_session=held_out, data_only_rpy_deg_xyz=fold_data.rpy_deg_xyz, prior_rpy_deg_xyz=fold_prior.rpy_deg_xyz, data_only_gyro_bias_rad_s_per_session=( fold_data.gyro_bias_rad_s_per_session ), prior_gyro_bias_rad_s_per_session=( fold_prior.gyro_bias_rad_s_per_session ), data_only_training_rms_deg=fold_data.residual_rms_deg, data_only_training_p95_deg=fold_data.residual_p95_deg, prior_training_rms_deg=fold_prior.residual_rms_deg, prior_training_p95_deg=fold_prior.residual_p95_deg, held_out_static_bg_rms_deg=held_rms, held_out_static_bg_p95_deg=held_p95, held_out_fitted_bg_rms_deg=held_fit.residual_rms_deg, held_out_fitted_bg_p95_deg=held_fit.residual_p95_deg, held_out_fitted_bg_rad_s=( held_fit.gyro_bias_rad_s_per_session[held_out] ), ) ) _emit( progress_callback, "leave_one_out_complete", fold=fold_index, folds=len(active_session_ids), held_out_session=held_out, data_only_yaw_deg=float(fold_data.rpy_deg_xyz[2]), prior_yaw_deg=float(fold_prior.rpy_deg_xyz[2]), held_out_static_bg_p95_deg=held_p95, held_out_fitted_bg_p95_deg=held_fit.residual_p95_deg, ) yaw_values = [ float(item.data_only_rpy_deg_xyz[2]) for item in leave_one_out ] loo_yaw_range = ( float(max(yaw_values) - min(yaw_values)) if yaw_values else float("nan") ) stability_basis = ( np.eye(3) if marginal.precision_rank == 3 else observable_basis ) observable_loo_values = [ float( np.degrees( np.linalg.norm( stability_basis.T @ so3_log( item_rotation @ data_only.R_IMU_lidar.T ) ) ) ) for item_rotation in leave_one_out_rotations ] loo_observable_max = ( max(observable_loo_values) if observable_loo_values else float("nan") ) yaw_difference = abs( _wrap_angle_deg( float( data_only.rpy_deg_xyz[2] - with_prior.rpy_deg_xyz[2] ) ) ) geodesic_difference = float( np.degrees( np.linalg.norm( so3_log( data_only.R_IMU_lidar.T @ with_prior.R_IMU_lidar ) ) ) ) loo_required = run_leave_one_out and len(active_session_ids) >= 3 sensitivity_half = next( (item for item in prior_sensitivity if item.sigma_deg == 0.5), None, ) checks = { "data_only_quality": bool(data_only.accepted), "marginal_rotation_rank": bool(marginal.rank == 3), "marginal_rotation_precision_rank": bool( marginal.precision_rank == 3 ), "marginal_yaw_std": bool( marginal.yaw_std_deg <= yaw_std_max_deg ), "data_vs_prior_yaw": bool( yaw_difference <= data_prior_difference_max_deg ), "data_vs_prior_geodesic": bool( geodesic_difference <= data_prior_difference_max_deg ), "leave_one_out_yaw_range": bool( (not loo_required) or ( np.isfinite(loo_yaw_range) and loo_yaw_range <= leave_one_out_yaw_range_max_deg ) ), "leave_one_out_observable_subspace": bool( (not loo_required) or ( np.isfinite(loo_observable_max) and loo_observable_max <= leave_one_out_yaw_range_max_deg ) ), "prior_sensitivity_0_5deg": bool( sensitivity_half is None or sensitivity_half.rotation_difference_from_data_deg <= data_prior_difference_max_deg ), } full_check_names = ( "data_only_quality", "marginal_rotation_precision_rank", "marginal_yaw_std", "leave_one_out_observable_subspace", "prior_sensitivity_0_5deg", ) full_accepted = all(checks[name] for name in full_check_names) partial_accepted = bool( not full_accepted and rotation_prior is not None and marginal.precision_rank >= 2 and partial_result is not None and partial_result.accepted and checks["leave_one_out_observable_subspace"] ) solution_status = ( "phase_a_full_accepted" if full_accepted else ( "phase_a_partial_accepted" if partial_accepted else "phase_a_rejected" ) ) recommended_result = ( "A1_session_bg_data_only" if full_accepted else ( "A3_observable_subspace_with_prior" if partial_accepted else "none" ) ) notes = ( ( "strong-rotation pairs require both IMU and LiDAR " f"relative rotation > {strong_rotation_min_deg:.3f} deg" ), ( "A1 data-only result is primary; A2 is only a " "prior-sensitivity comparison" ), ( "A3 is emitted only when one or more directions fail the precision " "gate; it optimizes the observable tangent subspace and explicitly " "inherits the complementary direction from the installation prior" ), ( f"overlapping factors are reduced to at most {max_pairs_per_block} " f"pair(s) per {decorrelation_block_s:.3f}s session block" ), ( f"thresholds: yaw_std<={yaw_std_max_deg:.3f} deg, " f"LOO yaw range<={leave_one_out_yaw_range_max_deg:.3f} deg, " f"data/prior difference<={data_prior_difference_max_deg:.3f} deg" ), ) return PhaseAComparisonResult( fixed_bg_data_only=fixed, session_bg_data_only=data_only, session_bg_with_rotation_prior=with_prior, observable_subspace_with_prior=partial_result, marginal_observability=marginal, leave_one_out=tuple(leave_one_out), strong_pair_counts_per_session={ sid: int(count) for sid, count in sorted(counts.items()) }, excluded_sessions=tuple(sorted(excluded)), data_vs_prior_yaw_diff_deg=yaw_difference, data_vs_prior_geodesic_deg=geodesic_difference, leave_one_out_yaw_range_deg=loo_yaw_range, leave_one_out_observable_max_deg=loo_observable_max, prior_sensitivity=tuple(prior_sensitivity), strong_pair_candidate_count=len(strong_candidates), decorrelated_pair_count=len(active_pairs), decorrelation_block_s=float(decorrelation_block_s), solution_status=solution_status, recommended_result=recommended_result, partial_accepted=partial_accepted, accepted=full_accepted, acceptance_checks=checks, notes=notes, ) def _array_map( values: Mapping[str, np.ndarray], ) -> dict[str, list[float]]: return { sid: np.asarray(value, dtype=float).reshape(3).tolist() for sid, value in values.items() } def phase_a_session_to_dict( item: PhaseASessionDiagnostics, ) -> dict[str, Any]: return { "session_id": item.session_id, "pair_count": item.pair_count, "gyro_bias0_rad_s": item.gyro_bias0_rad_s.tolist(), "gyro_bias_rad_s": item.gyro_bias_rad_s.tolist(), "residual_rms_deg": item.residual_rms_deg, "residual_median_deg": item.residual_median_deg, "residual_p95_deg": item.residual_p95_deg, "outlier_fraction_gt_5deg": item.outlier_fraction_gt_5deg, "accepted": item.accepted, "included_in_final": item.included_in_final, } def phase_a_variant_to_dict( item: PhaseAVariantResult, ) -> dict[str, Any]: return { "name": item.name, "R_IMU_lidar": item.R_IMU_lidar.tolist(), "rpy_deg_xyz": item.rpy_deg_xyz.tolist(), "gyro_bias_rad_s_per_session": _array_map( item.gyro_bias_rad_s_per_session ), "sessions": [ phase_a_session_to_dict(value) for value in item.sessions ], "residual_rms_deg": item.residual_rms_deg, "residual_median_deg": item.residual_median_deg, "residual_p95_deg": item.residual_p95_deg, "outlier_fraction_gt_5deg": ( item.outlier_fraction_gt_5deg ), "accepted": item.accepted, "rotation_prior_used": item.rotation_prior_used, "optimizer_success": item.optimizer_success, "optimizer_nfev": item.optimizer_nfev, "cost": item.cost, "residual_count": item.residual_count, "parameter_count": item.parameter_count, } def phase_a_comparison_to_dict( result: PhaseAComparisonResult, ) -> dict[str, Any]: marginal = result.marginal_observability variants = { "A0_fixed_bg_data_only": phase_a_variant_to_dict( result.fixed_bg_data_only ), "A1_session_bg_data_only": phase_a_variant_to_dict( result.session_bg_data_only ), "A2_session_bg_with_rotation_prior": phase_a_variant_to_dict( result.session_bg_with_rotation_prior ), } if result.observable_subspace_with_prior is not None: variants["A3_observable_subspace_with_prior"] = ( phase_a_variant_to_dict( result.observable_subspace_with_prior ) ) return { "status": result.solution_status, "accepted": result.accepted, "partial_accepted": result.partial_accepted, "recommended_result": result.recommended_result, "acceptance_checks": result.acceptance_checks, "variants": variants, "marginal_observability_A1": { "rank": marginal.rank, "condition": marginal.condition, "eigenvalues": marginal.eigenvalues.tolist(), "weakest_direction_left_tangent": ( marginal.weakest_direction_left_tangent.tolist() ), "covariance_rotvec_rad2": ( marginal.covariance_rotvec_rad2.tolist() ), "yaw_std_deg": marginal.yaw_std_deg, "direction_std_deg": marginal.direction_std_deg.tolist(), "precision_rank": marginal.precision_rank, "residual_variance_scale": ( marginal.residual_variance_scale ), "hessian_marginal": ( marginal.hessian_marginal.tolist() ), "ok": marginal.ok, "notes": list(marginal.notes), }, "leave_one_out": [ { "held_out_session": item.held_out_session, "data_only_rpy_deg_xyz": ( item.data_only_rpy_deg_xyz.tolist() ), "prior_rpy_deg_xyz": ( item.prior_rpy_deg_xyz.tolist() ), "data_only_gyro_bias_rad_s_per_session": ( _array_map( item.data_only_gyro_bias_rad_s_per_session ) ), "prior_gyro_bias_rad_s_per_session": ( _array_map( item.prior_gyro_bias_rad_s_per_session ) ), "data_only_training_rms_deg": ( item.data_only_training_rms_deg ), "data_only_training_p95_deg": ( item.data_only_training_p95_deg ), "prior_training_rms_deg": ( item.prior_training_rms_deg ), "prior_training_p95_deg": ( item.prior_training_p95_deg ), "held_out_static_bg_rms_deg": ( item.held_out_static_bg_rms_deg ), "held_out_static_bg_p95_deg": ( item.held_out_static_bg_p95_deg ), "held_out_fitted_bg_rms_deg": ( item.held_out_fitted_bg_rms_deg ), "held_out_fitted_bg_p95_deg": ( item.held_out_fitted_bg_p95_deg ), "held_out_fitted_bg_rad_s": ( item.held_out_fitted_bg_rad_s.tolist() ), } for item in result.leave_one_out ], "strong_pair_counts_per_session": ( result.strong_pair_counts_per_session ), "excluded_sessions": list(result.excluded_sessions), "data_vs_prior_yaw_diff_deg": ( result.data_vs_prior_yaw_diff_deg ), "data_vs_prior_geodesic_deg": ( result.data_vs_prior_geodesic_deg ), "leave_one_out_yaw_range_deg": ( result.leave_one_out_yaw_range_deg ), "leave_one_out_observable_max_deg": ( result.leave_one_out_observable_max_deg ), "prior_sensitivity": [ { "sigma_deg": item.sigma_deg, "rpy_deg_xyz": item.rpy_deg_xyz.tolist(), "rotation_difference_from_data_deg": ( item.rotation_difference_from_data_deg ), "residual_rms_deg": item.residual_rms_deg, "residual_p95_deg": item.residual_p95_deg, } for item in result.prior_sensitivity ], "strong_pair_candidate_count": result.strong_pair_candidate_count, "decorrelated_pair_count": result.decorrelated_pair_count, "decorrelation_block_s": result.decorrelation_block_s, "notes": list(result.notes), }