1436 lines
49 KiB
Python
1436 lines
49 KiB
Python
"""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),
|
|
}
|