完善Phase-A会话级联合优化并修正雷达相位中心高度先验

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
+      # X/Y：后轮轴中心 → 雷达
-      translation_m: [2.522276859, 0.000020526, 1.9165]
+      # 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_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
-  t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.730]
+  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

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）。
+- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
-- **旋转先验未因 Z 修正改变**；仅平移先验 Z 从旧值 0.8085 改为 0.730。
+- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
 
 ---
 

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）。
+- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
-- **旋转先验未因 Z 修正改变**；仅平移先验 Z 从旧值 0.8085 改为 0.730。
+- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
 
 ---
 

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)
+        SessionInput(
-        for sid, imu, lidar in zip(session_ids, imus, lidars)
+            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:

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

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
+        translation_accepted = False
-        notes.append("SE3 motion solve gated off; using CAD translation prior with refined rotation")
+        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),
+    )

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)

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

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"unsupported motion_pairs schema_version={version}; "
-            f"expected {SCHEMA_VERSION}"
+            f"expected 1 or {SCHEMA_VERSION}"
         )
     return payload
 

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.
+    singular = np.linalg.svd(j_r, compute_uv=False)
-    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)
     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.
+    # Translation lever arm is observable through stacked (R_A - I). Pure
-    axes = []
+    # planar yaw leaves its vertical column in the nullspace and must fail.
-    translations = []
+    translation_rows = [
-    for pair in usable:
+        np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
-        axis = so3_log(pair.R_B)
+        for pair in usable
-        n = np.linalg.norm(axis)
+        if pair.t_B_m is not None
-        if n > 1e-8:
+    ]
-            axes.append(axis / n)
+    if translation_rows:
-        if pair.t_B_m is not None:
+        j_t = np.vstack(translation_rows)
-            translations.append(pair.t_B_m)
+        singular_t = np.linalg.svd(j_t, compute_uv=False)
-    axis_rank = 0
+        cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
-    if axes:
+        translation_information = float(
-        axis_mat = np.asarray(axes, dtype=float)
+            singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
-        axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
+        )
-    trans_span = 0.0
+    else:
-    if translations:
+        cond_t = 1e9
-        tmat = np.asarray(translations, dtype=float)
+        translation_information = 0.0
-        trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
+    translation_ok = (
-    # For planar yaw-mostly motion, translation z is typically weak.
+        len(translation_rows) >= 5
-    translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
+        and cond_t < condition_threshold
-    cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))
+        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); "
+            f"translation not observable: condition={cond_t:.1f}, "
-            "V1 will reject full SE3 without strong priors"
+            f"min_information={translation_information:.3e}; "
+            "full SE3 will be rejected"
         )
     return ObservabilityReport(
         rotation_observable=rotation_ok,

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

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,11 +103,21 @@ 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,
@@ -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:
+    _emit_progress(
-        return finalize_result(
+        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)
+    gyro_bias_by_session = {
-    gravity_init = np.mean(np.stack([p["gravity_init_m_s2"] for p in prepared], axis=0), axis=0)
+        p["session_id"]: np.asarray(p["gyro_bias_rad_s"], dtype=float) for p in prepared
-    g_n = float(np.linalg.norm(gravity_init))
+    }
-    if g_n > 1e-6:
+    time_offset_by_session = {
-        gravity_init = gravity_init * (9.80665 / g_n)
+        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_by_session=gyro_bias_by_session,
-        gyro_bias_rad_s=gyro_bias,
+        time_offset_s_by_session=time_offset_by_session,
-        gravity_init_m_s2=gravity_init,
+        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
+                    if session_bias is None
-                    else np.asarray(joint.gyro_bias_rad_s, dtype=float).tolist(),
+                    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.
+    if request.requested_mode == CalibrationMode.ROTATION_ONLY:
-    delta_t = float(prepared[0]["time_offset_s"])
+        # A rotation-only result must never expose a seed/prior translation,
-    if request.requested_mode == CalibrationMode.FULL_SE3:
+        # 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),
     )
 

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}, "
+        "weighted hand-eye: normalized/clipped IMU confidence "
-        f"min={float(np.min(weights)):.3g}, max={float(np.max(weights)):.3g}"
+        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),

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) 交付。
 
 ---
 

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_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`                      | 同上           | `full_se3` 不崩溃      | 状态为 accepted / rejected / rotation_only 之一 |
+| `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）。  

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

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])

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

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)
+    progress_events: list[dict] = []
-    assert result.status.value == "rotation_only_accepted"
+    result = run_calibration(request, progress_callback=progress_events.append)
-    assert result.time_offset_s is not None
+    # The lightweight synthetic point cloud uses approximate ICP and has a
-    assert abs(result.time_offset_s - meta["delta_t_s"]) < 0.05
+    # roughly 3-degree P95 residual. The production gate must reject it rather
-    assert result.T_IMU_lidar is not None
+    # than expose a plausible-looking extrinsic.
-    # End-to-end uses approximate ICP; allow moderate absolute error but require consistency.
+    assert result.status.value == "blocked"
-    r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
+    assert result.T_IMU_lidar is None
-    err_deg = np.degrees(np.linalg.norm(_log(r_true.T @ result.T_IMU_lidar[:3, :3])))
-    assert err_deg < 15.0
     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.
+    # Phase-C fields appear only when the strict rotation gate passed.
-    if session0.get("ok"):
+    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):