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

imu_lidar/joint_optimizer.py

@@ -2,7 +2,9 @@
 
 from __future__ import annotations
 
-from dataclasses import dataclass
+from collections.abc import Callable, Mapping
+from dataclasses import dataclass, field
+from typing import Any
 
 import numpy as np
 from scipy.optimize import least_squares
@@ -17,10 +19,26 @@ from .imu_preintegration import (
     residual_whiten_matrix,
 )
 from .observability import ObservabilityReport, analyze_observability
+from .phase_a import phase_a_comparison_to_dict, solve_phase_a_comparison
+from .rotation_handeye import select_strong_rotation_pairs
 
 G_NORM = 9.80665
 
 
+@dataclass(frozen=True)
+class PhaseASessionResult:
+    session_id: str
+    pair_count: int
+    gyro_bias0_rad_s: np.ndarray
+    gyro_bias_rad_s: np.ndarray
+    residual_rms_deg: float
+    residual_median_deg: float
+    residual_p95_deg: float
+    outlier_fraction_gt_5deg: float
+    accepted: bool
+    included_in_final: bool
+
+
 @dataclass(frozen=True)
 class JointExtrinsicResult:
     T_IMU_lidar: np.ndarray
@@ -31,6 +49,10 @@ class JointExtrinsicResult:
     gyro_bias_rad_s: np.ndarray | None = None
     accel_bias_m_s2: np.ndarray | None = None
     gravity_m_s2: np.ndarray | None = None
+    gyro_bias_rad_s_per_session: dict[str, np.ndarray] = field(default_factory=dict)
+    phase_a_sessions: tuple[PhaseASessionResult, ...] = ()
+    phase_a_accepted: bool = False
+    phase_a_comparison: dict[str, Any] = field(default_factory=dict)
     notes: tuple[str, ...] = ()
 
 
@@ -174,7 +196,8 @@ def _solve_phase_c_se3(
     pairs: list[MotionPair],
     r_x: np.ndarray,
     *,
-    gyro_bias0: np.ndarray,
+    gyro_bias_linearization: np.ndarray,
+    gyro_bias_init: np.ndarray,
     gravity_init: np.ndarray,
     sigma_bg_rw: float = 1.0e-5,
     sigma_ba_rw: float = 1.0e-3,
@@ -189,7 +212,7 @@ def _solve_phase_c_se3(
     if len(usable) < 3:
         notes.append("phase-C skipped: need pairs with full preintegration metadata")
         t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
-        return r_x, t0, gravity_init, gyro_bias0, np.zeros(3), 1e9, 1e9, notes
+        return r_x, t0, gravity_init, gyro_bias_init, np.zeros(3), 1e9, 1e9, notes
 
     # Keyframes: group by session, sort each session by IMU time (no cross-session chain).
     stamp: dict[int, float] = {}
@@ -225,7 +248,8 @@ def _solve_phase_c_se3(
     g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9))
     basis = _gravity_basis(g0)
     ba0 = np.zeros(3)
-    bg0 = np.asarray(gyro_bias0, dtype=float).reshape(3)
+    bg0 = np.asarray(gyro_bias_linearization, dtype=float).reshape(3)
+    bg_init = np.asarray(gyro_bias_init, dtype=float).reshape(3)
 
     # State: dθ(3), t(3), g_xy(2), v(3K), bg(3K), ba(3K)
     n_v = 3 * k_count
@@ -243,7 +267,7 @@ def _solve_phase_c_se3(
             t_sigma = np.full(3, float(t_sigma[0]), dtype=float)
     # velocities start at 0; biases at prior
     for idx in range(k_count):
-        x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg0
+        x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg_init
 
     whitened = []
     for pair in usable:
@@ -312,7 +336,7 @@ def _solve_phase_c_se3(
         for sid in session_ids:
             first = next(kid for kid in keyframe_ids if kf_session[kid] == sid)
             idx0 = id_to_idx[first]
-            out.append(50.0 * (bgs[idx0] - bg0))
+            out.append(50.0 * (bgs[idx0] - bg_init))
             out.append(20.0 * bas[idx0])
         if t_prior_vec is not None:
             out.append((t_opt - t_prior_vec) / np.maximum(t_sigma, 1e-3))
@@ -357,7 +381,197 @@ def _solve_phase_c_se3(
     return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes
 
 
-def solve_joint_extrinsic(
+def _pair_gyro_bias0(pair: MotionPair, fallback: np.ndarray) -> np.ndarray:
+    raw = pair.metadata.get("gyro_bias0_rad_s")
+    if raw is None:
+        return np.asarray(fallback, dtype=float).reshape(3)
+    return np.asarray(raw, dtype=float).reshape(3)
+
+
+def _phase_a_bias_bases(
+    pairs: list[MotionPair],
+    *,
+    gyro_bias_rad_s: np.ndarray | None,
+    gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None,
+) -> dict[str, np.ndarray]:
+    session_ids = sorted({pair.session_id for pair in pairs})
+    scalar = None
+    if gyro_bias_rad_s is not None:
+        scalar = np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
+    supplied = {} if gyro_bias_rad_s_by_session is None else gyro_bias_rad_s_by_session
+    bases: dict[str, np.ndarray] = {}
+    for sid in session_ids:
+        if sid in supplied:
+            bases[sid] = np.asarray(supplied[sid], dtype=float).reshape(3)
+            continue
+        pair = next(
+            (
+                item
+                for item in pairs
+                if item.session_id == sid and "gyro_bias0_rad_s" in item.metadata
+            ),
+            None,
+        )
+        if pair is not None:
+            bases[sid] = np.asarray(pair.metadata["gyro_bias0_rad_s"], dtype=float).reshape(3)
+        elif scalar is not None:
+            bases[sid] = scalar.copy()
+        else:
+            bases[sid] = np.zeros(3)
+    return bases
+
+
+def _rotation_distribution(errs_deg: list[float]) -> tuple[float, float, float, float, bool]:
+    if not errs_deg:
+        return 1e9, 1e9, 1e9, 1.0, False
+    errs = np.asarray(errs_deg, dtype=float)
+    rms = float(np.sqrt(np.mean(errs**2)))
+    median = float(np.median(errs))
+    p95 = float(np.percentile(errs, 95.0))
+    outlier_fraction = float(np.mean(errs > 5.0))
+    accepted = (
+        len(errs) >= 3
+        and rms < 1.5
+        and median < 0.5
+        and p95 < 1.5
+        and outlier_fraction <= 0.005
+    )
+    return rms, median, p95, outlier_fraction, accepted
+
+
+def _solve_phase_a_rotation(
+    pairs: list[MotionPair],
+    r_seed: np.ndarray,
+    *,
+    bias_bases: Mapping[str, np.ndarray],
+    imu: ImuSeries | None,
+    bias_prior_sigma_rad_s: float,
+    preexcluded_session_ids: set[str] | None = None,
+) -> tuple[
+    np.ndarray,
+    dict[str, np.ndarray],
+    tuple[PhaseASessionResult, ...],
+    list[MotionPair],
+    float,
+    bool,
+    list[str],
+]:
+    notes: list[str] = []
+    all_session_ids = sorted({pair.session_id for pair in pairs})
+    prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
+
+    def optimize(
+        active_pairs: list[MotionPair],
+        r0: np.ndarray,
+        bias_seed: Mapping[str, np.ndarray],
+    ) -> tuple[np.ndarray, dict[str, np.ndarray]]:
+        session_ids = sorted({pair.session_id for pair in active_pairs})
+        session_index = {sid: index for index, sid in enumerate(session_ids)}
+        whiten = [residual_whiten_matrix(_pair_cov(pair)) for pair in active_pairs]
+        x0 = np.zeros(3 + 3 * len(session_ids))
+        for sid, index in session_index.items():
+            x0[3 + 3 * index : 6 + 3 * index] = np.asarray(bias_seed[sid], dtype=float)
+
+        def residual(vec: np.ndarray) -> np.ndarray:
+            r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r0)
+            out: list[np.ndarray] = []
+            for pair, sqrt_info in zip(active_pairs, whiten):
+                index = session_index[pair.session_id]
+                bias = vec[3 + 3 * index : 6 + 3 * index]
+                base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
+                delta_r = _corrected_delta_r(
+                    pair, bias - base, imu=imu, bias0=base
+                )
+                out.append(
+                    sqrt_info
+                    @ preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
+                )
+            for sid, index in session_index.items():
+                bias = vec[3 + 3 * index : 6 + 3 * index]
+                out.append(prior_w * (bias - bias_bases[sid]))
+            return np.concatenate(out)
+
+        opt = least_squares(residual, x0, loss="huber", f_scale=1.0, max_nfev=200)
+        r_opt = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r0)
+        biases = {
+            sid: opt.x[3 + 3 * index : 6 + 3 * index].copy()
+            for sid, index in session_index.items()
+        }
+        return r_opt, biases
+
+    def summarize(
+        r_opt: np.ndarray,
+        biases: Mapping[str, np.ndarray],
+        included: set[str],
+    ) -> tuple[PhaseASessionResult, ...]:
+        results: list[PhaseASessionResult] = []
+        for sid in all_session_ids:
+            local_pairs = [pair for pair in pairs if pair.session_id == sid]
+            bias = np.asarray(biases.get(sid, bias_bases[sid]), dtype=float).reshape(3)
+            errs: list[float] = []
+            for pair in local_pairs:
+                base = _pair_gyro_bias0(pair, bias_bases[sid])
+                delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
+                err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
+                errs.append(float(np.degrees(np.linalg.norm(err))))
+            rms, median, p95, outlier, accepted = _rotation_distribution(errs)
+            results.append(
+                PhaseASessionResult(
+                    session_id=sid,
+                    pair_count=len(local_pairs),
+                    gyro_bias0_rad_s=np.asarray(bias_bases[sid], dtype=float),
+                    gyro_bias_rad_s=bias,
+                    residual_rms_deg=rms,
+                    residual_median_deg=median,
+                    residual_p95_deg=p95,
+                    outlier_fraction_gt_5deg=outlier,
+                    accepted=accepted,
+                    included_in_final=sid in included,
+                )
+            )
+        return tuple(results)
+
+    if not pairs:
+        return r_seed, dict(bias_bases), (), [], 1e9, False, ["no pairs for phase-A"]
+
+    r_first, biases_first = optimize(pairs, r_seed, bias_bases)
+    first = summarize(r_first, biases_first, set(all_session_ids))
+    accepted_ids = {item.session_id for item in first if item.accepted}
+    preexcluded = set() if preexcluded_session_ids is None else set(preexcluded_session_ids)
+    accepted_ids -= preexcluded
+    active_ids = set(all_session_ids)
+    r_final = r_first
+    biases_final = dict(biases_first)
+    if preexcluded and not accepted_ids:
+        active_ids = set()
+        notes.append(f"phase-A pre-gate excluded all sessions: {sorted(preexcluded)}")
+    elif accepted_ids and accepted_ids != active_ids:
+        active_ids = accepted_ids
+        active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
+        r_final, active_biases = optimize(active_pairs, r_first, biases_first)
+        biases_final.update(active_biases)
+        excluded = sorted(set(all_session_ids) - active_ids)
+        notes.append(f"phase-A excluded sessions after local/pre residual gate: {excluded}")
+    active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
+    final = summarize(r_final, biases_final, active_ids)
+    active_results = [item for item in final if item.included_in_final]
+    global_errs: list[float] = []
+    for pair in active_pairs:
+        bias = biases_final[pair.session_id]
+        base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
+        delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
+        err = preintegration_rotation_residual(delta_r, r_final, pair.R_B)
+        global_errs.append(float(np.degrees(np.linalg.norm(err))))
+    rot_rms, _, _, _, global_ok = _rotation_distribution(global_errs)
+    accepted = bool(active_results and global_ok and all(item.accepted for item in active_results))
+    notes.append(
+        f"phase-A session-local bias refine: sessions={len(active_ids)}/{len(all_session_ids)}, "
+        f"pairs={len(active_pairs)}, rms={rot_rms:.3f} deg"
+    )
+    return r_final, biases_final, final, active_pairs, rot_rms, accepted, notes
+
+
+def _solve_joint_extrinsic_legacy(
     pairs: list[MotionPair] | tuple[MotionPair, ...],
     r_x: np.ndarray,
     *,
@@ -366,6 +580,8 @@ def solve_joint_extrinsic(
     delta_t_s: float = 0.0,
     gyro_bias_rad_s: np.ndarray | None = None,
     gravity_init_m_s2: np.ndarray | None = None,
+    gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
+    time_offset_s_by_session: Mapping[str, float] | None = None,
     bias_prior_sigma_rad_s: float = 0.02,
     enable_phase_c: bool | None = None,
     t_init_m: np.ndarray | None = None,
@@ -391,10 +607,10 @@ def solve_joint_extrinsic(
 
     def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray:
         residuals = []
-        for pair, weight, whiten in zip(usable, weights, whitens):
+        for pair, whiten in zip(usable, whitens):
             delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
             err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
-            residuals.append(np.sqrt(weight) * (whiten @ err))
+            residuals.append(whiten @ err)
         residuals.append(prior_w * delta_bias)
         return np.concatenate(residuals) if residuals else np.zeros(0)
 
@@ -407,16 +623,16 @@ def solve_joint_extrinsic(
             residual_rot_bias,
             np.zeros(6),
             loss="huber",
-            f_scale=np.deg2rad(1.0),
+            f_scale=1.0,
             max_nfev=200,
         )
         r = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r)
         delta_bias = opt.x[3:]
         bias_out = bias0 + delta_bias
         notes.append(
-            "phase-A joint refine (Σ-whitened + J_bg): "
+            "phase-A joint refine (single Σ whitening + J_bg): "
             f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
-            f"weighted pairs={len(usable)}"
+            f"pairs={len(usable)}"
         )
     else:
         bias_out = bias0
@@ -457,7 +673,8 @@ def solve_joint_extrinsic(
         r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3(
             usable,
             r,
-            gyro_bias0=bias_out,
+            gyro_bias_linearization=bias0,
+            gyro_bias_init=bias_out,
             gravity_init=gravity_init,
             t_init=t_seed if t_seed is not None else t_prior_m,
             t_prior=t_prior_m,
@@ -469,9 +686,9 @@ def solve_joint_extrinsic(
             # Prefer CAD prior over silent zero when motion SE3 is rejected.
             if t_prior_m is not None:
                 t = np.asarray(t_prior_m, dtype=float).reshape(3)
-                translation_accepted = True
                 notes.append(
-                    "phase-C translation residual/gate failed; keeping CAD translation prior"
+                    "phase-C translation residual/gate failed; CAD translation is reported "
+                    "as a prior only and is not accepted as calibration"
                 )
             else:
                 notes.append("phase-C translation residual/gate failed; keeping translation at zero")
@@ -520,15 +737,18 @@ def solve_joint_extrinsic(
             trans_errs.append(np.linalg.norm(pred - meas))
         rot_rms = float(np.sqrt(np.mean(np.square(rot_errs))))
         trans_rms = float(np.sqrt(np.mean(np.square(trans_errs))))
-        translation_accepted = trans_rms < 0.5 or t_prior_m is not None
+        translation_accepted = trans_rms < 0.5
         notes.append(f"legacy translation refine rms={trans_rms:.3f} m")
         if not translation_accepted:
             notes.append("translation residual too large; keeping translation at zero")
             t = np.zeros(3)
     elif not force_rotation_only and t_prior_m is not None:
         t = np.asarray(t_prior_m, dtype=float).reshape(3)
-        translation_accepted = True
-        notes.append("SE3 motion solve gated off; using CAD translation prior with refined rotation")
+        translation_accepted = False
+        notes.append(
+            "SE3 motion solve gated off; CAD translation is reported as a prior only "
+            "and is not accepted as calibration"
+        )
     else:
         notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")
 
@@ -536,10 +756,222 @@ def solve_joint_extrinsic(
         T_IMU_lidar=make_transform(t, r),
         translation_accepted=bool(translation_accepted and np.linalg.norm(t) > 0),
         residual_rms_rot_deg=rot_rms,
-        residual_rms_trans_m=0.0 if not translation_accepted else trans_rms,
+        residual_rms_trans_m=trans_rms,
         observability=observability,
         gyro_bias_rad_s=np.asarray(bias_out, dtype=float),
         accel_bias_m_s2=None if accel_bias_out is None else np.asarray(accel_bias_out, dtype=float),
         gravity_m_s2=None if gravity_out is None else np.asarray(gravity_out, dtype=float),
         notes=tuple(notes),
     )
+
+
+def solve_joint_extrinsic(
+    pairs: list[MotionPair] | tuple[MotionPair, ...],
+    r_x: np.ndarray,
+    *,
+    force_rotation_only: bool = False,
+    imu: ImuSeries | None = None,
+    delta_t_s: float = 0.0,
+    gyro_bias_rad_s: np.ndarray | None = None,
+    gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
+    time_offset_s_by_session: Mapping[str, float] | None = None,
+    preexcluded_session_ids: set[str] | None = None,
+    gravity_init_m_s2: np.ndarray | None = None,
+    bias_prior_sigma_rad_s: float = 0.002,
+    rotation_prior: np.ndarray | None = None,
+    rotation_prior_sigma_deg: float = 15.0,
+    phase_a_yaw_std_max_deg: float = 0.5,
+    phase_a_loo_yaw_range_max_deg: float = 1.0,
+    phase_a_data_prior_difference_max_deg: float = 1.0,
+    run_phase_a_leave_one_out: bool = True,
+    phase_a_progress_callback: (
+        Callable[[str, dict[str, Any]], None] | None
+    ) = None,
+    enable_phase_c: bool | None = None,
+    t_init_m: np.ndarray | None = None,
+    t_prior_m: np.ndarray | None = None,
+    t_prior_sigma_m: np.ndarray | float | None = None,
+) -> JointExtrinsicResult:
+    """Run the corrected session-aware Phase-A and gate unfinished SE(3) stages."""
+
+    del gravity_init_m_s2, t_init_m, t_prior_sigma_m, imu, r_x
+    usable_input = [pair for pair in pairs if pair.t_B_m is not None]
+    bias_bases = _phase_a_bias_bases(
+        usable_input,
+        gyro_bias_rad_s=gyro_bias_rad_s,
+        gyro_bias_rad_s_by_session=gyro_bias_rad_s_by_session,
+    )
+    comparison = solve_phase_a_comparison(
+        usable_input,
+        gyro_bias_rad_s_by_session=bias_bases,
+        rotation_prior=rotation_prior,
+        rotation_prior_sigma_deg=rotation_prior_sigma_deg,
+        preexcluded_session_ids=preexcluded_session_ids,
+        bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
+        yaw_std_max_deg=phase_a_yaw_std_max_deg,
+        leave_one_out_yaw_range_max_deg=(
+            phase_a_loo_yaw_range_max_deg
+        ),
+        data_prior_difference_max_deg=(
+            phase_a_data_prior_difference_max_deg
+        ),
+        run_leave_one_out=run_phase_a_leave_one_out,
+        progress_callback=phase_a_progress_callback,
+    )
+    primary = comparison.session_bg_data_only
+    r = primary.R_IMU_lidar
+    biases = primary.gyro_bias_rad_s_per_session
+    rot_rms = primary.residual_rms_deg
+    phase_a_accepted = comparison.accepted
+    notes = list(comparison.notes)
+    notes.append(
+        "phase-A primary=A1_session_bg_data_only; "
+        f"A0 RPY={comparison.fixed_bg_data_only.rpy_deg_xyz.tolist()}, "
+        f"A1 RPY={primary.rpy_deg_xyz.tolist()}, "
+        "A2 RPY="
+        f"{comparison.session_bg_with_rotation_prior.rpy_deg_xyz.tolist()}"
+    )
+    notes.append(
+        f"phase-A marginal yaw_std={comparison.marginal_observability.yaw_std_deg:.3f} deg, "
+        f"LOO yaw range={comparison.leave_one_out_yaw_range_deg:.3f} deg"
+    )
+
+    session_results_list: list[PhaseASessionResult] = [
+        PhaseASessionResult(
+            session_id=item.session_id,
+            pair_count=item.pair_count,
+            gyro_bias0_rad_s=item.gyro_bias0_rad_s,
+            gyro_bias_rad_s=item.gyro_bias_rad_s,
+            residual_rms_deg=item.residual_rms_deg,
+            residual_median_deg=item.residual_median_deg,
+            residual_p95_deg=item.residual_p95_deg,
+            outlier_fraction_gt_5deg=item.outlier_fraction_gt_5deg,
+            accepted=item.accepted,
+            included_in_final=True,
+        )
+        for item in primary.sessions
+    ]
+    preexcluded = (
+        set()
+        if preexcluded_session_ids is None
+        else set(preexcluded_session_ids)
+    )
+    strong_all = select_strong_rotation_pairs(usable_input)
+    for session_id in sorted(preexcluded):
+        local_pairs = [
+            pair for pair in strong_all if pair.session_id == session_id
+        ]
+        errors = [
+            float(
+                np.degrees(
+                    np.linalg.norm(
+                        preintegration_rotation_residual(
+                            pair.R_A, r, pair.R_B
+                        )
+                    )
+                )
+            )
+            for pair in local_pairs
+        ]
+        rms, median, p95, outlier, accepted = _rotation_distribution(
+            errors
+        )
+        base = np.asarray(
+            bias_bases.get(session_id, np.zeros(3)), dtype=float
+        ).reshape(3)
+        session_results_list.append(
+            PhaseASessionResult(
+                session_id=session_id,
+                pair_count=len(local_pairs),
+                gyro_bias0_rad_s=base,
+                gyro_bias_rad_s=base,
+                residual_rms_deg=rms,
+                residual_median_deg=median,
+                residual_p95_deg=p95,
+                outlier_fraction_gt_5deg=outlier,
+                accepted=accepted,
+                included_in_final=False,
+            )
+        )
+    session_results = tuple(
+        sorted(session_results_list, key=lambda item: item.session_id)
+    )
+    usable = [
+        pair
+        for pair in strong_all
+        if pair.session_id not in preexcluded
+    ]
+    base_observability = analyze_observability(usable, r)
+    marginal = comparison.marginal_observability
+    observability = ObservabilityReport(
+        rotation_observable=bool(
+            marginal.rank == 3
+            and marginal.yaw_std_deg <= phase_a_yaw_std_max_deg
+        ),
+        translation_observable=base_observability.translation_observable,
+        condition_rotation=marginal.condition,
+        condition_translation=base_observability.condition_translation,
+        notes=tuple(
+            list(marginal.notes)
+            + list(base_observability.notes)
+        ),
+    )
+    notes.extend(observability.notes)
+    if time_offset_s_by_session is None:
+        notes.append(
+            f"legacy scalar time offset fixed during pair construction: {float(delta_t_s):.6f}s"
+        )
+    else:
+        fixed_offsets = {
+            str(sid): float(value) for sid, value in time_offset_s_by_session.items()
+        }
+        notes.append(
+            f"time offsets fixed during pair construction (not optimized): {fixed_offsets}"
+        )
+
+    for item in session_results:
+        notes.append(
+            f"phase-A session {item.session_id}: included={item.included_in_final}, "
+            f"pairs={item.pair_count}, rms={item.residual_rms_deg:.3f} deg, "
+            f"p95={item.residual_p95_deg:.3f} deg, "
+            f"|bias-bias0|={float(np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)):.3e}"
+        )
+
+    phase_c_requested = (not force_rotation_only) if enable_phase_c is None else bool(enable_phase_c)
+    t = np.zeros(3)
+    if not force_rotation_only:
+        if phase_c_requested:
+            notes.append(
+                "phase-B/C gated off: session-aware translation/gravity/navigation "
+                "states are not implemented yet"
+            )
+        else:
+            notes.append("phase-C disabled; translation is not accepted")
+        if t_prior_m is not None:
+            t = np.asarray(t_prior_m, dtype=float).reshape(3)
+            notes.append(
+                "CAD translation is reported as a prior only and is not accepted as calibration"
+            )
+    else:
+        notes.append("rotation-only extrinsic returned after corrected phase-A")
+
+    single_bias = None
+    if len(biases) == 1:
+        single_bias = np.asarray(next(iter(biases.values())), dtype=float)
+    return JointExtrinsicResult(
+        T_IMU_lidar=make_transform(t, r),
+        translation_accepted=False,
+        residual_rms_rot_deg=rot_rms,
+        residual_rms_trans_m=1e9,
+        observability=observability,
+        gyro_bias_rad_s=single_bias,
+        accel_bias_m_s2=None,
+        gravity_m_s2=None,
+        gyro_bias_rad_s_per_session={
+            sid: np.asarray(value, dtype=float) for sid, value in biases.items()
+        },
+        phase_a_sessions=session_results,
+        phase_a_accepted=phase_a_accepted,
+        phase_a_comparison=phase_a_comparison_to_dict(comparison),
+        notes=tuple(notes),
+    )