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

Co-authored-by: Cursor <cursoragent@cursor.com>

README.md

@@ -73,16 +73,25 @@ powershell -File tools\reproduce_synthetic.ps1
 证明：链路可跑通，能收回已知 yaw / δt。  
 不证明：实车安装精度、平移可交付。
 
-产物在 `examples/synthetic_session/out/`。叠点查看：
+产物在 `examples/synthetic_session/out/`（含 `summary.json` 与 `motion_pairs.json`）。叠点查看：
 
 ```powershell
+# 优先读取 summary 同目录的 motion_pairs.json，按需加载点云（无需重算配准）
 python tools\visualize_pair_3d.py `
   --lidar examples\synthetic_session\lidar `
-  --imu examples\synthetic_session\imu.csv `
   --summary examples\synthetic_session\out\summary.json `
   --pair-index 0
 ```
 
+旧标定目录若缺少缓存，可只补导出运动对（不重求解外参）：
+
+```powershell
+python tools\export_motion_pairs_for_viz.py `
+  --lidar path\to\lidar `
+  --imu path\to\imu.csv `
+  --summary path\to\out\summary.json
+```
+
 键 `1`–`4` 切换叠点模式；`N`/`P` 切换运动对。
 
 ---

config/vehicle_hi13_h32_20260808.yaml

@@ -4,19 +4,24 @@ vehicle:
   vehicle_id: "outdoor_usable_20260808"
   body_frame:
     name: "base_link"
-    # CAD / 后轮轴中心测量系（与安装图 dX/dY/dZ 一致）
+    # 车体约定：后轮轴中心在地面投影为原点附近参考；X 前 / Y 左 / Z 上
+    # translation_m 的 Z 使用「离地高度」；后轮轴中心离地 294 mm
     axes: "X forward, Y left, Z up"
     unit: m
-    reference_point: "rear_axle_center"
+    reference_point: "rear_axle_center_xy__z_above_ground"
+    rear_axle_height_above_ground_m: 0.294
 
 installation:
   installation_id: "20260808_priority_windows"
   installed_at: "2026-08-08"
   notes: >
-    HI13R4 + H32 DLogCapture. CAD mounts are origins vs rear axle center
-    (translation only). LiDAR phase-center Z = CAD dZ + 63.5 mm.
+    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 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 assumed body-aligned.
+    LiDAR Cartesian in NPZ assumed body-aligned (X forward).
 
 sensors:
   imu:
@@ -26,14 +31,13 @@ sensors:
       axes: "X right, Y forward, Z up (RFU)"
       driver_axis_remapped: false
     mount_in_body:
-      # CAD 图二：后轮轴中心 → IMU，单位 m（mm/1000）
-      # dX=2574.126255, dY=36.5, dZ=892.5
-      translation_m: [2.574126255, 0.0365, 0.8925]
+      # X/Y：后轮轴中心 → IMU；Z：离地 = CAD 0.8925 + 0.294
+      translation_m: [2.574126255, 0.0365, 1.1865]
       # body <- imu : p_body = R_body_imu * p_imu
       # R_body_imu = [[0,1,0],[-1,0,0],[0,0,1]]  (fwd=imu_y, left=-imu_x, up=imu_z)
       rotation_matrix_body_imu: [[0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
       rotation_quaternion_xyzw: null
-      source: "CAD dX/dY/dZ + HI13R4 manual RFU"
+      source: "CAD X/Y in body (+X forward); Z = CAD axle-height + 294mm AGL + HI13R4 RFU"
 
   lidar:
     model: "RSLidarH32"
@@ -42,13 +46,12 @@ sensors:
       axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
       driver_axis_remapped: false
     mount_in_body:
-      # CAD 图一：后轮轴中心 → 雷达安装点，再加相位中心 +63.5 mm（仅 Z）
-      # dX=2522.276859, dY=0.020526, dZ=1637.499879+63.5=1700.999879
-      translation_m: [2.522276859, 0.000020526, 1.700999879]
-      # 假设雷达系与车体 CAD 轴一致（导出 XYZ 已按此约定）
+      # X/Y：后轮轴中心 → 雷达
+      # Z离地 = CAD dZ 1.637499879 + 后轮轴离地 0.294 + 相位中心偏移 0.0635
+      translation_m: [2.522276859, 0.000020526, 1.994999879]
       rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
       rotation_quaternion_xyzw: null
-      source: "CAD dX/dY/dZ + phase-center +63.5mm on Z; attitude assumed = 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: ""
@@ -69,9 +72,8 @@ derived_T_IMU_lidar_prior:
   t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
   t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
   notes: >
-    Rotation prior is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd).
-    Translation prior from CAD + LiDAR phase-center offset; use for full_se3 /
-    sanity, not as hard lock for rotation_only.
+    Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd).
+    Relative Z = 1.994999879 - 1.1865 = 0.808499879 m.
 
 initialization:
   translation_prior:

docs/20260808_LiDAR-IMU标定现状与问题.md

@@ -0,0 +1,209 @@
+# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题
+
+> 数据：`D:\data\calibration_usable_20260808`  
+> 可用会话：`sessions_v1_host_aligned`（三优先窗）  
+> 当前结果目录：各窗 `out_fixed_dt0/`  
+> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`  
+> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml`  
+> 约定外参：`p_IMU = T_IMU_lidar · p_lidar`
+
+---
+
+## 1. 一句话结论
+
+**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。**  
+三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。
+
+---
+
+## 2. 当前可用结果（`out_fixed_dt0`）
+
+约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。
+
+| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
+|----|------|----|---------------------|--------------|----------|--------------|
+| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
+| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° |
+| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° |
+
+- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。
+- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。
+- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。
+
+### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
+
+- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json`
+- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
+- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
+
+```text
+R =
+[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
+ [ 0.9999982088237712,  2.3798651350e-05,  0.0018925598731371 ],
+ [-0.0018923488575947, -0.0069474968493909,  0.9999740753156198 ]]
+t = [0, 0, 0]
+```
+
+### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
+
+- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json`
+- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
+- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
+
+```text
+R =
+[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
+ [ 0.9999810938467022,  1.2097239027e-07, -0.0061491421465438 ],
+ [ 0.0061490495645171, -0.0055071432752239,  0.9999659297008071 ]]
+t = [0, 0, 0]
+```
+
+### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
+
+- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json`
+- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
+- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
+
+```text
+R =
+[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
+ [ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
+ [ 0.0006258500675082, -0.0065162397345547,  0.9999785732361544 ]]
+t = [0, 0, 0]
+```
+
+### 相对历史失败轮次
+
+| 轮次 | 问题 | 结果 |
+|------|------|------|
+| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° |
+| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） |
+| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 |
+
+---
+
+## 3. 已澄清并写入配置的坐标系 / 先验
+
+### 3.1 车体与传感器
+
+- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。
+- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。
+- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。
+- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。
+
+### 3.2 安装量（`translation_m`）
+
+| 传感器 | X / Y（后轮轴中心） | Z（离地） |
+|--------|---------------------|-----------|
+| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
+| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
+
+- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
+- 雷达 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.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
+- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
+
+---
+
+## 4. 现存问题清单
+
+### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移）
+
+- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。
+- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。
+- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。
+- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。
+- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。
+
+### P2. 平面运动导致竖直平移弱可观
+
+- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。
+- 流水线门控已给出 `translation_accepted=false`。
+- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。
+
+### P3. 时间偏移若再自由估计会被带偏（已规避，需保持）
+
+- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。
+- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。
+- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。
+
+### P4. 单窗低残差 ≠ 外参正确（历史教训）
+
+- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。
+- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
+- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。
+
+### P5. 旋转软先验尚未做无先验对照
+
+- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。
+- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。
+
+### P6. 文档与操作约定未完全同步（工程）
+
+- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
+- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。
+
+---
+
+## 5. 不该做 / 可以做
+
+| 动作 | 建议 |
+|------|------|
+| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） |
+| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
+| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 |
+| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** |
+| 无先验 / 大 σ 旋转对照 | **建议做** |
+| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
+| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
+
+---
+
+## 6. 建议下一步顺序
+
+1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。  
+2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。  
+3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。  
+4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。
+
+---
+
+## 7. 常用路径与命令
+
+```text
+数据根：
+  D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
+
+结果：
+  ...\priority_XXXX\out_fixed_dt0\summary.json
+  ...\priority_XXXX\out_fixed_dt0\motion_pairs.json
+  ...\calibration_manifest_fixed_dt0.json
+```
+
+```powershell
+# 可视化（rotation_only 默认模式4用 GICP 平移）
+python tools\visualize_pair_3d.py `
+  --lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
+  --summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
+  --pair-index 0
+
+# 若要看「坏 Δp」导致的错层效果：
+#   --mode4-translation imu
+```
+
+---
+
+## 8. 问题优先级（跟踪用）
+
+| ID | 严重度 | 状态 | 标题 |
+|----|--------|------|------|
+| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 |
+| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 |
+| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） |
+| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
+| P5 | 中 | 待做 | 无旋转先验对照 |
+| P6 | 低 | 待做 | README/交付物同步 |

docs/问题清单.md

@@ -0,0 +1,209 @@
+# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题
+
+> 数据：`D:\data\calibration_usable_20260808`  
+> 可用会话：`sessions_v1_host_aligned`（三优先窗）  
+> 当前结果目录：各窗 `out_fixed_dt0/`  
+> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`  
+> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml`  
+> 约定外参：`p_IMU = T_IMU_lidar · p_lidar`
+
+---
+
+## 1. 一句话结论
+
+**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。**  
+三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。
+
+---
+
+## 2. 当前可用结果（`out_fixed_dt0`）
+
+约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。
+
+| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
+|----|------|----|---------------------|--------------|----------|--------------|
+| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
+| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° |
+| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° |
+
+- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。
+- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。
+- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。
+
+### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
+
+- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json`
+- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
+- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
+
+```text
+R =
+[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
+ [ 0.9999982088237712,  2.3798651350e-05,  0.0018925598731371 ],
+ [-0.0018923488575947, -0.0069474968493909,  0.9999740753156198 ]]
+t = [0, 0, 0]
+```
+
+### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
+
+- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json`
+- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
+- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
+
+```text
+R =
+[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
+ [ 0.9999810938467022,  1.2097239027e-07, -0.0061491421465438 ],
+ [ 0.0061490495645171, -0.0055071432752239,  0.9999659297008071 ]]
+t = [0, 0, 0]
+```
+
+### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
+
+- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json`
+- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
+- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
+
+```text
+R =
+[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
+ [ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
+ [ 0.0006258500675082, -0.0065162397345547,  0.9999785732361544 ]]
+t = [0, 0, 0]
+```
+
+### 相对历史失败轮次
+
+| 轮次 | 问题 | 结果 |
+|------|------|------|
+| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° |
+| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） |
+| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 |
+
+---
+
+## 3. 已澄清并写入配置的坐标系 / 先验
+
+### 3.1 车体与传感器
+
+- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。
+- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。
+- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。
+- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。
+
+### 3.2 安装量（`translation_m`）
+
+| 传感器 | X / Y（后轮轴中心） | Z（离地） |
+|--------|---------------------|-----------|
+| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
+| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
+
+- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
+- 雷达 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.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
+- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
+
+---
+
+## 4. 现存问题清单
+
+### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移）
+
+- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。
+- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。
+- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。
+- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。
+- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。
+
+### P2. 平面运动导致竖直平移弱可观
+
+- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。
+- 流水线门控已给出 `translation_accepted=false`。
+- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。
+
+### P3. 时间偏移若再自由估计会被带偏（已规避，需保持）
+
+- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。
+- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。
+- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。
+
+### P4. 单窗低残差 ≠ 外参正确（历史教训）
+
+- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。
+- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
+- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。
+
+### P5. 旋转软先验尚未做无先验对照
+
+- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。
+- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。
+
+### P6. 文档与操作约定未完全同步（工程）
+
+- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
+- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。
+
+---
+
+## 5. 不该做 / 可以做
+
+| 动作 | 建议 |
+|------|------|
+| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） |
+| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
+| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 |
+| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** |
+| 无先验 / 大 σ 旋转对照 | **建议做** |
+| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
+| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
+
+---
+
+## 6. 建议下一步顺序
+
+1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。  
+2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。  
+3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。  
+4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。
+
+---
+
+## 7. 常用路径与命令
+
+```text
+数据根：
+  D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
+
+结果：
+  ...\priority_XXXX\out_fixed_dt0\summary.json
+  ...\priority_XXXX\out_fixed_dt0\motion_pairs.json
+  ...\calibration_manifest_fixed_dt0.json
+```
+
+```powershell
+# 可视化（rotation_only 默认模式4用 GICP 平移）
+python tools\visualize_pair_3d.py `
+  --lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
+  --summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
+  --pair-index 0
+
+# 若要看「坏 Δp」导致的错层效果：
+#   --mode4-translation imu
+```
+
+---
+
+## 8. 问题优先级（跟踪用）
+
+| ID | 严重度 | 状态 | 标题 |
+|----|--------|------|------|
+| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 |
+| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 |
+| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） |
+| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
+| P5 | 中 | 待做 | 无旋转先验对照 |
+| P6 | 低 | 待做 | README/交付物同步 |

imu_lidar/CHANGELOG.md

@@ -5,6 +5,31 @@
 
 ---
 
+## 2026-08-11 10:55 (UTC+8)
+
+### 运动对缓存：标定落盘，可视化直读
+
+- **原本**：`visualize_pair_3d` 每次启动都重新关键帧+配准+预积分，等同半次标定。
+- **改成**：
+  - 标定成功后写出 `motion_pairs.json`（`motion_pairs_io.py` / `finalize`）。
+  - 可视化优先读缓存并对点云懒加载；`--rebuild-pairs` 可回退旧路径。
+  - 旧结果可用 `tools/export_motion_pairs_for_viz.py` 只补导出运动对，无需重求解外参。
+
+---
+
+## 2026-08-11 08:55 (UTC+8)
+
+### 主机桥接后冻结 δt + 旋转先验软约束
+
+- **原本**：手眼后 signed δt 精修可在弱 MSE 下降下连走数步（最远约 0.5 s）；旋转手眼无 CAD 先验，平面运动下 yaw 易掉进低残差错解。
+- **改成**：
+  - CLI：`--fixed-time-offset-s`、`--no-signed-time-refine`、`--max-signed-refine-shift-s`。
+  - signed refine：默认 `|Δδt|≤0.05 s`，且要求 MSE 至少降约 2%。
+  - `rotation_handeye` 读取配置 `rotation_prior` 作初值/软约束。
+  - 主机 UTC 桥接会话建议：`--fixed-time-offset-s 0 --no-signed-time-refine`。
+
+---
+
 ## 2026-08-09 14:30 (UTC+8)
 
 ### 导出：HI13 IMU + recovered dlog zip + 墙钟切窗

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)
@@ -52,8 +122,95 @@ def build_parser() -> argparse.ArgumentParser:
     )
     run.add_argument("--max-iterations", type=int, default=2)
     run.add_argument("--time-offset-search-s", type=float, default=1.0)
+    run.add_argument(
+        "--fixed-time-offset-s",
+        type=float,
+        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",
+        help="Disable signed 3-axis δt refine after hand-eye (recommended for host-bridged data)",
+    )
+    run.add_argument(
+        "--max-signed-refine-shift-s",
+        type=float,
+        default=0.05,
+        help="Max |Δδt| accepted by signed refine from the coarse estimate",
+    )
     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
 
 
@@ -70,9 +227,23 @@ def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]:
             raise SystemExit(
                 f"--session-id count ({len(session_ids)}) must match --imu/--lidar ({len(imus)})"
             )
+    if args.session_time_offset_s is None:
+        session_offsets: list[float | None] = [None] * len(imus)
+    else:
+        session_offsets = list(args.session_time_offset_s)
+        if len(session_offsets) != len(imus):
+            raise SystemExit(
+                f"--session-time-offset-s count ({len(session_offsets)}) must match "
+                f"--imu/--lidar ({len(imus)})"
+            )
     return tuple(
-        SessionInput(session_id=sid, imu_source=imu, lidar_source=lidar)
-        for sid, imu, lidar in zip(session_ids, imus, lidars)
+        SessionInput(
+            session_id=sid,
+            imu_source=imu,
+            lidar_source=lidar,
+            fixed_time_offset_s=offset,
+        )
+        for sid, imu, lidar, offset in zip(session_ids, imus, lidars, session_offsets)
     )
 
 
@@ -91,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(
@@ -101,9 +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,7 +54,16 @@ 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
+    # Signed 3-axis refine after hand-eye; disable for already-bridged timelines.
+    enable_signed_time_refine: bool = True
+    # Reject signed refine steps that walk farther than this from the coarse δt.
+    max_signed_refine_shift_s: float = 0.05
 
 
 @dataclass

imu_lidar/finalize.py

@@ -34,6 +34,7 @@ def finalize_result(
     T_IMU_lidar: np.ndarray | None = None,
     time_offset_s: float | None = None,
     output_directory: Path | None = None,
+    motion_pairs_payload: dict[str, Any] | None = None,
 ) -> CalibrationResult:
     """Build the result envelope and optionally write report files."""
 
@@ -72,5 +73,10 @@ def finalize_result(
                 json.dumps({"delta_t_s": time_offset_s, "definition": "t_imu = t_lidar + delta_t"}, indent=2),
                 encoding="utf-8",
             )
+        if motion_pairs_payload is not None:
+            from .motion_pairs_io import save_motion_pairs
+
+            save_motion_pairs(output_directory / "motion_pairs.json", motion_pairs_payload)
+            summary["motion_pairs_file"] = "motion_pairs.json"
         (output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
     return result

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

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/lidar_io.py

@@ -19,10 +19,7 @@ import numpy as np
 from .contracts import LidarFrame
 
 
-def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
-    """Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
-
-    root = Path(path)
+def _read_frames_index(root: Path) -> tuple[np.ndarray, str]:
     index_path = root / "frames_index.csv"
     if not index_path.exists():
         raise FileNotFoundError(f"missing frames_index.csv under {root}")
@@ -38,6 +35,54 @@ def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
         raise ValueError(
             f"frames_index.csv must contain frame_id,{file_key}/filename,t_start,t_end; got {sorted(names)}"
         )
+    return rows, file_key
+
+
+def list_lidar_frame_entries(path: Path | str) -> list[tuple[str, float, float, Path]]:
+    """Return ``(frame_id, t_start, t_end, npz_path)`` sorted by mid time (same as ``load_lidar_frames``)."""
+
+    root = Path(path)
+    rows, file_key = _read_frames_index(root)
+    entries: list[tuple[str, float, float, Path]] = []
+    for row in rows:
+        t0 = float(row["t_start"])
+        t1 = float(row["t_end"])
+        entries.append((str(row["frame_id"]), t0, t1, root / str(row[file_key])))
+    entries.sort(key=lambda item: 0.5 * (item[1] + item[2]))
+    return entries
+
+
+def load_lidar_frame_at(root: Path | str, index: int) -> LidarFrame:
+    """Load one frame by index in mid-time-sorted order (matches motion-pair ``i``/``j``)."""
+
+    entries = list_lidar_frame_entries(root)
+    if index < 0 or index >= len(entries):
+        raise IndexError(f"frame index {index} outside [0, {len(entries) - 1}] for {root}")
+    frame_id, t0, t1, npz_path = entries[index]
+    with np.load(npz_path) as payload:
+        if "points" not in payload.files:
+            raise ValueError(f"{npz_path} must contain array 'points'")
+        points = np.asarray(payload["points"], dtype=float)
+    if points.ndim != 2 or points.shape[1] < 3:
+        raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
+    return LidarFrame(
+        frame_id=frame_id,
+        t_start_s=t0,
+        t_end_s=t1,
+        points_xyz=points[:, :3],
+        path=npz_path,
+    )
+
+
+def lidar_frame_count(path: Path | str) -> int:
+    return len(list_lidar_frame_entries(path))
+
+
+def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
+    """Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
+
+    root = Path(path)
+    rows, file_key = _read_frames_index(root)
 
     frames: list[LidarFrame] = []
     for row in rows:

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

@@ -0,0 +1,154 @@
+"""Serialize / deserialize motion pairs for fast visualization."""
+
+from __future__ import annotations
+
+import json
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from .contracts import MotionPair
+
+SCHEMA_VERSION = 2
+
+# 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",
+        "rotation_deg_A",
+        "rotation_deg_B",
+        "translation_m_B",
+        "weight",
+        "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",
+    }
+)
+
+
+def _to_list(value: Any) -> Any:
+    if isinstance(value, np.ndarray):
+        return value.tolist()
+    if isinstance(value, (np.floating, np.integer, np.bool_)):
+        return value.item()
+    return value
+
+
+def pair_to_dict(pair: MotionPair) -> dict[str, Any]:
+    meta = {
+        str(k): _to_list(v)
+        for k, v in (pair.metadata or {}).items()
+        if str(k) in _METADATA_KEEP
+    }
+    return {
+        "session_id": pair.session_id,
+        "i": int(pair.i),
+        "j": int(pair.j),
+        "t_i_s": float(pair.t_i_s),
+        "t_j_s": float(pair.t_j_s),
+        "R_A": np.asarray(pair.R_A, dtype=float).reshape(3, 3).tolist(),
+        "R_B": np.asarray(pair.R_B, dtype=float).reshape(3, 3).tolist(),
+        "t_A_m": None if pair.t_A_m is None else np.asarray(pair.t_A_m, dtype=float).reshape(3).tolist(),
+        "t_B_m": None if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float).reshape(3).tolist(),
+        "fitness": float(pair.fitness),
+        "metadata": meta,
+    }
+
+
+def pair_from_dict(payload: dict[str, Any]) -> MotionPair:
+    t_a = payload.get("t_A_m")
+    t_b = payload.get("t_B_m")
+    return MotionPair(
+        session_id=str(payload.get("session_id", "")),
+        i=int(payload["i"]),
+        j=int(payload["j"]),
+        t_i_s=float(payload["t_i_s"]),
+        t_j_s=float(payload["t_j_s"]),
+        R_A=np.asarray(payload["R_A"], dtype=float).reshape(3, 3),
+        R_B=np.asarray(payload["R_B"], dtype=float).reshape(3, 3),
+        t_A_m=None if t_a is None else np.asarray(t_a, dtype=float).reshape(3),
+        t_B_m=None if t_b is None else np.asarray(t_b, dtype=float).reshape(3),
+        fitness=float(payload.get("fitness", 0.0)),
+        metadata=dict(payload.get("metadata") or {}),
+    )
+
+
+def build_motion_pairs_payload(
+    *,
+    prepared_sessions: list[dict[str, Any]],
+) -> dict[str, Any]:
+    """Build a JSON-serializable cache from pipeline ``prepared`` session dicts."""
+
+    sessions_out: list[dict[str, Any]] = []
+    for prep in prepared_sessions:
+        pairs = prep.get("pairs") or ()
+        sessions_out.append(
+            {
+                "session_id": prep.get("session_id"),
+                "delta_t_s": float(prep.get("time_offset_s", 0.0)),
+                "gyro_bias_rad_s": np.asarray(prep.get("gyro_bias_rad_s", np.zeros(3)), dtype=float)
+                .reshape(3)
+                .tolist(),
+                "pair_count": len(pairs),
+                "pairs": [pair_to_dict(pair) for pair in pairs],
+            }
+        )
+    return {
+        "schema_version": SCHEMA_VERSION,
+        "sessions": sessions_out,
+        "note": "Cached motion pairs for visualization; A=IMU preintegration, B=LiDAR registration",
+    }
+
+
+def save_motion_pairs(path: Path | str, payload: dict[str, Any]) -> Path:
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    destination.write_text(json.dumps(payload, indent=2), encoding="utf-8")
+    return destination
+
+
+def load_motion_pairs(path: Path | str) -> dict[str, Any]:
+    payload = json.loads(Path(path).read_text(encoding="utf-8"))
+    version = int(payload.get("schema_version", 0))
+    if version not in {1, SCHEMA_VERSION}:
+        raise ValueError(
+            f"unsupported motion_pairs schema_version={version}; "
+            f"expected 1 or {SCHEMA_VERSION}"
+        )
+    return payload
+
+
+def pairs_for_session(payload: dict[str, Any], session_id: str | None = None) -> list[MotionPair]:
+    sessions = payload.get("sessions") or []
+    if not sessions:
+        return []
+    if session_id is None:
+        chosen = sessions[0]
+    else:
+        chosen = next((s for s in sessions if s.get("session_id") == session_id), None)
+        if chosen is None:
+            raise KeyError(f"session_id {session_id!r} not found in motion_pairs cache")
+    return [pair_from_dict(item) for item in chosen.get("pairs") or []]
+
+
+def resolve_motion_pairs_path(summary_path: Path | str) -> Path | None:
+    """Return ``motion_pairs.json`` next to a summary if it exists."""
+
+    summary = Path(summary_path)
+    candidate = summary.parent / "motion_pairs.json"
+    return candidate if candidate.is_file() else None

imu_lidar/observability.py

@@ -57,43 +57,53 @@ def analyze_observability(
     if j_r.size == 0:
         return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
 
-    # Normalize columns.
-    col_norm = np.linalg.norm(j_r, axis=0) + 1e-12
-    j_r_n = j_r / col_norm
-    singular = np.linalg.svd(j_r_n, compute_uv=False)
+    singular = np.linalg.svd(j_r, compute_uv=False)
     cond_r = float(singular[0] / max(singular[-1], 1e-12))
-    rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3
+    rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1)))
+    rotation_ok = (
+        cond_r < condition_threshold
+        and rotation_information > 1e-3
+        and singular[-1] > 1e-6
+    )
 
-    # Translation observability proxy: diversity of rotation axes and presence of translation in B.
-    axes = []
-    translations = []
-    for pair in usable:
-        axis = so3_log(pair.R_B)
-        n = np.linalg.norm(axis)
-        if n > 1e-8:
-            axes.append(axis / n)
-        if pair.t_B_m is not None:
-            translations.append(pair.t_B_m)
-    axis_rank = 0
-    if axes:
-        axis_mat = np.asarray(axes, dtype=float)
-        axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
-    trans_span = 0.0
-    if translations:
-        tmat = np.asarray(translations, dtype=float)
-        trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
-    # For planar yaw-mostly motion, translation z is typically weak.
-    translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
-    cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))
+    # Translation lever arm is observable through stacked (R_A - I). Pure
+    # planar yaw leaves its vertical column in the nullspace and must fail.
+    translation_rows = [
+        np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
+        for pair in usable
+        if pair.t_B_m is not None
+    ]
+    if translation_rows:
+        j_t = np.vstack(translation_rows)
+        singular_t = np.linalg.svd(j_t, compute_uv=False)
+        cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
+        translation_information = float(
+            singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
+        )
+    else:
+        cond_t = 1e9
+        translation_information = 0.0
+    translation_ok = (
+        len(translation_rows) >= 5
+        and cond_t < condition_threshold
+        and translation_information > 0.02
+    )
 
     if not rotation_ok:
-        notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
+        notes.append(
+            f"rotation not observable: condition={cond_r:.1f}, "
+            f"min_information={rotation_information:.3e}"
+        )
     else:
-        notes.append(f"rotation condition {cond_r:.1f}")
+        notes.append(
+            f"rotation observable: condition={cond_r:.1f}, "
+            f"min_information={rotation_information:.3e}"
+        )
     if not translation_ok:
         notes.append(
-            f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); "
-            "V1 will reject full SE3 without strong priors"
+            f"translation not observable: condition={cond_t:.1f}, "
+            f"min_information={translation_information:.3e}; "
+            "full SE3 will be rejected"
         )
     return ObservabilityReport(
         rotation_observable=rotation_ok,

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
@@ -24,6 +26,7 @@ from .keyframes import build_keyframes
 from .lidar_deskew import deskew_lidar_frames
 from .lidar_io import load_lidar_frames
 from .motion_pairs import build_motion_pairs
+from .motion_pairs_io import build_motion_pairs_payload
 from .rotation_handeye import solve_rotation_handeye
 from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
 from .timestamp_audit import audit_timestamps
@@ -57,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."""
 
@@ -76,12 +101,24 @@ def _build_pairs_and_handeye(
     delta_t_s: float,
     gyro_bias_rad_s: np.ndarray,
     request: CalibrationRequest,
+    R_prior: np.ndarray | None = None,
+    prior_sigma_deg: float | None = None,
+    progress_callback: ProgressCallback | None = None,
 ):
     keyframes = build_keyframes(
         working_frames,
         min_translation_m=request.min_pair_translation_m,
         min_rotation_deg=request.min_pair_rotation_deg,
+        min_registration_fitness=request.min_registration_fitness,
     )
+    if progress_callback is not None:
+        progress_callback(
+            {
+                "event": "keyframes_ready",
+                "keyframe_count": len(keyframes.indices),
+                "lidar_frame_count": len(working_frames),
+            }
+        )
     pair_set = build_motion_pairs(
         session_id=session_id,
         keyframes=list(keyframes.frames),
@@ -91,8 +128,17 @@ 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,
+        R_prior=R_prior,
+        prior_sigma_deg=prior_sigma_deg,
     )
-    handeye = solve_rotation_handeye(pair_set.pairs)
     return keyframes, pair_set, handeye
 
 
@@ -108,42 +154,160 @@ def _translation_prior_from_config(
     return np.asarray(tp["t_IMU_lidar_m"], dtype=float).reshape(3), tp.get("sigma_m", [0.05, 0.05, 0.05])
 
 
+def _rotation_prior_from_config(
+    vehicle_config: dict[str, Any] | None,
+) -> tuple[np.ndarray | None, float | None]:
+    if vehicle_config is None or not prior_enabled(vehicle_config, "rotation_prior"):
+        return None, None
+    init_cfg = vehicle_config.get("initialization") or {}
+    rp = init_cfg.get("rotation_prior") or {}
+    if rp.get("R_IMU_lidar") is None:
+        return None, None
+    return np.asarray(rp["R_IMU_lidar"], dtype=float).reshape(3, 3), float(rp.get("sigma_deg", 15.0))
+
+
 def _prepare_session_pairs(
     session: SessionInput,
     request: CalibrationRequest,
+    *,
+    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)}
 
-    offset = estimate_time_offset(
-        imu,
-        frames,
-        gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
-        search_s=request.time_offset_search_s,
+    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 not offset.ok:
-        return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}
+    if fixed_time_offset_s is not None:
+        offset_source = "fixed"
+        offset = TimeOffsetResult(
+            delta_t_s=float(fixed_time_offset_s),
+            correlation_peak=1.0,
+            search_s=0.0,
+            notes=(
+                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,
+            gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+            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)
+    r_x = np.eye(3) if R_prior is None else np.asarray(R_prior, dtype=float).reshape(3, 3)
     handeye = None
     pair_set = None
     keyframes = None
     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,
@@ -151,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,
@@ -158,10 +329,33 @@ def _prepare_session_pairs(
             delta_t_s=offset.delta_t_s,
             gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
             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",
@@ -176,7 +370,17 @@ def _prepare_session_pairs(
             }
         r_x = handeye.R_IMU_lidar
 
-        for _ in range(2):
+        if not request.enable_signed_time_refine:
+            continue
+
+        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,
@@ -184,11 +388,37 @@ def _prepare_session_pairs(
                 R_IMU_lidar=r_x,
                 gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
                 search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
+                max_shift_s=request.max_signed_refine_shift_s,
             )
+            # Also bound total walk away from the original coarse estimate.
+            if abs(refined.delta_t_s - coarse_delta_t) > request.max_signed_refine_shift_s:
+                refined = TimeOffsetResult(
+                    delta_t_s=float(offset.delta_t_s),
+                    correlation_peak=refined.correlation_peak,
+                    search_s=refined.search_s,
+                    notes=tuple(
+                        list(refined.notes)
+                        + [
+                            f"signed refine clamped: |δt-coarse| would exceed "
+                            f"{request.max_signed_refine_shift_s:.3f}s"
+                        ]
+                    ),
+                    ok=True,
+                )
             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,
@@ -196,10 +426,33 @@ def _prepare_session_pairs(
                 delta_t_s=offset.delta_t_s,
                 gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
                 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",
@@ -213,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))
@@ -222,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,
@@ -242,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,
@@ -265,49 +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:
-        prep = _prepare_session_pairs(session, request)
+    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)
-    handeye = solve_rotation_handeye(all_pairs)
-    if not handeye.ok:
-        return finalize_result(
+    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,
+    )
+    _emit_progress(
+        progress_callback,
+        7,
+        "joint_handeye_complete",
+        pair_count=handeye.pair_count,
+        rms_deg=float(handeye.residual_rms_deg),
+        p95_deg=float(handeye.residual_p95_deg),
+        outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
+        ok=handeye.ok,
+        elapsed_s=perf_counter() - handeye_started_at,
+    )
+    if handeye.pair_count < 3:
+        return finish(
             status=CalibrationStatus.BLOCKED,
             message="blocked at stage rotation_handeye (joint)",
             details={
@@ -315,33 +681,124 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
                 "joint_handeye": asdict(handeye),
                 "merged_pair_count": len(all_pairs),
             },
-            output_directory=request.output_directory,
         )
 
     force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
     t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
-    gyro_bias = np.mean(np.stack([p["gyro_bias_rad_s"] for p in prepared], axis=0), axis=0)
-    gravity_init = np.mean(np.stack([p["gravity_init_m_s2"] for p in prepared], axis=0), axis=0)
-    g_n = float(np.linalg.norm(gravity_init))
-    if g_n > 1e-6:
-        gravity_init = gravity_init * (9.80665 / g_n)
+    gyro_bias_by_session = {
+        p["session_id"]: np.asarray(p["gyro_bias_rad_s"], dtype=float) for p in prepared
+    }
+    time_offset_by_session = {
+        p["session_id"]: float(p["time_offset_s"]) for p in prepared
+    }
+
+    preexcluded_session_ids = {
+        p["session_id"] for p in prepared if not p["handeye_local"]["ok"]
+    }
+    if len(preexcluded_session_ids) == len(prepared):
+        _emit_progress(
+            progress_callback,
+            8,
+            "phase_a_complete",
+            accepted=False,
+            reason="all_sessions_failed_local_handeye_gate",
+            excluded_sessions=sorted(preexcluded_session_ids),
+        )
+        return finish(
+            status=CalibrationStatus.BLOCKED,
+            message=(
+                "Phase-A blocked: all sessions failed the local "
+                "rotation residual gate"
+            ),
+            details={
+                "sessions": [_public_session(p) for p in prepared],
+                "joint_handeye": asdict(handeye),
+                "merged_pair_count": len(all_pairs),
+                "excluded_sessions": sorted(
+                    preexcluded_session_ids
+                ),
+            },
+        )
+    _emit_progress(
+        progress_callback,
+        8,
+        "phase_a_start",
+        session_count=len(prepared),
+        merged_pair_count=len(all_pairs),
+        preexcluded_sessions=sorted(preexcluded_session_ids),
+    )
+    phase_a_started_at = perf_counter()
+
+    def on_phase_a_progress(
+        event: str,
+        fields: dict[str, Any],
+    ) -> None:
+        _emit_progress(
+            progress_callback,
+            8,
+            event,
+            **fields,
+        )
 
     joint = solve_joint_extrinsic(
         all_pairs,
         handeye.R_IMU_lidar,
         force_rotation_only=force_rotation_only,
         imu=None,
-        delta_t_s=0.0,
-        gyro_bias_rad_s=gyro_bias,
-        gravity_init_m_s2=gravity_init,
+        gyro_bias_rad_s_by_session=gyro_bias_by_session,
+        time_offset_s_by_session=time_offset_by_session,
+        preexcluded_session_ids=preexcluded_session_ids,
+        rotation_prior=r_prior,
+        rotation_prior_sigma_deg=(
+            15.0 if prior_sigma_deg is None else prior_sigma_deg
+        ),
+        phase_a_progress_callback=on_phase_a_progress,
         enable_phase_c=not force_rotation_only,
         t_init_m=t_prior,
         t_prior_m=t_prior,
         t_prior_sigma_m=t_prior_sigma,
     )
+    included_sessions = [
+        item.session_id for item in joint.phase_a_sessions if item.included_in_final
+    ]
+    excluded_sessions = [
+        item.session_id for item in joint.phase_a_sessions if not item.included_in_final
+    ]
+    _emit_progress(
+        progress_callback,
+        8,
+        "phase_a_complete",
+        accepted=joint.phase_a_accepted,
+        joint_rms_deg=float(joint.residual_rms_rot_deg),
+        rotation_observable=joint.observability.rotation_observable,
+        included_sessions=included_sessions,
+        excluded_sessions=excluded_sessions,
+        elapsed_s=perf_counter() - phase_a_started_at,
+    )
+    for item in joint.phase_a_sessions:
+        _emit_progress(
+            progress_callback,
+            8,
+            "phase_a_session",
+            session=item.session_id,
+            included=item.included_in_final,
+            accepted=item.accepted,
+            pair_count=item.pair_count,
+            rms_deg=float(item.residual_rms_deg),
+            p95_deg=float(item.residual_p95_deg),
+            bias_delta_norm_rad_s=float(
+                np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)
+            ),
+            gyro_bias_rad_s=np.asarray(item.gyro_bias_rad_s, dtype=float).round(8).tolist(),
+        )
 
+    phase_a_by_session = {
+        item.session_id: item for item in joint.phase_a_sessions
+    }
     session_results = []
     for prep in prepared:
+        phase_a = phase_a_by_session.get(prep["session_id"])
+        session_bias = joint.gyro_bias_rad_s_per_session.get(prep["session_id"])
         session_results.append(
             {
                 **_public_session(prep),
@@ -349,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"]),
@@ -361,9 +820,10 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
                     "observability": asdict(joint.observability),
                     "notes": joint.notes,
                     "T_IMU_lidar": joint.T_IMU_lidar.tolist(),
+                    "phase_a": None if phase_a is None else asdict(phase_a),
                     "gyro_bias_rad_s": None
-                    if joint.gyro_bias_rad_s is None
-                    else np.asarray(joint.gyro_bias_rad_s, dtype=float).tolist(),
+                    if session_bias is None
+                    else np.asarray(session_bias, dtype=float).tolist(),
                     "accel_bias_m_s2": None
                     if joint.accel_bias_m_s2 is None
                     else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(),
@@ -372,14 +832,40 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
                     else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
                 },
                 "translation_accepted": joint.translation_accepted,
-                "rotation_ok": handeye.ok and joint.observability.rotation_observable,
+                "rotation_ok": (
+                    phase_a is not None
+                    and phase_a.included_in_final
+                    and phase_a.accepted
+                    and joint.phase_a_accepted
+                    and joint.observability.rotation_observable
+                ),
+                "rotation_prior_constrained": (
+                    phase_a is not None
+                    and phase_a.included_in_final
+                    and phase_a.accepted
+                    and joint.phase_a_accepted
+                    and not joint.observability.rotation_observable
+                    and r_prior is not None
+                ),
             }
         )
 
     T = np.asarray(joint.T_IMU_lidar, dtype=float)
-    # Report per-session δt list; keep first as scalar for backward-compatible field.
-    delta_t = float(prepared[0]["time_offset_s"])
-    if request.requested_mode == CalibrationMode.FULL_SE3:
+    if request.requested_mode == CalibrationMode.ROTATION_ONLY:
+        # A rotation-only result must never expose a seed/prior translation,
+        # including when the rotation itself is rejected by a later gate.
+        T = T.copy()
+        T[:3, 3] = 0.0
+    # Multi-session offsets stay in details; the legacy scalar is single-session only.
+    delta_t = float(prepared[0]["time_offset_s"]) if len(prepared) == 1 else None
+    joint_rotation_ok = joint.phase_a_accepted
+    if not joint_rotation_ok:
+        status = CalibrationStatus.BLOCKED
+        message = (
+            f"joint rotation rejected: RMS={joint.residual_rms_rot_deg:.3f} deg "
+            "or a retained session failed the Phase-A residual gates"
+        )
+    elif request.requested_mode == CalibrationMode.FULL_SE3:
         if joint.translation_accepted:
             status = CalibrationStatus.FULL_SE3_ACCEPTED
             message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
@@ -387,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={
@@ -406,12 +908,27 @@ 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:
@@ -57,26 +63,101 @@ def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
     return float(np.degrees(np.linalg.norm(err)))
 
 
-def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> RotationHandeyeResult:
-    """Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement."""
+def _rms_deg(r_x: np.ndarray, pairs: list[MotionPair]) -> float:
+    if not pairs:
+        return 1e9
+    errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in pairs], dtype=float)
+    return float(np.sqrt(np.mean(errs**2)))
 
-    usable = [pair for pair in pairs if rotation_angle_deg(pair.R_A) > 1.0 and rotation_angle_deg(pair.R_B) > 1.0]
+
+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, ...],
+    *,
+    R_prior: np.ndarray | None = None,
+    prior_sigma_deg: float | None = None,
+) -> RotationHandeyeResult:
+    """Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement.
+
+    Optional CAD / installation ``R_prior`` soft-constrains the extrinsic yaw that
+    is weakly observable under near-planar motion.
+    """
+
+    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)
-    weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
+    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))
+        rms_tsai = _rms_deg(r0, usable)
+        rms_prior = _rms_deg(r_prior, usable)
+        if rms_prior <= rms_tsai * 1.25:
+            r0 = r_prior
+            notes.append(
+                f"init from rotation prior (rms={rms_prior:.3f} deg vs Tsai {rms_tsai:.3f} deg)"
+            )
+        else:
+            notes.append(
+                f"init from Tsai (rms={rms_tsai:.3f} deg; prior {rms_prior:.3f} deg kept as soft constraint)"
+            )
+
     notes.append(
-        f"weighted hand-eye: weight median={float(np.median(weights)):.3g}, "
-        f"min={float(np.min(weights)):.3g}, max={float(np.max(weights)):.3g}"
+        "weighted hand-eye: normalized/clipped IMU confidence "
+        f"raw_median={median_raw_weight:.3g}, "
+        f"normalized_min={float(np.min(weights)):.3g}, "
+        f"normalized_max={float(np.max(weights)):.3g}"
     )
 
     def pack(r: np.ndarray) -> np.ndarray:
@@ -85,12 +166,21 @@ def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) ->
     def unpack(vec: np.ndarray) -> np.ndarray:
         return orthonormalize_rotation(so3_exp(vec))
 
+    sigma = 15.0 if prior_sigma_deg is None else float(prior_sigma_deg)
+    prior_w = 0.0
+    if r_prior is not None and sigma > 1e-6:
+        # Scale prior to a few strong pairs so it regularizes yaw without dominating.
+        prior_w = float(np.sqrt(np.median(weights)) / np.deg2rad(sigma))
+        notes.append(f"rotation prior soft constraint sigma={sigma:.1f} deg, weight={prior_w:.3g}")
+
     def residual(vec: np.ndarray) -> np.ndarray:
         r_x = unpack(vec)
         residuals = []
         for pair, weight in zip(usable, weights):
             err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
             residuals.append(np.sqrt(weight) * err)
+        if r_prior is not None and prior_w > 0:
+            residuals.append(prior_w * so3_log(r_prior.T @ r_x))
         return np.concatenate(residuals)
 
     opt = least_squares(residual, pack(r0), loss="huber", f_scale=np.deg2rad(1.0), max_nfev=200)
@@ -99,14 +189,28 @@ def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) ->
     # 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/time_offset.py

@@ -190,6 +190,7 @@ def refine_time_offset_signed(
     gyro_bias_rad_s: np.ndarray | None = None,
     search_s: float = 0.08,
     sample_hz: float = 50.0,
+    max_shift_s: float | None = 0.05,
 ) -> TimeOffsetResult:
     """Refine ``δt`` with signed 3-axis rates using a known ``R_IMU_lidar``.
 
@@ -293,9 +294,23 @@ def refine_time_offset_signed(
         f"corr={best_corr:.3f}, mag_corr={mag_at_best:.3f} (coarse_mag={mag_at_coarse:.3f}), "
         f"search=±{half:.3f}s"
     )
+    shift = abs(best_delta - float(delta_t_s))
+    if max_shift_s is not None and shift > float(max_shift_s):
+        notes.append(
+            f"signed refine rejected: |Δδt|={shift:.4f}s exceeds max_shift={float(max_shift_s):.4f}s; "
+            "keeping previous delta_t"
+        )
+        return TimeOffsetResult(
+            delta_t_s=float(delta_t_s),
+            correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
+            search_s=search_s,
+            notes=tuple(notes),
+            ok=True,
+        )
+    # Require a meaningful MSE drop so tiny downhill noise cannot walk δt across iterations.
     improved = (
         np.isfinite(best_cost)
-        and best_cost < coarse_cost * 0.999
+        and best_cost < coarse_cost * 0.98
         # Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
         and mag_at_best + 1e-4 >= mag_at_coarse
     )

imu_lidar/文件职责说明.md

@@ -41,10 +41,11 @@ python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output
 | 5   | `lidar_deskew.py`       | 可选点云去畸变（低速可关）                 |
 | 6   | `imu_preintegration.py` | IMU 预积分（旋转及速度/位移增量、协方差、零偏雅可比） |
 | 6   | `motion_pairs.py`       | 构造运动对；手眼使用其中的旋转               |
+| 6   | `motion_pairs_io.py`    | 运动对 JSON 缓存读写（供可视化直读）          |
 | 7   | `rotation_handeye.py`   | 加权旋转手眼                        |
 | 8   | `observability.py`      | 旋转 / 平移可观性检查                  |
 | 8   | `joint_optimizer.py`    | 联合精修；完整模式下可估计平移、重力、速度与时变零偏    |
-| 9   | `finalize.py`           | 写出结果 JSON                     |
+| 9   | `finalize.py`           | 写出结果 JSON（含 `motion_pairs.json`） |
 | —   | `pipeline.py`           | 编排全流程                         |
 | —   | `cli.py`                | 命令行入口                         |
 | —   | `CHANGELOG.md`          | 改动记录                          |
@@ -56,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_full_se3_smoke`                      | 同上           | `full_se3` 不崩溃      | 状态为 accepted / rejected / rotation_only 之一 |
+| `test_synthetic_pipeline_rejects_noisy_icp_but_keeps_time_audit` | synthetic end-to-end | strict rotation quality gate + time audit | noisy ICP is blocked; delta-t remains accurate |
+| `test_synthetic_pipeline_full_se3_smoke` | synthetic end-to-end | full-SE(3) smoke test | returns an explicit accepted/rejected/blocked status |
+| `test_planar_yaw_is_not_full_rotation_or_translation_observable` | pure-yaw motion pairs | degeneracy detection | full rotation/translation observability is rejected |
+| `test_multi_axis_motion_is_rotation_and_translation_observable` | multi-axis motion pairs | positive observability case | rotation and translation pass |
+| `test_translation_prior_is_reported_but_not_accepted_when_unobservable` | planar motion + CAD prior | prior semantics | prior is reported but not accepted as calibration |
+| `test_handeye_rejects_a_small_fraction_of_gross_rotation_outliers` | motion pairs with a gross outlier | residual-distribution gate | solve is rejected |
+| `test_motion_pairs_reject_low_fitness` | low-fitness registration | fitness gate | no motion pair is emitted |
+| `test_motion_pairs_reject_imu_and_lidar_discontinuities` | timestamp gaps | continuity gates | cross-gap pairs are rejected |
 
 
 合成数据由 `tools/generate_synthetic_session.py` 生成（墙面点云 + 已知外参 yaw 与 δt）。  

tests/test_motion_pairs_io.py

@@ -0,0 +1,78 @@
+"""Tests for motion-pair cache IO."""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+
+from imu_lidar.contracts import MotionPair
+from imu_lidar.motion_pairs_io import (
+    build_motion_pairs_payload,
+    load_motion_pairs,
+    pair_from_dict,
+    pair_to_dict,
+    pairs_for_session,
+    save_motion_pairs,
+)
+
+
+def test_pair_roundtrip(tmp_path: Path) -> None:
+    pair = MotionPair(
+        session_id="s0",
+        i=1,
+        j=4,
+        t_i_s=1.0,
+        t_j_s=2.5,
+        R_A=np.eye(3),
+        R_B=np.eye(3),
+        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,
+            "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=[
+            {
+                "session_id": "s0",
+                "time_offset_s": 0.0,
+                "gyro_bias_rad_s": np.zeros(3),
+                "pairs": (pair,),
+            }
+        ]
+    )
+    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)
-    assert result.status.value == "rotation_only_accepted"
-    assert result.time_offset_s is not None
-    assert abs(result.time_offset_s - meta["delta_t_s"]) < 0.05
-    assert result.T_IMU_lidar is not None
-    # End-to-end uses approximate ICP; allow moderate absolute error but require consistency.
-    r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
-    err_deg = np.degrees(np.linalg.norm(_log(r_true.T @ result.T_IMU_lidar[:3, :3])))
-    assert err_deg < 15.0
+    progress_events: list[dict] = []
+    result = run_calibration(request, progress_callback=progress_events.append)
+    # The lightweight synthetic point cloud uses approximate ICP and has a
+    # roughly 3-degree P95 residual. The production gate must reject it rather
+    # than expose a plausible-looking extrinsic.
+    assert result.status.value == "blocked"
+    assert result.T_IMU_lidar is None
     session0 = result.details["sessions"][0]
-    assert session0["handeye"]["residual_rms_deg"] < 5.0
+    assert abs(session0["time_offset_s"] - meta["delta_t_s"]) < 0.05
+    assert result.details["joint_handeye"]["residual_p95_deg"] > 1.5
+    assert not result.details["joint_handeye"]["ok"]
+    assert progress_events[0]["event"] == "pipeline_start"
+    assert any(
+        event["stage"] == "motion_pairs" and event["event"] == "complete"
+        for event in progress_events
+    )
+    assert any(
+        event["stage"] == "joint_optimizer" and event["event"] == "phase_a_complete"
+        for event in progress_events
+    )
+    assert progress_events[-1]["stage"] == "finalize"
+    assert progress_events[-1]["event"] == "complete"
 
 
 def test_time_offset_on_synthetic(tmp_path: Path):
@@ -176,13 +187,17 @@ def test_synthetic_pipeline_full_se3_smoke(tmp_path: Path):
         "full_se3_accepted",
         "full_se3_rejected_due_to_observability",
         "rotation_only_accepted",
+        "blocked",
     }
-    assert result.T_IMU_lidar is not None
     session0 = result.details["sessions"][0]
     assert "delta_v" in session0.get("pair_notes", []) or session0.get("pair_count", 0) >= 0
-    # Phase-C fields appear when joint ran successfully on pairs.
-    if session0.get("ok"):
+    # Phase-C fields appear only when the strict rotation gate passed.
+    if result.status.value != "blocked":
+        assert result.T_IMU_lidar is not None
         assert "gyro_bias_rad_s" in session0["joint"]
+    else:
+        assert result.T_IMU_lidar is None
+        assert not result.details["joint_handeye"]["ok"]
 
 
 def test_signed_time_offset_refine_improves_or_keeps(tmp_path: Path):

tools/export_motion_pairs_for_viz.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+"""Build motion_pairs.json next to an existing summary without re-solving extrinsic.
+
+Use this once for older calibration outputs that predate automatic pair caching.
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import sys
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+    sys.path.insert(0, str(ROOT))
+
+from imu_lidar.imu_audit import audit_imu
+from imu_lidar.imu_io import load_imu_samples
+from imu_lidar.keyframes import build_keyframes
+from imu_lidar.lidar_io import load_lidar_frames
+from imu_lidar.motion_pairs import build_motion_pairs
+from imu_lidar.motion_pairs_io import build_motion_pairs_payload, save_motion_pairs
+
+
+def _load_summary_meta(summary_path: Path) -> tuple[float, np.ndarray, str]:
+    summary = json.loads(summary_path.read_text(encoding="utf-8"))
+    delta_t = float(summary.get("time_offset_s") or 0.0)
+    session = (summary.get("details") or {}).get("sessions", [{}])[0]
+    session_id = str(session.get("session_id") or summary_path.parent.name)
+    bias = np.asarray(
+        (session.get("imu_audit") or {}).get("gyro_bias_rad_s")
+        or (session.get("joint") or {}).get("gyro_bias_rad_s")
+        or [0.0, 0.0, 0.0],
+        dtype=float,
+    ).reshape(3)
+    return delta_t, bias, session_id
+
+
+def export_one(
+    *,
+    lidar: Path,
+    imu: Path,
+    summary: Path,
+    output: Path | None,
+    min_rotation_deg: float,
+    min_translation_m: float,
+) -> Path:
+    delta_t, bias_from_summary, session_id = _load_summary_meta(summary)
+    imu_series = load_imu_samples(imu)
+    # Prefer freshly audited bias if summary bias is missing/zeros.
+    if float(np.linalg.norm(bias_from_summary)) < 1e-12:
+        bias = audit_imu(imu_series).gyro_bias_rad_s
+    else:
+        bias = bias_from_summary
+
+    frames = load_lidar_frames(lidar)
+    keyframes = build_keyframes(
+        frames,
+        min_translation_m=min_translation_m,
+        min_rotation_deg=min_rotation_deg,
+    )
+    pair_set = build_motion_pairs(
+        session_id=session_id,
+        keyframes=list(keyframes.frames),
+        keyframe_indices=keyframes.indices,
+        imu=imu_series,
+        delta_t_s=delta_t,
+        gyro_bias_rad_s=bias,
+        min_rotation_deg=min_rotation_deg,
+        min_translation_m=min_translation_m,
+    )
+    prepared = [
+        {
+            "session_id": session_id,
+            "time_offset_s": delta_t,
+            "gyro_bias_rad_s": np.asarray(bias, dtype=float).reshape(3),
+            "pairs": pair_set.pairs,
+        }
+    ]
+    payload = build_motion_pairs_payload(prepared_sessions=prepared)
+    out = output or (summary.parent / "motion_pairs.json")
+    save_motion_pairs(out, payload)
+    print(f"wrote {out} ({len(pair_set.pairs)} pairs, session={session_id}, dt={delta_t:.6f})")
+    return out
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--lidar", type=Path, required=True)
+    parser.add_argument("--imu", type=Path, required=True)
+    parser.add_argument("--summary", type=Path, required=True)
+    parser.add_argument("--output", type=Path, default=None, help="Default: <summary_dir>/motion_pairs.json")
+    parser.add_argument("--min-pair-rotation-deg", type=float, default=2.0)
+    parser.add_argument("--min-pair-translation-m", type=float, default=0.3)
+    args = parser.parse_args()
+    export_one(
+        lidar=args.lidar,
+        imu=args.imu,
+        summary=args.summary,
+        output=args.output,
+        min_rotation_deg=args.min_pair_rotation_deg,
+        min_translation_m=args.min_pair_translation_m,
+    )
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/visualize_pair_3d.py

@@ -9,6 +9,8 @@ Modes (keyboard):
   2          IMU prediction with X=I  (B_pred = A)
   3          LiDAR registration B     (reference)
   4          calibrated prediction    B_pred = X^{-1} A X
+             (rotation_only runs default to R conjug + t_B so bad IMU Δp
+              does not dominate the overlay)
   N / ]      next motion pair
   P / [      previous motion pair
   Q / Esc    exit
@@ -20,10 +22,16 @@ from __future__ import annotations
 
 import argparse
 import json
+import sys
 from pathlib import Path
+from typing import Any
 
 import numpy as np
 
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+    sys.path.insert(0, str(ROOT))
+
 from imu_lidar.geometry import (
     inverse_transform,
     make_transform,
@@ -34,12 +42,39 @@ from imu_lidar.geometry import (
 from imu_lidar.imu_io import load_imu_samples
 from imu_lidar.imu_preintegration import preintegrate_imu
 from imu_lidar.keyframes import build_keyframes
-from imu_lidar.lidar_io import load_lidar_frames
+from imu_lidar.lidar_io import load_lidar_frame_at, load_lidar_frames
 from imu_lidar.motion_pairs import build_motion_pairs
+from imu_lidar.motion_pairs_io import (
+    load_motion_pairs,
+    pairs_for_session,
+    resolve_motion_pairs_path,
+)
 from imu_lidar.registration import register_lidar_pair
 from imu_lidar.time_offset import lidar_time_to_imu_time
 
 
+class _LazyFrameStore:
+    """Load NPZ frames on demand; indices match mid-time-sorted ``load_lidar_frames``."""
+
+    def __init__(self, lidar_dir: Path, *, max_cached: int = 16):
+        self.lidar_dir = Path(lidar_dir)
+        self.max_cached = max_cached
+        self._cache: dict[int, object] = {}
+        self._order: list[int] = []
+
+    def __getitem__(self, index: int):
+        index = int(index)
+        if index in self._cache:
+            return self._cache[index]
+        frame = load_lidar_frame_at(self.lidar_dir, index)
+        self._cache[index] = frame
+        self._order.append(index)
+        while len(self._order) > self.max_cached:
+            old = self._order.pop(0)
+            self._cache.pop(old, None)
+        return frame
+
+
 COLORS = {
     "target": [0.10, 0.65, 1.00],
     "source": [1.00, 0.35, 0.05],
@@ -53,24 +88,45 @@ MODE_NAMES = (
 )
 
 
-def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray]:
+def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray, dict[str, Any]]:
     summary = json.loads(summary_path.read_text(encoding="utf-8"))
     t_block = summary.get("T_IMU_lidar")
+    meta: dict[str, Any] = {
+        "rotation_only": False,
+        "translation_accepted": False,
+        "status": str(summary.get("status") or ""),
+    }
     if t_block is None:
         matrix = summary.get("matrix")
         if matrix is not None:
-            return np.asarray(matrix, dtype=float), 0.0, np.zeros(3)
+            return np.asarray(matrix, dtype=float), 0.0, np.zeros(3), meta
         raise ValueError(f"no T_IMU_lidar in {summary_path}")
     t_mat = np.asarray(t_block["matrix"], dtype=float)
     dt = float(summary.get("time_offset_s") or 0.0)
     session = (summary.get("details") or {}).get("sessions", [{}])[0]
+    joint = session.get("joint") or {}
     bias = np.asarray(
         (session.get("imu_audit") or {}).get("gyro_bias_rad_s")
-        or (session.get("joint") or {}).get("gyro_bias_rad_s")
+        or joint.get("gyro_bias_rad_s")
         or [0.0, 0.0, 0.0],
         dtype=float,
     ).reshape(3)
-    return t_mat, dt, bias
+    status = str(summary.get("status") or "")
+    translation_accepted = bool(
+        joint.get("translation_accepted")
+        or (summary.get("details") or {}).get("joint", {}).get("translation_accepted")
+    )
+    rotation_only = ("rotation_only" in status) or (
+        not translation_accepted and float(np.linalg.norm(t_mat[:3, 3])) < 1e-9
+    )
+    meta.update(
+        {
+            "rotation_only": rotation_only,
+            "translation_accepted": translation_accepted,
+            "status": status,
+        }
+    )
+    return t_mat, dt, bias, meta
 
 
 def _delta_components(reference: np.ndarray, candidate: np.ndarray) -> dict:
@@ -169,16 +225,42 @@ def _pair_from_indices(
     return frame_i, frame_j, a, reg.transform
 
 
-def _transforms_for_pair(x: np.ndarray, a_ij: np.ndarray, b_gicp: np.ndarray) -> dict[str, np.ndarray]:
+def _transforms_for_pair(
+    x: np.ndarray,
+    a_ij: np.ndarray,
+    b_gicp: np.ndarray,
+    *,
+    mode4_translation: str = "imu",
+) -> dict[str, np.ndarray]:
+    """Build overlay transforms.
+
+    ``mode4_translation``:
+      - ``imu``: full SE3 conjug ``X^{-1} A X`` (needs trustworthy IMU Δp)
+      - ``gicp``: rotation conjug only; translation taken from LiDAR B
+        (correct check for rotation_only calibrations)
+    """
+
+    calibrated = inverse_transform(x) @ a_ij @ x
+    if mode4_translation == "gicp":
+        calibrated = make_transform(b_gicp[:3, 3], calibrated[:3, :3])
+    elif mode4_translation != "imu":
+        raise ValueError(f"unknown mode4_translation={mode4_translation!r}")
     return {
         MODE_NAMES[0]: np.eye(4),
         MODE_NAMES[1]: a_ij.copy(),
         MODE_NAMES[2]: b_gicp.copy(),
-        MODE_NAMES[3]: inverse_transform(x) @ a_ij @ x,
+        MODE_NAMES[3]: calibrated,
     }
 
 
-def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray):
+def _resolve_pair(
+    frames,
+    pairs,
+    pair_index: int,
+    x: np.ndarray,
+    *,
+    mode4_translation: str = "imu",
+):
     pair = pairs[pair_index]
     frame_i = frames[pair.i]
     frame_j = frames[pair.j]
@@ -190,7 +272,9 @@ def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray):
         pair.t_B_m if pair.t_B_m is not None else np.zeros(3),
         pair.R_B,
     )
-    transforms = _transforms_for_pair(x, a_ij, b_gicp)
+    transforms = _transforms_for_pair(
+        x, a_ij, b_gicp, mode4_translation=mode4_translation
+    )
     label = (
         f"pair {pair_index + 1}/{len(pairs)}  "
         f"frames {pair.i} <- {pair.j}  "
@@ -255,12 +339,15 @@ def _run_gui(
     start_index: int,
     voxel: float,
     fixed_single_pair: tuple | None,
+    mode4_translation: str = "imu",
 ) -> None:
     import open3d as o3d
 
     if fixed_single_pair is not None:
         frame_i, frame_j, a_ij, b_gicp = fixed_single_pair
-        transforms = _transforms_for_pair(x, a_ij, b_gicp)
+        transforms = _transforms_for_pair(
+            x, a_ij, b_gicp, mode4_translation=mode4_translation
+        )
         label = f"fixed frames (no pair switching)"
         pair_index = 0
         n_pairs = 1
@@ -270,7 +357,7 @@ def _run_gui(
         n_pairs = len(pairs)
         use_list = True
         frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
-            frames, pairs, pair_index, x
+            frames, pairs, pair_index, x, mode4_translation=mode4_translation
         )
 
     viewer = o3d.visualization.VisualizerWithKeyCallback()
@@ -311,7 +398,7 @@ def _run_gui(
             return
         new_index = int(new_index) % n_pairs
         frame_i, frame_j, _a, b_gicp, transforms, label = _resolve_pair(
-            frames, pairs, new_index, x
+            frames, pairs, new_index, x, mode4_translation=mode4_translation
         )
         state["pair_index"] = new_index
         state["transforms"] = transforms
@@ -357,13 +444,34 @@ def _run_gui(
 def main(argv: list[str] | None = None) -> int:
     parser = argparse.ArgumentParser(description=__doc__)
     parser.add_argument("--lidar", required=True, type=Path, help="LiDAR session directory")
-    parser.add_argument("--imu", required=True, type=Path, help="IMU CSV")
+    parser.add_argument(
+        "--imu",
+        type=Path,
+        default=None,
+        help="IMU CSV (only needed when rebuilding pairs without motion_pairs.json)",
+    )
     parser.add_argument(
         "--summary",
         required=True,
         type=Path,
         help="summary.json (or T_IMU_lidar.json) from a calibration run",
     )
+    parser.add_argument(
+        "--motion-pairs",
+        type=Path,
+        default=None,
+        help="Cached motion_pairs.json (default: next to --summary)",
+    )
+    parser.add_argument(
+        "--session-id",
+        default=None,
+        help="Session id inside multi-session motion_pairs.json",
+    )
+    parser.add_argument(
+        "--rebuild-pairs",
+        action="store_true",
+        help="Ignore cache and rebuild pairs from IMU/LiDAR (slow)",
+    )
     parser.add_argument("--pair-index", type=int, default=0, help="Starting motion-pair index")
     parser.add_argument("--frame-i", type=int, default=None, help="Optional explicit frame index i")
     parser.add_argument("--frame-j", type=int, default=None, help="Optional explicit frame index j")
@@ -381,45 +489,104 @@ def main(argv: list[str] | None = None) -> int:
         action="store_true",
         help="Skip Open3D window (use with --save-png)",
     )
+    parser.add_argument(
+        "--mode4-translation",
+        choices=("auto", "gicp", "imu"),
+        default="auto",
+        help=(
+            "Mode-4 translation source: gicp=R conjug + t_B (rotation check); "
+            "imu=full X^-1 A X; auto=gicp for rotation_only summaries"
+        ),
+    )
     args = parser.parse_args(argv)
 
-    x, delta_t_s, gyro_bias = _load_extrinsic(args.summary)
-    frames, imu, keyframes, pair_set = _build_pair_list(
-        lidar_dir=args.lidar,
-        imu_path=args.imu,
-        delta_t_s=delta_t_s,
-        gyro_bias=gyro_bias,
-        min_rotation_deg=args.min_pair_rotation_deg,
-        min_translation_m=args.min_pair_translation_m,
-    )
+    x, delta_t_s, gyro_bias, extr_meta = _load_extrinsic(args.summary)
+    if args.mode4_translation == "auto":
+        mode4_translation = "gicp" if extr_meta.get("rotation_only") else "imu"
+    else:
+        mode4_translation = args.mode4_translation
+    cache_path = args.motion_pairs or resolve_motion_pairs_path(args.summary)
+    use_cache = (not args.rebuild_pairs) and cache_path is not None and args.frame_i is None
 
+    frames = None
+    pairs: tuple = ()
     fixed_single_pair = None
-    if args.frame_i is not None and args.frame_j is not None:
-        frame_i, frame_j, a_ij, b_gicp = _pair_from_indices(
-            frames,
-            imu,
-            i=args.frame_i,
-            j=args.frame_j,
+    label = ""
+    b_gicp = np.eye(4)
+    transforms: dict[str, np.ndarray] = {}
+    frame_i = frame_j = None
+
+    if use_cache:
+        payload = load_motion_pairs(cache_path)
+        pair_list = pairs_for_session(payload, args.session_id)
+        if not pair_list:
+            raise SystemExit(f"no pairs in cache: {cache_path}")
+        if not 0 <= args.pair_index < len(pair_list):
+            raise SystemExit(
+                f"pair-index {args.pair_index} outside [0, {len(pair_list) - 1}] "
+                f"({len(pair_list)} pairs in cache)"
+            )
+        frames = _LazyFrameStore(args.lidar)
+        pairs = tuple(pair_list)
+        frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
+            frames, pairs, args.pair_index, x, mode4_translation=mode4_translation
+        )
+        print(f"loaded {len(pairs)} cached pairs from {cache_path}")
+    else:
+        if args.imu is None:
+            raise SystemExit("--imu is required when motion_pairs.json is missing (or use --rebuild-pairs with --imu)")
+        frames, imu, keyframes, pair_set = _build_pair_list(
+            lidar_dir=args.lidar,
+            imu_path=args.imu,
             delta_t_s=delta_t_s,
             gyro_bias=gyro_bias,
+            min_rotation_deg=args.min_pair_rotation_deg,
+            min_translation_m=args.min_pair_translation_m,
         )
-        transforms = _transforms_for_pair(x, a_ij, b_gicp)
-        label = f"frames {args.frame_i} <- {args.frame_j}"
-        fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp)
-        pairs = ()
-    else:
-        if not pair_set.pairs:
-            raise SystemExit("no motion pairs rebuilt; loosen min-pair thresholds or check data")
-        if not 0 <= args.pair_index < len(pair_set.pairs):
-            raise SystemExit(
-                f"pair-index {args.pair_index} outside [0, {len(pair_set.pairs) - 1}] "
-                f"({len(pair_set.pairs)} pairs available)"
+        if args.frame_i is not None and args.frame_j is not None:
+            frame_i, frame_j, a_ij, b_gicp = _pair_from_indices(
+                frames,
+                imu,
+                i=args.frame_i,
+                j=args.frame_j,
+                delta_t_s=delta_t_s,
+                gyro_bias=gyro_bias,
             )
-        pairs = pair_set.pairs
-        frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
-            frames, pairs, args.pair_index, x
+            transforms = _transforms_for_pair(
+                x, a_ij, b_gicp, mode4_translation=mode4_translation
+            )
+            label = f"frames {args.frame_i} <- {args.frame_j}"
+            fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp)
+            pairs = ()
+        else:
+            if not pair_set.pairs:
+                raise SystemExit("no motion pairs rebuilt; loosen min-pair thresholds or check data")
+            if not 0 <= args.pair_index < len(pair_set.pairs):
+                raise SystemExit(
+                    f"pair-index {args.pair_index} outside [0, {len(pair_set.pairs) - 1}] "
+                    f"({len(pair_set.pairs)} pairs available)"
+                )
+            pairs = pair_set.pairs
+            frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
+                frames, pairs, args.pair_index, x, mode4_translation=mode4_translation
+            )
+            print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes")
+
+    print(
+        f"mode4 translation={mode4_translation}  "
+        f"(status={extr_meta.get('status') or 'n/a'}, "
+        f"rotation_only={bool(extr_meta.get('rotation_only'))})"
+    )
+    if mode4_translation == "gicp":
+        print(
+            "note: mode4 uses R conjug + t_B; IMU Δp is ignored "
+            "(typical for rotation_only — raw Δp often has large Z drift)."
+        )
+    if mode4_translation == "imu":
+        print(
+            "note: mode4 uses full X^-1 A X. If clouds stack vertically, "
+            "IMU Δp is likely bad; retry with --mode4-translation gicp."
         )
-        print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes")
 
     if args.save_png is not None:
         _print_pair_header(label, b_gicp, transforms)
@@ -444,6 +611,7 @@ def main(argv: list[str] | None = None) -> int:
         start_index=args.pair_index,
         voxel=args.voxel,
         fixed_single_pair=fixed_single_pair,
+        mode4_translation=mode4_translation,
     )
     return 0