diff --git a/config/vehicle_hi13_h32_20260808.yaml b/config/vehicle_hi13_h32_20260808.yaml index c1213d1..f874da0 100644 --- a/config/vehicle_hi13_h32_20260808.yaml +++ b/config/vehicle_hi13_h32_20260808.yaml @@ -18,7 +18,8 @@ installation: HI13R4 + H32 DLogCapture. Body +X forward: LiDAR and IMU at positive X. CAD sheet may draw +X rearward; numbers below are body-frame. Z is height above ground = CAD height at axle + 0.294 m (axle AGL). - LiDAR phase-center AGL is the measured 1.9165 m (not CAD dZ+63.5). + LiDAR CAD dZ is 1637.499879 mm relative to the axle reference. Phase-center + AGL adds rear-axle height 294 mm and the 63.5 mm phase-center offset. IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up). LiDAR Cartesian in NPZ assumed body-aligned (X forward). @@ -45,11 +46,12 @@ sensors: axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)" driver_axis_remapped: false mount_in_body: - # X/Y：后轮轴中心 → 雷达；Z：相位中心真实离地 1.9165 m - translation_m: [2.522276859, 0.000020526, 1.9165] + # X/Y：后轮轴中心 → 雷达 + # Z离地 = CAD dZ 1.637499879 + 后轮轴离地 0.294 + 相位中心偏移 0.0635 + translation_m: [2.522276859, 0.000020526, 1.994999879] rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] rotation_quaternion_xyzw: null - source: "CAD X/Y in body (+X forward); Z = measured phase-center AGL 1.9165 m; attitude = body" + source: "CAD X/Y in body (+X forward); Z AGL = CAD dZ 1.637499879 + axle AGL 0.294 + phase-center offset 0.0635; attitude = body" rtk: frame_definition: "" @@ -67,17 +69,17 @@ time: # t_IMU_lidar = R_IMU_body * t_body, R_IMU_lidar = R_IMU_body * R_body_lidar derived_T_IMU_lidar_prior: R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] - t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730] - t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.730] + t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879] + t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879] notes: > Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd). - Relative Z = 1.9165 - 1.1865 = 0.730 m (was 0.8085 m with old CAD+63.5mm Z). + Relative Z = 1.994999879 - 1.1865 = 0.808499879 m. initialization: translation_prior: enabled: true sigma_m: [0.05, 0.05, 0.05] - t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730] + t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879] rotation_prior: enabled: true sigma_deg: 15.0 diff --git a/docs/20260808_LiDAR-IMU标定现状与问题.md b/docs/20260808_LiDAR-IMU标定现状与问题.md index 9ea0262..08b23a0 100644 --- a/docs/20260808_LiDAR-IMU标定现状与问题.md +++ b/docs/20260808_LiDAR-IMU标定现状与问题.md @@ -96,16 +96,16 @@ t = [0, 0, 0] | 传感器 | X / Y（后轮轴中心） | Z（离地） | |--------|---------------------|-----------| | IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** | -| 雷达 | 2.522 / 0.00002 m | 相位中心实测 **1.9165 m** | +| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** | - 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。 -- 雷达相位中心以实测 1.9165 m 为准（不再用「CAD dZ + 63.5 mm」作为最终离地高）。 +- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。 ### 3.3 导出外参先验 - `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。 -- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.730]` m**（相对 Z = 1.9165 − 1.1865）。 -- **旋转先验未因 Z 修正改变**；仅平移先验 Z 从旧值 0.8085 改为 0.730。 +- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。 +- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。 --- diff --git a/docs/问题清单.md b/docs/问题清单.md index 9ea0262..08b23a0 100644 --- a/docs/问题清单.md +++ b/docs/问题清单.md @@ -96,16 +96,16 @@ t = [0, 0, 0] | 传感器 | X / Y（后轮轴中心） | Z（离地） | |--------|---------------------|-----------| | IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** | -| 雷达 | 2.522 / 0.00002 m | 相位中心实测 **1.9165 m** | +| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** | - 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。 -- 雷达相位中心以实测 1.9165 m 为准（不再用「CAD dZ + 63.5 mm」作为最终离地高）。 +- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。 ### 3.3 导出外参先验 - `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。 -- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.730]` m**（相对 Z = 1.9165 − 1.1865）。 -- **旋转先验未因 Z 修正改变**；仅平移先验 Z 从旧值 0.8085 改为 0.730。 +- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。 +- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。 --- diff --git a/imu_lidar/cli.py b/imu_lidar/cli.py index 696a202..0de67b6 100644 --- a/imu_lidar/cli.py +++ b/imu_lidar/cli.py @@ -3,12 +3,82 @@ from __future__ import annotations import argparse +from datetime import datetime from pathlib import Path +from typing import Any from .contracts import CalibrationMode, CalibrationRequest, CalibrationStatus, SessionInput +from .phase_a_replay import run_phase_a_replay from .pipeline import describe_pipeline, run_calibration +def _format_progress_value(value: Any) -> str: + if isinstance(value, float): + return f"{value:.3f}" + if isinstance(value, (list, tuple, set)): + return "[" + ",".join(str(item) for item in value) + "]" + return str(value) + + +def _print_progress(event: dict[str, Any]) -> None: + """Print one compact, immediately flushed progress line.""" + + timestamp = datetime.now().strftime("%H:%M:%S") + stage_index = event.get("stage_index", "?") + stage_total = event.get("stage_total", "?") + stage_name = event.get("stage", "unknown") + message = event.get("event", "progress") + fields = " ".join( + f"{key}={_format_progress_value(value)}" + for key, value in event.items() + if key not in {"stage_index", "stage_total", "stage", "event"} + and value is not None + ) + suffix = f" | {fields}" if fields else "" + print( + f"[{timestamp}] [stage {stage_index}/{stage_total} {stage_name}] {message}{suffix}", + flush=True, + ) + + +def _parse_session_imu_specs( + specs: list[str] | None, +) -> dict[str, Path]: + result: dict[str, Path] = {} + for spec in specs or []: + if "=" not in spec: + raise SystemExit( + "--session-imu must use SESSION_ID=PATH syntax" + ) + session_id, raw_path = spec.split("=", 1) + session_id = session_id.strip() + if not session_id or not raw_path.strip(): + raise SystemExit( + "--session-imu must use non-empty SESSION_ID=PATH" + ) + if session_id in result: + raise SystemExit( + f"duplicate --session-imu for {session_id}" + ) + result[session_id] = Path(raw_path.strip()) + return result + + +def _print_phase_a_progress( + event: str, + fields: dict[str, Any], +) -> None: + _print_progress( + { + "stage_index": "A", + "stage_total": "A", + "stage": "phase_a_replay", + "event": event, + **fields, + } + ) + + def build_parser() -> argparse.ArgumentParser: parser = argparse.ArgumentParser(description="LiDAR–IMU extrinsic calibration (V1)") subcommands = parser.add_subparsers(dest="command", required=True) @@ -58,6 +128,13 @@ def build_parser() -> argparse.ArgumentParser: default=None, help="Skip |ω| δt search and use this constant (use 0 after host-UTC bridge)", ) + run.add_argument( + "--session-time-offset-s", + action="append", + type=float, + default=None, + help="Per-session fixed time offset; repeat once per --imu/--lidar input", + ) run.add_argument( "--no-signed-time-refine", action="store_true", @@ -71,6 +148,69 @@ def build_parser() -> argparse.ArgumentParser: ) run.add_argument("--min-pair-rotation-deg", type=float, default=3.0) run.add_argument("--min-pair-translation-m", type=float, default=0.3) + run.add_argument("--min-registration-fitness", type=float, default=0.5) + run.add_argument("--max-imu-gap-s", type=float, default=0.05) + run.add_argument("--max-lidar-gap-s", type=float, default=1.0) + + replay = subcommands.add_parser( + "phase-a-replay", + help="Replay Phase-A from cached motion pairs without rerunning GICP", + ) + replay.add_argument("--motion-pairs", type=Path, required=True) + replay.add_argument("--vehicle-config", type=Path, required=True) + replay.add_argument("--output", type=Path, required=True) + replay.add_argument( + "--session-imu", + action="append", + default=None, + metavar="SESSION_ID=PATH", + help="Raw IMU mapping used only when cache lacks J_bg/cov", + ) + replay.add_argument( + "--exclude-session", + action="append", + default=None, + help="Session ID to exclude; may be repeated", + ) + replay.add_argument( + "--strong-rotation-min-deg", + type=float, + default=1.0, + ) + replay.add_argument( + "--decorrelation-block-s", + type=float, + default=3.0, + help="Per-session time-block length used to decorrelate factors", + ) + replay.add_argument( + "--max-pairs-per-block", + type=int, + default=1, + help="Maximum factors kept in each decorrelation block", + ) + replay.add_argument( + "--bias-prior-sigma-rad-s", + type=float, + default=0.002, + ) + replay.add_argument( + "--yaw-std-max-deg", + type=float, + default=0.5, + ) + replay.add_argument( + "--loo-yaw-range-max-deg", + type=float, + default=1.0, + ) + replay.add_argument( + "--data-prior-difference-max-deg", + type=float, + default=1.0, + ) + replay.add_argument("--max-nfev", type=int, default=200) + return parser @@ -87,9 +227,23 @@ def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]: raise SystemExit( f"--session-id count ({len(session_ids)}) must match --imu/--lidar ({len(imus)})" ) + if args.session_time_offset_s is None: + session_offsets: list[float | None] = [None] * len(imus) + else: + session_offsets = list(args.session_time_offset_s) + if len(session_offsets) != len(imus): + raise SystemExit( + f"--session-time-offset-s count ({len(session_offsets)}) must match " + f"--imu/--lidar ({len(imus)})" + ) return tuple( - SessionInput(session_id=sid, imu_source=imu, lidar_source=lidar) - for sid, imu, lidar in zip(session_ids, imus, lidars) + SessionInput( + session_id=sid, + imu_source=imu, + lidar_source=lidar, + fixed_time_offset_s=offset, + ) + for sid, imu, lidar, offset in zip(session_ids, imus, lidars, session_offsets) ) @@ -108,6 +262,48 @@ def main(argv: list[str] | None = None) -> int: print(f"{index}. {stage.name}: {stage.responsibility}") return 0 + if args.command == "phase-a-replay": + summary = run_phase_a_replay( + motion_pairs_path=args.motion_pairs, + vehicle_config_path=args.vehicle_config, + output_directory=args.output, + imu_paths_by_session=_parse_session_imu_specs( + args.session_imu + ), + excluded_sessions=set(args.exclude_session or []), + strong_rotation_min_deg=args.strong_rotation_min_deg, + decorrelation_block_s=args.decorrelation_block_s, + max_pairs_per_block=args.max_pairs_per_block, + bias_prior_sigma_rad_s=args.bias_prior_sigma_rad_s, + yaw_std_max_deg=args.yaw_std_max_deg, + leave_one_out_yaw_range_max_deg=( + args.loo_yaw_range_max_deg + ), + data_prior_difference_max_deg=( + args.data_prior_difference_max_deg + ), + max_nfev=args.max_nfev, + progress_callback=_print_phase_a_progress, + ) + print(f"status: {summary['status']}") + print(f"acceptance_checks: {summary['acceptance_checks']}") + for name, variant in summary["variants"].items(): + print( + f"{name}: rpy_deg_xyz={variant['rpy_deg_xyz']} " + f"RMS={variant['residual_rms_deg']:.6f} " + f"P95={variant['residual_p95_deg']:.6f}" + ) + print( + "A1 marginalized yaw_std_deg: " + f"{summary['marginal_observability_A1']['yaw_std_deg']}" + ) + print( + "leave_one_out_yaw_range_deg: " + f"{summary['leave_one_out_yaw_range_deg']}" + ) + print(f"report directory: {args.output}") + return 0 if (summary["accepted"] or summary.get("partial_accepted")) else 2 + if args.command == "run": sessions = _build_sessions(args) request = CalibrationRequest( @@ -118,12 +314,15 @@ def main(argv: list[str] | None = None) -> int: max_iterations=args.max_iterations, min_pair_rotation_deg=args.min_pair_rotation_deg, min_pair_translation_m=args.min_pair_translation_m, + min_registration_fitness=args.min_registration_fitness, + max_imu_gap_s=args.max_imu_gap_s, + max_lidar_gap_s=args.max_lidar_gap_s, time_offset_search_s=args.time_offset_search_s, fixed_time_offset_s=args.fixed_time_offset_s, enable_signed_time_refine=not args.no_signed_time_refine, max_signed_refine_shift_s=args.max_signed_refine_shift_s, ) - result = run_calibration(request) + result = run_calibration(request, progress_callback=_print_progress) print(f"status: {result.status.value}") print(f"message: {result.message}") if result.time_offset_s is not None: diff --git a/imu_lidar/contracts.py b/imu_lidar/contracts.py index dad7e5a..5865433 100644 --- a/imu_lidar/contracts.py +++ b/imu_lidar/contracts.py @@ -25,6 +25,7 @@ class CalibrationStatus(str, Enum): NOT_RUN = "not_run" BLOCKED = "blocked" ROTATION_ONLY_ACCEPTED = "rotation_only_accepted" + ROTATION_ONLY_PRIOR_CONSTRAINED = "rotation_only_prior_constrained" FULL_SE3_ACCEPTED = "full_se3_accepted" FULL_SE3_REJECTED = "full_se3_rejected_due_to_observability" @@ -37,6 +38,9 @@ class SessionInput: imu_source: Path lidar_source: Path board_configuration_id: str | None = None + # Optional session-local override. The request-level value remains a + # backward-compatible fallback for batches whose timelines are all aligned. + fixed_time_offset_s: float | None = None @dataclass(frozen=True) @@ -50,6 +54,9 @@ class CalibrationRequest: max_iterations: int = 2 min_pair_rotation_deg: float = 3.0 min_pair_translation_m: float = 0.3 + min_registration_fitness: float = 0.5 + max_imu_gap_s: float = 0.05 + max_lidar_gap_s: float = 1.0 time_offset_search_s: float = 1.0 # If set, skip |ω| search and use this constant (host-UTC-bridged sessions: 0). fixed_time_offset_s: float | None = None diff --git a/imu_lidar/joint_optimizer.py b/imu_lidar/joint_optimizer.py index 3cbfafa..6ce4dd3 100644 --- a/imu_lidar/joint_optimizer.py +++ b/imu_lidar/joint_optimizer.py @@ -2,7 +2,9 @@ from __future__ import annotations -from dataclasses import dataclass +from collections.abc import Callable, Mapping +from dataclasses import dataclass, field +from typing import Any import numpy as np from scipy.optimize import least_squares @@ -17,10 +19,26 @@ from .imu_preintegration import ( residual_whiten_matrix, ) from .observability import ObservabilityReport, analyze_observability +from .phase_a import phase_a_comparison_to_dict, solve_phase_a_comparison +from .rotation_handeye import select_strong_rotation_pairs G_NORM = 9.80665 +@dataclass(frozen=True) +class PhaseASessionResult: + 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 + + @dataclass(frozen=True) class JointExtrinsicResult: T_IMU_lidar: np.ndarray @@ -31,6 +49,10 @@ class JointExtrinsicResult: gyro_bias_rad_s: np.ndarray | None = None accel_bias_m_s2: np.ndarray | None = None gravity_m_s2: np.ndarray | None = None + gyro_bias_rad_s_per_session: dict[str, np.ndarray] = field(default_factory=dict) + phase_a_sessions: tuple[PhaseASessionResult, ...] = () + phase_a_accepted: bool = False + phase_a_comparison: dict[str, Any] = field(default_factory=dict) notes: tuple[str, ...] = () @@ -174,7 +196,8 @@ def _solve_phase_c_se3( pairs: list[MotionPair], r_x: np.ndarray, *, - gyro_bias0: np.ndarray, + gyro_bias_linearization: np.ndarray, + gyro_bias_init: np.ndarray, gravity_init: np.ndarray, sigma_bg_rw: float = 1.0e-5, sigma_ba_rw: float = 1.0e-3, @@ -189,7 +212,7 @@ def _solve_phase_c_se3( if len(usable) < 3: notes.append("phase-C skipped: need pairs with full preintegration metadata") t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3) - return r_x, t0, gravity_init, gyro_bias0, np.zeros(3), 1e9, 1e9, notes + return r_x, t0, gravity_init, gyro_bias_init, np.zeros(3), 1e9, 1e9, notes # Keyframes: group by session, sort each session by IMU time (no cross-session chain). stamp: dict[int, float] = {} @@ -225,7 +248,8 @@ def _solve_phase_c_se3( g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9)) basis = _gravity_basis(g0) ba0 = np.zeros(3) - bg0 = np.asarray(gyro_bias0, dtype=float).reshape(3) + bg0 = np.asarray(gyro_bias_linearization, dtype=float).reshape(3) + bg_init = np.asarray(gyro_bias_init, dtype=float).reshape(3) # State: dθ(3), t(3), g_xy(2), v(3K), bg(3K), ba(3K) n_v = 3 * k_count @@ -243,7 +267,7 @@ def _solve_phase_c_se3( t_sigma = np.full(3, float(t_sigma[0]), dtype=float) # velocities start at 0; biases at prior for idx in range(k_count): - x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg0 + x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg_init whitened = [] for pair in usable: @@ -312,7 +336,7 @@ def _solve_phase_c_se3( for sid in session_ids: first = next(kid for kid in keyframe_ids if kf_session[kid] == sid) idx0 = id_to_idx[first] - out.append(50.0 * (bgs[idx0] - bg0)) + out.append(50.0 * (bgs[idx0] - bg_init)) out.append(20.0 * bas[idx0]) if t_prior_vec is not None: out.append((t_opt - t_prior_vec) / np.maximum(t_sigma, 1e-3)) @@ -357,7 +381,197 @@ def _solve_phase_c_se3( return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes -def solve_joint_extrinsic( +def _pair_gyro_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_bias_bases( + pairs: list[MotionPair], + *, + gyro_bias_rad_s: np.ndarray | None, + gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None, +) -> dict[str, np.ndarray]: + session_ids = sorted({pair.session_id for pair in pairs}) + scalar = None + if gyro_bias_rad_s is not None: + scalar = np.asarray(gyro_bias_rad_s, dtype=float).reshape(3) + supplied = {} if gyro_bias_rad_s_by_session is None else gyro_bias_rad_s_by_session + bases: dict[str, np.ndarray] = {} + for sid in session_ids: + if sid in supplied: + bases[sid] = np.asarray(supplied[sid], dtype=float).reshape(3) + continue + pair = next( + ( + item + for item in pairs + if item.session_id == sid and "gyro_bias0_rad_s" in item.metadata + ), + None, + ) + if pair is not None: + bases[sid] = np.asarray(pair.metadata["gyro_bias0_rad_s"], dtype=float).reshape(3) + elif scalar is not None: + bases[sid] = scalar.copy() + else: + bases[sid] = np.zeros(3) + return bases + + +def _rotation_distribution(errs_deg: list[float]) -> tuple[float, float, float, float, bool]: + if not errs_deg: + return 1e9, 1e9, 1e9, 1.0, False + errs = np.asarray(errs_deg, dtype=float) + rms = float(np.sqrt(np.mean(errs**2))) + median = float(np.median(errs)) + p95 = float(np.percentile(errs, 95.0)) + outlier_fraction = float(np.mean(errs > 5.0)) + accepted = ( + len(errs) >= 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 _solve_phase_a_rotation( + pairs: list[MotionPair], + r_seed: np.ndarray, + *, + bias_bases: Mapping[str, np.ndarray], + imu: ImuSeries | None, + bias_prior_sigma_rad_s: float, + preexcluded_session_ids: set[str] | None = None, +) -> tuple[ + np.ndarray, + dict[str, np.ndarray], + tuple[PhaseASessionResult, ...], + list[MotionPair], + float, + bool, + list[str], +]: + notes: list[str] = [] + all_session_ids = sorted({pair.session_id for pair in pairs}) + prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4) + + def optimize( + active_pairs: list[MotionPair], + r0: np.ndarray, + bias_seed: Mapping[str, np.ndarray], + ) -> tuple[np.ndarray, dict[str, np.ndarray]]: + session_ids = sorted({pair.session_id for pair in active_pairs}) + session_index = {sid: index for index, sid in enumerate(session_ids)} + whiten = [residual_whiten_matrix(_pair_cov(pair)) for pair in active_pairs] + x0 = np.zeros(3 + 3 * len(session_ids)) + for sid, index in session_index.items(): + x0[3 + 3 * index : 6 + 3 * index] = np.asarray(bias_seed[sid], dtype=float) + + def residual(vec: np.ndarray) -> np.ndarray: + r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r0) + out: list[np.ndarray] = [] + for pair, sqrt_info in zip(active_pairs, whiten): + index = session_index[pair.session_id] + bias = vec[3 + 3 * index : 6 + 3 * index] + base = _pair_gyro_bias0(pair, bias_bases[pair.session_id]) + delta_r = _corrected_delta_r( + pair, bias - base, imu=imu, bias0=base + ) + out.append( + sqrt_info + @ preintegration_rotation_residual(delta_r, r_opt, pair.R_B) + ) + for sid, index in session_index.items(): + bias = vec[3 + 3 * index : 6 + 3 * index] + out.append(prior_w * (bias - bias_bases[sid])) + return np.concatenate(out) + + opt = least_squares(residual, x0, loss="huber", f_scale=1.0, max_nfev=200) + r_opt = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r0) + biases = { + sid: opt.x[3 + 3 * index : 6 + 3 * index].copy() + for sid, index in session_index.items() + } + return r_opt, biases + + def summarize( + r_opt: np.ndarray, + biases: Mapping[str, np.ndarray], + included: set[str], + ) -> tuple[PhaseASessionResult, ...]: + results: list[PhaseASessionResult] = [] + for sid in all_session_ids: + local_pairs = [pair for pair in pairs if pair.session_id == sid] + bias = np.asarray(biases.get(sid, bias_bases[sid]), dtype=float).reshape(3) + errs: list[float] = [] + for pair in local_pairs: + base = _pair_gyro_bias0(pair, bias_bases[sid]) + delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base) + err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B) + errs.append(float(np.degrees(np.linalg.norm(err)))) + rms, median, p95, outlier, accepted = _rotation_distribution(errs) + results.append( + PhaseASessionResult( + session_id=sid, + pair_count=len(local_pairs), + gyro_bias0_rad_s=np.asarray(bias_bases[sid], dtype=float), + gyro_bias_rad_s=bias, + residual_rms_deg=rms, + residual_median_deg=median, + residual_p95_deg=p95, + outlier_fraction_gt_5deg=outlier, + accepted=accepted, + included_in_final=sid in included, + ) + ) + return tuple(results) + + if not pairs: + return r_seed, dict(bias_bases), (), [], 1e9, False, ["no pairs for phase-A"] + + r_first, biases_first = optimize(pairs, r_seed, bias_bases) + first = summarize(r_first, biases_first, set(all_session_ids)) + accepted_ids = {item.session_id for item in first if item.accepted} + preexcluded = set() if preexcluded_session_ids is None else set(preexcluded_session_ids) + accepted_ids -= preexcluded + active_ids = set(all_session_ids) + r_final = r_first + biases_final = dict(biases_first) + if preexcluded and not accepted_ids: + active_ids = set() + notes.append(f"phase-A pre-gate excluded all sessions: {sorted(preexcluded)}") + elif accepted_ids and accepted_ids != active_ids: + active_ids = accepted_ids + active_pairs = [pair for pair in pairs if pair.session_id in active_ids] + r_final, active_biases = optimize(active_pairs, r_first, biases_first) + biases_final.update(active_biases) + excluded = sorted(set(all_session_ids) - active_ids) + notes.append(f"phase-A excluded sessions after local/pre residual gate: {excluded}") + active_pairs = [pair for pair in pairs if pair.session_id in active_ids] + final = summarize(r_final, biases_final, active_ids) + active_results = [item for item in final if item.included_in_final] + global_errs: list[float] = [] + for pair in active_pairs: + bias = biases_final[pair.session_id] + base = _pair_gyro_bias0(pair, bias_bases[pair.session_id]) + delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base) + err = preintegration_rotation_residual(delta_r, r_final, pair.R_B) + global_errs.append(float(np.degrees(np.linalg.norm(err)))) + rot_rms, _, _, _, global_ok = _rotation_distribution(global_errs) + accepted = bool(active_results and global_ok and all(item.accepted for item in active_results)) + notes.append( + f"phase-A session-local bias refine: sessions={len(active_ids)}/{len(all_session_ids)}, " + f"pairs={len(active_pairs)}, rms={rot_rms:.3f} deg" + ) + return r_final, biases_final, final, active_pairs, rot_rms, accepted, notes + + +def _solve_joint_extrinsic_legacy( pairs: list[MotionPair] | tuple[MotionPair, ...], r_x: np.ndarray, *, @@ -366,6 +580,8 @@ def solve_joint_extrinsic( delta_t_s: float = 0.0, gyro_bias_rad_s: np.ndarray | None = None, gravity_init_m_s2: np.ndarray | None = None, + gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None, + time_offset_s_by_session: Mapping[str, float] | None = None, bias_prior_sigma_rad_s: float = 0.02, enable_phase_c: bool | None = None, t_init_m: np.ndarray | None = None, @@ -391,10 +607,10 @@ def solve_joint_extrinsic( def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray: residuals = [] - for pair, weight, whiten in zip(usable, weights, whitens): + for pair, whiten in zip(usable, whitens): delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0) err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B) - residuals.append(np.sqrt(weight) * (whiten @ err)) + residuals.append(whiten @ err) residuals.append(prior_w * delta_bias) return np.concatenate(residuals) if residuals else np.zeros(0) @@ -407,16 +623,16 @@ def solve_joint_extrinsic( residual_rot_bias, np.zeros(6), loss="huber", - f_scale=np.deg2rad(1.0), + f_scale=1.0, max_nfev=200, ) r = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r) delta_bias = opt.x[3:] bias_out = bias0 + delta_bias notes.append( - "phase-A joint refine (Σ-whitened + J_bg): " + "phase-A joint refine (single Σ whitening + J_bg): " f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, " - f"weighted pairs={len(usable)}" + f"pairs={len(usable)}" ) else: bias_out = bias0 @@ -457,7 +673,8 @@ def solve_joint_extrinsic( r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3( usable, r, - gyro_bias0=bias_out, + gyro_bias_linearization=bias0, + gyro_bias_init=bias_out, gravity_init=gravity_init, t_init=t_seed if t_seed is not None else t_prior_m, t_prior=t_prior_m, @@ -469,9 +686,9 @@ def solve_joint_extrinsic( # Prefer CAD prior over silent zero when motion SE3 is rejected. if t_prior_m is not None: t = np.asarray(t_prior_m, dtype=float).reshape(3) - translation_accepted = True notes.append( - "phase-C translation residual/gate failed; keeping CAD translation prior" + "phase-C translation residual/gate failed; CAD translation is reported " + "as a prior only and is not accepted as calibration" ) else: notes.append("phase-C translation residual/gate failed; keeping translation at zero") @@ -520,15 +737,18 @@ def solve_joint_extrinsic( trans_errs.append(np.linalg.norm(pred - meas)) rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) trans_rms = float(np.sqrt(np.mean(np.square(trans_errs)))) - translation_accepted = trans_rms < 0.5 or t_prior_m is not None + translation_accepted = trans_rms < 0.5 notes.append(f"legacy translation refine rms={trans_rms:.3f} m") if not translation_accepted: notes.append("translation residual too large; keeping translation at zero") t = np.zeros(3) elif not force_rotation_only and t_prior_m is not None: t = np.asarray(t_prior_m, dtype=float).reshape(3) - translation_accepted = True - notes.append("SE3 motion solve gated off; using CAD translation prior with refined rotation") + translation_accepted = False + notes.append( + "SE3 motion solve gated off; CAD translation is reported as a prior only " + "and is not accepted as calibration" + ) else: notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)") @@ -536,10 +756,222 @@ def solve_joint_extrinsic( T_IMU_lidar=make_transform(t, r), translation_accepted=bool(translation_accepted and np.linalg.norm(t) > 0), residual_rms_rot_deg=rot_rms, - residual_rms_trans_m=0.0 if not translation_accepted else trans_rms, + residual_rms_trans_m=trans_rms, observability=observability, gyro_bias_rad_s=np.asarray(bias_out, dtype=float), accel_bias_m_s2=None if accel_bias_out is None else np.asarray(accel_bias_out, dtype=float), gravity_m_s2=None if gravity_out is None else np.asarray(gravity_out, dtype=float), notes=tuple(notes), ) + + +def solve_joint_extrinsic( + pairs: list[MotionPair] | tuple[MotionPair, ...], + r_x: np.ndarray, + *, + force_rotation_only: bool = False, + imu: ImuSeries | None = None, + delta_t_s: float = 0.0, + gyro_bias_rad_s: np.ndarray | None = None, + gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None, + time_offset_s_by_session: Mapping[str, float] | None = None, + preexcluded_session_ids: set[str] | None = None, + gravity_init_m_s2: np.ndarray | None = None, + bias_prior_sigma_rad_s: float = 0.002, + rotation_prior: np.ndarray | None = None, + rotation_prior_sigma_deg: float = 15.0, + phase_a_yaw_std_max_deg: float = 0.5, + phase_a_loo_yaw_range_max_deg: float = 1.0, + phase_a_data_prior_difference_max_deg: float = 1.0, + run_phase_a_leave_one_out: bool = True, + phase_a_progress_callback: ( + Callable[[str, dict[str, Any]], None] | None + ) = None, + enable_phase_c: bool | None = None, + t_init_m: np.ndarray | None = None, + t_prior_m: np.ndarray | None = None, + t_prior_sigma_m: np.ndarray | float | None = None, +) -> JointExtrinsicResult: + """Run the corrected session-aware Phase-A and gate unfinished SE(3) stages.""" + + del gravity_init_m_s2, t_init_m, t_prior_sigma_m, imu, r_x + usable_input = [pair for pair in pairs if pair.t_B_m is not None] + bias_bases = _phase_a_bias_bases( + usable_input, + gyro_bias_rad_s=gyro_bias_rad_s, + gyro_bias_rad_s_by_session=gyro_bias_rad_s_by_session, + ) + comparison = solve_phase_a_comparison( + usable_input, + gyro_bias_rad_s_by_session=bias_bases, + rotation_prior=rotation_prior, + rotation_prior_sigma_deg=rotation_prior_sigma_deg, + preexcluded_session_ids=preexcluded_session_ids, + bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, + yaw_std_max_deg=phase_a_yaw_std_max_deg, + leave_one_out_yaw_range_max_deg=( + phase_a_loo_yaw_range_max_deg + ), + data_prior_difference_max_deg=( + phase_a_data_prior_difference_max_deg + ), + run_leave_one_out=run_phase_a_leave_one_out, + progress_callback=phase_a_progress_callback, + ) + primary = comparison.session_bg_data_only + r = primary.R_IMU_lidar + biases = primary.gyro_bias_rad_s_per_session + rot_rms = primary.residual_rms_deg + phase_a_accepted = comparison.accepted + notes = list(comparison.notes) + notes.append( + "phase-A primary=A1_session_bg_data_only; " + f"A0 RPY={comparison.fixed_bg_data_only.rpy_deg_xyz.tolist()}, " + f"A1 RPY={primary.rpy_deg_xyz.tolist()}, " + "A2 RPY=" + f"{comparison.session_bg_with_rotation_prior.rpy_deg_xyz.tolist()}" + ) + notes.append( + f"phase-A marginal yaw_std={comparison.marginal_observability.yaw_std_deg:.3f} deg, " + f"LOO yaw range={comparison.leave_one_out_yaw_range_deg:.3f} deg" + ) + + session_results_list: list[PhaseASessionResult] = [ + PhaseASessionResult( + session_id=item.session_id, + pair_count=item.pair_count, + gyro_bias0_rad_s=item.gyro_bias0_rad_s, + gyro_bias_rad_s=item.gyro_bias_rad_s, + 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=True, + ) + for item in primary.sessions + ] + preexcluded = ( + set() + if preexcluded_session_ids is None + else set(preexcluded_session_ids) + ) + strong_all = select_strong_rotation_pairs(usable_input) + for session_id in sorted(preexcluded): + local_pairs = [ + pair for pair in strong_all if pair.session_id == session_id + ] + errors = [ + float( + np.degrees( + np.linalg.norm( + preintegration_rotation_residual( + pair.R_A, r, pair.R_B + ) + ) + ) + ) + for pair in local_pairs + ] + rms, median, p95, outlier, accepted = _rotation_distribution( + errors + ) + base = np.asarray( + bias_bases.get(session_id, np.zeros(3)), dtype=float + ).reshape(3) + session_results_list.append( + PhaseASessionResult( + session_id=session_id, + pair_count=len(local_pairs), + gyro_bias0_rad_s=base, + gyro_bias_rad_s=base, + residual_rms_deg=rms, + residual_median_deg=median, + residual_p95_deg=p95, + outlier_fraction_gt_5deg=outlier, + accepted=accepted, + included_in_final=False, + ) + ) + session_results = tuple( + sorted(session_results_list, key=lambda item: item.session_id) + ) + usable = [ + pair + for pair in strong_all + if pair.session_id not in preexcluded + ] + base_observability = analyze_observability(usable, r) + marginal = comparison.marginal_observability + observability = ObservabilityReport( + rotation_observable=bool( + marginal.rank == 3 + and marginal.yaw_std_deg <= phase_a_yaw_std_max_deg + ), + translation_observable=base_observability.translation_observable, + condition_rotation=marginal.condition, + condition_translation=base_observability.condition_translation, + notes=tuple( + list(marginal.notes) + + list(base_observability.notes) + ), + ) + notes.extend(observability.notes) + if time_offset_s_by_session is None: + notes.append( + f"legacy scalar time offset fixed during pair construction: {float(delta_t_s):.6f}s" + ) + else: + fixed_offsets = { + str(sid): float(value) for sid, value in time_offset_s_by_session.items() + } + notes.append( + f"time offsets fixed during pair construction (not optimized): {fixed_offsets}" + ) + + for item in session_results: + notes.append( + f"phase-A session {item.session_id}: included={item.included_in_final}, " + f"pairs={item.pair_count}, rms={item.residual_rms_deg:.3f} deg, " + f"p95={item.residual_p95_deg:.3f} deg, " + f"|bias-bias0|={float(np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)):.3e}" + ) + + phase_c_requested = (not force_rotation_only) if enable_phase_c is None else bool(enable_phase_c) + t = np.zeros(3) + if not force_rotation_only: + if phase_c_requested: + notes.append( + "phase-B/C gated off: session-aware translation/gravity/navigation " + "states are not implemented yet" + ) + else: + notes.append("phase-C disabled; translation is not accepted") + if t_prior_m is not None: + t = np.asarray(t_prior_m, dtype=float).reshape(3) + notes.append( + "CAD translation is reported as a prior only and is not accepted as calibration" + ) + else: + notes.append("rotation-only extrinsic returned after corrected phase-A") + + single_bias = None + if len(biases) == 1: + single_bias = np.asarray(next(iter(biases.values())), dtype=float) + return JointExtrinsicResult( + T_IMU_lidar=make_transform(t, r), + translation_accepted=False, + residual_rms_rot_deg=rot_rms, + residual_rms_trans_m=1e9, + observability=observability, + gyro_bias_rad_s=single_bias, + accel_bias_m_s2=None, + gravity_m_s2=None, + gyro_bias_rad_s_per_session={ + sid: np.asarray(value, dtype=float) for sid, value in biases.items() + }, + phase_a_sessions=session_results, + phase_a_accepted=phase_a_accepted, + phase_a_comparison=phase_a_comparison_to_dict(comparison), + notes=tuple(notes), + ) diff --git a/imu_lidar/keyframes.py b/imu_lidar/keyframes.py index 97b3d6c..ed1e1f1 100644 --- a/imu_lidar/keyframes.py +++ b/imu_lidar/keyframes.py @@ -21,6 +21,7 @@ def build_keyframes( *, min_translation_m: float = 0.3, min_rotation_deg: float = 3.0, + min_registration_fitness: float = 0.5, max_frame_gap: int = 8, ) -> KeyframeSet: """Select keyframes with enough relative motion for hand-eye pairs.""" @@ -36,7 +37,7 @@ def build_keyframes( last = index continue result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz) - if not result.ok: + if not result.ok or result.fitness < min_registration_fitness: continue if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg: selected.append(index) diff --git a/imu_lidar/motion_pairs.py b/imu_lidar/motion_pairs.py index b8c39d4..146241b 100644 --- a/imu_lidar/motion_pairs.py +++ b/imu_lidar/motion_pairs.py @@ -2,7 +2,10 @@ from __future__ import annotations +from collections.abc import Callable from dataclasses import dataclass +from time import perf_counter +from typing import Any import numpy as np @@ -30,7 +33,12 @@ def build_motion_pairs( acc_bias_m_s2: np.ndarray | None = None, min_rotation_deg: float = 3.0, min_translation_m: float = 0.3, + min_registration_fitness: float = 0.5, + max_imu_gap_s: float = 0.05, + max_lidar_gap_s: float = 1.0, + all_frame_times_s: np.ndarray | None = None, max_index_span: int = 4, + progress_callback: Callable[[dict[str, Any]], None] | None = None, ) -> MotionPairSet: """Create A/B motion pairs between nearby keyframes. @@ -40,21 +48,74 @@ def build_motion_pairs( notes: list[str] = [] pairs: list[MotionPair] = [] + rejected_fitness = 0 + rejected_imu_gap = 0 + rejected_lidar_gap = 0 + frame_times = ( + None + if all_frame_times_s is None + else np.asarray(all_frame_times_s, dtype=float).reshape(-1) + ) bias_g = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float) bias_a = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float) n = len(keyframes) if n < 2: return MotionPairSet((), ("need at least two keyframes",)) + total_candidates = sum(max(n - span, 0) for span in range(1, max_index_span + 1)) + processed_candidates = 0 + started_at = perf_counter() + last_progress_at = started_at + + def report_progress(*, event: str, span: int, force: bool = False) -> None: + nonlocal last_progress_at + if progress_callback is None: + return + now = perf_counter() + if not force and processed_candidates > 1 and now - last_progress_at < 10.0: + return + last_progress_at = now + progress_callback( + { + "event": event, + "processed_candidates": processed_candidates, + "total_candidates": total_candidates, + "progress_pct": 100.0 * processed_candidates / max(total_candidates, 1), + "current_span": span, + "max_span": max_index_span, + "accepted_pairs": len(pairs), + "rejected_fitness": rejected_fitness, + "rejected_imu_gap": rejected_imu_gap, + "rejected_lidar_gap": rejected_lidar_gap, + "elapsed_s": now - started_at, + } + ) + + report_progress(event="start", span=1, force=True) + for span in range(1, max_index_span + 1): for start in range(0, n - span): + processed_candidates += 1 + report_progress(event="running", span=span) i = start j = start + span frame_i = keyframes[i] frame_j = keyframes[j] + source_i = int(keyframe_indices[i]) + source_j = int(keyframe_indices[j]) + if frame_times is not None: + lo = min(source_i, source_j) + hi = max(source_i, source_j) + local_times = frame_times[lo : hi + 1] + if local_times.size >= 2 and np.any(np.diff(local_times) > max_lidar_gap_s): + rejected_lidar_gap += 1 + continue reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz) if not reg.ok: continue + if reg.fitness < min_registration_fitness: + rejected_fitness += 1 + continue if reg.rotation_deg < min_rotation_deg and reg.translation_m < min_translation_m: continue @@ -64,6 +125,16 @@ def build_motion_pairs( continue if t_i_imu < imu.t_s[0] or t_j_imu > imu.t_s[-1]: continue + imu_lo = max(int(np.searchsorted(imu.t_s, t_i_imu, side="right")) - 1, 0) + imu_hi = min( + int(np.searchsorted(imu.t_s, t_j_imu, side="left")) + 1, + imu.t_s.size, + ) + if imu_hi - imu_lo >= 2 and np.any( + np.diff(imu.t_s[imu_lo:imu_hi]) > max_imu_gap_s + ): + rejected_imu_gap += 1 + continue preint = preintegrate_imu( imu.t_s, @@ -111,14 +182,27 @@ def build_motion_pairs( "delta_p": preint.delta_p.tolist(), "t_i_imu_s": t_i_imu, "t_j_imu_s": t_j_imu, + "gyro_bias0_rad_s": bias_g.tolist(), + "accel_bias0_m_s2": bias_a.tolist(), + "time_offset_s": float(delta_t_s), + "keyframe_span": int(span), + "is_consecutive": bool(span == 1), "modeling": "imu_preintegration_factor_phase_c", }, ) ) + report_progress(event="complete", span=max_index_span, force=True) + notes.append( f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)" ) + notes.append( + "quality rejects: " + f"fitness<{min_registration_fitness:.2f}: {rejected_fitness}, " + f"IMU gap>{max_imu_gap_s:.3f}s: {rejected_imu_gap}, " + f"LiDAR gap>{max_lidar_gap_s:.3f}s: {rejected_lidar_gap}" + ) return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes)) diff --git a/imu_lidar/motion_pairs_io.py b/imu_lidar/motion_pairs_io.py index a9a038d..251ea0e 100644 --- a/imu_lidar/motion_pairs_io.py +++ b/imu_lidar/motion_pairs_io.py @@ -10,9 +10,11 @@ import numpy as np from .contracts import MotionPair -SCHEMA_VERSION = 1 +SCHEMA_VERSION = 2 -# Keep viz-relevant fields; drop large cov / Jacobians. +# Keep visualization fields plus the compact 3x3 rotation metadata needed to +# rerun Phase-A without repeating LiDAR registration. Full 9x9 Phase-C matrices +# remain excluded from this cache. _METADATA_KEEP = frozenset( { "backend", @@ -23,8 +25,17 @@ _METADATA_KEEP = frozenset( "duration_s", "mean_gyro_norm", "preint_sigma_rad", + "cov", + "J_bg", + "phase_a_metadata_rehydrated", + "rehydrated_R_A_error_deg", "t_i_imu_s", "t_j_imu_s", + "gyro_bias0_rad_s", + "accel_bias0_m_s2", + "time_offset_s", + "keyframe_span", + "is_consecutive", "modeling", } ) @@ -113,10 +124,11 @@ def save_motion_pairs(path: Path | str, payload: dict[str, Any]) -> Path: def load_motion_pairs(path: Path | str) -> dict[str, Any]: payload = json.loads(Path(path).read_text(encoding="utf-8")) - if int(payload.get("schema_version", 0)) != SCHEMA_VERSION: + version = int(payload.get("schema_version", 0)) + if version not in {1, SCHEMA_VERSION}: raise ValueError( - f"unsupported motion_pairs schema_version={payload.get('schema_version')}; " - f"expected {SCHEMA_VERSION}" + f"unsupported motion_pairs schema_version={version}; " + f"expected 1 or {SCHEMA_VERSION}" ) return payload diff --git a/imu_lidar/observability.py b/imu_lidar/observability.py index dd623a5..c6c5e9d 100644 --- a/imu_lidar/observability.py +++ b/imu_lidar/observability.py @@ -57,43 +57,53 @@ def analyze_observability( if j_r.size == 0: return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",)) - # Normalize columns. - col_norm = np.linalg.norm(j_r, axis=0) + 1e-12 - j_r_n = j_r / col_norm - singular = np.linalg.svd(j_r_n, compute_uv=False) + singular = np.linalg.svd(j_r, compute_uv=False) cond_r = float(singular[0] / max(singular[-1], 1e-12)) - rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3 + rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1))) + rotation_ok = ( + cond_r < condition_threshold + and rotation_information > 1e-3 + and singular[-1] > 1e-6 + ) - # Translation observability proxy: diversity of rotation axes and presence of translation in B. - axes = [] - translations = [] - for pair in usable: - axis = so3_log(pair.R_B) - n = np.linalg.norm(axis) - if n > 1e-8: - axes.append(axis / n) - if pair.t_B_m is not None: - translations.append(pair.t_B_m) - axis_rank = 0 - if axes: - axis_mat = np.asarray(axes, dtype=float) - axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1)) - trans_span = 0.0 - if translations: - tmat = np.asarray(translations, dtype=float) - trans_span = float(np.linalg.norm(np.std(tmat, axis=0))) - # For planar yaw-mostly motion, translation z is typically weak. - translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5 - cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3))) + # Translation lever arm is observable through stacked (R_A - I). Pure + # planar yaw leaves its vertical column in the nullspace and must fail. + translation_rows = [ + np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3) + for pair in usable + if pair.t_B_m is not None + ] + if translation_rows: + j_t = np.vstack(translation_rows) + singular_t = np.linalg.svd(j_t, compute_uv=False) + cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12)) + translation_information = float( + singular_t[-1] / np.sqrt(max(len(translation_rows), 1)) + ) + else: + cond_t = 1e9 + translation_information = 0.0 + translation_ok = ( + len(translation_rows) >= 5 + and cond_t < condition_threshold + and translation_information > 0.02 + ) if not rotation_ok: - notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}") + notes.append( + f"rotation not observable: condition={cond_r:.1f}, " + f"min_information={rotation_information:.3e}" + ) else: - notes.append(f"rotation condition {cond_r:.1f}") + notes.append( + f"rotation observable: condition={cond_r:.1f}, " + f"min_information={rotation_information:.3e}" + ) if not translation_ok: notes.append( - f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); " - "V1 will reject full SE3 without strong priors" + f"translation not observable: condition={cond_t:.1f}, " + f"min_information={translation_information:.3e}; " + "full SE3 will be rejected" ) return ObservabilityReport( rotation_observable=rotation_ok, diff --git a/imu_lidar/phase_a.py b/imu_lidar/phase_a.py new file mode 100644 index 0000000..7cbaf16 --- /dev/null +++ b/imu_lidar/phase_a.py @@ -0,0 +1,1435 @@ +"""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), + } diff --git a/imu_lidar/phase_a_replay.py b/imu_lidar/phase_a_replay.py new file mode 100644 index 0000000..c592a86 --- /dev/null +++ b/imu_lidar/phase_a_replay.py @@ -0,0 +1,290 @@ +"""Cached Phase-A replay: rehydrate gyro factors, compare variants, write reports.""" + +from __future__ import annotations + +import json +from collections import defaultdict +from pathlib import Path +from typing import Any + +import numpy as np + +from .imu_io import load_imu_samples +from .motion_pairs_io import ( + build_motion_pairs_payload, + load_motion_pairs, + pair_from_dict, + save_motion_pairs, +) +from .phase_a import ( + ProgressCallback, + phase_a_comparison_to_dict, + phase_a_metadata_complete, + rehydrate_phase_a_pairs, + solve_phase_a_comparison, +) +from .vehicle_config import load_vehicle_config, prior_enabled + + +def _rotation_prior( + vehicle_config_path: Path, +) -> tuple[np.ndarray | None, float]: + config = load_vehicle_config(vehicle_config_path) + if not prior_enabled(config, "rotation_prior"): + return None, 15.0 + prior = (config.get("initialization") or {}).get("rotation_prior") or {} + matrix = prior.get("R_IMU_lidar") + if matrix is None: + return None, float(prior.get("sigma_deg", 15.0)) + return ( + np.asarray(matrix, dtype=float).reshape(3, 3), + float(prior.get("sigma_deg", 15.0)), + ) + + +def _sanitize_json(value: Any) -> Any: + if isinstance(value, dict): + return {str(key): _sanitize_json(item) for key, item in value.items()} + if isinstance(value, (list, tuple)): + return [_sanitize_json(item) for item in value] + if isinstance(value, np.ndarray): + return _sanitize_json(value.tolist()) + if isinstance(value, (np.floating, float)): + number = float(value) + return number if np.isfinite(number) else None + if isinstance(value, (np.integer, np.bool_)): + return value.item() + return value + + +def _write_json(path: Path, payload: Any) -> None: + path.write_text( + json.dumps(_sanitize_json(payload), indent=2, ensure_ascii=False) + "\n", + encoding="utf-8", + ) + + +def _load_cached_sessions( + motion_pairs_path: Path, +) -> tuple[ + dict[str, Any], + list, + dict[str, np.ndarray], + dict[str, float], +]: + payload = load_motion_pairs(motion_pairs_path) + pairs = [] + biases: dict[str, np.ndarray] = {} + offsets: dict[str, float] = {} + for session in payload.get("sessions") or []: + session_id = str(session["session_id"]) + biases[session_id] = np.asarray( + session.get("gyro_bias_rad_s", np.zeros(3)), + dtype=float, + ).reshape(3) + offsets[session_id] = float(session.get("delta_t_s", 0.0)) + pairs.extend( + pair_from_dict(item) + for item in session.get("pairs") or [] + ) + if not pairs: + raise ValueError(f"motion-pair cache is empty: {motion_pairs_path}") + return payload, pairs, biases, offsets + + +def run_phase_a_replay( + *, + motion_pairs_path: Path, + vehicle_config_path: Path, + output_directory: Path, + imu_paths_by_session: dict[str, Path] | None = None, + excluded_sessions: set[str] | None = None, + strong_rotation_min_deg: float = 1.0, + decorrelation_block_s: float = 3.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, + max_nfev: int = 200, + progress_callback: ProgressCallback | None = None, +) -> dict[str, Any]: + """Run Phase-A only. Existing LiDAR relative motions are never recomputed.""" + + output_directory.mkdir(parents=True, exist_ok=True) + source_payload, pairs, bias0, offsets = _load_cached_sessions( + motion_pairs_path + ) + session_ids = sorted(bias0) + if progress_callback is not None: + progress_callback( + "cache_loaded", + { + "schema_version": source_payload.get("schema_version"), + "sessions": len(session_ids), + "pairs": len(pairs), + }, + ) + + rehydration_report: dict[str, Any] = { + "required": not phase_a_metadata_complete(pairs), + "pair_count": len(pairs), + } + if not phase_a_metadata_complete(pairs): + supplied_paths = {} if imu_paths_by_session is None else imu_paths_by_session + missing = [sid for sid in session_ids if sid not in supplied_paths] + if missing: + raise ValueError( + "v1 cache lacks J_bg/cov; provide --session-imu for: " + + ", ".join(missing) + ) + imu_by_session = { + sid: load_imu_samples(supplied_paths[sid]) + for sid in session_ids + } + pairs, details = rehydrate_phase_a_pairs( + pairs, + imu_by_session=imu_by_session, + bias0_by_session=bias0, + progress_callback=progress_callback, + ) + rehydration_report.update(details) + if float(details["max_R_A_error_deg"]) > 0.05: + raise ValueError( + "rehydrated IMU rotations do not match cached R_A: " + f"max error={details['max_R_A_error_deg']:.6f} deg; " + "check session-to-IMU path mapping" + ) + + grouped: dict[str, list] = defaultdict(list) + for pair in pairs: + grouped[pair.session_id].append(pair) + enriched_payload = build_motion_pairs_payload( + prepared_sessions=[ + { + "session_id": sid, + "time_offset_s": offsets[sid], + "gyro_bias_rad_s": bias0[sid], + "pairs": tuple(grouped[sid]), + } + for sid in session_ids + ] + ) + enriched_cache_path = save_motion_pairs( + output_directory / "motion_pairs_phase_a_v2.json", + enriched_payload, + ) + + rotation_prior, rotation_prior_sigma_deg = _rotation_prior( + vehicle_config_path + ) + comparison = solve_phase_a_comparison( + pairs, + gyro_bias_rad_s_by_session=bias0, + rotation_prior=rotation_prior, + rotation_prior_sigma_deg=rotation_prior_sigma_deg, + preexcluded_session_ids=excluded_sessions, + strong_rotation_min_deg=strong_rotation_min_deg, + decorrelation_block_s=decorrelation_block_s, + max_pairs_per_block=max_pairs_per_block, + bias_prior_sigma_rad_s=bias_prior_sigma_rad_s, + yaw_std_max_deg=yaw_std_max_deg, + leave_one_out_yaw_range_max_deg=( + leave_one_out_yaw_range_max_deg + ), + data_prior_difference_max_deg=data_prior_difference_max_deg, + run_leave_one_out=True, + max_nfev=max_nfev, + progress_callback=progress_callback, + ) + full = phase_a_comparison_to_dict(comparison) + full["input"] = { + "motion_pairs": str(motion_pairs_path), + "source_schema_version": source_payload.get("schema_version"), + "vehicle_config": str(vehicle_config_path), + "session_imu_paths": { + sid: str(path) + for sid, path in (imu_paths_by_session or {}).items() + }, + "excluded_sessions": sorted(excluded_sessions or set()), + } + full["rehydration"] = rehydration_report + full["enriched_cache"] = str(enriched_cache_path) + full["parameters"] = { + "strong_rotation_min_deg": strong_rotation_min_deg, + "decorrelation_block_s": decorrelation_block_s, + "max_pairs_per_block": max_pairs_per_block, + "bias_prior_sigma_rad_s": bias_prior_sigma_rad_s, + "rotation_prior_sigma_deg": rotation_prior_sigma_deg, + "yaw_std_max_deg": yaw_std_max_deg, + "leave_one_out_yaw_range_max_deg": ( + leave_one_out_yaw_range_max_deg + ), + "data_prior_difference_max_deg": ( + data_prior_difference_max_deg + ), + "max_nfev": max_nfev, + } + + variants = full["variants"] + summary = { + "status": comparison.solution_status, + "accepted": comparison.accepted, + "partial_accepted": comparison.partial_accepted, + "acceptance_checks": comparison.acceptance_checks, + "primary_result": comparison.recommended_result, + "variants": { + name: { + "rpy_deg_xyz": item["rpy_deg_xyz"], + "R_IMU_lidar": item["R_IMU_lidar"], + "residual_rms_deg": item["residual_rms_deg"], + "residual_p95_deg": item["residual_p95_deg"], + "accepted": item["accepted"], + "gyro_bias_rad_s_per_session": item[ + "gyro_bias_rad_s_per_session" + ], + } + for name, item in variants.items() + if item is not None + }, + "marginal_observability_A1": full[ + "marginal_observability_A1" + ], + "data_vs_prior_yaw_diff_deg": ( + comparison.data_vs_prior_yaw_diff_deg + ), + "data_vs_prior_geodesic_deg": ( + comparison.data_vs_prior_geodesic_deg + ), + "leave_one_out_yaw_range_deg": ( + comparison.leave_one_out_yaw_range_deg + ), + "leave_one_out_observable_max_deg": ( + comparison.leave_one_out_observable_max_deg + ), + "strong_pair_candidate_count": ( + comparison.strong_pair_candidate_count + ), + "decorrelated_pair_count": comparison.decorrelated_pair_count, + "strong_pair_counts_per_session": ( + comparison.strong_pair_counts_per_session + ), + "excluded_sessions": list(comparison.excluded_sessions), + "rehydration": rehydration_report, + "comparison_file": "phase_a_comparison.json", + "observability_file": "phase_a_observability.json", + "leave_one_out_file": "phase_a_leave_one_out.json", + "enriched_cache_file": enriched_cache_path.name, + } + + _write_json(output_directory / "phase_a_comparison.json", full) + _write_json( + output_directory / "phase_a_observability.json", + full["marginal_observability_A1"], + ) + _write_json( + output_directory / "phase_a_leave_one_out.json", + full["leave_one_out"], + ) + _write_json(output_directory / "phase_a_summary.json", summary) + return summary diff --git a/imu_lidar/pipeline.py b/imu_lidar/pipeline.py index a81f079..a5f8d5b 100644 --- a/imu_lidar/pipeline.py +++ b/imu_lidar/pipeline.py @@ -2,8 +2,10 @@ from __future__ import annotations +from collections.abc import Callable from dataclasses import asdict, dataclass, replace from pathlib import Path +from time import perf_counter from typing import Any import numpy as np @@ -58,11 +60,33 @@ STAGES = ( PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"), PipelineStage("motion_pairs", "各会话构造运动对，再合并"), PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"), - PipelineStage("joint_optimizer", "用全部会话运动对联合精修；完整模式估平移"), + PipelineStage("joint_optimizer", "Phase-A 会话级零偏联合精修；Phase-B/C 暂时门控"), PipelineStage("finalize", "写出结果与质量报告"), ) +ProgressCallback = Callable[[dict[str, Any]], None] + + +def _emit_progress( + callback: ProgressCallback | None, + stage_index: int, + event: str, + **fields: Any, +) -> None: + if callback is None: + return + callback( + { + "stage_index": stage_index, + "stage_total": len(STAGES), + "stage": STAGES[stage_index - 1].name, + "event": event, + **fields, + } + ) + + def describe_pipeline(_: CalibrationRequest) -> tuple[PipelineStage, ...]: """Return the planned stages.""" @@ -79,12 +103,22 @@ def _build_pairs_and_handeye( request: CalibrationRequest, R_prior: np.ndarray | None = None, prior_sigma_deg: float | None = None, + progress_callback: ProgressCallback | None = None, ): keyframes = build_keyframes( working_frames, min_translation_m=request.min_pair_translation_m, min_rotation_deg=request.min_pair_rotation_deg, + min_registration_fitness=request.min_registration_fitness, ) + if progress_callback is not None: + progress_callback( + { + "event": "keyframes_ready", + "keyframe_count": len(keyframes.indices), + "lidar_frame_count": len(working_frames), + } + ) pair_set = build_motion_pairs( session_id=session_id, keyframes=list(keyframes.frames), @@ -94,6 +128,11 @@ def _build_pairs_and_handeye( gyro_bias_rad_s=gyro_bias_rad_s, min_rotation_deg=request.min_pair_rotation_deg, min_translation_m=request.min_pair_translation_m, + min_registration_fitness=request.min_registration_fitness, + max_imu_gap_s=request.max_imu_gap_s, + max_lidar_gap_s=request.max_lidar_gap_s, + all_frame_times_s=np.asarray([frame.t_mid_s for frame in working_frames], dtype=float), + progress_callback=progress_callback, ) handeye = solve_rotation_handeye( pair_set.pairs, @@ -133,32 +172,81 @@ def _prepare_session_pairs( *, R_prior: np.ndarray | None = None, prior_sigma_deg: float | None = None, + progress_callback: ProgressCallback | None = None, + session_index: int = 1, + session_total: int = 1, ) -> dict[str, Any]: """Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet.""" + started_at = perf_counter() + + def emit(stage_index: int, event: str, **fields: Any) -> None: + _emit_progress( + progress_callback, + stage_index, + event, + session=session.session_id, + session_index=session_index, + session_total=session_total, + **fields, + ) + + emit( + 2, + "session_start", + imu_source=str(session.imu_source), + lidar_source=str(session.lidar_source), + ) imu = load_imu_samples(session.imu_source) frames = load_lidar_frames(session.lidar_source) + emit( + 2, + "data_loaded", + imu_samples=int(imu.t_s.size), + lidar_frames=len(frames), + imu_span_s=float(imu.t_s[-1] - imu.t_s[0]) if imu.t_s.size >= 2 else 0.0, + lidar_span_s=( + float(frames[-1].t_mid_s - frames[0].t_mid_s) if len(frames) >= 2 else 0.0 + ), + elapsed_s=perf_counter() - started_at, + ) ts = audit_timestamps(imu, frames) + emit(2, "audit_complete", ok=ts.ok) if not ts.ok: + emit(2, "blocked", reason="timestamp_audit") return {"ok": False, "stage": "timestamp_audit", "session_id": session.session_id, "report": asdict(ts)} imu_report = audit_imu(imu) + emit( + 3, + "audit_complete", + ok=imu_report.ok, + gyro_bias_norm_rad_s=float(np.linalg.norm(imu_report.gyro_bias_rad_s)), + ) if not imu_report.ok: + emit(3, "blocked", reason="imu_audit") return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)} - if request.fixed_time_offset_s is not None: + fixed_time_offset_s = ( + session.fixed_time_offset_s + if session.fixed_time_offset_s is not None + else request.fixed_time_offset_s + ) + if fixed_time_offset_s is not None: + offset_source = "fixed" offset = TimeOffsetResult( - delta_t_s=float(request.fixed_time_offset_s), + delta_t_s=float(fixed_time_offset_s), correlation_peak=1.0, search_s=0.0, notes=( - f"fixed_time_offset_s={float(request.fixed_time_offset_s):.6f} " + f"fixed_time_offset_s={float(fixed_time_offset_s):.6f} " "(skip |ω| search; intended for host-UTC-bridged sessions)", ), ok=True, ) else: + offset_source = "estimated" offset = estimate_time_offset( imu, frames, @@ -166,8 +254,22 @@ def _prepare_session_pairs( search_s=request.time_offset_search_s, ) if not offset.ok: + emit( + 4, + "blocked", + reason="time_offset", + time_offset_s=float(offset.delta_t_s), + correlation_peak=float(offset.correlation_peak), + ) return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)} + emit( + 4, + "offset_ready", + source=offset_source, + time_offset_s=float(offset.delta_t_s), + correlation_peak=float(offset.correlation_peak), + ) coarse_delta_t = float(offset.delta_t_s) working_frames = frames r_x = np.eye(3) if R_prior is None else np.asarray(R_prior, dtype=float).reshape(3, 3) @@ -177,8 +279,35 @@ def _prepare_session_pairs( pairs_notes: list[str] = [] pair_count = 0 - for iteration in range(max(1, request.max_iterations)): + iterations_total = max(1, request.max_iterations) + build_pass = "outer" + + def on_build_progress(payload: dict[str, Any]) -> None: + event = str(payload.get("event", "running")) + stage_index = 5 if event == "keyframes_ready" else 6 + fields = {key: value for key, value in payload.items() if key != "event"} + emit( + stage_index, + event, + iteration=iteration + 1, + iterations_total=iterations_total, + build_pass=build_pass, + **fields, + ) + + for iteration in range(iterations_total): + build_pass = "outer" + emit( + 5, + "iteration_start", + iteration=iteration + 1, + iterations_total=iterations_total, + deskew=iteration > 0, + time_offset_s=float(offset.delta_t_s), + ) if iteration > 0: + deskew_started_at = perf_counter() + emit(5, "deskew_start", iteration=iteration + 1) working_frames = deskew_lidar_frames( frames, imu, @@ -186,6 +315,13 @@ def _prepare_session_pairs( R_IMU_lidar=r_x, gyro_bias_rad_s=imu_report.gyro_bias_rad_s, ) + emit( + 5, + "deskew_complete", + iteration=iteration + 1, + lidar_frames=len(working_frames), + elapsed_s=perf_counter() - deskew_started_at, + ) keyframes, pair_set, handeye = _build_pairs_and_handeye( session_id=session.session_id, working_frames=working_frames, @@ -195,10 +331,31 @@ def _prepare_session_pairs( request=request, R_prior=R_prior, prior_sigma_deg=prior_sigma_deg, + progress_callback=on_build_progress, ) pairs_notes = list(pair_set.notes) pair_count = len(pair_set.pairs) + emit( + 7, + "local_handeye", + iteration=iteration + 1, + build_pass=build_pass, + keyframes=len(keyframes.indices), + pair_count=pair_count, + rms_deg=float(handeye.residual_rms_deg), + p95_deg=float(handeye.residual_p95_deg), + outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg), + ok=handeye.ok, + ) if pair_count < 3: + emit( + 6, + "blocked", + reason="insufficient_motion_pairs", + iteration=iteration + 1, + keyframes=len(keyframes.indices), + pair_count=pair_count, + ) return { "ok": False, "stage": "motion_pairs", @@ -216,7 +373,14 @@ def _prepare_session_pairs( if not request.enable_signed_time_refine: continue - for _ in range(2): + for refine_step in range(1, 3): + emit( + 4, + "signed_refine_start", + iteration=iteration + 1, + refine_step=refine_step, + time_offset_s=float(offset.delta_t_s), + ) refined = refine_time_offset_signed( imu, frames, @@ -243,8 +407,18 @@ def _prepare_session_pairs( ) delta_shift = abs(refined.delta_t_s - offset.delta_t_s) offset = _merge_time_offset(offset, refined) + emit( + 4, + "signed_refine_complete", + iteration=iteration + 1, + refine_step=refine_step, + time_offset_s=float(offset.delta_t_s), + shift_s=float(delta_shift), + correlation_peak=float(refined.correlation_peak), + ) if delta_shift < 1e-3: break + build_pass = f"signed_refine_{refine_step}" keyframes, pair_set, handeye = _build_pairs_and_handeye( session_id=session.session_id, working_frames=working_frames, @@ -254,10 +428,31 @@ def _prepare_session_pairs( request=request, R_prior=R_prior, prior_sigma_deg=prior_sigma_deg, + progress_callback=on_build_progress, ) pairs_notes = list(pair_set.notes) pair_count = len(pair_set.pairs) + emit( + 7, + "local_handeye", + iteration=iteration + 1, + build_pass=build_pass, + keyframes=len(keyframes.indices), + pair_count=pair_count, + rms_deg=float(handeye.residual_rms_deg), + p95_deg=float(handeye.residual_p95_deg), + outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg), + ok=handeye.ok, + ) if pair_count < 3: + emit( + 6, + "blocked", + reason="insufficient_motion_pairs_after_signed_refine", + iteration=iteration + 1, + keyframes=len(keyframes.indices), + pair_count=pair_count, + ) return { "ok": False, "stage": "motion_pairs", @@ -271,7 +466,6 @@ def _prepare_session_pairs( "handeye": asdict(handeye), } r_x = handeye.R_IMU_lidar - assert handeye is not None and pair_set is not None and keyframes is not None acc_mean = np.asarray(imu_report.static_acc_mean_m_s2, dtype=float).reshape(3) acc_n = float(np.linalg.norm(acc_mean)) @@ -280,6 +474,15 @@ def _prepare_session_pairs( else: gravity_init = np.array([0.0, 0.0, -9.80665]) + emit( + 7, + "session_complete", + keyframes=len(keyframes.indices), + pair_count=pair_count, + time_offset_s=float(offset.delta_t_s), + local_handeye_ok=handeye.ok, + elapsed_s=perf_counter() - started_at, + ) return { "ok": True, "session_id": session.session_id, @@ -300,6 +503,8 @@ def _prepare_session_pairs( "handeye_local": { "residual_rms_deg": handeye.residual_rms_deg, "residual_median_deg": handeye.residual_median_deg, + "residual_p95_deg": handeye.residual_p95_deg, + "outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg, "pair_count": handeye.pair_count, "ok": handeye.ok, "notes": handeye.notes, @@ -323,60 +528,152 @@ def _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]: return merged -def run_calibration(request: CalibrationRequest) -> CalibrationResult: +def run_calibration( + request: CalibrationRequest, + *, + progress_callback: ProgressCallback | None = None, +) -> CalibrationResult: """Run the V1 calibration pipeline for one or more sessions. Multi-session: each session estimates its own δt and builds motion pairs; rotation hand-eye and joint SE3 are solved once on the merged pair set. """ + overall_started_at = perf_counter() + + def finish( + *, + status: CalibrationStatus, + message: str, + details: dict[str, Any], + T_IMU_lidar: np.ndarray | None = None, + time_offset_s: float | None = None, + motion_pairs_payload: dict[str, Any] | None = None, + ) -> CalibrationResult: + _emit_progress( + progress_callback, + 9, + "writing_result", + status=status.value, + output_directory=str(request.output_directory), + ) + result = finalize_result( + status=status, + message=message, + details=details, + T_IMU_lidar=T_IMU_lidar, + time_offset_s=time_offset_s, + output_directory=request.output_directory, + motion_pairs_payload=motion_pairs_payload, + ) + _emit_progress( + progress_callback, + 9, + "complete", + status=result.status.value, + elapsed_s=perf_counter() - overall_started_at, + ) + return result + + _emit_progress( + progress_callback, + 1, + "pipeline_start", + mode=request.requested_mode.value, + session_count=len(request.sessions), + max_iterations=max(1, request.max_iterations), + output_directory=str(request.output_directory), + ) if not request.sessions: - return finalize_result( + return finish( status=CalibrationStatus.BLOCKED, message="no sessions provided", details={}, - output_directory=request.output_directory, ) vehicle_config = None if request.vehicle_config is not None: + _emit_progress( + progress_callback, + 1, + "loading_vehicle_config", + path=str(request.vehicle_config), + ) try: vehicle_config = load_vehicle_config(request.vehicle_config) except Exception as exc: # noqa: BLE001 - surface config problems as blocked - return finalize_result( + _emit_progress( + progress_callback, + 1, + "blocked", + reason="vehicle_config", + error=str(exc), + ) + return finish( status=CalibrationStatus.BLOCKED, message=f"vehicle config failed: {exc}", details={}, - output_directory=request.output_directory, ) + _emit_progress( + progress_callback, + 1, + "vehicle_config_ready", + loaded=vehicle_config is not None, + ) r_prior, prior_sigma_deg = _rotation_prior_from_config(vehicle_config) prepared: list[dict[str, Any]] = [] - for session in request.sessions: + session_total = len(request.sessions) + for session_index, session in enumerate(request.sessions, start=1): prep = _prepare_session_pairs( session, request, R_prior=r_prior, prior_sigma_deg=prior_sigma_deg, + progress_callback=progress_callback, + session_index=session_index, + session_total=session_total, ) if not prep.get("ok"): - return finalize_result( + return finish( status=CalibrationStatus.BLOCKED, message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})", details={"sessions": [prep]}, - output_directory=request.output_directory, ) prepared.append(prep) all_pairs = _remap_pairs_for_joint(prepared) + pair_counts_per_session = { + p["session_id"]: int(p["pair_count"]) for p in prepared + } + _emit_progress( + progress_callback, + 7, + "joint_handeye_start", + session_count=len(prepared), + merged_pair_count=len(all_pairs), + pair_counts_per_session=pair_counts_per_session, + ) + handeye_started_at = perf_counter() handeye = solve_rotation_handeye( all_pairs, R_prior=r_prior, prior_sigma_deg=prior_sigma_deg, ) - if not handeye.ok: - return finalize_result( + _emit_progress( + progress_callback, + 7, + "joint_handeye_complete", + pair_count=handeye.pair_count, + rms_deg=float(handeye.residual_rms_deg), + p95_deg=float(handeye.residual_p95_deg), + outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg), + ok=handeye.ok, + elapsed_s=perf_counter() - handeye_started_at, + ) + if handeye.pair_count < 3: + return finish( status=CalibrationStatus.BLOCKED, message="blocked at stage rotation_handeye (joint)", details={ @@ -384,33 +681,124 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: "joint_handeye": asdict(handeye), "merged_pair_count": len(all_pairs), }, - output_directory=request.output_directory, ) force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config) - gyro_bias = np.mean(np.stack([p["gyro_bias_rad_s"] for p in prepared], axis=0), axis=0) - gravity_init = np.mean(np.stack([p["gravity_init_m_s2"] for p in prepared], axis=0), axis=0) - g_n = float(np.linalg.norm(gravity_init)) - if g_n > 1e-6: - gravity_init = gravity_init * (9.80665 / g_n) + gyro_bias_by_session = { + p["session_id"]: np.asarray(p["gyro_bias_rad_s"], dtype=float) for p in prepared + } + time_offset_by_session = { + p["session_id"]: float(p["time_offset_s"]) for p in prepared + } + + preexcluded_session_ids = { + p["session_id"] for p in prepared if not p["handeye_local"]["ok"] + } + if len(preexcluded_session_ids) == len(prepared): + _emit_progress( + progress_callback, + 8, + "phase_a_complete", + accepted=False, + reason="all_sessions_failed_local_handeye_gate", + excluded_sessions=sorted(preexcluded_session_ids), + ) + return finish( + status=CalibrationStatus.BLOCKED, + message=( + "Phase-A blocked: all sessions failed the local " + "rotation residual gate" + ), + details={ + "sessions": [_public_session(p) for p in prepared], + "joint_handeye": asdict(handeye), + "merged_pair_count": len(all_pairs), + "excluded_sessions": sorted( + preexcluded_session_ids + ), + }, + ) + _emit_progress( + progress_callback, + 8, + "phase_a_start", + session_count=len(prepared), + merged_pair_count=len(all_pairs), + preexcluded_sessions=sorted(preexcluded_session_ids), + ) + phase_a_started_at = perf_counter() + + def on_phase_a_progress( + event: str, + fields: dict[str, Any], + ) -> None: + _emit_progress( + progress_callback, + 8, + event, + **fields, + ) joint = solve_joint_extrinsic( all_pairs, handeye.R_IMU_lidar, force_rotation_only=force_rotation_only, imu=None, - delta_t_s=0.0, - gyro_bias_rad_s=gyro_bias, - gravity_init_m_s2=gravity_init, + gyro_bias_rad_s_by_session=gyro_bias_by_session, + time_offset_s_by_session=time_offset_by_session, + preexcluded_session_ids=preexcluded_session_ids, + rotation_prior=r_prior, + rotation_prior_sigma_deg=( + 15.0 if prior_sigma_deg is None else prior_sigma_deg + ), + phase_a_progress_callback=on_phase_a_progress, enable_phase_c=not force_rotation_only, t_init_m=t_prior, t_prior_m=t_prior, t_prior_sigma_m=t_prior_sigma, ) + included_sessions = [ + item.session_id for item in joint.phase_a_sessions if item.included_in_final + ] + excluded_sessions = [ + item.session_id for item in joint.phase_a_sessions if not item.included_in_final + ] + _emit_progress( + progress_callback, + 8, + "phase_a_complete", + accepted=joint.phase_a_accepted, + joint_rms_deg=float(joint.residual_rms_rot_deg), + rotation_observable=joint.observability.rotation_observable, + included_sessions=included_sessions, + excluded_sessions=excluded_sessions, + elapsed_s=perf_counter() - phase_a_started_at, + ) + for item in joint.phase_a_sessions: + _emit_progress( + progress_callback, + 8, + "phase_a_session", + session=item.session_id, + included=item.included_in_final, + accepted=item.accepted, + pair_count=item.pair_count, + rms_deg=float(item.residual_rms_deg), + p95_deg=float(item.residual_p95_deg), + bias_delta_norm_rad_s=float( + np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s) + ), + gyro_bias_rad_s=np.asarray(item.gyro_bias_rad_s, dtype=float).round(8).tolist(), + ) + phase_a_by_session = { + item.session_id: item for item in joint.phase_a_sessions + } session_results = [] for prep in prepared: + phase_a = phase_a_by_session.get(prep["session_id"]) + session_bias = joint.gyro_bias_rad_s_per_session.get(prep["session_id"]) session_results.append( { **_public_session(prep), @@ -418,6 +806,8 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: "handeye": { "residual_rms_deg": handeye.residual_rms_deg, "residual_median_deg": handeye.residual_median_deg, + "residual_p95_deg": handeye.residual_p95_deg, + "outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg, "pair_count": handeye.pair_count, "ok": handeye.ok, "notes": tuple(list(handeye.notes) + [f"joint over {len(request.sessions)} sessions"]), @@ -430,9 +820,10 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: "observability": asdict(joint.observability), "notes": joint.notes, "T_IMU_lidar": joint.T_IMU_lidar.tolist(), + "phase_a": None if phase_a is None else asdict(phase_a), "gyro_bias_rad_s": None - if joint.gyro_bias_rad_s is None - else np.asarray(joint.gyro_bias_rad_s, dtype=float).tolist(), + if session_bias is None + else np.asarray(session_bias, dtype=float).tolist(), "accel_bias_m_s2": None if joint.accel_bias_m_s2 is None else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(), @@ -441,14 +832,40 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: else np.asarray(joint.gravity_m_s2, dtype=float).tolist(), }, "translation_accepted": joint.translation_accepted, - "rotation_ok": handeye.ok and joint.observability.rotation_observable, + "rotation_ok": ( + phase_a is not None + and phase_a.included_in_final + and phase_a.accepted + and joint.phase_a_accepted + and joint.observability.rotation_observable + ), + "rotation_prior_constrained": ( + phase_a is not None + and phase_a.included_in_final + and phase_a.accepted + and joint.phase_a_accepted + and not joint.observability.rotation_observable + and r_prior is not None + ), } ) T = np.asarray(joint.T_IMU_lidar, dtype=float) - # Report per-session δt list; keep first as scalar for backward-compatible field. - delta_t = float(prepared[0]["time_offset_s"]) - if request.requested_mode == CalibrationMode.FULL_SE3: + if request.requested_mode == CalibrationMode.ROTATION_ONLY: + # A rotation-only result must never expose a seed/prior translation, + # including when the rotation itself is rejected by a later gate. + T = T.copy() + T[:3, 3] = 0.0 + # Multi-session offsets stay in details; the legacy scalar is single-session only. + delta_t = float(prepared[0]["time_offset_s"]) if len(prepared) == 1 else None + joint_rotation_ok = joint.phase_a_accepted + if not joint_rotation_ok: + status = CalibrationStatus.BLOCKED + message = ( + f"joint rotation rejected: RMS={joint.residual_rms_rot_deg:.3f} deg " + "or a retained session failed the Phase-A residual gates" + ) + elif request.requested_mode == CalibrationMode.FULL_SE3: if joint.translation_accepted: status = CalibrationStatus.FULL_SE3_ACCEPTED message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)" @@ -456,15 +873,31 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: status = CalibrationStatus.FULL_SE3_REJECTED message = ( f"rotation accepted jointly ({len(prepared)} sessions); " - "translation rejected by observability/residual gates" + "translation deferred until Phase-B/C session-state redesign" ) - else: + elif joint.observability.rotation_observable: status = CalibrationStatus.ROTATION_ONLY_ACCEPTED - message = f"rotation-only calibration accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)" + message = ( + f"rotation-only calibration accepted " + f"(joint {len(prepared)} sessions, {len(all_pairs)} pairs)" + ) + T = T.copy() + T[:3, 3] = 0.0 + elif r_prior is not None: + status = CalibrationStatus.ROTATION_ONLY_PRIOR_CONSTRAINED + message = ( + "rotation residuals passed, but motion does not independently observe all " + "rotation axes; result remains constrained by the installation prior" + ) + T = T.copy() + T[:3, 3] = 0.0 + else: + status = CalibrationStatus.BLOCKED + message = "rotation residuals passed but rotation observability failed without a prior" T = T.copy() T[:3, 3] = 0.0 - return finalize_result( + return finish( status=status, message=message, details={ @@ -475,12 +908,26 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: "pair_counts_per_session": {p["session_id"]: p["pair_count"] for p in prepared}, "time_offset_s_per_session": {p["session_id"]: p["time_offset_s"] for p in prepared}, "handeye_rms_deg": handeye.residual_rms_deg, + "handeye_p95_deg": handeye.residual_p95_deg, + "handeye_outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg, + "phase_a_accepted": joint.phase_a_accepted, + "phase_a_comparison": joint.phase_a_comparison, + "phase_a_sessions": [asdict(item) for item in joint.phase_a_sessions], + "gyro_bias_rad_s_per_session": { + sid: np.asarray(value, dtype=float).tolist() + for sid, value in joint.gyro_bias_rad_s_per_session.items() + }, + "excluded_sessions": [ + item.session_id for item in joint.phase_a_sessions if not item.included_in_final + ], + "joint_rotation_rms_deg": joint.residual_rms_rot_deg, + "rotation_observable": joint.observability.rotation_observable, "translation_accepted": joint.translation_accepted, }, + "joint_handeye": asdict(handeye), }, - T_IMU_lidar=T, + T_IMU_lidar=None if status == CalibrationStatus.BLOCKED else T, time_offset_s=delta_t, - output_directory=request.output_directory, motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared), ) diff --git a/imu_lidar/rotation_handeye.py b/imu_lidar/rotation_handeye.py index 0289274..99eb3e3 100644 --- a/imu_lidar/rotation_handeye.py +++ b/imu_lidar/rotation_handeye.py @@ -16,6 +16,8 @@ class RotationHandeyeResult: R_IMU_lidar: np.ndarray residual_rms_deg: float residual_median_deg: float + residual_p95_deg: float + outlier_fraction_gt_5deg: float pair_count: int ok: bool notes: tuple[str, ...] = () @@ -28,17 +30,21 @@ def _pair_weight(pair: MotionPair) -> float: return weight -def _tsai_rotation_initial(pairs: list[MotionPair]) -> np.ndarray: +def _tsai_rotation_initial( + pairs: list[MotionPair], + pair_weights: np.ndarray | None = None, +) -> np.ndarray: """Closed-form rotation hand-eye initial guess (Tsai-style linear solve).""" rows: list[np.ndarray] = [] rhs: list[np.ndarray] = [] - for pair in pairs: + weights = np.ones(len(pairs)) if pair_weights is None else np.asarray(pair_weights, dtype=float) + for pair, pair_weight in zip(pairs, weights): alpha = so3_log(pair.R_A) beta = so3_log(pair.R_B) if np.linalg.norm(alpha) < 1e-6 or np.linalg.norm(beta) < 1e-6: continue - w = np.sqrt(_pair_weight(pair)) + w = np.sqrt(float(pair_weight)) rows.append(w * skew(alpha + beta)) rhs.append(w * (beta - alpha)) if len(rows) < 2: @@ -64,6 +70,44 @@ def _rms_deg(r_x: np.ndarray, pairs: list[MotionPair]) -> float: return float(np.sqrt(np.mean(errs**2))) +def select_strong_rotation_pairs( + pairs: list[MotionPair] | tuple[MotionPair, ...], + *, + min_rotation_deg: float = 1.0, +) -> list[MotionPair]: + """Return pairs that independently excite rotation on both sensor sides.""" + + threshold = float(min_rotation_deg) + return [ + pair + for pair in pairs + if rotation_angle_deg(pair.R_A) > threshold + and rotation_angle_deg(pair.R_B) > threshold + ] + + +def estimate_rotation_handeye_initial( + pairs: list[MotionPair] | tuple[MotionPair, ...], + *, + min_rotation_deg: float = 1.0, +) -> np.ndarray: + """Return the fast data-only Tsai initialization without nonlinear refine.""" + + usable = select_strong_rotation_pairs( + pairs, + min_rotation_deg=min_rotation_deg, + ) + if not usable: + return np.eye(3) + raw_weights = np.asarray( + [_pair_weight(pair) for pair in usable], + dtype=float, + ) + median = max(float(np.median(raw_weights)), 1e-12) + weights = np.clip(raw_weights / median, 0.1, 10.0) + return _tsai_rotation_initial(usable, weights) + + def solve_rotation_handeye( pairs: list[MotionPair] | tuple[MotionPair, ...], *, @@ -76,19 +120,24 @@ def solve_rotation_handeye( is weakly observable under near-planar motion. """ - usable = [pair for pair in pairs if rotation_angle_deg(pair.R_A) > 1.0 and rotation_angle_deg(pair.R_B) > 1.0] + usable = select_strong_rotation_pairs(pairs) notes: list[str] = [] if len(usable) < 3: return RotationHandeyeResult( R_IMU_lidar=np.eye(3), residual_rms_deg=1e9, residual_median_deg=1e9, + residual_p95_deg=1e9, + outlier_fraction_gt_5deg=1.0, pair_count=len(usable), ok=False, notes=("need at least 3 motion pairs with meaningful rotation",), ) - r0 = _tsai_rotation_initial(usable) + raw_weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float) + median_raw_weight = max(float(np.median(raw_weights)), 1e-12) + weights = np.clip(raw_weights / median_raw_weight, 0.1, 10.0) + r0 = _tsai_rotation_initial(usable, weights) r_prior = None if R_prior is not None: r_prior = orthonormalize_rotation(np.asarray(R_prior, dtype=float).reshape(3, 3)) @@ -104,10 +153,11 @@ def solve_rotation_handeye( f"init from Tsai (rms={rms_tsai:.3f} deg; prior {rms_prior:.3f} deg kept as soft constraint)" ) - weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float) notes.append( - f"weighted hand-eye: weight median={float(np.median(weights)):.3g}, " - f"min={float(np.min(weights)):.3g}, max={float(np.max(weights)):.3g}" + "weighted hand-eye: normalized/clipped IMU confidence " + f"raw_median={median_raw_weight:.3g}, " + f"normalized_min={float(np.min(weights)):.3g}, " + f"normalized_max={float(np.max(weights)):.3g}" ) def pack(r: np.ndarray) -> np.ndarray: @@ -139,14 +189,28 @@ def solve_rotation_handeye( # Report unweighted RMS/median for interpretability. rms = float(np.sqrt(np.mean(errs**2))) med = float(np.median(errs)) + p95 = float(np.percentile(errs, 95.0)) + outlier_fraction = float(np.mean(errs > 5.0)) notes.append(f"optimized over {len(usable)} pairs") - ok = rms < 5.0 and len(usable) >= 3 + notes.append( + f"rotation residual quality: rms={rms:.3f} deg, median={med:.3f} deg, " + f"p95={p95:.3f} deg, >5deg={100.0 * outlier_fraction:.2f}%" + ) + ok = ( + len(usable) >= 3 + and rms < 1.5 + and med < 0.5 + and p95 < 1.5 + and outlier_fraction <= 0.005 + ) if not ok: - notes.append("rotation residual RMS too high or too few pairs") + notes.append("rotation residual distribution failed acceptance gates") return RotationHandeyeResult( R_IMU_lidar=r_x, residual_rms_deg=rms, residual_median_deg=med, + residual_p95_deg=p95, + outlier_fraction_gt_5deg=outlier_fraction, pair_count=len(usable), ok=ok, notes=tuple(notes), diff --git a/imu_lidar/文件职责说明.md b/imu_lidar/文件职责说明.md index a36e32c..b63df12 100644 --- a/imu_lidar/文件职责说明.md +++ b/imu_lidar/文件职责说明.md @@ -57,7 +57,7 @@ python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output - **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。 - `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。 -- `--mode full_se3`：在可观时再估计重力、关键帧速度、时变零偏与平移；结果写入 `summary.json` 的 joint 字段。 +- `--mode full_se3`：当前完成 Phase-A 后明确拒绝平移；待 Phase-B/C 会话状态重构完成后再恢复完整 SE(3) 交付。 --- diff --git a/tests/README.md b/tests/README.md index 5d2aee6..b9cf276 100644 --- a/tests/README.md +++ b/tests/README.md @@ -28,8 +28,14 @@ python -m pytest -q | `test_preintegration_bias_jacobian_matches_finite_difference` | 随机陀螺序列 | 旋转预积分 `J_bg` | 与有限差分一致（松阈值） | | `test_imu_preintegration_recovers_constant_accel_translation` | 常值加速度 | 完整预积分 Δv/Δp | 接近解析值 | | `test_imu_preintegration_bias_jacobian_finite_difference` | 随机 IMU | `J_bg`/`J_ba` 一阶修正 | 与重积分接近 | -| `test_synthetic_pipeline_rotation_and_time_offset` | 端到端合成会话 | `rotation_only` 全流程 | `rotation_only_accepted`；δt 准；手眼 RMS < 5° | -| `test_synthetic_pipeline_full_se3_smoke` | 同上 | `full_se3` 不崩溃 | 状态为 accepted / rejected / rotation_only 之一 | +| `test_synthetic_pipeline_rejects_noisy_icp_but_keeps_time_audit` | synthetic end-to-end | strict rotation quality gate + time audit | noisy ICP is blocked; delta-t remains accurate | +| `test_synthetic_pipeline_full_se3_smoke` | synthetic end-to-end | full-SE(3) smoke test | returns an explicit accepted/rejected/blocked status | +| `test_planar_yaw_is_not_full_rotation_or_translation_observable` | pure-yaw motion pairs | degeneracy detection | full rotation/translation observability is rejected | +| `test_multi_axis_motion_is_rotation_and_translation_observable` | multi-axis motion pairs | positive observability case | rotation and translation pass | +| `test_translation_prior_is_reported_but_not_accepted_when_unobservable` | planar motion + CAD prior | prior semantics | prior is reported but not accepted as calibration | +| `test_handeye_rejects_a_small_fraction_of_gross_rotation_outliers` | motion pairs with a gross outlier | residual-distribution gate | solve is rejected | +| `test_motion_pairs_reject_low_fitness` | low-fitness registration | fitness gate | no motion pair is emitted | +| `test_motion_pairs_reject_imu_and_lidar_discontinuities` | timestamp gaps | continuity gates | cross-gap pairs are rejected | 合成数据由 `tools/generate_synthetic_session.py` 生成（墙面点云 + 已知外参 yaw 与 δt）。 diff --git a/tests/test_motion_pairs_io.py b/tests/test_motion_pairs_io.py index 718dfe6..81390b1 100644 --- a/tests/test_motion_pairs_io.py +++ b/tests/test_motion_pairs_io.py @@ -29,14 +29,29 @@ def test_pair_roundtrip(tmp_path: Path) -> None: t_A_m=np.array([0.1, 0.0, 0.0]), t_B_m=np.array([0.1, 0.0, 0.0]), fitness=0.8, - metadata={"weight": 12.0, "cov9": [[0.0] * 9] * 9, "backend": "test"}, + metadata={ + "weight": 12.0, + "cov": (np.eye(3) * 1e-4).tolist(), + "J_bg": (-np.eye(3)).tolist(), + "cov9": [[0.0] * 9] * 9, + "backend": "test", + "gyro_bias0_rad_s": [0.01, -0.02, 0.03], + "accel_bias0_m_s2": [0.1, 0.2, -0.1], + "time_offset_s": 0.004, + "keyframe_span": 3, + "is_consecutive": False, + }, ) encoded = pair_to_dict(pair) assert "cov9" not in encoded["metadata"] + assert "cov" in encoded["metadata"] + assert "J_bg" in encoded["metadata"] assert encoded["metadata"]["weight"] == 12.0 restored = pair_from_dict(encoded) assert restored.i == 1 and restored.j == 4 np.testing.assert_allclose(restored.t_A_m, [0.1, 0.0, 0.0]) + np.testing.assert_allclose(restored.metadata["gyro_bias0_rad_s"], [0.01, -0.02, 0.03]) + assert restored.metadata["keyframe_span"] == 3 payload = build_motion_pairs_payload( prepared_sessions=[ @@ -50,6 +65,14 @@ def test_pair_roundtrip(tmp_path: Path) -> None: ) path = save_motion_pairs(tmp_path / "motion_pairs.json", payload) loaded = load_motion_pairs(path) + assert loaded["schema_version"] == 2 pairs = pairs_for_session(loaded, "s0") assert len(pairs) == 1 assert pairs[0].session_id == "s0" + + payload["schema_version"] = 1 + legacy_path = save_motion_pairs( + tmp_path / "motion_pairs_v1.json", payload + ) + legacy = load_motion_pairs(legacy_path) + assert legacy["schema_version"] == 1 diff --git a/tests/test_phase_a.py b/tests/test_phase_a.py new file mode 100644 index 0000000..a30bcf3 --- /dev/null +++ b/tests/test_phase_a.py @@ -0,0 +1,194 @@ +"""Tests for cached, session-balanced Phase-A comparison.""" + +from __future__ import annotations + +import numpy as np + +from imu_lidar.contracts import ImuSeries, MotionPair +from imu_lidar.geometry import so3_exp, so3_log +from imu_lidar.imu_preintegration import preintegrate_gyro +from imu_lidar.phase_a import ( + rehydrate_phase_a_pairs, + select_decorrelated_phase_a_pairs, + solve_phase_a_comparison, +) + + +def _phase_a_pair( + session_id: str, + index: int, + r_true: np.ndarray, + vector_deg: tuple[float, float, float], + bias0: np.ndarray, +) -> MotionPair: + r_b = so3_exp(np.deg2rad(np.asarray(vector_deg, dtype=float))) + return MotionPair( + session_id=session_id, + i=index, + j=index + 1, + t_i_s=float(index), + t_j_s=float(index + 1), + R_A=r_true @ r_b @ r_true.T, + R_B=r_b, + t_A_m=np.zeros(3), + t_B_m=np.zeros(3), + metadata={ + "J_bg": (-np.eye(3)).tolist(), + "cov": (np.eye(3) * 1e-5).tolist(), + "gyro_bias0_rad_s": bias0.tolist(), + }, + ) + + +def test_phase_a_reports_three_variants_and_leave_one_session_out() -> None: + r_true = so3_exp(np.deg2rad(np.array([3.0, -2.0, 25.0]))) + prior = so3_exp(np.deg2rad(np.array([0.0, 0.0, 0.2]))) @ r_true + vectors = ( + (12.0, 0.0, 0.0), + (0.0, 15.0, 0.0), + (0.0, 0.0, 18.0), + (10.0, 8.0, 0.0), + (0.0, 11.0, 9.0), + (7.0, 0.0, 13.0), + (9.0, -5.0, 6.0), + (-6.0, 8.0, 11.0), + (5.0, 7.0, -9.0), + ) + biases = { + "s0": np.array([0.001, -0.0005, 0.0002]), + "s1": np.array([-0.0004, 0.0008, -0.0001]), + "s2": np.array([0.0002, 0.0001, -0.0006]), + } + pairs: list[MotionPair] = [] + index = 0 + for sid, count in (("s0", 18), ("s1", 9), ("s2", 6)): + for local_index in range(count): + pairs.append( + _phase_a_pair( + sid, + index, + r_true, + vectors[local_index % len(vectors)], + biases[sid], + ) + ) + index += 1 + + result = solve_phase_a_comparison( + pairs, + gyro_bias_rad_s_by_session=biases, + rotation_prior=prior, + rotation_prior_sigma_deg=15.0, + yaw_std_max_deg=1.0, + leave_one_out_yaw_range_max_deg=1.0, + data_prior_difference_max_deg=1.0, + decorrelation_block_s=0.0, + max_nfev=80, + ) + + assert result.accepted + assert result.strong_pair_counts_per_session == { + "s0": 18, + "s1": 9, + "s2": 6, + } + assert len(result.leave_one_out) == 3 + assert result.marginal_observability.rank == 3 + assert result.leave_one_out_yaw_range_deg < 0.1 + for variant in ( + result.fixed_bg_data_only, + result.session_bg_data_only, + result.session_bg_with_rotation_prior, + ): + error_deg = np.degrees( + np.linalg.norm( + so3_log(r_true.T @ variant.R_IMU_lidar) + ) + ) + assert error_deg < 0.1 + + +def test_rehydrate_phase_a_pairs_recovers_jacobian_without_lidar() -> None: + t_s = np.linspace(0.0, 1.0, 201) + gyro = np.tile(np.array([0.12, -0.04, 0.2]), (t_s.size, 1)) + bias0 = np.array([0.01, -0.005, 0.002]) + imu = ImuSeries( + t_s=t_s, + gyro_rad_s=gyro, + acc_m_s2=np.zeros((t_s.size, 3)), + ) + preint = preintegrate_gyro(t_s, gyro, 0.1, 0.8, bias0) + pair = MotionPair( + session_id="s0", + i=0, + j=1, + t_i_s=0.1, + t_j_s=0.8, + R_A=preint.delta_R, + R_B=preint.delta_R, + metadata={ + "t_i_imu_s": 0.1, + "t_j_imu_s": 0.8, + "gyro_bias0_rad_s": bias0.tolist(), + "preint_sigma_rad": preint.sigma_rad, + }, + ) + + enriched, report = rehydrate_phase_a_pairs( + [pair], + imu_by_session={"s0": imu}, + bias0_by_session={"s0": bias0}, + ) + + assert "J_bg" in enriched[0].metadata + assert "cov" in enriched[0].metadata + assert report["max_R_A_error_deg"] < 1e-8 + + +def test_phase_a_time_blocks_do_not_count_overlapping_pairs_as_independent() -> None: + r_true = so3_exp(np.deg2rad(np.array([1.0, -2.0, 20.0]))) + bias = np.zeros(3) + pairs = [ + _phase_a_pair("s0", index, r_true, (5.0 + index, 2.0, 1.0), bias) + for index in range(9) + ] + selected = select_decorrelated_phase_a_pairs( + pairs, + block_s=3.0, + max_pairs_per_block=1, + ) + assert len(selected) == 3 + assert all(pair in pairs for pair in selected) + + +def test_phase_a_planar_motion_is_partial_and_keeps_weak_direction_from_prior() -> None: + r_true = so3_exp(np.deg2rad(np.array([4.0, -3.0, 31.0]))) + prior = so3_exp(np.deg2rad(np.array([0.2, -0.1, 0.4]))) @ r_true + biases = {"s0": np.zeros(3), "s1": np.zeros(3)} + pairs: list[MotionPair] = [] + for session_index, sid in enumerate(biases): + for index in range(12): + pairs.append( + _phase_a_pair( + sid, + session_index * 100 + index, + r_true, + (0.0, 0.0, 8.0 + index), + biases[sid], + ) + ) + result = solve_phase_a_comparison( + pairs, + gyro_bias_rad_s_by_session=biases, + rotation_prior=prior, + decorrelation_block_s=0.0, + yaw_std_max_deg=0.5, + run_leave_one_out=False, + max_nfev=80, + ) + assert not result.accepted + assert result.partial_accepted + assert result.solution_status == "phase_a_partial_accepted" + assert result.marginal_observability.precision_rank == 2 + assert result.observable_subspace_with_prior is not None + assert np.isinf(result.marginal_observability.direction_std_deg[0]) diff --git a/tests/test_quality_gates.py b/tests/test_quality_gates.py new file mode 100644 index 0000000..00caf1b --- /dev/null +++ b/tests/test_quality_gates.py @@ -0,0 +1,260 @@ +"""Regression tests for calibration quality, continuity, and observability gates.""" + +from __future__ import annotations + +import numpy as np + +from imu_lidar.contracts import ImuSeries, LidarFrame, MotionPair +from imu_lidar.geometry import make_transform, so3_exp, so3_log +from imu_lidar.joint_optimizer import solve_joint_extrinsic +from imu_lidar.motion_pairs import build_motion_pairs +from imu_lidar.observability import analyze_observability +from imu_lidar.registration import RegistrationResult +from imu_lidar.rotation_handeye import solve_rotation_handeye + + +def _motion_pair(index: int, rotation_vector: np.ndarray) -> MotionPair: + rotation = so3_exp(np.asarray(rotation_vector, dtype=float)) + return MotionPair( + session_id="synthetic", + i=index, + j=index + 1, + t_i_s=float(index), + t_j_s=float(index + 1), + R_A=rotation, + R_B=rotation, + t_A_m=np.zeros(3), + t_B_m=np.array([0.1, -0.03, 0.0]), + fitness=0.9, + metadata={ + "J_bg": (-np.eye(3)).tolist(), + "cov": (np.eye(3) * 1e-4).tolist(), + "gyro_bias0_rad_s": [0.0, 0.0, 0.0], + }, + ) + + +def _frame(frame_id: str, mid_s: float) -> LidarFrame: + return LidarFrame( + frame_id=frame_id, + t_start_s=mid_s - 0.01, + t_end_s=mid_s + 0.01, + points_xyz=np.zeros((64, 3)), + ) + + +def _registration(*, fitness: float = 0.9) -> RegistrationResult: + rotation = so3_exp(np.deg2rad(np.array([0.0, 0.0, 10.0]))) + return RegistrationResult( + transform=make_transform(np.array([0.4, 0.0, 0.0]), rotation), + fitness=fitness, + rotation_deg=10.0, + translation_m=0.4, + backend="test", + ok=True, + ) + + +def test_planar_yaw_is_not_full_rotation_or_translation_observable(): + pairs = [ + _motion_pair(i, np.deg2rad(np.array([0.0, 0.0, angle_deg]))) + for i, angle_deg in enumerate((5.0, 8.0, 12.0, 17.0, 23.0, 31.0)) + ] + + report = analyze_observability(pairs, np.eye(3)) + + assert not report.rotation_observable + assert not report.translation_observable + + +def test_multi_axis_motion_is_rotation_and_translation_observable(): + vectors_deg = ( + (12.0, 0.0, 0.0), + (0.0, 15.0, 0.0), + (0.0, 0.0, 18.0), + (10.0, 8.0, 0.0), + (0.0, 11.0, 9.0), + (7.0, 0.0, 13.0), + ) + pairs = [ + _motion_pair(i, np.deg2rad(np.asarray(vector_deg))) + for i, vector_deg in enumerate(vectors_deg) + ] + + report = analyze_observability(pairs, np.eye(3)) + + assert report.rotation_observable + assert report.translation_observable + + +def test_translation_prior_is_reported_but_not_accepted_when_unobservable(): + pairs = [ + _motion_pair(i, np.deg2rad(np.array([0.0, 0.0, angle_deg]))) + for i, angle_deg in enumerate((5.0, 8.0, 12.0, 17.0, 23.0, 31.0)) + ] + prior = np.array([0.3, -0.2, 0.5]) + + result = solve_joint_extrinsic( + pairs, + np.eye(3), + force_rotation_only=False, + enable_phase_c=False, + t_prior_m=prior, + ) + + assert not result.translation_accepted + np.testing.assert_allclose(result.T_IMU_lidar[:3, 3], prior) + assert any("prior only" in note for note in result.notes) + + +def test_handeye_rejects_a_small_fraction_of_gross_rotation_outliers(): + rng = np.random.default_rng(7) + r_true = so3_exp(np.deg2rad(np.array([2.0, -3.0, 20.0]))) + pairs: list[MotionPair] = [] + for index in range(100): + axis = rng.normal(size=3) + axis /= np.linalg.norm(axis) + r_b = so3_exp(axis * np.deg2rad(rng.uniform(8.0, 30.0))) + r_a = r_true @ r_b @ r_true.T + if index == 0: + r_a = so3_exp(np.deg2rad(np.array([18.0, 0.0, 0.0]))) @ r_a + pairs.append( + MotionPair( + session_id="outlier", + i=index, + j=index + 1, + t_i_s=float(index), + t_j_s=float(index + 1), + R_A=r_a, + R_B=r_b, + ) + ) + + result = solve_rotation_handeye(pairs) + + assert not result.ok + assert result.outlier_fraction_gt_5deg > 0.005 + + +def test_motion_pairs_reject_low_fitness(monkeypatch): + monkeypatch.setattr( + "imu_lidar.motion_pairs.register_lidar_pair", + lambda *_args, **_kwargs: _registration(fitness=0.3), + ) + imu = ImuSeries( + t_s=np.linspace(0.0, 1.2, 121), + gyro_rad_s=np.zeros((121, 3)), + acc_m_s2=np.zeros((121, 3)), + ) + + result = build_motion_pairs( + session_id="fitness", + keyframes=[_frame("0", 0.1), _frame("1", 1.1)], + keyframe_indices=[0, 1], + imu=imu, + delta_t_s=0.0, + min_registration_fitness=0.5, + ) + + assert not result.pairs + assert any("fitness<0.50: 1" in note for note in result.notes) + + +def test_motion_pairs_reject_imu_and_lidar_discontinuities(monkeypatch): + monkeypatch.setattr( + "imu_lidar.motion_pairs.register_lidar_pair", + lambda *_args, **_kwargs: _registration(), + ) + imu_with_gap = ImuSeries( + t_s=np.array([0.0, 0.1, 0.2, 0.3, 0.4, 0.8, 0.9, 1.0, 1.1, 1.2]), + gyro_rad_s=np.zeros((10, 3)), + acc_m_s2=np.zeros((10, 3)), + ) + imu_result = build_motion_pairs( + session_id="imu-gap", + keyframes=[_frame("0", 0.1), _frame("1", 1.1)], + keyframe_indices=[0, 1], + imu=imu_with_gap, + delta_t_s=0.0, + max_imu_gap_s=0.2, + ) + + assert not imu_result.pairs + assert any("IMU gap>0.200s: 1" in note for note in imu_result.notes) + + continuous_imu = ImuSeries( + t_s=np.linspace(0.0, 2.2, 221), + gyro_rad_s=np.zeros((221, 3)), + acc_m_s2=np.zeros((221, 3)), + ) + lidar_result = build_motion_pairs( + session_id="lidar-gap", + keyframes=[_frame("0", 0.1), _frame("2", 2.1)], + keyframe_indices=[0, 2], + imu=continuous_imu, + delta_t_s=0.0, + all_frame_times_s=np.array([0.1, 0.2, 2.1]), + max_lidar_gap_s=0.5, + ) + + assert not lidar_result.pairs + assert any("LiDAR gap>0.500s: 1" in note for note in lidar_result.notes) + +def test_phase_a_keeps_session_bias_linearization_points_independent(): + r_true = so3_exp(np.deg2rad(np.array([2.0, -3.0, 20.0]))) + bias0_by_session = { + "s0": np.array([0.010, -0.004, 0.002]), + "s1": np.array([-0.006, 0.008, -0.003]), + } + vectors_deg = ( + (12.0, 0.0, 0.0), + (0.0, 15.0, 0.0), + (0.0, 0.0, 18.0), + (10.0, 8.0, 0.0), + (0.0, 11.0, 9.0), + (7.0, 0.0, 13.0), + ) + pairs: list[MotionPair] = [] + for session_index, (session_id, bias0) in enumerate(bias0_by_session.items()): + for pair_index, vector_deg in enumerate(vectors_deg): + r_b = so3_exp(np.deg2rad(np.asarray(vector_deg))) + r_a = r_true @ r_b @ r_true.T + index = session_index * 100 + pair_index + pairs.append( + MotionPair( + session_id=session_id, + i=index, + j=index + 1, + t_i_s=float(pair_index), + t_j_s=float(pair_index + 1), + R_A=r_a, + R_B=r_b, + t_A_m=np.zeros(3), + t_B_m=np.zeros(3), + metadata={ + "J_bg": np.eye(3).tolist(), + "cov": (np.eye(3) * 1e-4).tolist(), + "gyro_bias0_rad_s": bias0.tolist(), + }, + ) + ) + + result = solve_joint_extrinsic( + pairs, + r_true, + force_rotation_only=True, + gyro_bias_rad_s_by_session=bias0_by_session, + ) + + assert result.phase_a_accepted + assert set(result.phase_a_comparison["variants"]) == { + "A0_fixed_bg_data_only", + "A1_session_bg_data_only", + "A2_session_bg_with_rotation_prior", + } + assert set(result.gyro_bias_rad_s_per_session) == {"s0", "s1"} + for session_id, bias0 in bias0_by_session.items(): + np.testing.assert_allclose( + result.gyro_bias_rad_s_per_session[session_id], bias0, atol=1e-8 + ) + assert np.linalg.norm(so3_log(r_true.T @ result.T_IMU_lidar[:3, :3])) < 1e-8 diff --git a/tests/test_v1_pipeline.py b/tests/test_v1_pipeline.py index ea8788b..6191d81 100644 --- a/tests/test_v1_pipeline.py +++ b/tests/test_v1_pipeline.py @@ -54,7 +54,7 @@ def _log(rotation: np.ndarray) -> np.ndarray: return so3_log(rotation) -def test_synthetic_pipeline_rotation_and_time_offset(tmp_path: Path): +def test_synthetic_pipeline_rejects_noisy_icp_but_keeps_time_audit(tmp_path: Path): meta = generate_synthetic_session(tmp_path, delta_t_s=0.17, yaw_extrinsic_deg=25.0) config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml" out = tmp_path / "out" @@ -74,17 +74,28 @@ def test_synthetic_pipeline_rotation_and_time_offset(tmp_path: Path): min_pair_rotation_deg=2.0, min_pair_translation_m=0.05, ) - result = run_calibration(request) - assert result.status.value == "rotation_only_accepted" - assert result.time_offset_s is not None - assert abs(result.time_offset_s - meta["delta_t_s"]) < 0.05 - assert result.T_IMU_lidar is not None - # End-to-end uses approximate ICP; allow moderate absolute error but require consistency. - r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]]))) - err_deg = np.degrees(np.linalg.norm(_log(r_true.T @ result.T_IMU_lidar[:3, :3]))) - assert err_deg < 15.0 + progress_events: list[dict] = [] + result = run_calibration(request, progress_callback=progress_events.append) + # The lightweight synthetic point cloud uses approximate ICP and has a + # roughly 3-degree P95 residual. The production gate must reject it rather + # than expose a plausible-looking extrinsic. + assert result.status.value == "blocked" + assert result.T_IMU_lidar is None session0 = result.details["sessions"][0] - assert session0["handeye"]["residual_rms_deg"] < 5.0 + assert abs(session0["time_offset_s"] - meta["delta_t_s"]) < 0.05 + assert result.details["joint_handeye"]["residual_p95_deg"] > 1.5 + assert not result.details["joint_handeye"]["ok"] + assert progress_events[0]["event"] == "pipeline_start" + assert any( + event["stage"] == "motion_pairs" and event["event"] == "complete" + for event in progress_events + ) + assert any( + event["stage"] == "joint_optimizer" and event["event"] == "phase_a_complete" + for event in progress_events + ) + assert progress_events[-1]["stage"] == "finalize" + assert progress_events[-1]["event"] == "complete" def test_time_offset_on_synthetic(tmp_path: Path): @@ -176,13 +187,17 @@ def test_synthetic_pipeline_full_se3_smoke(tmp_path: Path): "full_se3_accepted", "full_se3_rejected_due_to_observability", "rotation_only_accepted", + "blocked", } - assert result.T_IMU_lidar is not None session0 = result.details["sessions"][0] assert "delta_v" in session0.get("pair_notes", []) or session0.get("pair_count", 0) >= 0 - # Phase-C fields appear when joint ran successfully on pairs. - if session0.get("ok"): + # Phase-C fields appear only when the strict rotation gate passed. + if result.status.value != "blocked": + assert result.T_IMU_lidar is not None assert "gyro_bias_rad_s" in session0["joint"] + else: + assert result.T_IMU_lidar is None + assert not result.details["joint_handeye"]["ok"] def test_signed_time_offset_refine_improves_or_keeps(tmp_path: Path):