清理历史雷达RTK资料并归档当前车辆结果

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

.gitignore

@@ -1,10 +1,24 @@
+# Python
 __pycache__/
 *.py[cod]
 .pytest_cache/
-*.egg-info/
-.eggs/
-dist/
-build/
-examples/synthetic_session/
 .venv/
 venv/
+
+# IDE / OS
+.idea/
+.vscode/
+.DS_Store
+Thumbs.db
+
+# Raw data and generated outputs
+data/raw/
+work/
+outputs/
+*.rscap
+*.dorec
+*.log
+
+# Large generated point clouds outside the archived reference result
+**/frames/
+**/frames_all/

.vs/VSWorkspaceState.json

@@ -0,0 +1,6 @@
+{
+  "ExpandedNodes": [
+    ""
+  ],
+  "PreviewInSolutionExplorer": false
+}

.vs/calibration.slnx/v18/DocumentLayout.backup.json

@@ -0,0 +1,49 @@
+{
+  "Version": 1,
+  "WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\",
+  "Documents": [
+    {
+      "AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}",
+      "RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}"
+    }
+  ],
+  "DocumentGroupContainers": [
+    {
+      "Orientation": 0,
+      "VerticalTabListWidth": 256,
+      "DocumentGroups": [
+        {
+          "DockedWidth": 200,
+          "SelectedChildIndex": 3,
+          "Children": [
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{1c4feeaa-4718-4aa9-859d-94ce25d182ba}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}"
+            },
+            {
+              "$type": "Document",
+              "DocumentIndex": 0,
+              "Title": "README.md",
+              "DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md",
+              "RelativeDocumentMoniker": "README.md",
+              "ToolTip": "D:\\First-dev-dept\\calibration\\README.md",
+              "RelativeToolTip": "README.md",
+              "ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==",
+              "Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|",
+              "WhenOpened": "2026-07-24T03:20:21.56Z",
+              "EditorCaption": ""
+            }
+          ]
+        }
+      ]
+    }
+  ]
+}

.vs/calibration.slnx/v18/DocumentLayout.json

@@ -0,0 +1,45 @@
+{
+  "Version": 1,
+  "WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\",
+  "Documents": [
+    {
+      "AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}",
+      "RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}"
+    }
+  ],
+  "DocumentGroupContainers": [
+    {
+      "Orientation": 0,
+      "VerticalTabListWidth": 256,
+      "DocumentGroups": [
+        {
+          "DockedWidth": 200,
+          "SelectedChildIndex": 2,
+          "Children": [
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
+            },
+            {
+              "$type": "Bookmark",
+              "Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}"
+            },
+            {
+              "$type": "Document",
+              "DocumentIndex": 0,
+              "Title": "README.md",
+              "DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md",
+              "RelativeDocumentMoniker": "README.md",
+              "ToolTip": "D:\\First-dev-dept\\calibration\\README.md",
+              "RelativeToolTip": "README.md",
+              "ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==",
+              "Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|",
+              "WhenOpened": "2026-07-24T03:20:21.56Z",
+              "EditorCaption": ""
+            }
+          ]
+        }
+      ]
+    }
+  ]
+}

README.md

@@ -1,184 +1,292 @@
-# LiDAR–IMU 外参标定
+# 双天线RTK—3D LiDAR直接手眼标定
 
-用连续行驶中的 LiDAR 与 IMU 相对运动，估计安装外参与时间偏置：
+本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **车头向前的 RTK 车体系**（主天线原点）。  
+求解不使用 RTK 到后轮轴的 XY 杆臂；与雷达–IMU 外参对照时旋转系一致，平移仍差天线原点。
+
+当前交付标定（2026-08 室外车，27 站）约定如下：
+
+| 项 | 值 |
+|---|---|
+| RTK 坐标系 | **车头向前**（`HeadingOffsetDeg = -90`；主从装反、基线朝右） |
+| 天线相位中心离地高 | **1.9165 m**（1916.5 mm） |
+| 机械初值（车头系） | \(t=(+0.21086,-0.41418,+0.07850)\) m，yaw=**0°**（CAD 纵向已按车头正向取 +X） |
+| 物理基线 | `baseline_points=vehicle_right`（主天线车左，从天线车右，后轴中心左右对称） |
+| 姿态 | 双天线 pitch/roll：`R = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(+90°)` |
+| 地面点 ROI | LiDAR 系 **`z ∈ [-2.5, -1.5]`**（约 2 m 车顶安装） |
+| pair 配准 | **禁止**使用外参 seed；B 与 X 独立 |
+
+数据下载：https://fs.fairylandtech.com:5001/FRLD/#file_id=966776353886090246  
+账号：lichun.qu@fairylandtech.com　密码：lichun.qu
+
+---
+
+## 1. 输出坐标约定（车头向前）
+
+统一约定 `T_A_B` 把 B 系点变换到 A 系：
 
 ```text
-p_IMU = T_IMU_lidar · p_lidar
+p_RTK = T_RTK_lidar · p_lidar
 ```
 
-**当前阶段：** 算法与合成自检已闭环；已提供 `tools/export_rscap_to_v1.py`（N300 `.rscap` + H32 dlog/MSOP → 中间格式）；**合格实车验收尚未完成**，故正式外参尚未对实车落盘交付。
+本仓库默认 RTK 导航系（**车头向前 / vehicle_forward_heading_offset**）：
+
+- 原点：GGA 位置参考点（主天线 / ANT1 相位中心）；
+- X 轴：车头向前（`rawHeading + HeadingOffsetDeg`，本车 `HeadingOffsetDeg = -90`）；
+- Y 轴：左；
+- Z 轴：上；
+- 姿态：先在基线系应用双天线 pitch/roll，再乘固定 `Rz(-heading_offset)`；不是 IMU 融合姿态。
+
+> 改 `HeadingOffsetDeg` 或姿态模型后必须从 **prepare** 起重跑；禁止事后只改 JSON 里的 yaw。  
+> 旧基线系结果（`HeadingOffsetDeg = 0`）与车头系外参不可混用。
+
+机械初值文件：[`run/rtk_lidar_mechanical_initial.json`](run/rtk_lidar_mechanical_initial.json)  
+**仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。**
 
 ---
 
-## 先看什么（对外三份就够）
+## 2. 算法流程
-
-
-| 顺序  | 文档                                     | 用途                   |
-| --- | -------------------------------------- | -------------------- |
-| 1   | **本 README**                           | 做什么、怎么跑、结果怎么判        |
-| 2   | [docs/V1_数据格式.md](docs/V1_数据格式.md)     | 中间格式 + 原始数据导出命令 |
-| 3   | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采（合格数据要求）        |
-
-
-其余（方法细述、测试说明、源码职责、CHANGELOG）给深入阅读 / 改代码时用，见文末。
-
----
-
-
-
-## 1. 输入 / 输出
-
-
-| 输入        | 说明                                                          |
-| --------- | ----------------------------------------------------------- |
-| `imu.csv` | `t,gx,gy,gz,ax,ay,az`（秒；rad/s；m/s²）；`t` 用设备时间               |
-| 雷达会话目录    | `frames_index.csv` + `frames/*.npz`（米制 XYZ）                 |
-| 车辆 YAML   | 轴向与时间语义；外参真值可空（`config/vehicle_installation.template.yaml`） |
-
-
-
-| 输出                 | 说明                     |
-| ------------------ | ---------------------- |
-| `T_IMU_lidar.json` | 外参                     |
-| `time_offset.json` | `t_imu = t_lidar + δt` |
-| `summary.json`     | 状态、残差、可观性              |
-
-
-新车原始数据导出（H32 dlog/zip + HI13 rscap）：
-
-```powershell
-python tools\export_rscap_to_v1.py `
-  --imu-rscap path\to\hi13r4-imu.rscap `
-  --imu-kind hi13 `
-  --lidar-dlog path\to\session_or_recovered.zip `
-  --host-start 2026-08-08T17:40:05 `
-  --host-end 2026-08-08T17:45:15 `
-  --out path\to\session_v1 `
-  --require-difop
-```
-
----
-
-
-
-## 2. 一键复现（合成，不需实车）
-
-```powershell
-cd <本仓库根目录>
-python -m pip install -e ".[dev]"
-python -m pip install -e ".[open3d]"   # 推荐
-powershell -File tools\reproduce_synthetic.ps1
-```
-
-证明：链路可跑通，能收回已知 yaw / δt。  
-不证明：实车安装精度、平移可交付。
-
-产物在 `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 `
-  --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` 切换运动对。
-
----
-
-
-
-## 3. 真实数据怎么跑
-
-1. 按采集清单录制（设备时间；静止 + 低速转弯；有结构场景）
-2. 导出中间格式（第1节命令）
-3. 填写车辆 YAML 的轴向与时间语义
-4. 标定：
-
-```powershell
-python -m imu_lidar.cli run `
-  --vehicle-config config\vehicle_installation.template.yaml `
-  --imu path\to\session_v1\imu.csv `
-  --lidar path\to\session_v1\lidar `
-  --output path\to\out `
-  --mode rotation_only `
-  --time-offset-search-s 2.0
-```
-
-1. 看 `summary.json`，再叠点 / 用验证会话复核后才交付
-
-
-| 模式                  | 交付      | 成功标志                        |
-| ------------------- | ------- | --------------------------- |
-| `rotation_only`（先做） | 旋转 + δt | `rotation_only_accepted`    |
-| `full_se3`（激励够再试）   | + 可观平移  | `full_se3_accepted`（否则平移拒绝） |
-
-
-
-| `summary.json` 状态                              | 含义          |
-| ---------------------------------------------- | ----------- |
-| `rotation_only_accepted` / `full_se3_accepted` | 可进入验证       |
-| `full_se3_rejected_due_to_observability`       | 旋转可用，平移不交   |
-| `blocked`                                      | **不可作安装参数** |
-
-
-预期量级：旋转约 0.5°–2°；水平平移数厘米～十几厘米；无坡时竖直常不可观。
-
----
-
-
-
-## 4. 方法（一句话）
-
-关键帧雷达配准得 **B**，同区间 IMU 预积分得 **A**，解 `R_A R_X ≈ R_X R_B`；再估 δt。可观时才在 `full_se3` 下交平移。
-
----
-
-
-
-## 5. 试验边界（勿误读）
-
-
-|        | 合成 pytest                | 旧车 S2 线下          |
-| ------ | ------------------------ | ----------------- |
-| 目的     | 回归算法                     | 验证旧主机时间数据上链路能跑完   |
-| 期望     | `rotation_only_accepted` | `blocked`**（预期）** |
-| 当安装参数？ | 否                        | **否**             |
-
-
-细节：[tests/README.md](tests/README.md)。
-
----
-
-
-
-## 6. 仓库结构与其余文档
 
 ```text
-imu_lidar/   算法与 CLI
+原始雷达 + RTK（+ 可选 IMU）
-config/      车辆配置模板
+  → combined/（按站关联的多传感器 NPZ）
-docs/        采集清单、数据格式、方法细述
+  → 每站选一帧静态点云 + RTK pose（车头向前，含双天线 pitch/roll）
-tools/       导出、合成复现、可视化
+  → Open3D GICP 与 small_gicp 分别求 B_ij = T_Li_Lj（无外参 seed）
-tests/       自动化测试
+  → 留出点、正反向、旋转共轭不变量等精筛
+  → 双后端共识边 → consensus B
+  → A_ij X = X B_ij + 地面法向/高度约束 → X = T_RTK_lidar
+  → bootstrap、双后端差异、逐对残差与 3D 可视化
 ```
 
+```text
+A_ij = inv(T_W_Ri) · T_W_Rj = T_Ri_Rj
+B_ij = T_Li_Lj
+A_ij · X = X · B_ij
+X = T_RTK_lidar
+```
 
-| 文档                                               | 何时看              |
+---
-| ------------------------------------------------ | ---------------- |
+
-| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md)       | 要看方法约定与实现状态表     |
+## 3. 原始数据与导出
-| [tests/README.md](tests/README.md)               | 要看合成用例 / S2 烟测记录 |
+
-| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md)       | 要改源码             |
+大体积数据不提交 Git。常见两种采集形态：
-| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 要查改动史            |
+
+### 3.1 每站独立雷达目录（旧/标准站目录）
+
+```text
+raw_dataset/
+├── stations/001|002|.../   # H32 dlog 或 h32.rscap
+└── captures/
+    ├── rtk.rscap
+    └── imu.rscap           # 仅关联，不参与外参求解
+```
+
+```powershell
+python tools\export_raw_to_combined.py `
+  --stations-root "$Raw\stations" `
+  --rtk-rscap "$Raw\captures\rtk.rscap" `
+  --imu-rscap "$Raw\captures\imu.rscap" `
+  --out "$Out\exported" `
+  --overwrite
+```
+
+默认时间基：`-TimeBasis device_gnss`（雷达设备时 ↔ GNSS week/TOW）。
+
+### 3.2 G90 连续录制 + H32 DLog 按站时间窗（本次 27 站）
+
+站不在独立目录，而在多个 Medulla DLog ZIP 与 G90 `.rscap` 中时：
+
+```powershell
+python tools\export_g90_h32_windows_to_combined.py `
+  --segments-csv <rtk_lidar_station_segments.csv> `
+  --lidar-dlog <dump_1.zip> --lidar-dlog <dump_2.zip> `
+  --rtk-rscap <g90_1.rscap> --rtk-rscap <g90_2.rscap> `
+  --out <output_root> --expected-stations 27 --frame-stride 5
+```
+
+该入口用 **主机接收 UTC** 做近邻关联（`time_basis_mode: host`），并保留设备时间供审计。  
+可加 `--reuse-export` 在已有 `export/` 上续跑。
+
+采集建议：有效静站 ≥30（更好 40～60）；相邻站转角约 **15°～30°**；避免一长串同朝向停车；场内宜有墙/立柱及 2～3 块法向不同的固定平面板。
+
+---
+
+## 4. 环境安装
+
+Windows + PowerShell + Python 3.11：
+
+```powershell
+python -m pip install -r requirements.txt
+```
+
+依赖：NumPy、SciPy、Open3D、small_gicp。完整共识需要两个配准后端。
+
+---
+
+## 5. 一键复现（匹配本次标定）
+
+### 5.1 已有 `combined/`（推荐复现本次结果）
+
+```powershell
+$Repo = (Resolve-Path ".").Path
+$Data = "D:\data\rtk_lidar_run"   # 含 combined/
+
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
+  -CombinedRoot "$Data\combined" `
+  -WorkRoot "$Data\prepared_vehicle_h19165" `
+  -OutputRoot "$Data\outputs_vehicle_h19165" `
+  -RtkReferenceHeightAboveGroundM 1.9165 `
+  -HeadingOffsetDeg -90 `
+  -ExpectedStations 27 `
+  -MinStations 20 `
+  -GroundZMin -2.5 `
+  -GroundZMax -1.5 `
+  -Bootstrap 200
+```
+
+关键参数：
+
+| 参数 | 本次取值 | 说明 |
+|---|---|---|
+| `-RtkReferenceHeightAboveGroundM` | **1.9165** | GGA/ANT1 相位中心离地高（m），必填 |
+| `-HeadingOffsetDeg` | **-90** | 车头向前（主从装反、基线朝右）；`0` 才是基线系 |
+| `-GroundZMin/Max` | **-2.5 / -1.5** | 约 2 m 车顶雷达；旧默认 `[-1.4,-0.4]` 会拟合到墙 |
+| `-ExpectedStations` | **27** | 本批站数 |
+| `-MinStations` | **20** | 远程旧脚本曾写死 30，会跑不了本批 |
+
+pair 阶段**不会**传入 `--initial-extrinsic`；机械初值只进最终 AX=XB。
+
+### 5.2 站目录原始数据一键（导出 + 求解）
+
+```powershell
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" `
+  -DataRoot "$Raw\stations" `
+  -RtkCapture "$Raw\captures\rtk.rscap" `
+  -ImuCapture "$Raw\captures\imu.rscap" `
+  -OutputRoot $Out `
+  -RtkReferenceHeightAboveGroundM 1.9165 `
+  -ExpectedStations 27 `
+  -GroundZMin -2.5 `
+  -GroundZMax -1.5
+```
+
+主要输出：
+
+```text
+$Out/
+├── exported/combined/          # 或外部已有 combined/
+├── prepared_*/frames_all/
+├── prepared_*/reference_poses_rtk_gga_raw_heading.csv
+└── calibration/ 或 outputs_*/
+    ├── open3d_gicp/  small_gicp/  consensus/
+    ├── common/ground_planes.csv
+    ├── summary.json
+    └── final_T_RTK_lidar.json
+```
+
+---
+
+## 6. 3D 可视化
+
+查看本次结果：
+
+```powershell
+$Repo = "D:\First-dev-dept\calibration-rtk-run"
+$Out  = "D:\data\rtk_lidar_run\outputs_vehicle_h19165"
+$Work = "D:\data\rtk_lidar_run\prepared_vehicle_h19165"
+
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
+  -Frames "$Work\frames_all" `
+  -Pairs "$Out\consensus\B_consensus.npz" `
+  -Extrinsic "$Out\final_T_RTK_lidar.json" `
+  -PairIndex 0
+```
+
+通用模板（把路径换成你的 `WorkRoot` / `OutputRoot`）：
+
+```powershell
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
+  -Frames "$WorkRoot\frames_all" `
+  -Pairs "$OutputRoot\consensus\B_consensus.npz" `
+  -Extrinsic "$OutputRoot\final_T_RTK_lidar.json" `
+  -PairIndex 0
+```
+
+| 按键 | 含义 |
+|---|---|
+| `1` | 原始点云 |
+| `2` | 仅用 RTK 运动作初值 |
+| `3` | GICP 测得的 B |
+| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） |
+| `N` / `]` | 下一运动对 |
+| `P` / `[` | 上一运动对 |
+| `Q` / `Esc` | 退出 |
+
+蓝 = 站 i，橙 = 站 j。请用 `N`/`P` **多看大转角对**，不要只看前几对同朝向站。
+
+---
+
+## 7. 当前标定结果（车头向前，h = 1.9165 m）
+
+> **状态：可作车头系候选交付**（`recommended_for_deployment: true`）。  
+> 约定：`HeadingOffsetDeg=-90`，双天线 pitch/roll，机械初值 \(t=(+0.21086,-0.41418,+0.07850)\)，yaw=0。  
+
+仓库内结果：[`results/vehicle_20260808/`](results/vehicle_20260808/)（来自本机 `outputs_vehicle_h19165`）。
+
+```text
+translation_m = [0.217822250, -0.411347802, 0.106542337]
+RPY_deg_xyz   = [0.066239, 0.809662, -0.551322]
+
+T_RTK_lidar ≈
+ 0.999854   0.009639   0.014119   0.217822
+-0.009621   0.999953  -0.001292  -0.411348
+-0.014131   0.001156   0.999899   0.106542
+ 0          0          0          1
+```
+
+| 指标 | 值 |
+|---|---:|
+| 有效站点 / 共识对 | 27 / 20 |
+| 平移残差 RMS | ≈ 0.071 m |
+| 旋转残差 RMS | ≈ 0.982 ° |
+| 双后端差 | ≈ 3.1 mm / 0.12° |
+| `frame_mode` | `vehicle_forward_heading_offset` |
+| 相对机械初值 | XY 近机械杆臂；yaw≈0；无近 180° 冲突 |
+
+与机械平移初值 XY 相差约数毫米；z 由天线高度约束，CAD 的 4 mm 不能代替实测 1.9165 m。
+
+### 为何 RMS 尚可、尾部（P95/max）较差？
+
+1. **前段多站几乎同航向**（STATION-01～05 约 250°～255°）。最差对（如 2→4）站间转角仅约 5°，小转角对平均平移残差约 7.4 cm，大转角对约 3.7 cm。  
+2. **GICP heldout RMSE** 本身多在 0.11～0.14 m，场景重叠/结构限制了配准下限。  
+3. 本批导出为 **host 时间关联**，静站可用，但仍可能引入厘米级位姿—点云错位。  
+4. AX 残差衡量的是「RTK 运动 A」与「外参预测 XBX」的一致性，**不是**相对 CAD 的毫米误差，也不能单独证明 ±3 cm 绝对真值。
+
+改进方向：相邻站转角 15°～30°、站数 ≥40、固定平面板、有条件改用 `device_gnss`。
+
+---
+
+## 8. z 与精度限制
+
+平面阿克曼运动不能独立观测 z。z 由「LiDAR 地面平面 + 外供 RTK 参考点离地高」约束：
+
+- 本次：**1.9165 m**（相位中心离地）；
+- 不得复用其他车辆或历史采集的天线离地高度。
+
+更改高度后必须重新求解，禁止只改 JSON 里的 z。  
+GGA 对应哪根天线、`rawHeading` 方向须现场确认；搞反会导致 yaw 差约 180°。
+
+---
 
 
-改算法请同步职责说明与 CHANGELOG；改对外用法请更新本 README。
+## 9. 仓库目录
+
+| 目录 | 职责 |
+|---|---|
+| [`code/`](code/) | GICP、运动对质量、AX=XB、结果封装、3D 可视化 |
+| [`tools/`](tools/) | dlog/rscap 解析、G90 窗导出、combined / prepared |
+| [`run/`](run/) | PowerShell 入口；路径与高度均由参数传入 |
+| [`results/`](results/) | 当前车辆的最终外参与质量摘要；不含原始数据和中间点云 |
+| `tests/` | 坐标契约、G90 host 关联等回归 |
+| `work/`、`outputs/` | 本地生成物（`.gitignore`） |
+
+命令索引见 [`run/README.md`](run/README.md)，工具说明见 [`tools/README.md`](tools/README.md)，操作手册见 [`雷达与RTK标定说明书.md`](雷达与RTK标定说明书.md)。

code/README.md

@@ -0,0 +1,13 @@
+# code目录
+
+| 文件 | 职责 |
+|---|---|
+| `rigorous_calibration.py` | 核心CLI：读取静态点云/RTK位姿，Open3D或small_gicp求B，拟合地面，求解/验证AX=XB |
+| `refine_pairs.py` | 不使用最终X，按留出点重叠率、RMSE、旋转共轭不变量和正反向一致性精筛运动对 |
+| `cross_backend_filter.py` | 保留Open3D与small_gicp共同认可且变换接近的边；共识B数值取Open3D结果 |
+| `finalize_direct_rtk_lidar.py` | 将三路求解结果封装为明确方向的`T_RTK_lidar`，选择consensus为最终结果 |
+| `visualize_pair_3d.py` | 交互显示原始、RTK初值、GICP B和`X^-1AX`，并打印增量 |
+| `compare_extrinsics.py` | 计算两套外参的SE(3)平移/旋转差异 |
+| `build_joint_rtk_lidar_inputs.py` | 合并多个独立批次的批内A/B运动对和地面平面，并保留批次索引与汇总信息 |
+
+核心约定：`A=T_Ri_Rj`、`B=T_Li_Lj`、`X=T_RTK_lidar`，满足`A X = X B`。点云配准以i为target、j为source，B将j帧点云变换到i帧。

code/build_joint_rtk_lidar_inputs.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+"""Combine independent RTK-direct hand-eye batches for a shared extrinsic.
+
+Each batch contributes only its within-batch A/B motion pairs and LiDAR ground
+planes.  No cross-batch motion pair is created, so different ENU origins and
+capture locations are valid as long as every batch uses the same RTK-direct
+frame definition and unchanged physical sensor installation.
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+from pathlib import Path
+
+import numpy as np
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--batch-name", action="append", required=True)
+    parser.add_argument("--pairs", action="append", required=True, type=Path)
+    parser.add_argument("--ground-planes", action="append", required=True, type=Path)
+    parser.add_argument("--output-pairs", required=True, type=Path)
+    parser.add_argument("--output-ground-planes", required=True, type=Path)
+    parser.add_argument("--summary", required=True, type=Path)
+    return parser.parse_args()
+
+
+def load_planes(path: Path, batch_name: str) -> list[dict[str, str]]:
+    with path.open(encoding="utf-8-sig", newline="") as stream:
+        rows = list(csv.DictReader(stream))
+    if not rows:
+        raise ValueError(f"no ground planes in {path}")
+    for row in rows:
+        for key in ("nx", "ny", "nz", "d"):
+            if key not in row or row[key] in (None, ""):
+                raise ValueError(f"missing {key} in {path}")
+        row["source_batch"] = batch_name
+    return rows
+
+
+def main() -> int:
+    args = parse_args()
+    count = len(args.batch_name)
+    if count < 2 or len(args.pairs) != count or len(args.ground_planes) != count:
+        raise ValueError("provide the same number of --batch-name, --pairs, and --ground-planes (at least two)")
+
+    pair_parts: list[dict[str, np.ndarray]] = []
+    plane_rows: list[dict[str, str]] = []
+    batch_summaries: list[dict[str, object]] = []
+    for index, (name, pairs_path, planes_path) in enumerate(zip(args.batch_name, args.pairs, args.ground_planes)):
+        with np.load(pairs_path, allow_pickle=False) as source:
+            required = ("A", "B", "meta", "station_times", "rtk_nearest_dt_s")
+            missing = [key for key in required if key not in source]
+            if missing:
+                raise ValueError(f"{pairs_path} missing {missing}")
+            a = np.asarray(source["A"], float)
+            b = np.asarray(source["B"], float)
+            meta = np.asarray(source["meta"], float)
+            times = np.asarray(source["station_times"], float)
+            rtk_dt = np.asarray(source["rtk_nearest_dt_s"], float)
+        if len(a) == 0 or len(a) != len(b) or len(a) != len(meta):
+            raise ValueError(f"invalid A/B/meta sizes in {pairs_path}")
+        pair_parts.append({"A": a, "B": b, "meta": meta, "station_times": times, "rtk_dt": rtk_dt})
+        rows = load_planes(planes_path, name)
+        plane_rows.extend(rows)
+        batch_summaries.append({
+            "name": name,
+            "pairs_path": str(pairs_path.resolve()),
+            "ground_planes_path": str(planes_path.resolve()),
+            "pairs": len(a),
+            "stations": len(times),
+            "ground_planes": len(rows),
+            "pair_offset": sum(item["A"].shape[0] for item in pair_parts[:-1]),
+        })
+
+    output_pairs = args.output_pairs
+    output_pairs.parent.mkdir(parents=True, exist_ok=True)
+    batch_index = np.concatenate([np.full(len(part["A"]), index, np.int32) for index, part in enumerate(pair_parts)])
+    np.savez_compressed(
+        output_pairs,
+        A=np.concatenate([part["A"] for part in pair_parts]),
+        B=np.concatenate([part["B"] for part in pair_parts]),
+        meta=np.concatenate([part["meta"] for part in pair_parts]),
+        station_times=np.concatenate([part["station_times"] for part in pair_parts]),
+        rtk_nearest_dt_s=np.concatenate([part["rtk_dt"] for part in pair_parts]),
+        batch_index=batch_index,
+        batch_names=np.asarray(args.batch_name),
+        backend=np.asarray("independent_batch_consensus"),
+    )
+
+    output_planes = args.output_ground_planes
+    output_planes.parent.mkdir(parents=True, exist_ok=True)
+    fieldnames = ["nx", "ny", "nz", "d", "source_batch"]
+    with output_planes.open("w", encoding="utf-8", newline="") as stream:
+        writer = csv.DictWriter(stream, fieldnames=fieldnames)
+        writer.writeheader()
+        for row in plane_rows:
+            writer.writerow({key: row[key] for key in fieldnames})
+
+    summary = {
+        "schema_version": 1,
+        "convention": "Shared T_RTK_lidar; only within-batch A_ij and B_ij are combined.",
+        "batches": batch_summaries,
+        "total_pairs": int(len(batch_index)),
+        "total_ground_planes": len(plane_rows),
+        "output_pairs": str(output_pairs.resolve()),
+        "output_ground_planes": str(output_planes.resolve()),
+    }
+    args.summary.parent.mkdir(parents=True, exist_ok=True)
+    args.summary.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
+    print(json.dumps(summary, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

code/compare_extrinsics.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+"""Compare two homogeneous-extrinsic JSON files in parameter space and on SE(3)."""
+
+import argparse
+import json
+from pathlib import Path
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--reference", type=Path, required=True)
+    parser.add_argument("--candidate", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    args = parser.parse_args()
+    reference = json.loads(args.reference.read_text(encoding="utf-8-sig"))
+    candidate = json.loads(args.candidate.read_text(encoding="utf-8-sig"))
+    a = np.asarray(reference["matrix_4x4"], dtype=float)
+    b = np.asarray(candidate["matrix_4x4"], dtype=float)
+    delta = np.linalg.inv(a) @ b
+    result = {
+        "convention": "delta = inverse(reference) @ candidate",
+        "reference": str(args.reference.resolve()),
+        "candidate": str(args.candidate.resolve()),
+        "candidate_minus_reference_translation_xyz_m": (b[:3, 3] - a[:3, 3]).tolist(),
+        "candidate_minus_reference_rpy_xyz_deg": (
+            np.asarray(candidate["rotation_rpy_deg_xyz"], float)
+            - np.asarray(reference["rotation_rpy_deg_xyz"], float)
+        ).tolist(),
+        "relative_translation_norm_m": float(np.linalg.norm(delta[:3, 3])),
+        "relative_rotation_deg": float(np.degrees(Rotation.from_matrix(delta[:3, :3]).magnitude())),
+        "relative_matrix_4x4": delta.tolist(),
+    }
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    args.output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
+    print(json.dumps(result, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

code/cross_backend_filter.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+"""Keep common A/B edges on which Open3D and small_gicp agree, without using X."""
+import argparse
+import json
+from pathlib import Path
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def key(meta):
+    return int(meta[0]), int(meta[1])
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--open3d-pairs", required=True)
+    parser.add_argument("--small-pairs", required=True)
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--audit")
+    parser.add_argument("--max-translation", type=float, default=0.05)
+    parser.add_argument("--max-rotation", type=float, default=0.50)
+    parser.add_argument("--min-pairs", type=int, default=25)
+    args = parser.parse_args()
+
+    with np.load(args.open3d_pairs, allow_pickle=False) as source:
+        open_a = np.asarray(source["A"], float)
+        open_b = np.asarray(source["B"], float)
+        open_meta = np.asarray(source["meta"], float)
+        station_times = np.asarray(source["station_times"])
+        rtk_dt = np.asarray(source["rtk_nearest_dt_s"])
+    with np.load(args.small_pairs, allow_pickle=False) as source:
+        small = {key(meta): np.asarray(b, float)
+                 for meta, b in zip(source["meta"], source["B"])}
+
+    keep, audit = [], []
+    for meta, b_open in zip(open_meta, open_b):
+        edge = key(meta)
+        if edge not in small:
+            audit.append({"i": edge[0], "j": edge[1], "accepted": False,
+                          "reason": "not_in_small_gicp_refined"})
+            keep.append(False)
+            continue
+        delta = np.linalg.inv(b_open) @ small[edge]
+        translation = float(np.linalg.norm(delta[:3, 3]))
+        rotation = float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude()))
+        accepted = translation <= args.max_translation and rotation <= args.max_rotation
+        keep.append(accepted)
+        audit.append({
+            "i": edge[0], "j": edge[1],
+            "open3d_small_translation_m": translation,
+            "open3d_small_rotation_deg": rotation,
+            "accepted": accepted,
+            "reason": "" if accepted else "backend_disagreement",
+        })
+    keep = np.asarray(keep, bool)
+    output = Path(args.output)
+    output.parent.mkdir(parents=True, exist_ok=True)
+    np.savez_compressed(
+        output, A=open_a[keep], B=open_b[keep], meta=open_meta[keep],
+        station_times=station_times, rtk_nearest_dt_s=rtk_dt,
+        backend=np.asarray("open3d_gicp_cross_backend_consensus"),
+    )
+    audit_path = Path(args.audit or output.with_suffix(".consensus.json"))
+    audit_path.write_text(json.dumps({
+        "selection_is_X_independent": True,
+        "B_source": "Open3D; small_gicp is used only as an agreement gate",
+        "max_translation_m": args.max_translation,
+        "max_rotation_deg": args.max_rotation,
+        "input_open3d_pairs": len(open_b),
+        "accepted_pairs": int(np.count_nonzero(keep)),
+        "pairs": audit,
+    }, ensure_ascii=False, indent=2), encoding="utf-8")
+    if np.count_nonzero(keep) < args.min_pairs:
+        raise RuntimeError(f"only {np.count_nonzero(keep)} consensus pairs")
+    print(json.dumps({"accepted_pairs": int(np.count_nonzero(keep)),
+                      "output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
+
+
+if __name__ == "__main__":
+    main()

code/finalize_direct_rtk_lidar.py

@@ -0,0 +1,349 @@
+from __future__ import annotations
+
+import argparse
+import json
+import math
+from pathlib import Path
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def load(path: Path) -> dict:
+    return json.loads(path.read_text(encoding="utf-8-sig"))
+
+
+def write(path: Path, document: dict) -> None:
+    path.parent.mkdir(parents=True, exist_ok=True)
+
+    def default(obj):
+        if isinstance(obj, (np.bool_, np.integer)):
+            return obj.item()
+        if isinstance(obj, np.floating):
+            return float(obj)
+        if isinstance(obj, np.ndarray):
+            return obj.tolist()
+        raise TypeError(f"Object of type {type(obj).__name__} is not JSON serializable")
+
+    path.write_text(json.dumps(document, ensure_ascii=False, indent=2, default=default), encoding="utf-8")
+
+
+def inverse(t: np.ndarray) -> np.ndarray:
+    result = np.eye(4)
+    result[:3, :3] = t[:3, :3].T
+    result[:3, 3] = -result[:3, :3] @ t[:3, 3]
+    return result
+
+
+def delta(a: np.ndarray, b: np.ndarray) -> dict:
+    d = inverse(a) @ b
+    return {
+        "translation_m": float(np.linalg.norm(d[:3, 3])),
+        "rotation_deg": float(np.linalg.norm(Rotation.from_matrix(d[:3, :3]).as_rotvec()) * 180.0 / math.pi),
+        "delta_matrix_4x4": d.tolist(),
+    }
+
+
+def wrap180(deg: float) -> float:
+    return (deg + 180.0) % 360.0 - 180.0
+
+
+def yaw_deg_of(transform: np.ndarray) -> float:
+    return float(Rotation.from_matrix(transform[:3, :3]).as_euler("xyz", degrees=True)[2])
+
+
+def mechanical_self_consistency(document: dict) -> dict:
+    """Reject mechanical JSON that mixes incompatible baseline / body definitions."""
+    translation = np.asarray(document["translation_m"], float)
+    yaw = float(document["rotation_rpy_deg_xyz"][2])
+    side = str(document.get("baseline_points", "")).strip().lower()
+    frame_mode = str(document.get("frame_mode", "")).strip().lower()
+    heading_offset = float(document.get("heading_offset_deg", 0.0) or 0.0)
+    vehicle_forward = (
+        frame_mode == "vehicle_forward_heading_offset"
+        or abs(heading_offset) > 1e-6
+    )
+    issues: list[str] = []
+
+    if vehicle_forward:
+        if abs(wrap180(yaw)) > 15.0:
+            issues.append(
+                f"vehicle-forward mechanical initial requires yaw≈0°, got {yaw:g}°"
+            )
+        lever = document.get("vehicle_flu_lever_master_to_lidar_m")
+        if lever is not None:
+            if float(np.linalg.norm(translation - np.asarray(lever, float))) > 0.05:
+                issues.append(
+                    "vehicle-forward translation_m must match vehicle_flu_lever_master_to_lidar_m"
+                )
+        if abs(heading_offset + 90.0) > 1e-6 and abs(heading_offset - 90.0) > 1e-6:
+            issues.append(
+                f"vehicle-forward heading_offset_deg should be ±90 for left/right baseline, got {heading_offset:g}"
+            )
+    elif side in {"vehicle_left", "left"}:
+        if abs(wrap180(yaw - (-90.0))) > 15.0:
+            issues.append(
+                f"baseline_points=vehicle_left requires yaw≈-90°, got {yaw:g}°"
+            )
+        if translation[0] <= 0.0 or translation[1] <= 0.0:
+            issues.append(
+                "baseline_points=vehicle_left expects +X/+Y lever in RTK baseline frame "
+                f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
+            )
+    elif side in {"vehicle_right", "right"}:
+        if abs(wrap180(yaw - 90.0)) > 15.0:
+            issues.append(
+                f"baseline_points=vehicle_right requires yaw≈+90°, got {yaw:g}°"
+            )
+        # Swapped but centerline-symmetric master (vehicle left): +X / -Y in baseline frame.
+        if translation[0] <= 0.0 or translation[1] >= 0.0:
+            issues.append(
+                "baseline_points=vehicle_right (master on vehicle left, baseline to the right) "
+                "expects +X/-Y lever in RTK baseline frame "
+                f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
+            )
+    else:
+        left_xy = translation[0] > 0.05 and translation[1] > 0.05
+        right_xy = translation[0] < -0.05 and translation[1] < -0.05
+        swapped_right_xy = translation[0] > 0.05 and translation[1] < -0.05
+        if left_xy and abs(wrap180(yaw - 90.0)) <= 15.0:
+            issues.append(
+                "mixed baseline definition: +X/+Y translation (left-baseline) combined with yaw≈+90° (right-baseline)"
+            )
+        if right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
+            issues.append(
+                "mixed baseline definition: -X/-Y translation combined with yaw≈-90°"
+            )
+        if swapped_right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
+            issues.append(
+                "mixed baseline definition: +X/-Y translation (swapped-master right-baseline) "
+                "combined with yaw≈-90° (left-baseline)"
+            )
+
+    return {
+        "baseline_points": side or None,
+        "frame_mode": frame_mode or None,
+        "heading_offset_deg": heading_offset,
+        "consistent": not issues,
+        "issues": issues,
+    }
+
+
+def solution_matches_declared_side(solution: np.ndarray, document: dict) -> dict:
+    """Check whether the solved extrinsic agrees with the mechanical baseline side."""
+    side = str(document.get("baseline_points", "")).strip().lower()
+    yaw = yaw_deg_of(solution)
+    t = solution[:3, 3]
+    expected_yaw = float(document["rotation_rpy_deg_xyz"][2])
+    yaw_err = abs(wrap180(yaw - expected_yaw))
+    xy_err = float(np.linalg.norm(t[:2] - np.asarray(document["translation_m"][:2], float)))
+    z_err = float(abs(t[2] - float(document["translation_m"][2])))
+
+    opposite_yaw = abs(wrap180(yaw - expected_yaw) - 180.0) <= 15.0 or abs(
+        wrap180(yaw - expected_yaw) + 180.0
+    ) <= 15.0
+    # Same XY sign as mechanical but yaw flipped ~180° (classic mixed inheritance).
+    same_xy_sign = (t[0] * float(document["translation_m"][0]) > 0.0) and (
+        t[1] * float(document["translation_m"][1]) > 0.0
+    )
+    mixed_inheritance = same_xy_sign and opposite_yaw
+
+    return {
+        "baseline_points": side or None,
+        "solution_yaw_deg": yaw,
+        "expected_yaw_deg": expected_yaw,
+        "yaw_error_deg": yaw_err,
+        "xy_error_m": xy_err,
+        "z_error_m": z_err,
+        "mixed_translation_rotation_inheritance": bool(mixed_inheritance),
+        "near_expected_pose": bool(yaw_err <= 15.0 and xy_err <= 0.25),
+    }
+
+
+def coordinate_contract_audit(raw: dict) -> dict:
+    """Audit mechanical self-consistency and solution agreement.
+
+    A near-180-degree disagreement is not auto-corrected: it normally means
+    that one physical forward-axis / baseline-direction statement is reversed.
+    """
+    path_text = raw.get("solver_initial_extrinsic")
+    if not path_text:
+        return {
+            "status": "mechanical_initial_not_available",
+            "requires_physical_axis_confirmation": False,
+        }
+    path = Path(path_text)
+    if not path.exists():
+        return {
+            "status": "mechanical_initial_file_missing",
+            "requires_physical_axis_confirmation": False,
+            "mechanical_initial_path": str(path),
+        }
+    initial_document = load(path)
+    initial = np.asarray(initial_document["matrix_4x4"], float)
+    solution = np.asarray(raw["matrix_4x4"], float)
+    comparison = delta(initial, solution)
+    near_180 = abs(comparison["rotation_deg"] - 180.0) <= 15.0
+    mech_check = mechanical_self_consistency(initial_document)
+    match = solution_matches_declared_side(solution, initial_document)
+
+    if not mech_check["consistent"]:
+        status = "mechanical_initial_inconsistent"
+    elif match["mixed_translation_rotation_inheritance"] or near_180:
+        status = "near_180_degree_axis_conflict"
+    elif not match["near_expected_pose"]:
+        status = "solution_disagrees_with_mechanical_baseline_side"
+    else:
+        status = "no_near_180_degree_axis_conflict"
+
+    requires = status != "no_near_180_degree_axis_conflict"
+    return {
+        "status": status,
+        "requires_physical_axis_confirmation": requires,
+        "mechanical_initial_path": str(path.resolve()),
+        "mechanical_self_consistency": mech_check,
+        "solution_vs_declared_baseline_side": match,
+        "solution_relative_to_mechanical_initial": comparison,
+        "note": (
+            "No automatic 180-degree correction was applied. Confirm static GNHPR "
+            "left/right vs vehicle heading and Helios +X vs vehicle forward before deployment."
+        ),
+    }
+
+
+def corrected(raw: dict, backend: str, reference_height: float, heading_offset_deg: float) -> dict:
+    baseline_frame = abs(heading_offset_deg) <= 1e-12
+    x_axis = (
+        "horizontal projection of the rawHeading baseline direction reported by the receiver"
+        if baseline_frame else
+        "vehicle forward after applying the configured G90 heading offset"
+    )
+    return {
+        "schema_version": 1,
+        "success": bool(raw["success"]),
+        "convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
+        "equation": "A_RTK_ij X = X B_LiDAR_ij",
+        "frames": {
+            "RTK": {
+                "origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
+                "x_axis": x_axis,
+                "y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
+                "z_axis": "up",
+                "yaw_enu_deg": f"90 - (rawHeadingDeg + {heading_offset_deg:g})",
+                "frame_mode": "baseline_raw_heading" if baseline_frame else "vehicle_forward_heading_offset",
+            },
+            "LiDAR": {
+                "description": "raw Helios sensor frame from points_raw polar decode",
+                "x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
+                "y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
+                "z_axis": "up",
+                "origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ",
+            },
+        },
+        "backend": backend,
+        "measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")),
+        "solver_initial_extrinsic": raw.get("solver_initial_extrinsic"),
+        "body_heading_offset_deg": heading_offset_deg,
+        "body_heading_offset_used": abs(heading_offset_deg) > 1e-12,
+        "body_antenna_lever_xy_used": False,
+        "translation_m": raw["translation_m"],
+        "rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
+        "quaternion_xyzw": raw["quaternion_xyzw"],
+        "coordinate_contract_audit": coordinate_contract_audit(raw),
+        "matrix_4x4": raw["matrix_4x4"],
+        "quality": {
+            "stations": raw["estimation"]["stations"],
+            "pairs": raw["estimation"]["pairs"],
+            "residuals": raw["estimation"]["residuals"],
+            "weighted_jacobian_condition_number": raw["weighted_jacobian_condition_number"],
+            "linearized_one_sigma": raw["linearized_one_sigma"],
+            "bootstrap": raw["bootstrap"],
+        },
+        "z_constraint": {
+            "observable_from_planar_AX_XB": False,
+            "method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
+            "rtk_reference_height_above_ground_m": reference_height,
+            "warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion",
+        },
+        "important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
+    }
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--result-root", type=Path, required=True)
+    parser.add_argument("--reference-height", type=float, required=True)
+    parser.add_argument("--heading-offset-deg", type=float, required=True)
+    args = parser.parse_args()
+
+    def solver_output(directory: str) -> Path:
+        raw = args.result_root / directory / "extrinsic_raw.json"
+        standard = args.result_root / directory / "extrinsic.json"
+        return raw if raw.exists() else standard
+
+    paths = {
+        "open3d_gicp": solver_output("open3d_gicp"),
+        "small_gicp": solver_output("small_gicp"),
+        "consensus": solver_output("consensus"),
+    }
+    docs = {}
+    for backend, path in paths.items():
+        document = corrected(
+            load(path), backend, args.reference_height, args.heading_offset_deg
+        )
+        write(path.with_name("extrinsic_rtk_lidar.json"), document)
+        docs[backend] = document
+
+    open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
+    small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
+    final = dict(docs["consensus"])
+    needs_axis_confirmation = bool(
+        final["coordinate_contract_audit"]["requires_physical_axis_confirmation"]
+    )
+    status = final["coordinate_contract_audit"]["status"]
+    reason_map = {
+        "mechanical_initial_inconsistent": (
+            "Mechanical initial mixes incompatible baseline-left/right translation and yaw; "
+            "fix run/rtk_lidar_mechanical_initial.json before trusting deployment"
+        ),
+        "near_180_degree_axis_conflict": (
+            "Physical axis confirmation is required because the data-driven solution differs "
+            "from the declared mechanical initial by approximately 180 degrees "
+            "(or inherits mixed translation/rotation signs)"
+        ),
+        "solution_disagrees_with_mechanical_baseline_side": (
+            "Solution yaw/XY disagree with the declared mechanical baseline side; "
+            "confirm static GNHPR direction before deployment"
+        ),
+    }
+    final["selection"] = {
+        "recommended": not needs_axis_confirmation,
+        "reason": (
+            reason_map.get(
+                status,
+                "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
+            )
+        ),
+        "open3d_vs_small_gicp": delta(open_t, small_t),
+    }
+
+    write(args.result_root / "final_T_RTK_lidar.json", final)
+    summary = {
+        "final": {
+            "translation_m": final["translation_m"],
+            "rotation_rpy_deg_xyz": final["rotation_rpy_deg_xyz"],
+            "pairs": final["quality"]["pairs"],
+            "translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"],
+            "rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"],
+            "condition_number": final["quality"]["weighted_jacobian_condition_number"],
+            "coordinate_contract_status": final["coordinate_contract_audit"]["status"],
+            "recommended_for_deployment": final["selection"]["recommended"],
+        },
+        "backend_difference": delta(open_t, small_t),
+    }
+    write(args.result_root / "summary.json", summary)
+    print(json.dumps(summary, ensure_ascii=False, indent=2))
+
+
+if __name__ == "__main__":
+    main()

code/refine_pairs.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+"""X-independent second-stage filter for stationary A/B pairs."""
+import argparse
+import json
+from pathlib import Path
+
+import numpy as np
+
+from rigorous_calibration import read_pairs, rotation_angle_deg
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--pairs", required=True)
+    parser.add_argument("--quality-json", required=True)
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--audit")
+    parser.add_argument("--min-pairs", type=int, default=25)
+    parser.add_argument("--min-inlier-ratio", type=float, default=0.70)
+    parser.add_argument("--max-inlier-rmse", type=float, default=0.13)
+    parser.add_argument("--max-rotation-invariant-error", type=float, default=0.75)
+    parser.add_argument("--reverse-translation-tolerance", type=float, default=0.05)
+    parser.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
+    args = parser.parse_args()
+
+    a_array, b_array, meta, _ = read_pairs(args.pairs)
+    quality = json.loads(Path(args.quality_json).read_text(encoding="utf-8-sig"))
+    reports = {(int(item["i"]), int(item["j"])): item for item in quality["pairs"]}
+    keep, audit = [], []
+    for a_ij, b_ij, item_meta in zip(a_array, b_array, meta):
+        key = (int(item_meta[0]), int(item_meta[1]))
+        report = reports[key]
+        heldout = report["heldout_symmetric"]
+        reverse = report["forward_reverse"]
+        invariant = abs(rotation_angle_deg(a_ij[:3, :3]) - rotation_angle_deg(b_ij[:3, :3]))
+        reasons = []
+        if heldout["inlier_ratio"] < args.min_inlier_ratio:
+            reasons.append("overlap_ratio")
+        if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
+            reasons.append("heldout_rmse")
+        if invariant > args.max_rotation_invariant_error:
+            reasons.append("rotation_conjugacy_invariant")
+        if reverse["translation_m"] > args.reverse_translation_tolerance:
+            reasons.append("forward_reverse_translation")
+        if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
+            reasons.append("forward_reverse_rotation")
+        accepted = not reasons
+        keep.append(accepted)
+        audit.append({
+            "i": key[0], "j": key[1], "heldout_inlier_ratio": heldout["inlier_ratio"],
+            "heldout_inlier_rmse_m": heldout["inlier_rmse_m"],
+            "rotation_invariant_error_deg": invariant,
+            "reverse_translation_m": reverse["translation_m"],
+            "reverse_rotation_deg": reverse["rotation_deg"],
+            "accepted": accepted, "rejection_reasons": reasons,
+        })
+    keep = np.asarray(keep, bool)
+    output = Path(args.output)
+    output.parent.mkdir(parents=True, exist_ok=True)
+    with np.load(args.pairs, allow_pickle=False) as source:
+        np.savez_compressed(
+            output, A=a_array[keep], B=b_array[keep], meta=meta[keep],
+            station_times=np.asarray(source["station_times"]),
+            rtk_nearest_dt_s=np.asarray(source["rtk_nearest_dt_s"]),
+            backend=np.asarray(source["backend"]),
+        )
+    audit_path = Path(args.audit or output.with_suffix(".refinement.json"))
+    audit_path.write_text(json.dumps({
+        "selection_is_X_independent": True,
+        "criteria": {
+            "min_inlier_ratio": args.min_inlier_ratio,
+            "max_inlier_rmse_m": args.max_inlier_rmse,
+            "max_rotation_invariant_error_deg": args.max_rotation_invariant_error,
+            "reverse_translation_tolerance_m": args.reverse_translation_tolerance,
+            "reverse_rotation_tolerance_deg": args.reverse_rotation_tolerance,
+        },
+        "input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
+        "pairs": audit,
+    }, ensure_ascii=False, indent=2), encoding="utf-8")
+    if np.count_nonzero(keep) < args.min_pairs:
+        raise RuntimeError(f"only {np.count_nonzero(keep)} refined pairs; need {args.min_pairs}")
+    print(json.dumps({"input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
+                      "output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
+
+
+if __name__ == "__main__":
+    main()

code/rigorous_calibration.py

@@ -0,0 +1,875 @@
+#!/usr/bin/env python3
+"""Rigorous stationary LiDAR / reference-trajectory hand-eye calibration.
+
+Convention: T_A_B maps points from frame B into frame A.
+For this repository the reference frame is the RTK navigation frame.
+X = T_RTK_lidar, A_ij = T_W_Ri^-1 T_W_Rj, B_ij = T_Li_Lj,
+therefore A_ij X = X B_ij. Raw sensor-frame points_raw are used.
+"""
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import math
+import time
+from dataclasses import dataclass
+from pathlib import Path
+
+import numpy as np
+from scipy.optimize import least_squares
+from scipy.spatial import cKDTree
+
+
+def skew(v):
+    x, y, z = v
+    return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
+
+
+def so3_exp(v):
+    angle = float(np.linalg.norm(v))
+    if angle < 1e-12:
+        return np.eye(3) + skew(v)
+    k = skew(np.asarray(v, float) / angle)
+    return np.eye(3) + math.sin(angle) * k + (1.0 - math.cos(angle)) * k @ k
+
+
+def so3_log(rotation):
+    cosine = float(np.clip((np.trace(rotation) - 1.0) / 2.0, -1.0, 1.0))
+    angle = math.acos(cosine)
+    vee = np.array([
+        rotation[2, 1] - rotation[1, 2],
+        rotation[0, 2] - rotation[2, 0],
+        rotation[1, 0] - rotation[0, 1],
+    ])
+    if angle < 1e-9:
+        return vee / 2.0
+    if abs(math.pi - angle) < 1e-5:
+        values, vectors = np.linalg.eigh((rotation + np.eye(3)) / 2.0)
+        return vectors[:, int(np.argmax(values))] * angle
+    return vee * angle / (2.0 * math.sin(angle))
+
+
+def quat_to_rotation(q):
+    x, y, z, w = np.asarray(q, float) / np.linalg.norm(q)
+    return np.array([
+        [1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
+        [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
+        [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)],
+    ])
+
+
+def rotation_to_quat(rotation):
+    from scipy.spatial.transform import Rotation
+    return Rotation.from_matrix(rotation).as_quat()
+
+
+def rpy_deg(rotation):
+    from scipy.spatial.transform import Rotation
+    return Rotation.from_matrix(rotation).as_euler("xyz", degrees=True).tolist()
+
+
+def make_transform(translation, rotation):
+    transform = np.eye(4)
+    transform[:3, :3] = rotation
+    transform[:3, 3] = translation
+    return transform
+
+
+def params_transform(params):
+    return make_transform(params[:3], so3_exp(params[3:]))
+
+
+def transform_params(transform):
+    from scipy.spatial.transform import Rotation
+    transform = np.asarray(transform, float)
+    return np.r_[transform[:3, 3], Rotation.from_matrix(transform[:3, :3]).as_rotvec()]
+
+
+def load_extrinsic_matrix(path):
+    document = json.loads(Path(path).read_text(encoding="utf-8-sig"))
+    transform = np.asarray(document["matrix_4x4"], dtype=float)
+    if transform.shape != (4, 4):
+        raise ValueError("initial extrinsic matrix_4x4 must be 4x4")
+    return transform
+
+
+def inverse_transform(transform):
+    answer = np.eye(4)
+    answer[:3, :3] = transform[:3, :3].T
+    answer[:3, 3] = -answer[:3, :3] @ transform[:3, 3]
+    return answer
+
+
+def transform_points(points, transform):
+    return points @ transform[:3, :3].T + transform[:3, 3]
+
+
+def rotation_angle_deg(rotation):
+    return math.degrees(np.linalg.norm(so3_log(rotation)))
+
+
+@dataclass
+class PoseSeries:
+    time: np.ndarray
+    transforms: np.ndarray
+
+
+def read_poses(path):
+    timestamps, transforms = [], []
+    with Path(path).open(encoding="utf-8-sig", newline="") as stream:
+        reader = csv.DictReader(stream)
+        required = ("time", "x", "y", "z", "qx", "qy", "qz", "qw")
+        missing = [key for key in required if key not in (reader.fieldnames or [])]
+        if missing:
+            raise ValueError(f"{path}: missing pose fields {missing}")
+        for row in reader:
+            timestamps.append(float(row["time"]))
+            translation = np.array([float(row[k]) for k in ("x", "y", "z")])
+            quaternion = np.array([float(row[k]) for k in ("qx", "qy", "qz", "qw")])
+            transforms.append(make_transform(translation, quat_to_rotation(quaternion)))
+    order = np.argsort(timestamps)
+    return PoseSeries(np.asarray(timestamps)[order], np.asarray(transforms)[order])
+
+
+def nearest_pose(series, timestamp):
+    index = int(np.argmin(np.abs(series.time - timestamp)))
+    return series.transforms[index], float(abs(series.time[index] - timestamp))
+
+
+def npz_files(root):
+    files = sorted(Path(root).rglob("*.npz"))
+    if not files:
+        raise FileNotFoundError(f"no NPZ files under {root}")
+    return files
+
+
+def load_npz_xyz(path, min_range=1.0, max_range=50.0):
+    with np.load(path, allow_pickle=False) as data:
+        if "points_raw" not in data:
+            raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden")
+        raw = np.asarray(data["points_raw"], dtype=np.float64)
+        time_key = "lidar_association_time_ns" if "lidar_association_time_ns" in data else "unix_time_ns"
+        timestamp = float(np.ravel(data[time_key])[0]) / 1e9
+        counter = int(np.ravel(data["frame_counter"])[0])
+    distance = raw[:, 0] * 0.001
+    azimuth = np.deg2rad(raw[:, 1])
+    altitude = np.deg2rad(raw[:, 2])
+    valid = (
+        np.isfinite(distance + azimuth + altitude)
+        & (distance >= min_range)
+        & (distance <= max_range)
+    )
+    distance, azimuth, altitude = distance[valid], azimuth[valid], altitude[valid]
+    xyz = np.column_stack((
+        distance * np.cos(altitude) * np.cos(azimuth),
+        distance * np.cos(altitude) * np.sin(azimuth),
+        distance * np.sin(altitude),
+    ))
+    return timestamp, counter, xyz
+
+
+def load_stations(root, min_range, max_range, z_min=None, z_max=None):
+    stations = []
+    for path in npz_files(root):
+        timestamp, counter, xyz = load_npz_xyz(path, min_range, max_range)
+        if z_min is not None:
+            xyz = xyz[(xyz[:, 2] >= z_min) & (xyz[:, 2] <= z_max)]
+        stations.append((timestamp, counter, path, xyz))
+    stations.sort(key=lambda item: item[0])
+    return stations
+
+
+def split_holdout(points, fraction, phase):
+    stride = max(int(round(1.0 / fraction)), 2)
+    index = np.arange(len(points))
+    holdout = ((index + phase) % stride) == 0
+    return points[~holdout], points[holdout]
+
+
+def make_o3d_cloud(points, voxel):
+    import open3d as o3d
+    cloud = o3d.geometry.PointCloud()
+    cloud.points = o3d.utility.Vector3dVector(np.asarray(points, float))
+    return cloud.voxel_down_sample(voxel)
+
+
+def make_global_features(points, voxel):
+    import open3d as o3d
+    cloud = make_o3d_cloud(points, voxel)
+    cloud.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(
+        radius=voxel * 2.5, max_nn=50
+    ))
+    features = o3d.pipelines.registration.compute_fpfh_feature(
+        cloud,
+        o3d.geometry.KDTreeSearchParamHybrid(radius=voxel * 5.0, max_nn=100),
+    )
+    return cloud, features
+
+
+def global_lidar_initialization(target_features, source_features, args, pair_seed):
+    """Estimate source-to-target motion from LiDAR geometry without RTK or an extrinsic."""
+    import open3d as o3d
+    registration = o3d.pipelines.registration
+    target_cloud, target_fpfh = target_features
+    source_cloud, source_fpfh = source_features
+    attempts = []
+    for attempt in range(args.global_ransac_attempts):
+        o3d.utility.random.seed(int(pair_seed + attempt))
+        answer = registration.registration_ransac_based_on_feature_matching(
+            source_cloud,
+            target_cloud,
+            source_fpfh,
+            target_fpfh,
+            True,
+            args.global_correspondence,
+            registration.TransformationEstimationPointToPoint(False),
+            4,
+            [
+                registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
+                registration.CorrespondenceCheckerBasedOnDistance(args.global_correspondence),
+            ],
+            registration.RANSACConvergenceCriteria(
+                args.global_ransac_iterations, args.global_ransac_confidence
+            ),
+        )
+        attempts.append({
+            "transform": np.asarray(answer.transformation, float),
+            "fitness": float(answer.fitness),
+            "inlier_rmse_m": float(answer.inlier_rmse),
+        })
+    best = max(attempts, key=lambda item: (item["fitness"], -item["inlier_rmse_m"]))
+    return {
+        "transform": best["transform"],
+        "method": "LiDAR-only FPFH RANSAC",
+        "fitness": best["fitness"],
+        "inlier_rmse_m": best["inlier_rmse_m"],
+        "attempts": [
+            {key: value for key, value in item.items() if key != "transform"}
+            for item in attempts
+        ],
+    }
+
+
+def align_open3d(target, source, initial, voxels, correspondences, iterations):
+    import open3d as o3d
+    registration = o3d.pipelines.registration
+    estimate = registration.TransformationEstimationForGeneralizedICP()
+    criteria = registration.ICPConvergenceCriteria(max_iteration=iterations)
+    transform, stages = np.asarray(initial, float), []
+    final_target = final_source = final_answer = None
+    started = time.perf_counter()
+    for voxel, correspondence in zip(voxels, correspondences):
+        target_cloud = make_o3d_cloud(target, voxel)
+        source_cloud = make_o3d_cloud(source, voxel)
+        answer = registration.registration_generalized_icp(
+            source_cloud, target_cloud, correspondence, transform, estimate, criteria
+        )
+        transform = np.asarray(answer.transformation, float)
+        stages.append({
+            "voxel_m": voxel,
+            "max_correspondence_m": correspondence,
+            "fitness": float(answer.fitness),
+            "inlier_rmse_m": float(answer.inlier_rmse),
+            "target_points": len(target_cloud.points),
+            "source_points": len(source_cloud.points),
+        })
+        final_target, final_source, final_answer = target_cloud, source_cloud, answer
+    information = registration.get_information_matrix_from_point_clouds(
+        final_source, final_target, correspondences[-1], transform
+    )
+    inliers = int(round(float(final_answer.fitness) * len(final_source.points)))
+    return {
+        "transform": transform,
+        "hessian": np.asarray(information, float),
+        "converged": None,
+        "iterations": None,
+        "num_inliers": inliers,
+        "objective": float(final_answer.inlier_rmse ** 2 * max(inliers, 1)),
+        "elapsed_sec": time.perf_counter() - started,
+        "stages": stages,
+    }
+
+
+def align_small_gicp(target, source, initial, voxels, correspondences, iterations, threads):
+    import small_gicp
+    transform, stages, result = np.asarray(initial, float), [], None
+    started = time.perf_counter()
+    for voxel, correspondence in zip(voxels, correspondences):
+        result = small_gicp.align(
+            np.ascontiguousarray(target),
+            np.ascontiguousarray(source),
+            transform,
+            registration_type="GICP",
+            downsampling_resolution=voxel,
+            max_correspondence_distance=correspondence,
+            num_threads=threads,
+            max_iterations=iterations,
+            rotation_epsilon=math.radians(0.005),
+            translation_epsilon=0.0005,
+            verbose=False,
+        )
+        transform = np.asarray(result.T_target_source, float)
+        stages.append({
+            "voxel_m": voxel,
+            "max_correspondence_m": correspondence,
+            "converged": bool(result.converged),
+            "iterations": int(result.iterations),
+            "num_inliers": int(result.num_inliers),
+            "objective": float(result.error),
+        })
+    return {
+        "transform": transform,
+        "hessian": np.asarray(result.H, float),
+        "converged": bool(result.converged),
+        "iterations": int(result.iterations),
+        "num_inliers": int(result.num_inliers),
+        "objective": float(result.error),
+        "elapsed_sec": time.perf_counter() - started,
+        "stages": stages,
+    }
+
+
+def align_backend(backend, target, source, initial, args):
+    if backend == "open3d":
+        return align_open3d(
+            target, source, initial, args.voxels, args.correspondences, args.iterations
+        )
+    return align_small_gicp(
+        target, source, initial, args.voxels, args.correspondences,
+        args.iterations, args.threads
+    )
+
+
+def symmetric_heldout_metrics(target_fit, target_holdout, source_fit, source_holdout,
+                              transform, threshold):
+    transformed_source_fit = transform_points(source_fit, transform)
+    transformed_source_holdout = transform_points(source_holdout, transform)
+    forward = cKDTree(target_fit).query(transformed_source_holdout, workers=-1)[0]
+    reverse = cKDTree(transformed_source_fit).query(target_holdout, workers=-1)[0]
+    distances = np.concatenate((forward, reverse))
+    inliers = distances[distances <= threshold]
+    return {
+        "evaluated": int(len(distances)),
+        "inliers": int(len(inliers)),
+        "inlier_ratio": float(len(inliers) / max(len(distances), 1)),
+        "inlier_rmse_m": float(np.sqrt(np.mean(inliers**2))) if len(inliers) else None,
+        "median_m": float(np.median(distances)),
+        "p90_m": float(np.quantile(distances, 0.90)),
+        "p95_m": float(np.quantile(distances, 0.95)),
+    }
+
+
+def hessian_metrics(hessian, characteristic_length=10.0):
+    hessian = 0.5 * (np.asarray(hessian, float) + np.asarray(hessian, float).T)
+    scale = np.diag([1.0 / characteristic_length] * 3 + [1.0] * 3)
+    scaled = scale.T @ hessian @ scale
+    values, vectors = np.linalg.eigh(scaled)
+    largest = max(float(np.max(np.abs(values))), np.finfo(float).eps)
+    positive = values[values > largest * 1e-9]
+    condition = float(positive[-1] / positive[0]) if len(positive) else float("inf")
+    return {
+        "native_order": ["rx_rad", "ry_rad", "rz_rad", "tx_m", "ty_m", "tz_m"],
+        "scaled_eigenvalues": values.tolist(),
+        "effective_rank": int(len(positive)),
+        "scaled_condition_number": condition,
+        "weakest_scaled_direction": vectors[:, int(np.argmin(values))].tolist(),
+    }
+
+
+def transform_difference(reference, candidate):
+    delta = inverse_transform(reference) @ candidate
+    return {
+        "translation_m": float(np.linalg.norm(delta[:3, 3])),
+        "rotation_deg": rotation_angle_deg(delta[:3, :3]),
+    }
+
+
+def loop_metrics(transforms):
+    loops = []
+    for (i, j), b_ij in transforms.items():
+        for (j2, k), b_jk in transforms.items():
+            if j2 != j or (i, k) not in transforms:
+                continue
+            loops.append(transform_difference(transforms[(i, k)], b_ij @ b_jk))
+    if not loops:
+        return {"count": 0}
+    translation = np.array([item["translation_m"] for item in loops])
+    rotation = np.array([item["rotation_deg"] for item in loops])
+    return {
+        "count": len(loops),
+        "translation_rms_m": float(np.sqrt(np.mean(translation**2))),
+        "translation_p95_m": float(np.quantile(translation, 0.95)),
+        "rotation_rms_deg": float(np.sqrt(np.mean(rotation**2))),
+        "rotation_p95_deg": float(np.quantile(rotation, 0.95)),
+    }
+
+
+def cmd_ground(args):
+    stations = load_stations(args.frames, args.min_range, args.max_range)
+    rows = []
+    for timestamp, counter, _, xyz in stations:
+        roi = xyz[(xyz[:, 2] >= args.z_min) & (xyz[:, 2] <= args.z_max)]
+        if len(roi) < args.min_inliers:
+            continue
+        cloud = make_o3d_cloud(roi, args.voxel)
+        plane, indexes = cloud.segment_plane(
+            args.distance_threshold, 3, args.ransac_iterations
+        )
+        normal = np.asarray(plane[:3], float)
+        norm = np.linalg.norm(normal)
+        normal, distance = normal / norm, float(plane[3] / norm)
+        if distance < 0:
+            normal, distance = -normal, -distance
+        points = np.asarray(cloud.points)[indexes]
+        rms = float(np.sqrt(np.mean((points @ normal + distance) ** 2)))
+        if len(indexes) >= args.min_inliers and rms <= args.max_rms:
+            rows.append([timestamp, *normal, distance, len(indexes), rms, counter])
+    output = Path(args.output)
+    output.parent.mkdir(parents=True, exist_ok=True)
+    with output.open("w", encoding="utf-8", newline="") as stream:
+        writer = csv.writer(stream)
+        writer.writerow(["time", "nx", "ny", "nz", "d", "inliers", "rms_m", "frame_counter"])
+        writer.writerows(rows)
+    print(json.dumps({"planes": len(rows), "output": str(output.resolve())}, indent=2))
+
+
+def cmd_pairs(args):
+    if len(args.voxels) != len(args.correspondences):
+        raise ValueError("--voxels and --correspondences must have equal lengths")
+    stations = load_stations(
+        args.frames, args.min_range, args.max_range, args.z_min, args.z_max
+    )
+    reference = read_poses(args.reference_poses)
+    if len(stations) < args.min_stations:
+        raise ValueError(f"need at least {args.min_stations} stations, got {len(stations)}")
+    reference_poses, reference_dt = [], []
+    for timestamp, _, _, xyz in stations:
+        if len(xyz) < args.min_roi_points:
+            raise ValueError(f"station at {timestamp} has only {len(xyz)} ROI points")
+        pose, dt = nearest_pose(reference, timestamp + args.time_offset)
+        reference_poses.append(pose)
+        reference_dt.append(dt)
+    reference_poses = np.asarray(reference_poses)
+    split = [split_holdout(station[3], args.holdout_fraction, i)
+             for i, station in enumerate(stations)]
+    global_features = [make_global_features(points[0], args.global_voxel)
+                       for points in split]
+    rng = np.random.default_rng(args.seed)
+    accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
+    accepted_transforms = {}
+    for i in range(len(stations)):
+        for j in range(i + args.min_gap, min(len(stations), i + args.max_gap + 1)):
+            a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j]
+            translation = float(np.linalg.norm(a_ij[:2, 3]))
+            rotation = rotation_angle_deg(a_ij[:3, :3])
+            if args.max_reference_translation is not None and translation > args.max_reference_translation:
+                continue
+            if translation < args.min_translation and rotation < args.min_rotation:
+                continue
+            global_initial = global_lidar_initialization(
+                global_features[i], global_features[j], args,
+                args.seed + i * 1009 + j * 9176,
+            )
+            initial_b = global_initial["transform"]
+            target_fit, target_holdout = split[i]
+            source_fit, source_holdout = split[j]
+            forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
+            heldout = symmetric_heldout_metrics(
+                target_fit, target_holdout, source_fit, source_holdout,
+                forward["transform"], args.evaluation_distance
+            )
+            hessian = hessian_metrics(forward["hessian"])
+            reverse_answer = align_backend(
+                args.backend, source_fit, target_fit, inverse_transform(initial_b), args
+            )
+            reverse = transform_difference(
+                forward["transform"], inverse_transform(reverse_answer["transform"])
+            )
+            multistart = []
+            for _ in range(args.multistart):
+                perturb = np.r_[
+                    rng.normal(0.0, args.multistart_translation_sigma, 3),
+                    np.deg2rad(rng.normal(0.0, args.multistart_rotation_sigma, 3)),
+                ]
+                candidate = align_backend(
+                    args.backend, target_fit, source_fit,
+                    params_transform(perturb) @ initial_b, args
+                )
+                multistart.append(transform_difference(forward["transform"], candidate["transform"]))
+            stable = [
+                item["translation_m"] <= args.multistart_translation_tolerance
+                and item["rotation_deg"] <= args.multistart_rotation_tolerance
+                for item in multistart
+            ]
+            success_rate = float(np.mean(stable)) if stable else 1.0
+            reasons = []
+            if forward["converged"] is False:
+                reasons.append("backend_not_converged")
+            if heldout["inlier_ratio"] < args.min_inlier_ratio:
+                reasons.append("heldout_inlier_ratio")
+            if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
+                reasons.append("heldout_inlier_rmse")
+            if hessian["effective_rank"] < 6:
+                reasons.append("hessian_rank")
+            if hessian["scaled_condition_number"] > args.max_hessian_condition:
+                reasons.append("hessian_condition")
+            if reverse["translation_m"] > args.reverse_translation_tolerance:
+                reasons.append("forward_reverse_translation")
+            if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
+                reasons.append("forward_reverse_rotation")
+            if success_rate < args.min_multistart_success:
+                reasons.append("multistart_instability")
+            accepted = not reasons
+            report = {
+                "i": i, "j": j,
+                "lidar_time_i": stations[i][0], "lidar_time_j": stations[j][0],
+                "frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1],
+                "rtk_translation_m": translation, "rtk_rotation_deg": rotation,
+                "nearest_rtk_dt_i_s": reference_dt[i], "nearest_rtk_dt_j_s": reference_dt[j],
+                "initial_B_source": global_initial["method"],
+                "global_lidar_initialization": {
+                    key: value for key, value in global_initial.items() if key != "transform"
+                },
+                "B_ij_4x4": forward["transform"].tolist(),
+                "backend": args.backend, "backend_converged": forward["converged"],
+                "backend_iterations": forward["iterations"],
+                "backend_num_inliers": forward["num_inliers"],
+                "backend_objective": forward["objective"],
+                "backend_elapsed_sec": forward["elapsed_sec"],
+                "multiscale_stages": forward["stages"],
+                "heldout_symmetric": heldout, "hessian": hessian,
+                "forward_reverse": reverse,
+                "multistart": {"runs": len(multistart), "success_rate": success_rate,
+                               "deltas": multistart},
+                "accepted": accepted, "rejection_reasons": reasons,
+            }
+            reports.append(report)
+            print(f"{args.backend} {i:02d}->{j:02d} rmse={heldout['inlier_rmse_m']} "
+                  f"ratio={heldout['inlier_ratio']:.3f} accepted={accepted}")
+            if accepted:
+                accepted_a.append(a_ij)
+                accepted_b.append(forward["transform"])
+                accepted_meta.append([i, j, stations[i][0], stations[j][0]])
+                accepted_transforms[(i, j)] = forward["transform"]
+    output = Path(args.output)
+    output.parent.mkdir(parents=True, exist_ok=True)
+    np.savez_compressed(
+        output, A=np.asarray(accepted_a), B=np.asarray(accepted_b),
+        meta=np.asarray(accepted_meta),
+        station_times=np.asarray([item[0] for item in stations]),
+        rtk_nearest_dt_s=np.asarray(reference_dt), backend=np.asarray(args.backend),
+    )
+    quality = {
+        "schema_version": 2,
+        "backend": args.backend,
+        "transform_convention": "B_ij=T_Li_Lj maps station j points into station i",
+        "raw_point_field": "points_raw",
+        "registration_initial_extrinsic": None,
+        "selection_is_X_independent": True,
+        "B_estimation_is_RTK_independent": True,
+        "candidate_pair_selection_uses_reference_motion": True,
+        "initialization_warning": None,
+        "stations": len(stations), "candidate_pairs": len(reports),
+        "accepted_pairs": len(accepted_a),
+        "parameters": vars(args),
+        "accepted_loop_closure": loop_metrics(accepted_transforms),
+        "pairs": reports,
+    }
+    quality["parameters"].pop("func", None)
+    quality_path = Path(args.quality_json or output.with_suffix(".quality.json"))
+    quality_path.write_text(json.dumps(quality, ensure_ascii=False, indent=2), encoding="utf-8")
+    csv_path = Path(args.quality_csv or output.with_suffix(".quality.csv"))
+    with csv_path.open("w", encoding="utf-8", newline="") as stream:
+        fields = ["i", "j", "rtk_translation_m", "rtk_rotation_deg",
+                  "heldout_inlier_ratio", "heldout_inlier_rmse_m",
+                  "hessian_rank", "hessian_condition", "reverse_translation_m",
+                  "reverse_rotation_deg", "multistart_success_rate", "accepted",
+                  "rejection_reasons"]
+        writer = csv.DictWriter(stream, fieldnames=fields)
+        writer.writeheader()
+        for item in reports:
+            writer.writerow({
+                "i": item["i"], "j": item["j"],
+                "rtk_translation_m": item["rtk_translation_m"],
+                "rtk_rotation_deg": item["rtk_rotation_deg"],
+                "heldout_inlier_ratio": item["heldout_symmetric"]["inlier_ratio"],
+                "heldout_inlier_rmse_m": item["heldout_symmetric"]["inlier_rmse_m"],
+                "hessian_rank": item["hessian"]["effective_rank"],
+                "hessian_condition": item["hessian"]["scaled_condition_number"],
+                "reverse_translation_m": item["forward_reverse"]["translation_m"],
+                "reverse_rotation_deg": item["forward_reverse"]["rotation_deg"],
+                "multistart_success_rate": item["multistart"]["success_rate"],
+                "accepted": item["accepted"],
+                "rejection_reasons": ";".join(item["rejection_reasons"]),
+            })
+    if len(accepted_a) < args.min_pairs:
+        raise RuntimeError(f"only {len(accepted_a)} accepted pairs; need {args.min_pairs}")
+    print(json.dumps({
+        "backend": args.backend, "stations": len(stations),
+        "candidate_pairs": len(reports), "accepted_pairs": len(accepted_a),
+        "output": str(output.resolve()), "quality_json": str(quality_path.resolve()),
+        "loop": quality["accepted_loop_closure"],
+    }, indent=2))
+
+
+def read_planes(path):
+    planes = []
+    with Path(path).open(encoding="utf-8-sig", newline="") as stream:
+        for row in csv.DictReader(stream):
+            normal = np.array([float(row[k]) for k in ("nx", "ny", "nz")])
+            norm = np.linalg.norm(normal)
+            normal, distance = normal / norm, float(row["d"]) / norm
+            if distance < 0:
+                normal, distance = -normal, -distance
+            planes.append([*normal, distance])
+    return np.asarray(planes)
+
+
+def read_pairs(path):
+    with np.load(path, allow_pickle=False) as data:
+        return (np.asarray(data["A"], float), np.asarray(data["B"], float),
+                np.asarray(data["meta"], float), len(data["station_times"]))
+
+
+def calibration_residual(params, a_array, b_array, planes, args):
+    x = params_transform(params)
+    values = []
+    for a_ij, b_ij in zip(a_array, b_array):
+        error = inverse_transform(a_ij @ x) @ x @ b_ij
+        values.extend((error[:3, 3] / args.translation_sigma).tolist())
+        values.extend((so3_log(error[:3, :3]) / math.radians(args.rotation_sigma)).tolist())
+    body_up = np.array([0.0, 0.0, 1.0])
+    for plane in planes:
+        normal_body = x[:3, :3] @ plane[:3]
+        values.extend((np.cross(normal_body, body_up) / args.plane_normal_sigma).tolist())
+        body_distance = plane[3] - float(normal_body @ x[:3, 3])
+        values.append((body_distance - args.reference_height) / args.plane_height_sigma)
+    return np.asarray(values)
+
+
+def pair_metrics(a_array, b_array, x):
+    translation, rotation, rows = [], [], []
+    for index, (a_ij, b_ij) in enumerate(zip(a_array, b_array)):
+        predicted = inverse_transform(x) @ a_ij @ x
+        delta = inverse_transform(b_ij) @ predicted
+        t = float(np.linalg.norm(delta[:3, 3]))
+        r = rotation_angle_deg(delta[:3, :3])
+        translation.append(t); rotation.append(r)
+        rows.append({"pair_index": index, "translation_m": t, "rotation_deg": r})
+    translation, rotation = np.asarray(translation), np.asarray(rotation)
+    def stats(values):
+        return {
+            "rms": float(np.sqrt(np.mean(values**2))),
+            "median": float(np.median(values)),
+            "p90": float(np.quantile(values, 0.90)),
+            "p95": float(np.quantile(values, 0.95)),
+            "max": float(np.max(values)),
+        }
+    return {"pairs": len(rows), "translation_m": stats(translation),
+            "rotation_deg": stats(rotation), "per_pair": rows}
+
+
+def solve_extrinsic(a_array, b_array, planes, args):
+    rng = np.random.default_rng(args.seed)
+    center = (transform_params(load_extrinsic_matrix(args.initial_extrinsic))
+              if args.initial_extrinsic else np.zeros(6))
+    starts = [center]
+    for _ in range(args.solver_multistart - 1):
+        starts.append(center + np.r_[
+            rng.normal(0.0, args.start_translation_sigma, 3),
+            np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
+        ])
+    candidates = []
+    lower = np.r_[[-5.0] * 3, [-math.pi] * 3]
+    upper = np.r_[[5.0] * 3, [math.pi] * 3]
+    for start in starts:
+        answer = least_squares(
+            calibration_residual, np.clip(start, lower, upper),
+            args=(a_array, b_array, planes, args),
+            bounds=(lower, upper), loss="huber", f_scale=1.5,
+            x_scale="jac", max_nfev=args.max_nfev,
+        )
+        candidates.append(answer)
+    best = min(candidates, key=lambda item: item.cost)
+    return best, candidates
+
+
+def cmd_calibrate(args):
+    a_array, b_array, meta, stations = read_pairs(args.pairs)
+    planes = read_planes(args.ground_planes)
+    best, candidates = solve_extrinsic(a_array, b_array, planes, args)
+    x = params_transform(best.x)
+    residual = calibration_residual(best.x, a_array, b_array, planes, args)
+    absolute = np.abs(residual)
+    weights = np.ones_like(residual)
+    weights[absolute > 1.5] = 1.5 / absolute[absolute > 1.5]
+    weighted_jacobian = best.jac * np.sqrt(weights)[:, None]
+    singular = np.linalg.svd(weighted_jacobian, compute_uv=False)
+    condition = float(singular[0] / max(singular[-1], 1e-15))
+    dof = max(len(residual) - 6, 1)
+    covariance = np.linalg.pinv(weighted_jacobian.T @ weighted_jacobian) * float(
+        np.sum(weights * residual**2) / dof
+    )
+    sigma = np.sqrt(np.maximum(np.diag(covariance), 0.0))
+    candidate_summary = []
+    for item in candidates:
+        candidate_x = params_transform(item.x)
+        candidate_summary.append({
+            "cost": float(item.cost), "success": bool(item.success),
+            **transform_difference(x, candidate_x),
+        })
+    bootstrap = []
+    rng = np.random.default_rng(args.seed + 1)
+    for _ in range(args.bootstrap):
+        indexes = rng.integers(0, len(a_array), len(a_array))
+        answer = least_squares(
+            calibration_residual, best.x,
+            args=(a_array[indexes], b_array[indexes], planes, args),
+            loss="huber", f_scale=1.5, x_scale="jac", max_nfev=args.max_nfev,
+        )
+        bootstrap.append(np.r_[answer.x[:3], rpy_deg(so3_exp(answer.x[3:]))])
+    bootstrap = np.asarray(bootstrap)
+    result = {
+        "schema_version": 2,
+        "success": bool(best.success),
+        "message": best.message,
+        "convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame",
+        "equation": "A_ij X = X B_ij",
+        "measured_extrinsic_used_as_initial": bool(args.initial_extrinsic),
+        "solver_initial_extrinsic": (
+            str(Path(args.initial_extrinsic).resolve()) if args.initial_extrinsic else None
+        ),
+        "translation_m": x[:3, 3].tolist(),
+        "rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
+        "quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
+        "matrix_4x4": x.tolist(),
+        "estimation": {"stations": stations, "pairs": len(a_array),
+                       "residuals": pair_metrics(a_array, b_array, x)},
+        "ground": {
+            "planes": len(planes),
+            "reference_origin_height_above_ground_m": args.reference_height,
+            "formula": "d_lidar - (R_X n_lidar)^T t_X - reference_height",
+        },
+        "linearized_one_sigma": {
+            "translation_m": sigma[:3].tolist(),
+            "rotation_deg": np.rad2deg(sigma[3:]).tolist(),
+            "warning": "conditional local estimate; bootstrap is the primary stability check",
+        },
+        "weighted_jacobian_condition_number": condition,
+        "solver_multistart": {
+            "runs": len(candidates), "candidates_relative_to_best": candidate_summary,
+        },
+        "bootstrap": {
+            "runs": len(bootstrap),
+            "order": ["x_m", "y_m", "z_m", "roll_deg", "pitch_deg", "yaw_deg"],
+            "std": np.std(bootstrap, axis=0, ddof=1).tolist() if len(bootstrap) > 1 else None,
+            "p025": np.quantile(bootstrap, 0.025, axis=0).tolist() if len(bootstrap) else None,
+            "p975": np.quantile(bootstrap, 0.975, axis=0).tolist() if len(bootstrap) else None,
+        },
+    }
+    output = Path(args.output)
+    output.parent.mkdir(parents=True, exist_ok=True)
+    output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
+    print(json.dumps(result, ensure_ascii=False, indent=2))
+
+
+def cmd_validate(args):
+    result = json.loads(Path(args.extrinsic).read_text(encoding="utf-8-sig"))
+    x = np.asarray(result["matrix_4x4"], float)
+    a_array, b_array, meta, stations = read_pairs(args.pairs)
+    metrics = pair_metrics(a_array, b_array, x)
+    for row, pair_meta in zip(metrics["per_pair"], meta):
+        row.update({"i": int(pair_meta[0]), "j": int(pair_meta[1])})
+    report = {
+        "role": "auxiliary check only; first-batch RTK is sparse",
+        "blind_with_respect_to_X": True,
+        "note": "No AX residual was used to select these pairs",
+        "stations": stations, "metrics": metrics,
+    }
+    output = Path(args.output)
+    output.parent.mkdir(parents=True, exist_ok=True)
+    output.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
+    print(json.dumps(report, ensure_ascii=False, indent=2))
+
+
+def build_parser():
+    parser = argparse.ArgumentParser(description=__doc__)
+    commands = parser.add_subparsers(dest="command", required=True)
+    ground = commands.add_parser("ground")
+    ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True)
+    ground.add_argument("--min-range", type=float, default=1.0); ground.add_argument("--max-range", type=float, default=30.0)
+    # Default ROI for ~2 m roof LiDAR (Z-up). Override for other mounting heights.
+    ground.add_argument("--z-min", type=float, default=-2.5); ground.add_argument("--z-max", type=float, default=-1.5)
+    ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025)
+    ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500)
+    ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground)
+
+    pairs = commands.add_parser("pairs")
+    pairs.add_argument("--backend", choices=["open3d", "small_gicp"], required=True)
+    pairs.add_argument("--frames", required=True)
+    pairs.add_argument("--reference-poses", "--body", dest="reference_poses", required=True)
+    pairs.add_argument("--output", required=True); pairs.add_argument("--quality-json"); pairs.add_argument("--quality-csv")
+    pairs.add_argument("--time-offset", type=float, default=0.0)
+    pairs.add_argument("--min-stations", type=int, default=30); pairs.add_argument("--min-pairs", type=int, default=25)
+    pairs.add_argument("--min-gap", type=int, default=1); pairs.add_argument("--max-gap", type=int, default=5)
+    pairs.add_argument("--max-reference-translation", type=float)
+    pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0)
+    pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0)
+    pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0)
+    pairs.add_argument("--min-roi-points", type=int, default=1000)
+    pairs.add_argument("--global-voxel", type=float, default=0.50)
+    pairs.add_argument("--global-correspondence", type=float, default=1.25)
+    pairs.add_argument("--global-ransac-attempts", type=int, default=3)
+    pairs.add_argument("--global-ransac-iterations", type=int, default=100000)
+    pairs.add_argument("--global-ransac-confidence", type=float, default=0.999)
+    pairs.add_argument("--holdout-fraction", type=float, default=0.20)
+    pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08])
+    pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25])
+    pairs.add_argument("--iterations", type=int, default=60); pairs.add_argument("--threads", type=int, default=8)
+    pairs.add_argument("--evaluation-distance", type=float, default=0.25)
+    pairs.add_argument("--min-inlier-ratio", type=float, default=0.35); pairs.add_argument("--max-inlier-rmse", type=float, default=0.16)
+    pairs.add_argument("--max-hessian-condition", type=float, default=1e8)
+    pairs.add_argument("--reverse-translation-tolerance", type=float, default=0.08)
+    pairs.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
+    pairs.add_argument("--multistart", type=int, default=2)
+    pairs.add_argument("--multistart-translation-sigma", type=float, default=0.30)
+    pairs.add_argument("--multistart-rotation-sigma", type=float, default=3.0)
+    pairs.add_argument("--multistart-translation-tolerance", type=float, default=0.08)
+    pairs.add_argument("--multistart-rotation-tolerance", type=float, default=0.50)
+    pairs.add_argument("--min-multistart-success", type=float, default=0.50)
+    pairs.add_argument("--seed", type=int, default=20260721); pairs.set_defaults(func=cmd_pairs)
+
+    calibrate = commands.add_parser("calibrate")
+    calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True)
+    calibrate.add_argument("--output", required=True)
+    calibrate.add_argument("--initial-extrinsic")
+    calibrate.add_argument("--translation-sigma", type=float, default=0.05)
+    calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
+    calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
+    calibrate.add_argument("--plane-height-sigma", type=float, default=0.03)
+    calibrate.add_argument(
+        "--reference-height", "--body-height", dest="reference_height",
+        type=float, required=True,
+        help="measured RTK/GGA reference-origin height above the local ground in metres",
+    )
+    calibrate.add_argument("--solver-multistart", type=int, default=12)
+    calibrate.add_argument("--start-translation-sigma", type=float, default=1.0)
+    calibrate.add_argument("--start-rotation-sigma", type=float, default=20.0)
+    calibrate.add_argument("--bootstrap", type=int, default=100)
+    calibrate.add_argument("--max-nfev", type=int, default=1000)
+    calibrate.add_argument("--seed", type=int, default=20260721); calibrate.set_defaults(func=cmd_calibrate)
+
+    validate = commands.add_parser("validate")
+    validate.add_argument("--pairs", required=True); validate.add_argument("--extrinsic", required=True)
+    validate.add_argument("--output", required=True); validate.set_defaults(func=cmd_validate)
+    return parser
+
+
+def main():
+    args = build_parser().parse_args()
+    args.func(args)
+
+
+if __name__ == "__main__":
+    main()

code/visualize_pair_3d.py

@@ -0,0 +1,294 @@
+#!/usr/bin/env python3
+"""Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion.
+
+Modes (keyboard), aligned with the LiDAR–IMU viewer:
+  1          raw source (no transform)
+  2          RTK prediction with X=I  (B_pred = A)
+  3          LiDAR registration B     (reference)
+  4          calibrated prediction    B_pred = X^{-1} A X
+  5          optional body-left RPY test (only if --left-rpy-deg is non-zero)
+  N / ]      next motion pair
+  P / [      previous motion pair
+  Q / Esc    exit
+
+Blue = target station i; orange = source station j after the selected transform.
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+from rigorous_calibration import (
+    inverse_transform,
+    load_stations,
+    rotation_angle_deg,
+    rpy_deg,
+)
+
+
+COLORS = {
+    "target": [0.10, 0.65, 1.00],
+    "source": [1.00, 0.35, 0.05],
+}
+
+MODE_NAMES = (
+    "1 raw",
+    "2 RTK initial (X=I)",
+    "3 GICP B",
+    "4 calibrated X^-1 A X",
+)
+
+
+def cloud(o3d, points, color, voxel):
+    item = o3d.geometry.PointCloud()
+    item.points = o3d.utility.Vector3dVector(points)
+    if voxel > 0:
+        item = item.voxel_down_sample(voxel)
+    item.paint_uniform_color(color)
+    return item
+
+
+def set_cloud_points(cloud_geom, points, color, voxel, o3d) -> None:
+    tmp = cloud(o3d, points, color, voxel)
+    cloud_geom.points = tmp.points
+    cloud_geom.colors = tmp.colors
+
+
+def delta_components(reference, candidate):
+    """Components of reference^-1*candidate, plus coordinate-invariant norms."""
+    delta = inverse_transform(reference) @ candidate
+    translation = np.asarray(delta[:3, 3], float)
+    return {
+        "translation_xyz_cm": (translation * 100.0).tolist(),
+        "translation_norm_cm": float(np.linalg.norm(translation) * 100.0),
+        "rotation_rpy_deg_xyz": rpy_deg(delta[:3, :3]),
+        "rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
+    }
+
+
+def body_left_rpy(x, rpy_correction_deg):
+    correction = np.eye(4)
+    correction[:3, :3] = Rotation.from_euler(
+        "xyz", np.asarray(rpy_correction_deg, float), degrees=True
+    ).as_matrix()
+    return correction @ x
+
+
+def print_delta(name, reference, candidate):
+    item = delta_components(reference, candidate)
+    tx, ty, tz = item["translation_xyz_cm"]
+    roll, pitch, yaw = item["rotation_rpy_deg_xyz"]
+    print(
+        f"{name}: B^-1*motion  "
+        f"t_xyz=[{tx:+.3f}, {ty:+.3f}, {tz:+.3f}] cm  "
+        f"rpy=[{roll:+.3f}, {pitch:+.3f}, {yaw:+.3f}] deg  "
+        f"|t|={item['translation_norm_cm']:.3f} cm  "
+        f"|R|={item['rotation_angle_deg']:.4f} deg"
+    )
+    return item
+
+
+def transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg):
+    b_calibrated = inverse_transform(x) @ a_ij @ x
+    transforms = {
+        MODE_NAMES[0]: np.eye(4),
+        MODE_NAMES[1]: a_ij.copy(),
+        MODE_NAMES[2]: b_gicp.copy(),
+        MODE_NAMES[3]: b_calibrated,
+    }
+    correction = np.asarray(left_rpy_deg, float)
+    test_name = None
+    if np.any(np.abs(correction) > 0.0):
+        x_test = body_left_rpy(x, correction)
+        test_name = f"5 test body-left RPY {correction.tolist()} deg"
+        transforms[test_name] = inverse_transform(x_test) @ a_ij @ x_test
+    return transforms, test_name
+
+
+def resolve_pair(stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy_deg):
+    a_ij = np.asarray(pairs_a[pair_index], float)
+    b_gicp = np.asarray(pairs_b[pair_index], float)
+    i, j = np.asarray(pairs_meta[pair_index, :2], int)
+    transforms, test_name = transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg)
+    label = (
+        f"pair {pair_index + 1}/{len(pairs_a)}  "
+        f"station {i} <- {j}  "
+        f"rotB={rotation_angle_deg(b_gicp[:3, :3]):.2f} deg  "
+        f"|tB|={float(np.linalg.norm(b_gicp[:3, 3])):.3f} m"
+    )
+    return i, j, a_ij, b_gicp, transforms, test_name, label
+
+
+def print_pair_header(label, b_gicp, transforms, test_name, a_ij):
+    print("-" * 72)
+    print(label)
+    print("blue=target i | orange=source j")
+    mode_hint = "1-4"
+    if test_name is not None:
+        mode_hint = "1-5"
+    print(f"{mode_hint}: overlay mode | N/]: next pair | P/[: prev pair | Q/Esc: exit")
+    print(
+        "IMPORTANT: delta xyz/rpy are components of B^-1*(X^-1*A*X), expressed "
+        "in station-j LiDAR coordinates; screen-left/right depends on the 3D camera view."
+    )
+    baseline = print_delta("mode4 minus mode3", b_gicp, transforms[MODE_NAMES[3]])
+    roll, pitch, yaw = np.abs(baseline["rotation_rpy_deg_xyz"])
+    if max(roll, pitch) > max(0.10, 2.0 * yaw):
+        print("note: roll/pitch dominate yaw on this pair.")
+    tx, ty, tz = np.abs(baseline["translation_xyz_cm"])
+    if tz > max(tx, ty):
+        print("note: largest translation component is Z for this pair.")
+    body_up = np.array([0.0, 0.0, 1.0])
+    if np.linalg.norm(a_ij[:3, :3] @ body_up - body_up) < 1e-8:
+        print(
+            "observability: this A preserves the body Z axis, so body-left X.z "
+            "translation is unobservable from this pair; use ground/external height constraints."
+        )
+    if test_name is not None:
+        print_delta("mode5 minus mode3", b_gicp, transforms[test_name])
+
+
+def main():
+    import open3d as o3d
+
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--frames", required=True)
+    parser.add_argument("--pairs", required=True)
+    parser.add_argument("--extrinsic", required=True)
+    parser.add_argument("--pair-index", type=int, default=0, help="Starting motion-pair index")
+    parser.add_argument("--voxel", type=float, default=0.10)
+    parser.add_argument(
+        "--left-rpy-deg",
+        nargs=3,
+        type=float,
+        default=[0.0, 0.0, 0.0],
+        metavar=("ROLL", "PITCH", "YAW"),
+        help="optional body-frame left correction applied as DeltaR_body * X",
+    )
+    args = parser.parse_args()
+
+    stations = load_stations(args.frames, 1.0, 60.0)
+    with np.load(args.pairs, allow_pickle=False) as data:
+        if len(stations) != len(data["station_times"]):
+            raise ValueError(
+                f"frames contain {len(stations)} stations but pair file records "
+                f"{len(data['station_times'])}"
+            )
+        pairs_a = np.asarray(data["A"], float)
+        pairs_b = np.asarray(data["B"], float)
+        pairs_meta = np.asarray(data["meta"])
+        n_pairs = len(pairs_a)
+        if not 0 <= args.pair_index < n_pairs:
+            raise IndexError(f"pair-index {args.pair_index} outside [0,{n_pairs - 1}]")
+
+    with open(args.extrinsic, encoding="utf-8-sig") as stream:
+        result = json.load(stream)
+    x = np.asarray(result["matrix_4x4"], float)
+    left_rpy = np.asarray(args.left_rpy_deg, float)
+
+    pair_index = int(args.pair_index)
+    i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair(
+        stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy
+    )
+
+    viewer = o3d.visualization.VisualizerWithKeyCallback()
+    viewer.create_window("RTK–LiDAR registration inspection", 1400, 900)
+    target_cloud = cloud(o3d, stations[i][3], COLORS["target"], args.voxel)
+    source_cloud = cloud(o3d, stations[j][3], COLORS["source"], args.voxel)
+    viewer.add_geometry(target_cloud)
+    viewer.add_geometry(source_cloud)
+    viewer.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0))
+    viewer.get_render_option().background_color = np.array([0.02, 0.02, 0.02])
+    viewer.get_render_option().point_size = 2.0
+
+    state = {
+        "pair_index": pair_index,
+        "mode_name": MODE_NAMES[3],
+        "current": np.eye(4),
+        "transforms": transforms,
+        "b_gicp": b_gicp,
+        "a_ij": a_ij,
+        "test_name": test_name,
+    }
+
+    def apply_mode(vis, mode_name: str, *, announce: bool = True) -> None:
+        desired = state["transforms"][mode_name]
+        source_cloud.transform(desired @ inverse_transform(state["current"]))
+        state["current"] = desired
+        state["mode_name"] = mode_name
+        vis.update_geometry(source_cloud)
+        if announce:
+            if mode_name == MODE_NAMES[2]:
+                print(f"{mode_name}: registration reference; delta = 0")
+            else:
+                print_delta(mode_name + " minus mode3", state["b_gicp"], desired)
+
+    def load_pair(vis, new_index: int) -> None:
+        new_index = int(new_index) % n_pairs
+        i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair(
+            stations, pairs_a, pairs_b, pairs_meta, new_index, x, left_rpy
+        )
+        state["pair_index"] = new_index
+        state["transforms"] = transforms
+        state["b_gicp"] = b_gicp
+        state["a_ij"] = a_ij
+        state["test_name"] = test_name
+        state["current"] = np.eye(4)
+        set_cloud_points(target_cloud, stations[i][3], COLORS["target"], args.voxel, o3d)
+        set_cloud_points(source_cloud, stations[j][3], COLORS["source"], args.voxel, o3d)
+        vis.update_geometry(target_cloud)
+        vis.update_geometry(source_cloud)
+        # Keep current mode if still available (mode 5 may vanish when correction is zero).
+        mode_name = state["mode_name"]
+        if mode_name not in transforms:
+            mode_name = MODE_NAMES[3]
+        print_pair_header(label, b_gicp, transforms, test_name, a_ij)
+        apply_mode(vis, mode_name, announce=True)
+
+    def make_mode_cb(mode_name: str):
+        def callback(vis):
+            if mode_name not in state["transforms"]:
+                print(f"{mode_name}: unavailable (pass non-zero --left-rpy-deg for mode 5)")
+                return False
+            apply_mode(vis, mode_name, announce=True)
+            return False
+
+        return callback
+
+    def next_pair(vis):
+        load_pair(vis, state["pair_index"] + 1)
+        return False
+
+    def prev_pair(vis):
+        load_pair(vis, state["pair_index"] - 1)
+        return False
+
+    print_pair_header(label, b_gicp, transforms, test_name, a_ij)
+    for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4")), MODE_NAMES):
+        viewer.register_key_callback(key, make_mode_cb(name))
+
+    def mode5(vis):
+        name = state["test_name"]
+        if name is None or name not in state["transforms"]:
+            print("5: unavailable (pass non-zero --left-rpy-deg for mode 5)")
+            return False
+        apply_mode(vis, name, announce=True)
+        return False
+
+    viewer.register_key_callback(ord("5"), mode5)
+    for key in (ord("N"), ord("n"), ord("]")):
+        viewer.register_key_callback(key, next_pair)
+    for key in (ord("P"), ord("p"), ord("[")):
+        viewer.register_key_callback(key, prev_pair)
+
+    apply_mode(viewer, MODE_NAMES[3], announce=False)
+    viewer.run()
+    viewer.destroy_window()
+
+
+if __name__ == "__main__":
+    main()

config/s2_old_smoke.yaml

@@ -1,51 +0,0 @@
-schema_version: 1
-
-vehicle:
-  vehicle_id: "S2_old_validation"
-  body_frame:
-    name: "rear_axle_center"
-    axes: "X forward, Y left, Z up"
-    unit: m
-
-installation:
-  installation_id: "S2_old_smoke"
-  installed_at: "unknown"
-  notes: "Smoke-test on old S2 host-time data only. Not for delivery."
-
-sensors:
-  imu:
-    model: "HI13_old_S2"
-    raw_frame:
-      axes: "as exported HI91"
-      driver_axis_remapped: false
-    mount_in_body:
-      translation_m: null
-      rotation_quaternion_xyzw: null
-
-  lidar:
-    model: "frontlidar"
-    points_field: points
-    raw_frame:
-      axes: "Cartesian metres from points_raw spherical conversion"
-      driver_axis_remapped: false
-    mount_in_body:
-      translation_m: null
-      rotation_quaternion_xyzw: null
-
-  rtk:
-    frame_definition: ""
-    reference_point: ""
-    existing_T_RTK_LIDAR_file: ""
-
-time:
-  imu_timestamp_source: "host_utc_receive_of_serial_chunk"
-  lidar_timestamp_source: "unix_time_ns_from_dlog_export"
-  lidar_frame_time_definition: "frame midpoint approx from unix_time_ns"
-
-initialization:
-  translation_prior:
-    enabled: false
-    sigma_m: null
-  rotation_prior:
-    enabled: false
-    sigma_deg: null

config/vehicle_hi13_h32_20260808.yaml

@@ -1,86 +0,0 @@
-schema_version: 1
-
-vehicle:
-  vehicle_id: "outdoor_usable_20260808"
-  body_frame:
-    name: "base_link"
-    # 车体约定：后轮轴中心在地面投影为原点附近参考；X 前 / Y 左 / Z 上
-    # translation_m 的 Z 使用「离地高度」；后轮轴中心离地 294 mm
-    axes: "X forward, Y left, Z up"
-    unit: m
-    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. 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 in NPZ assumed body-aligned (X forward).
-
-sensors:
-  imu:
-    model: "HI13R4"
-    raw_frame:
-      # HI13R4 用户手册 2.4：右-前-上 (RFU)
-      axes: "X right, Y forward, Z up (RFU)"
-      driver_axis_remapped: false
-    mount_in_body:
-      # 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 X/Y in body (+X forward); Z = CAD axle-height + 294mm AGL + HI13R4 RFU"
-
-  lidar:
-    model: "RSLidarH32"
-    points_field: points
-    raw_frame:
-      axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
-      driver_axis_remapped: false
-    mount_in_body:
-      # X/Y：后轮轴中心 → 雷达
-      # Z离地 = CAD dZ 1.637499879 + 后轮轴离地 0.294 + 相位中心偏移 0.0635
-      translation_m: [2.522276859, 0.000020526, 1.994999879]
-      rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
-      rotation_quaternion_xyzw: null
-      source: "CAD X/Y in body (+X forward); Z AGL = CAD dZ 1.637499879 + axle AGL 0.294 + phase-center offset 0.0635; attitude = body"
-
-  rtk:
-    frame_definition: ""
-    reference_point: ""
-    existing_T_RTK_LIDAR_file: ""
-
-time:
-  imu_timestamp_source: "hi13_device_timestamp_ms_seconds"
-  lidar_timestamp_source: "h32_msop_device_timestamp_seconds"
-  lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint"
-  host_bridge: "MSOP HostReceiveUtcTicks + IMU receive_utc_ticks"
-
-# Derived prior for p_IMU = R_IMU_lidar * p_lidar + t_IMU_lidar
-# t_body = t_lidar_body - t_imu_body
-# t_IMU_lidar = R_IMU_body * t_body,  R_IMU_lidar = R_IMU_body * R_body_lidar
-derived_T_IMU_lidar_prior:
-  R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
-  t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
-  t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
-  notes: >
-    Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd).
-    Relative Z = 1.994999879 - 1.1865 = 0.808499879 m.
-
-initialization:
-  translation_prior:
-    enabled: true
-    sigma_m: [0.05, 0.05, 0.05]
-    t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
-  rotation_prior:
-    enabled: true
-    sigma_deg: 15.0
-    R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]

config/vehicle_installation.template.yaml

@@ -1,51 +0,0 @@
-schema_version: 1
-
-vehicle:
-  vehicle_id: "example_vehicle"
-  body_frame:
-    name: "base_link"
-    axes: "X forward, Y left, Z up"
-    unit: m
-
-installation:
-  installation_id: "example_install"
-  installed_at: "unknown"
-  notes: "V1 example config. Mount translations may stay null."
-
-sensors:
-  imu:
-    model: "unknown_imu"
-    raw_frame:
-      axes: "declare after black-box tests: e.g. out_x=forward, out_y=left, out_z=up"
-      driver_axis_remapped: false
-    mount_in_body:
-      translation_m: null
-      rotation_quaternion_xyzw: null
-
-  lidar:
-    model: "unknown_lidar"
-    points_field: points
-    raw_frame:
-      axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
-      driver_axis_remapped: false
-    mount_in_body:
-      translation_m: null
-      rotation_quaternion_xyzw: null
-
-  rtk:
-    frame_definition: ""
-    reference_point: ""
-    existing_T_RTK_LIDAR_file: ""
-
-time:
-  imu_timestamp_source: "device_or_file_clock_seconds"
-  lidar_timestamp_source: "frame_midpoint_seconds"
-  lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint"
-
-initialization:
-  translation_prior:
-    enabled: false
-    sigma_m: null
-  rotation_prior:
-    enabled: false
-    sigma_deg: null

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

@@ -1,209 +0,0 @@
-# 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/IMU-LiDAR标定.md

@@ -1,195 +0,0 @@
-# LiDAR–IMU 外参标定说明
-
-**用途：** 方法约定与实现状态（深入阅读）。日常使用请先看根目录 [`README.md`](../README.md)。
-
-本文说明标定目标、约定、流水线与实现状态。代码在 `imu_lidar/`。场地采集见 [`标定流程与采集清单.md`](标定流程与采集清单.md)。
-
----
-
-## 1. 目标与约定
-
-估计安装外参：
-
-```text
-p_IMU = T_IMU_lidar · p_lidar
-```
-
-约定：`T_A_B` 表示把 **B 系点**变换到 **A 系**。
-
-相对运动手眼模型：
-
-```text
-A_ij ≈ IMU 在 [t_i, t_j] 的相对运动（预积分）
-B_ij ≈ LiDAR 在同时间段的相对运动（关键帧配准）
-A X ≈ X B
-X = T_IMU_lidar
-```
-
-旋转子问题（常规主交付）：
-
-```text
-R_A R_X = R_X R_B
-→ R_IMU_lidar
-```
-
-若已有完整六自由度外参，且另有 `T_RTK_lidar`，可链式得到：
-
-```text
-T_lidar_IMU = inverse(T_IMU_lidar)
-T_RTK_IMU   = T_RTK_lidar @ T_lidar_IMU
-```
-
-**不要**把 IMU 加速度二次积分成轨迹，再当作绝对位姿去做完整六自由度手眼。
-
----
-
-## 2. 交付分层
-
-| 层级 | 交付 | 数据最低要求 |
-|---|---|---|
-| 第一步 | 旋转 + 时间偏置 δt | **设备时间戳**；静止 + 低速转弯 /「8」字；结构化场景 |
-| 第二步 | 上一步 + 可观的水平平移 | 更多转弯半径与加减速 |
-| 第三步 | 完整六自由度（含可靠竖直分量） | 缓坡俯仰激励，或外测垂直杆臂先验 |
-
-可观性不过关 → 只交旋转，不强交“假精确”六自由度。  
-仅主机接收时间、或残差未过门控的结果 → **不要当作正式安装参数**。
-
----
-
-## 3. 总体原则
-
-1. **时间同步优先于外参**：δt 未对齐时，旋转与平移都不可信。使用设备时间戳。
-2. **先求旋转，再求平移**：旋转通常更稳；平面运动下竖直平移常常不可观。
-3. **用连续运动标定**：停车多站、再靠站间长积分，不适合作为纯 IMU 外参主流程。
-4. **可观性门控**：过不了就降级交付。
-5. **残差小 ≠ 标定对**：需叠点云 / 跨会话等独立验证。
-6. **首轮建议低速**：先保证配准与时间对齐；点云去畸变可选。
-7. **安装参数不写死在源码**：轴向与时间语义进 YAML；机械尺寸可作检查，不能伪装成已标定平移。
-8. **注意耦合**：时间相关峰很弱时，δt、航向角与陀螺零偏可能互相补偿，结果不可当真。
-
----
-
-## 4. 流水线（现行实现）
-
-```text
-vehicle_config
-  → timestamp_audit
-  → imu_audit（静止零偏等）
-  → time_offset：粗估 δt
-  → keyframes / [可选] deskew
-  → motion_pairs：完整 IMU 预积分 + 配准 B
-  → rotation_handeye：加权求解 R
-  → （有候选 R 时）精修 δt，必要时交替重建运动对
-  → joint_optimizer：精修旋转与陀螺零偏；可观且 full_se3 时再估平移等
-  → finalize
-```
-
-| 模块 | 文件 | 职责 |
-|---|---|---|
-| 配置 | `vehicle_config.py` | 读安装 YAML |
-| IO | `imu_io.py` / `lidar_io.py` | 标准 CSV / 帧目录 |
-| 质检 | `timestamp_audit.py` / `imu_audit.py` | 时间域、静止零偏 |
-| δt | `time_offset.py` | 粗估 + 有符号精修 |
-| 运动 | `keyframes.py` / `registration.py` / `lidar_deskew.py` | 关键帧、配准、可选去畸变 |
-| IMU 侧 | `imu_preintegration.py` / `motion_pairs.py` | 预积分与运动对 |
-| 求解 | `rotation_handeye.py` / `joint_optimizer.py` / `observability.py` | 手眼、联合精修、门控 |
-| 编排 | `pipeline.py` / `cli.py` / `finalize.py` | 入口与落盘 |
-
-输入中间格式见 [V1_数据格式.md](V1_数据格式.md)。改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
-
-### 运行示例
-
-```powershell
-python -m pip install -e ".[dev]"
-python -m pip install -e ".[open3d]"   # 可选
-
-python -m imu_lidar.cli plan --mode rotation_only
-
-python -m imu_lidar.cli run `
-  --vehicle-config config\vehicle_installation.template.yaml `
-  --imu path\to\imu.csv `
-  --lidar path\to\lidar_session `
-  --output path\to\output `
-  --mode rotation_only `
-  --time-offset-search-s 2.0
-```
-
-合成自检：`python tools\generate_synthetic_session.py` 后跑 CLI，再 `python -m pytest -q`。
-
-### 结果状态
-
-| status | 含义 |
-|---|---|
-| `rotation_only_accepted` | 旋转过门，可交旋转与报告中的 δt |
-| `full_se3_accepted` | 可观且联合优化通过，可交完整 `T` |
-| `full_se3_rejected_due_to_observability` | 旋转可用，平移未接受 |
-| `blocked` | 质检 / δt / 手眼残差等硬门失败，**不交付** |
-
-`rotation_only` 模式下：不要把未标定的机械平移拼进 4×4 伪装成完整标定。
-
----
-
-## 5. 采集要点
-
-单趟动态会话：
-
-```text
-[静止 20–30 s] → [低速激励 3–8 min] → [再静止 10–20 s]
-```
-
-优先激励：
-
-- 低速「8」字 / 左右圆（旋转主激励）
-- 直线加减速（有助于时间对齐与水平平移）
-- 缓坡（仅完整六自由度需要）：约 3°～8°，连续长度优先 ≥ 20～30 m
-
-硬条件：IMU / LiDAR **设备时间戳**；结构化场景；标定全程安装不得改动。
-
-更完整的现场清单见 [标定流程与采集清单.md](标定流程与采集清单.md)。
-
----
-
-## 6. 数学上允许与禁止
-
-禁止：加速度二次积分当真值轨迹；把只有旋转的相对运动硬补成完整六自由度；用最终外参反向筛边掩盖失败。
-
-允许：预积分旋转手眼求旋转；在可观时用预积分残差联合估计平移，并精修零偏等辅助量。
-
----
-
-## 7. 车辆配置
-
-使用 `--vehicle-config vehicle_installation.yaml`。  
-模板中安装平移/旋转保持未标定状态，直至实测确认；禁止写死某车杆臂冒充结果。
-
----
-
-## 8. 实现状态（当前阶段）
-
-本仓库**仅此一条**标定路径：连续运动关键帧 + IMU 预积分。对外总览与「合成 / 旧车 / 合格数据预期」见根目录 [`README.md`](../README.md) §0。
-
-| 项 | 状态 |
-|---|---|
-| 连续运动关键帧标定流水线 | 已实现（现行唯一路径） |
-| 加权预积分 / 加权手眼 | 已实现 |
-| 旋转预积分因子 + 有符号 δt 精修 | 已实现 |
-| 完整预积分（含速度/位移增量）与可观时的平移优化 | 已实现 |
-| 可观性门控 / `rotation_only` | 已实现 |
-| 合成数据 pytest / 一键复现 | 已实现（证明链路与已知 yaw/δt，不证明实车精度） |
-| 旧车主机时间烟测（S2） | 线下可跑；预期 `blocked`，不当交付 |
-| 设备时间新车数据正式验收 | **待做** |
-| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`（N300 rscap + H32 dlog/MSOP → V1；dlog 用 DIFOP 通道角） |
-
-细项见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
-
----
-
-## 9. 相关文档
-
-| 内容 | 路径 | 备注 |
-|---|---|---|
-| 对外总览（优先） | 根目录 [`README.md`](../README.md) | 日常入口 |
-| 采集清单 | [`标定流程与采集清单.md`](标定流程与采集清单.md) | 现场 |
-| 数据格式 / 导出 | [`V1_数据格式.md`](V1_数据格式.md) | 中间格式 |
-| 文件职责说明 | [`imu_lidar/文件职责说明.md`](../imu_lidar/文件职责说明.md) | 改代码 |
-| 改动史 | [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md) | 改代码 |

docs/V1_数据格式.md

@@ -1,109 +0,0 @@
-# V1 标准中间数据格式
-
-**用途：** 标定程序读入的 CSV/NPZ 约定，以及新车原始数据如何导出。总览见根目录 [README](../README.md)。
-
-## 从原始数据导出
-
-**推荐（新 H32 + HI13）：** HI13 `.rscap` + 雷达 Medulla dlog / recovered zip（raw MSOP + DIFOP）。
-
-```powershell
-python tools\export_rscap_to_v1.py `
-  --imu-rscap path\to\hi13r4-imu.rscap `
-  --imu-kind hi13 `
-  --lidar-dlog path\to\session_or_dlog_or_recovered.zip `
-  --host-start 2026-08-08T17:40:05 `
-  --host-end 2026-08-08T17:45:15 `
-  --out path\to\session_v1 `
-  --frame-stride 5 `
-  --require-difop
-```
-
-`--lidar-dlog` 可为：标准 `dobject/`+`dobject_recording/` 目录，或 recovered zip（`indices.log` + `data.bin`）。  
-`--imu-kind`：`hi13` / `n300` / `auto`（默认按文件名推断）。  
-`--host-start/end`：按本地墙钟切窗（仅裁剪；标定主轴仍是设备时间）。  
-默认 DObject：`frontlidar-msop-raw`、`frontlidar-difop-raw`。
-
-**兼容旧 MSOP-only `.rscap`：**
-
-```powershell
-python tools\export_rscap_to_v1.py `
-  --imu-rscap path\to\n300.rscap `
-  --lidar-rscap path\to\h32_msop.rscap `
-  --out path\to\session_v1 `
-  --frame-stride 1
-```
-
-产出：`imu.csv`、`lidar/`（含 `frames_index.csv`）、`export_summary.json`。  
-标定主轴仍是**设备时间**；同时写出**主机 UTC 接收时间**，用于把雷达帧桥接到 IMU 设备钟（禁止把两边设备时间第一帧强行重合）。
-
-## IMU
-
-文件：`imu.csv` 或 `imu.npz`
-
-### CSV
-
-```text
-t,gx,gy,gz,ax,ay,az,t_host_utc_s,receive_utc_ticks
-0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81,1754646005.123,6389...
-...
-```
-
-| 列 | 含义 | 单位 |
-|---|---|---|
-| t | IMU 设备时钟时间 | s |
-| gx,gy,gz | 角速度 | rad/s |
-| ax,ay,az | 比力/加速度 | m/s² |
-| t_host_utc_s | 主机 UTC 接收时间（Unix） | s |
-| receive_utc_ticks | 同上，.NET UTC ticks | — |
-
-### NPZ
-
-数组：`t (N,)`, `gyro (N,3)`, `acc (N,3)`，含义同上。
-
-> IMU 与 LiDAR 的时间原点可以不同。流水线会估计常值偏置：`t_imu = t_lidar + delta_t`。
-
-## LiDAR
-
-目录结构：
-
-```text
-lidar_session/
-├── frames_index.csv
-└── frames/
-    ├── frame_00000.npz
-    ├── frame_00001.npz
-    └── ...
-```
-
-### frames_index.csv
-
-```text
-frame_id,filename,t_start,t_end,host_receive_utc_ticks,t_host_utc_s,host_receive_utc_end_ticks,t_host_utc_end_s
-0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22
-```
-
-| 列 | 含义 |
-|---|---|
-| t_start / t_end | H32 MSOP **设备时间**（秒） |
-| t_host_utc_s / t_host_utc_end_s | 帧首/末包 **HostReceiveUtcTicks** → Unix 秒 |
-
-也兼容旧列名 `file`。对齐脚本用主机 UTC 把 `t_*` 重写到 IMU 设备钟后再跑标定。
-
-### 每帧 NPZ
-
-- `points`: `float64/float32`，形状 `(N, 3)`，LiDAR 直角坐标系，单位米
-
-## 时间不同步能不能用？
-
-可以，前提是：
-
-1. 两边都覆盖同一段**有角速度激励**的物理运动（尤其是转弯）；
-2. 偏置近似为**常数**（短会话）；
-3. `--time-offset-search-s` 足够覆盖可能的偏移（默认 ±1 s，可加大）。
-
-若两段数据完全不是同一趟行驶，或只有静止 IMU、没有重叠运动，则无法估 δt，标定会被 `blocked`。
-
-## 最小可用会话
-
-- IMU：建议含静止段 + 运动段，采样率稳定
-- LiDAR：建议 ≥ 20 帧，场景有墙/柱等结构，含转弯

docs/标定流程与采集清单.md

@@ -1,157 +0,0 @@
-# 纯 LiDAR–IMU：标定流程与采集清单
-
-**用途：** 现场怎么采合格数据（看完根目录 [README](../README.md) 后再看本文即可）。  
-算法命令与结果判读以 README 为准；改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
-
-仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐采集与流程要点。  
-目标外参：`p_IMU = T_IMU_lidar · p_lidar`（`T_A_B` 表示把 B 系点变到 A 系）。
-
----
-
-## 1. 原则（先读）
-
-1. **时间对齐优先**：时钟偏置未对准时，旋转与平移都不可信；正式数据用**设备时间戳**。
-2. **先旋转，再平移**：旋转通常更稳；平面低速时竖直方向平移常常不可观。
-3. **用连续运动**：停车多站适合 RTK 手眼，不适合作为纯 IMU 外参主流程。
-4. **可观才交平移**：激励不够就只交旋转，不强交“假精确”六自由度。
-5. **残差小 ≠ 标定对**：需叠点云、跨会话等独立验证。
-
-相对运动模型：
-
-```text
-A ≈ IMU 预积分相对运动（关键帧区间）
-B ≈ 雷达关键帧配准相对运动
-R_A · R_X ≈ R_X · R_B     → 先求旋转
-完整模式且可观时再求平移 t
-```
-
----
-
-## 2. 端到端流程（与现行代码一致）
-
-```text
-确认轴向 / 单位 / 时间语义
-  → 现场采集（首尾静止 + 低速多转弯；建议 ≥2 段独立会话）
-  → 导出标准中间格式（imu.csv + lidar 会话目录）
-  → 质检（时间 / IMU）不通过则停
-  → 粗估时间偏置 δt
-  → 关键帧 → 配准得 B；完整 IMU 预积分得 A（旋转/速度/位移增量）
-  → 加权旋转手眼得 R
-  → 用 R 精修 δt，必要时重新组对再解 R（可交替数轮）
-  → 联合精修 R 与常值陀螺零偏
-  → full_se3 且可观：再估重力、关键帧速度、时变零偏与平移 t
-  → 写出 T / δt / summary → 叠点云 / 跨会话验证后交付
-```
-
-
-| 步骤  | 现行模块                                                                 | 说明                                      |
-| --- | -------------------------------------------------------------------- | --------------------------------------- |
-| 质检  | `timestamp_audit` / `imu_audit`                                      | 含静止段陀螺零偏初值                              |
-| 时间  | `time_offset`                                                        | 模长相关粗估 + 有符号三轴精修                        |
-| 运动对 | `keyframes` / `registration` / `imu_preintegration` / `motion_pairs` | 预积分始终算满；手眼先用旋转                          |
-| 旋转  | `rotation_handeye`                                                   | 加权手眼                                    |
-| 精修  | `joint_optimizer` / `observability`                                  | `rotation_only` 到旋转为止；`full_se3` 可观才碰平移 |
-
-
-点云去畸变（`lidar_deskew`）可选；低速首轮可不依赖。
-
----
-
-
-
-## 3. 采集设计
-
-
-
-### 3.1 单趟会话结构
-
-```text
-静止 20–30 s → 连续运动 3–8 min → 再静止 10–20 s
-```
-
-运动优先：低速「8」字 / 左右圆；再补加减速直线。  
-要可靠竖直方向外参时，另加缓坡（约 3°～8°，有效长度优先 ≥20～30 m），或改用外测垂直尺寸先验。
-
-### 3.2 会话安排
-
-
-| 会话    | 作用                                 |
-| ----- | ---------------------------------- |
-| A     | 主标定                                |
-| B     | 独立验证（**同一场地**换一条不完全相同的路线即可，不参与求外参） |
-| C（可选） | 不同速度/路线，测稳定性                       |
-
-
-安装全程不得改动。多会话不要跨会话拼运动对。
-
-### 3.3 录制字段（原始，勿先做姿态融合）
-
-- IMU：设备时间、陀螺、加速度（建议同时留主机接收时间便于排查）
-- LiDAR：每帧起止时间（最好有包级/逐点时间）、原始点云
-
-中间格式见 `[V1_数据格式.md](V1_数据格式.md)`。
-
----
-
-
-
-## 4. 现场 Checklist
-
-**出发前**
-
-- [ ] 安装固定；草图/卷尺粗测仅作参考，不当真值
-- [ ] 单位与轴向确认；设备时间可写盘
-- [ ] 结构化路线（墙/杆/路缘），避开空旷无特征区
-- [ ] 存储与供电充足
-
-**录制中**
-
-- [ ] 首尾静止；中间有明显左右转与加减速
-- [ ] 不改安装、不切换时间源
-- [ ] 记录会话 ID、天气、异常（急刹、掉包等）
-
-**当场快查**
-
-- [ ] IMU 静止段平稳，转弯时角速度明显
-- [ ] 点云帧数/点数正常，无明显大面积丢帧
-- [ ] 雷达与 IMU 时间覆盖同一时段
-
-**回实验室**
-
-- [ ] 已导出中间格式并通过质检
-- [ ] 本次目标：`rotation_only` 还是尝试 `full_se3`
-- [ ] 若要竖直方向：确认真有俯仰/高度激励，否则降级交付
-
----
-
-
-
-## 5. 精度预期
-
-
-| 量          | 较现实范围     | 说明               |
-| ---------- | --------- | ---------------- |
-| 旋转         | 约 0.5°–2° | 最精确部分            |
-| 水平平移       | 数厘米～十几厘米  | 强依赖配准、激励与同步      |
-| 竖直 / 部分杠杆臂 | 往往更差甚至不可观 | 无高度激励时无法得出“精确 z” |
-
-
-如果条件有限，优先交付：**可靠旋转 + δt + 可观的平移分量（若有）+ 明确限制说明**。
-
-满足条件后的模式与成功标志见根目录 [`README.md`](../README.md) §0。
-
----
-
-
-
-## 6. 交付物建议
-
-程序默认写出：`T_IMU_lidar.json`、`time_offset.json`、`summary.json`。  
-完整报告目录还可补充：可观性结论、跨会话对比、运动对质量表、限制说明（尤其竖直方向与时间同步方式）。
-
-```text
-静止 + 激励录制（多会话）
-  → 质检 → 估 δt → 关键帧 A/B → 先解 R
-  → 精修 δt 与 R → 可观则求 t → 验证后交付
-```
-

docs/问题清单.md

@@ -1,209 +0,0 @@
-# 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

@@ -1,270 +0,0 @@
-# `imu_lidar` 改动记录
-
-本文件专门记录 `imu_lidar` 目录内的实现改动。  
-每条包含：**时间戳**、**改动内容**（以「原本怎么做 → 改成怎么做」书写）。
-
----
-
-## 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 只解 N300 FDILink；dlog 只认标准 `*.dorec`；无法按图上时段切窗。
-- **改成**：
-  - 新增 `tools/rscap_v2/hi13_imu.py`（HI91：g→m/s²、°/s→rad/s、设备 ms）。
-  - `h32_dlog` 支持 recovered zip（`indices.log` + `data.bin`），ZIP_STORED 成员按文件绝对 offset 直读。
-  - `export_rscap_to_v1.py`：`--imu-kind hi13|n300|auto`、多段 `--imu-rscap`、`--host-start/end` 切窗。
-  - 辅助脚本 `tools/export_usable_20260808_windows.py` 导出优先运动段。
-- **未推送**（按用户要求本地改完即可）。
-
----
-
-## 2026-08-05 09:00 (UTC+8)
-
-### 导出：支持 H32 DLogCapture（MSOP+DIFOP）→ V1
-
-- **原本**：导出只读 H32 MSOP V2 `.rscap`，无 DIFOP，垂直角用默认 −16°…+16°。
-- **改成**：
-  - 新增 `tools/h32_dlog/`（dobject 索引、MSOP/DIFOP payload V1、DIFOP 通道角）。
-  - `export_rscap_to_v1.py` 增加 `--lidar-dlog`（与 `--lidar-rscap` 二选一）；默认用 DIFOP 角做 XYZ。
-  - `h32_msop.iter_h32_frames_from_packets` 供 dlog/rscap 共用拼帧。
-  - 单测 `tests/test_h32_dlog_export.py`；文档改为推荐 dlog 导出命令。
-- **标定核心**（`imu_lidar/` 读 V1）未改。
-
----
-
-## 2026-08-03 17:30 (UTC+8)
-
-### 文档：精简对外阅读路径
-
-- **原本**：README 很长，多份文档职责不清，外人易觉复杂。
-- **改成**：README 改为短入口 +「对外三份就够」；采集清单 / 方法说明 / 测试说明 / 职责说明文首标明用途；细节仍保留在原文件。
-
----
-
-## 2026-08-03 11:40 (UTC+8)
-
-### 新增：N300/H32 `.rscap` → V1 中间格式导出
-
-- **原本**：标定只接受 CSV/NPZ；新车原始录制需手工转换，无仓库内导出器。
-- **改成**：
-  - 新增 `tools/rscap_v2/`（V2 读取、N300 IMU、H32 MSOP 拼帧）与 `tools/export_rscap_to_v1.py`。
-  - 导出写入设备时间轴的 `imu.csv` + `lidar/`；支持 `--frame-stride` / `--max-points-per-frame`。
-  - 单元测试 `tests/test_export_rscap_helpers.py`。
-
----
-
-
-
-## 2026-08-01 11:40 (UTC+8)
-
-
-
-### 文档：现状一览补充「合格数据」定义
-
-- **原本**：§0 只写「合格数据拿到后」怎么跑，未写清何为合格。
-- **改成**：根 `[README.md](../README.md)` §0 增加「什么叫合格数据」表（时间戳 / 会话 / 场景 / 格式 / 反例）及拿到后的模式与预期。
-
----
-
-
-
-## 2026-08-01 11:30 (UTC+8)
-
-
-
-### 文档：现状一览 + 去掉「方案」二分表述
-
-- **原本**：对外说明仍偶发「方案二」等旧称呼；根 README 缺少一眼可读的阶段 / 合成 vs 旧车 / 合格数据预期；烟测配置与对比脚本文件名带 `scheme2`。
-- **改成**：
-  - 根 `[README.md](../README.md)` 增加 §0「现状一览」；明确仓库只有一条连续运动标定路径。
-  - `[tests/README.md](../tests/README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明同步边界与阶段。
-  - `config/s2_old_smoke.yaml`、`tools/compare_s2_runs.py` 替换旧 `*scheme2*` 命名。
-
----
-
-
-
-## 2026-07-31 18:10 (UTC+8)
-
-
-
-### 配准可视化工具 + tests 说明（含 S2 线下记录）
-
-- **原本**：无类似 RTK 仓库的运动对叠点 3D 查看；`tests/` 未说明合成 pytest 与 S2 旧数据线下试验的区别与结果。
-- **改成**：
-  - 新增 `tools/visualize_pair_3d.py` / `view_pair.ps1`（键 1–4：原始 / IMU(X=I) / 雷达 B / `X⁻¹AX`；可 `--save-png`）。
-  - 新增 `[tests/README.md](../tests/README.md)`：自动化用例表 + S2 主机时间数据做了什么、结果为何 `blocked`。
-
----
-
-
-
-## 2026-07-31 17:20 (UTC+8)
-
-
-
-### 文档同步 + 合成数据一键复现
-
-- **原本**：`docs/标定流程与采集清单.md` 仍偏旧版「待写代码 / 因子图设想」；根 README 缺少清晰的一键复现入口与输入输出总表。
-- **改成**：
-  - 采集清单与现行流水线对齐（完整预积分、δt↔R 交替、可观时再估平移）。
-  - 新增 `tools/reproduce_synthetic.py` / `.ps1`、`tools/show_calibration_report.py`；合成生成写入 `meta.json`；根 README 增加「系统输入输出 + 一键复现」。
-
----
-
-
-
-## 2026-07-31 16:30 (UTC+8)
-
-
-
-### 文档：移除已删除的静站路径表述，对外 README 重写
-
-- **原本**：根 README / `docs` / 包说明仍对照已删除的静站路径与内部阶段黑话；`pyproject` 仍声明已删除的 `static_station` 包。
-- **改成**：
-  - 删除旧静站文档；采集清单定为 `[docs/标定流程与采集清单.md](../docs/标定流程与采集清单.md)`。
-  - 根 `[README.md](../README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明改为对外可读，只保留连续运动标定路径。
-  - `pyproject.toml` 仅保留 `imu_lidar` / `tools`。
-
----
-
-
-
-## 2026-07-31 14:00 (UTC+8)
-
-
-
-### Phase-C：完整 IMU 预积分 + 重力/速度/动态零偏（full_se3）
-
-- **原本**：
-  - 运动对仅陀螺旋转预积分（`ΔR/Σ/J_bg`）；`t_A` 为空。
-  - 联合精修只估常值陀螺零偏修正；SE(3) 平移用经典手眼式 `(R_A-I)t ≈ R_X t_B`，无重力/速度/`b_a`。
-- **改成**：
-  - `imu_preintegration.preintegrate_imu`：中值法积分 `ΔR/Δv/Δp`，传播 15 维误差态后输出 9×9 `Σ`（含 bias RW 过程噪声）与 9×3 `J_bg/J_ba`；保留 `preintegrate_gyro`。
-  - `motion_pairs` 始终调用完整预积分，写入 `delta_v/delta_p/cov9/J_bg9/J_ba`，`t_A_m=Δp`。
-  - `joint_optimizer`：`rotation_only` 仍 Phase-A；`full_se3` 可观时 Phase-C 联合估 `R_X,t_X,g,v_k,b_g,k,b_a,k`（关键帧 RW 先验）。
-  - `pipeline` 用静止加速度推重力初值；`summary.joint` 增加 `gravity_m_s2` / `accel_bias_m_s2`。
-
----
-
-
-
-## 2026-07-31 11:20 (UTC+8)
-
-
-
-### 文档维护约定 + README 与现行实现对齐
-
-- **原本**：根 README 与已删除的静站目录说明仍按「双路径并行」表述；部分模块说明未写明有符号 δt；改代码时 README 更新不完整。
-- **改成**：
-  - 对外说明统一为**唯一连续运动标定路径**；流水线描述对齐有符号 δt 与联合精修。
-  - 根 README 增加「文档维护」表：每次改代码必须同步涉及的 README / 本 CHANGELOG。
-
----
-
-
-
-## 2026-07-31 09:40 (UTC+8)
-
-
-
-### 流水线：手眼未过门时仍尝试有符号 δt 精修
-
-- **原本**：`rotation_handeye.ok=false`（如 RMS>5°）时立即 `blocked` 返回，阶段 A 的有符号 δt 精修根本不会执行。
-- **改成**：只要可用运动对数 ≥3，即使用当前候选 `R` 做最多 2 轮有符号 δt 精修并重建运动对；精修后再按手眼门控决定是否 `blocked`。保证阶段 A 在困难数据上也能完整参与。
-
----
-
-
-
-## 2026-07-31 09:20 (UTC+8)
-
-
-
-### 阶段 A：标准旋转预积分因子 + 精确时间边界 + 有符号 δt 精修
-
-- **原本**：
-  - 预积分只输出 `ΔR` 与启发式标量 weight/`σ`，区间端点用邻近 IMU 样本，无 `Σ`、无 `J_bg`。
-  - δt 仅靠角速度模长互相关粗估；手眼得到 `R` 后不再回头精修时间。
-  - 联合精修对零偏多用重积分或 `Exp(-δbΔt)` 近似，残差未按协方差白化，也无 `δb` 先验。
-- **改成**：
-  - `imu_preintegration.preintegrate_gyro`：区间端点 **线性插值** 到精确 `t0/t1`；离散中值更新同时传播 `cov(Σ)` 与 `J_bg`（`ΔR(b+δb)≈ΔR Exp(J_bg δb)`）；weight 由 `trace(Σ)` + 激励/时长构造。
-  - `motion_pairs` metadata 增加 `cov`、`J_bg`，modeling 标记为 `gyro_preintegration_factor_phase_a`。
-  - `time_offset.refine_time_offset_signed`：用当前 `R_IMU_lidar` 把 LiDAR 角速度变到 IMU 系，在粗 δt 邻域做 **三轴有符号 MSE 精修**；仅当 MSE 下降且 **模长相关不劣化** 时才接受，避免 ICP 噪声带偏；`pipeline` 在手眼后与构对交替最多 2 轮。
-  - `joint_optimizer`：残差按 `Σ` **信息白化**；零偏用 `J_bg` 一阶修正；增加弱 `δb` 先验。
-
----
-
-
-
-## 2026-07-30 17:50 (UTC+8)
-
-
-
-### 第 1 步：帧间 IMU 轻量加强（加权预积分手眼）
-
-- **原本**：`motion_pairs` 用 `integrate_gyro_rotation` 直接得到 `R_A`，各运动对等权进入 `rotation_handeye`；手眼残差不区分长短间隔与激励强弱。
-- **改成**：
-  - 新增 `imu_preintegration.py`：对 `[t_i, t_j]` 做中值陀螺预积分，估计 `σ` 与 **pair weight**（偏短间隔、有角速度、低不确定度）。
-  - `motion_pairs` 改为调用 `preintegrate_gyro`，在 `metadata` 写入 `weight/duration_s/mean_gyro_norm/preint_sigma_rad/t_*_imu_s`，并增加 A/B 转角粗一致性过滤。
-  - `rotation_handeye` 改为 **√weight 加权** 的 Tsai 初值与 Huber 非线性精修；报告仍给未加权 RMS/中位数便于解读。
-
-
-
-### 第 2 步：预积分残差联合精修（外参 + 陀螺零偏）
-
-- **原本**：`joint_optimizer` 在手眼 `R_X` 基础上，仅在可观时用离散手眼平移式尝试 SE(3)；旋转侧不再用 IMU 过程模型，也不联合估零偏。
-- **改成**：
-  - `joint_optimizer.solve_joint_extrinsic` 增加预积分旋转残差：`log(ΔRᵀ · R_X R_B R_Xᵀ)`，按 weight 加权。
-  - 联合变量增加陀螺零偏修正 `δb`：有 `imu` 时按区间 **重预积分**；否则用一阶修正 `ΔR(b+δb)≈ΔR Exp(-δbΔt)`。
-  - `pipeline` 把 `imu`、静止零偏、`δt` 传入 joint；summary 增加 `gyro_bias_rad_s`。
-  - 平移仍受可观性门控；`rotation_only` 时不交付平移。
-
-
-
-### 文档
-
-- **原本**：`imu_lidar/README.md` 仅模块列表，无逐次改动史。
-- **改成**：新增本文件 `CHANGELOG.md`；模块说明中补充 `imu_preintegration.py` 与建模步骤描述。
-
----
-
-
-
-## 模板（以后追加用）
-
-```markdown
-## YYYY-MM-DD HH:MM (UTC+8)
-
-### 标题
-
-- **原本**：...
-- **改成**：...
-```
-

imu_lidar/__init__.py

@@ -1,5 +0,0 @@
-"""LiDAR–IMU calibration package (V1 runnable pipeline)."""
-
-from .contracts import CalibrationMode, CalibrationStatus, TransformConvention
-
-__all__ = ["CalibrationMode", "CalibrationStatus", "TransformConvention"]

imu_lidar/cli.py

@@ -1,345 +0,0 @@
-"""Command-line entry point for LiDAR–IMU calibration."""
-
-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)
-
-    plan = subcommands.add_parser("plan", help="显示标定阶段，不读取数据")
-    plan.add_argument("--vehicle-config", help="车辆配置路径（仅展示，plan 不读取）")
-    plan.add_argument(
-        "--mode",
-        choices=[mode.value for mode in CalibrationMode],
-        default=CalibrationMode.ROTATION_ONLY.value,
-    )
-
-    run = subcommands.add_parser(
-        "run",
-        help="执行 V1 标定流水线（可重复 --imu/--lidar/--session-id 做多会话联合）",
-    )
-    run.add_argument(
-        "--session-id",
-        action="append",
-        default=None,
-        help="会话 ID（可重复；与 --imu/--lidar 一一对应）",
-    )
-    run.add_argument(
-        "--imu",
-        action="append",
-        required=True,
-        help="IMU CSV/NPZ 路径（可重复）",
-    )
-    run.add_argument(
-        "--lidar",
-        action="append",
-        required=True,
-        help="LiDAR 会话目录（可重复）",
-    )
-    run.add_argument("--vehicle-config", required=True, help="车辆配置 YAML")
-    run.add_argument("--output", required=True, help="输出目录")
-    run.add_argument(
-        "--mode",
-        choices=[mode.value for mode in CalibrationMode],
-        default=CalibrationMode.ROTATION_ONLY.value,
-    )
-    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
-
-
-def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]:
-    imus = [Path(p) for p in args.imu]
-    lidars = [Path(p) for p in args.lidar]
-    if len(imus) != len(lidars):
-        raise SystemExit(f"--imu count ({len(imus)}) must match --lidar count ({len(lidars)})")
-    if args.session_id is None:
-        session_ids = [f"session{i}" for i in range(len(imus))]
-    else:
-        session_ids = list(args.session_id)
-        if len(session_ids) != len(imus):
-            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,
-            fixed_time_offset_s=offset,
-        )
-        for sid, imu, lidar, offset in zip(session_ids, imus, lidars, session_offsets)
-    )
-
-
-def main(argv: list[str] | None = None) -> int:
-    parser = build_parser()
-    args = parser.parse_args(argv)
-
-    if args.command == "plan":
-        request = CalibrationRequest(
-            vehicle_config=Path(args.vehicle_config) if args.vehicle_config else None,
-            requested_mode=CalibrationMode(args.mode),
-        )
-        print("LiDAR–IMU calibration stages:")
-        print(f"requested mode: {request.requested_mode.value}")
-        for index, stage in enumerate(describe_pipeline(request), start=1):
-            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(
-            vehicle_config=Path(args.vehicle_config),
-            sessions=sessions,
-            requested_mode=CalibrationMode(args.mode),
-            output_directory=Path(args.output),
-            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, progress_callback=_print_progress)
-        print(f"status: {result.status.value}")
-        print(f"message: {result.message}")
-        if result.time_offset_s is not None:
-            print(f"time_offset_s (first session; t_imu = t_lidar + dt): {result.time_offset_s:.6f}")
-        joint = (result.details or {}).get("joint") or {}
-        if joint:
-            print(f"merged_pair_count: {joint.get('merged_pair_count')}")
-            print(f"pair_counts_per_session: {joint.get('pair_counts_per_session')}")
-        if result.T_IMU_lidar is not None:
-            print("T_IMU_lidar:")
-            print(result.T_IMU_lidar)
-        print(f"report directory: {args.output}")
-        return 0 if result.status != CalibrationStatus.BLOCKED else 2
-
-    parser.error(f"unknown command {args.command}")
-    return 2
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

imu_lidar/contracts.py

@@ -1,130 +0,0 @@
-"""Shared contracts for the LiDAR–IMU calibration pipeline."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass, field
-from enum import Enum
-from pathlib import Path
-from typing import Any
-
-import numpy as np
-
-
-class TransformConvention(str, Enum):
-    """The only transform convention used by this project."""
-
-    T_A_B = "T_A_B maps points from frame B into frame A"
-
-
-class CalibrationMode(str, Enum):
-    ROTATION_ONLY = "rotation_only"
-    FULL_SE3 = "full_se3"
-
-
-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"
-
-
-@dataclass(frozen=True)
-class SessionInput:
-    """Input paths for one independently recorded session."""
-
-    session_id: str
-    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)
-class CalibrationRequest:
-    """Top-level calibration request."""
-
-    vehicle_config: Path | None
-    sessions: tuple[SessionInput, ...] = ()
-    requested_mode: CalibrationMode = CalibrationMode.ROTATION_ONLY
-    output_directory: Path | None = None
-    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
-class CalibrationResult:
-    """Result envelope written by finalize after pipeline gates."""
-
-    status: CalibrationStatus = CalibrationStatus.NOT_RUN
-    message: str = "Calibration has not been executed."
-    details: dict[str, Any] = field(default_factory=dict)
-    T_IMU_lidar: np.ndarray | None = None
-    time_offset_s: float | None = None
-
-
-@dataclass(frozen=True)
-class ImuSeries:
-    """Normalized IMU samples.
-
-    ``t_s`` is the native IMU clock in seconds (need not match LiDAR epoch).
-    Gyro must be rad/s; accelerometer must be m/s^2.
-    """
-
-    t_s: np.ndarray
-    gyro_rad_s: np.ndarray
-    acc_m_s2: np.ndarray
-
-    def __post_init__(self) -> None:
-        object.__setattr__(self, "t_s", np.asarray(self.t_s, dtype=float).reshape(-1))
-        object.__setattr__(self, "gyro_rad_s", np.asarray(self.gyro_rad_s, dtype=float).reshape(-1, 3))
-        object.__setattr__(self, "acc_m_s2", np.asarray(self.acc_m_s2, dtype=float).reshape(-1, 3))
-        n = self.t_s.size
-        if self.gyro_rad_s.shape != (n, 3) or self.acc_m_s2.shape != (n, 3):
-            raise ValueError("IMU arrays must share the same length and have shape (N, 3)")
-
-
-@dataclass(frozen=True)
-class LidarFrame:
-    """One LiDAR sweep in Cartesian sensor coordinates."""
-
-    frame_id: str
-    t_start_s: float
-    t_end_s: float
-    points_xyz: np.ndarray
-    path: Path | None = None
-
-    @property
-    def t_mid_s(self) -> float:
-        return 0.5 * (self.t_start_s + self.t_end_s)
-
-
-@dataclass(frozen=True)
-class MotionPair:
-    """One relative-motion observation between keyframes i and j."""
-
-    session_id: str
-    i: int
-    j: int
-    t_i_s: float
-    t_j_s: float
-    R_A: np.ndarray
-    R_B: np.ndarray
-    t_A_m: np.ndarray | None = None
-    t_B_m: np.ndarray | None = None
-    fitness: float = 0.0
-    metadata: dict[str, Any] = field(default_factory=dict)

imu_lidar/finalize.py

@@ -1,82 +0,0 @@
-"""Package calibration outputs as JSON-friendly artifacts."""
-
-from __future__ import annotations
-
-import json
-from pathlib import Path
-from typing import Any
-
-import numpy as np
-
-from .contracts import CalibrationResult, CalibrationStatus
-from .geometry import rotation_matrix_to_quaternion_xyzw, rpy_deg_xyz
-
-
-def _to_serializable(value: Any) -> Any:
-    if isinstance(value, np.ndarray):
-        return value.tolist()
-    if isinstance(value, (np.floating, np.integer, np.bool_)):
-        return value.item()
-    if isinstance(value, Path):
-        return str(value)
-    if isinstance(value, dict):
-        return {str(k): _to_serializable(v) for k, v in value.items()}
-    if isinstance(value, (list, tuple)):
-        return [_to_serializable(v) for v in value]
-    return value
-
-
-def finalize_result(
-    *,
-    status: CalibrationStatus,
-    message: str,
-    details: dict[str, Any],
-    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."""
-
-    result = CalibrationResult(
-        status=status,
-        message=message,
-        details=_to_serializable(details),
-        T_IMU_lidar=None if T_IMU_lidar is None else np.asarray(T_IMU_lidar, dtype=float),
-        time_offset_s=time_offset_s,
-    )
-
-    if output_directory is not None:
-        output_directory = Path(output_directory)
-        output_directory.mkdir(parents=True, exist_ok=True)
-        summary = {
-            "status": status.value,
-            "message": message,
-            "time_offset_s": time_offset_s,
-            "details": result.details,
-        }
-        if result.T_IMU_lidar is not None:
-            t = result.T_IMU_lidar
-            summary["T_IMU_lidar"] = {
-                "matrix": t.tolist(),
-                "translation_m": t[:3, 3].tolist(),
-                "rotation_quaternion_xyzw": rotation_matrix_to_quaternion_xyzw(t[:3, :3]).tolist(),
-                "rpy_deg_xyz": rpy_deg_xyz(t[:3, :3]).tolist(),
-                "convention": "p_IMU = T_IMU_lidar * p_lidar",
-            }
-            (output_directory / "T_IMU_lidar.json").write_text(
-                json.dumps(summary["T_IMU_lidar"], indent=2),
-                encoding="utf-8",
-            )
-        if time_offset_s is not None:
-            (output_directory / "time_offset.json").write_text(
-                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/geometry.py

@@ -1,207 +0,0 @@
-"""SE(3)/SO(3) utilities for LiDAR–IMU calibration."""
-
-from __future__ import annotations
-
-import math
-
-import numpy as np
-
-
-def skew(vector: np.ndarray) -> np.ndarray:
-    """Return the skew-symmetric matrix such that ``skew(v) @ w == v x w``."""
-
-    x, y, z = np.asarray(vector, dtype=float).reshape(3)
-    return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]], dtype=float)
-
-
-def so3_exp(rotation_vector: np.ndarray) -> np.ndarray:
-    """Map a rotation vector in radians onto SO(3)."""
-
-    vector = np.asarray(rotation_vector, dtype=float).reshape(3)
-    angle = float(np.linalg.norm(vector))
-    if angle < 1e-12:
-        return np.eye(3) + skew(vector)
-    axis_cross = skew(vector / angle)
-    return np.eye(3) + math.sin(angle) * axis_cross + (1.0 - math.cos(angle)) * axis_cross @ axis_cross
-
-
-def so3_log(rotation: np.ndarray) -> np.ndarray:
-    """Map an SO(3) matrix to a rotation vector in radians."""
-
-    rotation = np.asarray(rotation, dtype=float).reshape(3, 3)
-    cos_angle = float(np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0))
-    angle = math.acos(cos_angle)
-    if angle < 1e-12:
-        return 0.5 * np.array(
-            [
-                rotation[2, 1] - rotation[1, 2],
-                rotation[0, 2] - rotation[2, 0],
-                rotation[1, 0] - rotation[0, 1],
-            ],
-            dtype=float,
-        )
-    if abs(angle - math.pi) < 1e-6:
-        # Near 180°: use eigenvector of the +1 eigenvalue.
-        eigvals, eigvecs = np.linalg.eigh(0.5 * (rotation + rotation.T))
-        axis = eigvecs[:, int(np.argmax(eigvals))]
-        return axis * angle
-    return (
-        0.5
-        * angle
-        / math.sin(angle)
-        * np.array(
-            [
-                rotation[2, 1] - rotation[1, 2],
-                rotation[0, 2] - rotation[2, 0],
-                rotation[1, 0] - rotation[0, 1],
-            ],
-            dtype=float,
-        )
-    )
-
-
-def rotation_angle_deg(rotation: np.ndarray) -> float:
-    """Return the rotation angle in degrees."""
-
-    return float(np.degrees(np.linalg.norm(so3_log(rotation))))
-
-
-def inverse_transform(transform: np.ndarray) -> np.ndarray:
-    """Return the inverse of a rigid 4x4 transform."""
-
-    transform = np.asarray(transform, dtype=float)
-    if transform.shape != (4, 4):
-        raise ValueError("a rigid transform must have shape (4, 4)")
-    result = np.eye(4)
-    result[:3, :3] = transform[:3, :3].T
-    result[:3, 3] = -result[:3, :3] @ transform[:3, 3]
-    return result
-
-
-def make_transform(translation_m: np.ndarray, rotation: np.ndarray) -> np.ndarray:
-    """Build ``T_A_B`` from its translation and rotation components."""
-
-    translation_m = np.asarray(translation_m, dtype=float).reshape(3)
-    rotation = np.asarray(rotation, dtype=float)
-    if rotation.shape != (3, 3):
-        raise ValueError("a rotation matrix must have shape (3, 3)")
-    result = np.eye(4)
-    result[:3, :3] = rotation
-    result[:3, 3] = translation_m
-    return result
-
-
-def transform_points(points: np.ndarray, transform: np.ndarray) -> np.ndarray:
-    """Apply ``T_A_B`` to an ``(N, 3)`` point array expressed in frame B."""
-
-    points = np.asarray(points, dtype=float)
-    if points.ndim != 2 or points.shape[1] != 3:
-        raise ValueError("points must have shape (N, 3)")
-    return points @ transform[:3, :3].T + transform[:3, 3]
-
-
-def orthonormalize_rotation(rotation: np.ndarray) -> np.ndarray:
-    """Project a near-rotation matrix onto SO(3)."""
-
-    u, _, vt = np.linalg.svd(np.asarray(rotation, dtype=float).reshape(3, 3))
-    result = u @ vt
-    if np.linalg.det(result) < 0:
-        u[:, -1] *= -1
-        result = u @ vt
-    return result
-
-
-def integrate_gyro_rotation(
-    times_s: np.ndarray,
-    gyro_rad_s: np.ndarray,
-    t0: float,
-    t1: float,
-    bias_rad_s: np.ndarray | None = None,
-) -> np.ndarray:
-    """Integrate gyroscope samples on ``[t0, t1]`` and return ``R(t0<-t1)`` wait.
-
-    Returns ``R_i_j`` that maps vectors from the IMU frame at ``t1`` into the
-    IMU frame at ``t0`` using right-invariant discrete integration:
-
-        R <- R @ Exp(omega * dt)
-    """
-
-    times_s = np.asarray(times_s, dtype=float).reshape(-1)
-    gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
-    if times_s.size < 2:
-        return np.eye(3)
-    bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
-
-    if t1 < t0:
-        raise ValueError("t1 must be >= t0")
-
-    # Include one sample before t0 and after t1 when possible for interpolation.
-    left = int(np.searchsorted(times_s, t0, side="left") - 1)
-    right = int(np.searchsorted(times_s, t1, side="right"))
-    left = max(left, 0)
-    right = min(right, times_s.size - 1)
-    if right <= left:
-        return np.eye(3)
-
-    rotation = np.eye(3)
-    for index in range(left, right):
-        t_a = float(times_s[index])
-        t_b = float(times_s[index + 1])
-        if t_b <= t0 or t_a >= t1:
-            continue
-        seg0 = max(t_a, t0)
-        seg1 = min(t_b, t1)
-        dt = seg1 - seg0
-        if dt <= 0:
-            continue
-        omega = 0.5 * (gyro_rad_s[index] + gyro_rad_s[index + 1]) - bias
-        rotation = rotation @ so3_exp(omega * dt)
-    return orthonormalize_rotation(rotation)
-
-
-def rotation_matrix_to_quaternion_xyzw(rotation: np.ndarray) -> np.ndarray:
-    """Convert SO(3) to quaternion ``[x, y, z, w]``."""
-
-    rotation = orthonormalize_rotation(rotation)
-    trace = float(np.trace(rotation))
-    if trace > 0:
-        s = math.sqrt(trace + 1.0) * 2.0
-        w = 0.25 * s
-        x = (rotation[2, 1] - rotation[1, 2]) / s
-        y = (rotation[0, 2] - rotation[2, 0]) / s
-        z = (rotation[1, 0] - rotation[0, 1]) / s
-    elif rotation[0, 0] > rotation[1, 1] and rotation[0, 0] > rotation[2, 2]:
-        s = math.sqrt(1.0 + rotation[0, 0] - rotation[1, 1] - rotation[2, 2]) * 2.0
-        w = (rotation[2, 1] - rotation[1, 2]) / s
-        x = 0.25 * s
-        y = (rotation[0, 1] + rotation[1, 0]) / s
-        z = (rotation[0, 2] + rotation[2, 0]) / s
-    elif rotation[1, 1] > rotation[2, 2]:
-        s = math.sqrt(1.0 + rotation[1, 1] - rotation[0, 0] - rotation[2, 2]) * 2.0
-        w = (rotation[0, 2] - rotation[2, 0]) / s
-        x = (rotation[0, 1] + rotation[1, 0]) / s
-        y = 0.25 * s
-        z = (rotation[1, 2] + rotation[2, 1]) / s
-    else:
-        s = math.sqrt(1.0 + rotation[2, 2] - rotation[0, 0] - rotation[1, 1]) * 2.0
-        w = (rotation[1, 0] - rotation[0, 1]) / s
-        x = (rotation[0, 2] + rotation[2, 0]) / s
-        y = (rotation[1, 2] + rotation[2, 1]) / s
-        z = 0.25 * s
-    return np.array([x, y, z, w], dtype=float)
-
-
-def rpy_deg_xyz(rotation: np.ndarray) -> np.ndarray:
-    """Intrinsic XYZ Euler angles in degrees from a rotation matrix."""
-
-    rotation = orthonormalize_rotation(rotation)
-    sy = math.sqrt(rotation[0, 0] ** 2 + rotation[1, 0] ** 2)
-    if sy > 1e-8:
-        roll = math.atan2(rotation[2, 1], rotation[2, 2])
-        pitch = math.atan2(-rotation[2, 0], sy)
-        yaw = math.atan2(rotation[1, 0], rotation[0, 0])
-    else:
-        roll = math.atan2(-rotation[1, 2], rotation[1, 1])
-        pitch = math.atan2(-rotation[2, 0], sy)
-        yaw = 0.0
-    return np.degrees(np.array([roll, pitch, yaw], dtype=float))

imu_lidar/imu_audit.py

@@ -1,86 +0,0 @@
-"""IMU unit, axis, bias, and saturation audit."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-
-from .contracts import ImuSeries
-
-G = 9.80665
-
-
-@dataclass(frozen=True)
-class ImuAuditReport:
-    ok: bool
-    gyro_bias_rad_s: np.ndarray
-    static_acc_mean_m_s2: np.ndarray
-    static_acc_norm_m_s2: float
-    suggested_up_axis: int
-    suggested_up_sign: float
-    static_ratio: float
-    notes: tuple[str, ...] = ()
-
-
-def _static_mask(gyro: np.ndarray, acc: np.ndarray) -> np.ndarray:
-    gyro_norm = np.linalg.norm(gyro, axis=1)
-    acc_norm = np.linalg.norm(acc, axis=1)
-    gyro_thr = max(0.02, float(np.percentile(gyro_norm, 20)) * 1.5)
-    acc_thr_low = 0.7 * G
-    acc_thr_high = 1.3 * G
-    return (gyro_norm < gyro_thr) & (acc_norm > acc_thr_low) & (acc_norm < acc_thr_high)
-
-
-def audit_imu(imu: ImuSeries) -> ImuAuditReport:
-    """Audit normalized IMU samples and estimate a static gyro bias."""
-
-    notes: list[str] = []
-    mask = _static_mask(imu.gyro_rad_s, imu.acc_m_s2)
-    static_ratio = float(np.mean(mask)) if mask.size else 0.0
-    if static_ratio < 0.02:
-        # Fall back to lowest-gyro percentile window.
-        gyro_norm = np.linalg.norm(imu.gyro_rad_s, axis=1)
-        cutoff = float(np.percentile(gyro_norm, 10))
-        mask = gyro_norm <= cutoff
-        notes.append("few gravity-consistent static samples; using lowest-gyro percentile")
-        static_ratio = float(np.mean(mask))
-
-    if not np.any(mask):
-        notes.append("no static samples found")
-        bias = np.zeros(3)
-        acc_mean = np.zeros(3)
-        acc_norm = 0.0
-        up_axis = 2
-        up_sign = 1.0
-        ok = False
-    else:
-        bias = np.mean(imu.gyro_rad_s[mask], axis=0)
-        acc_mean = np.mean(imu.acc_m_s2[mask], axis=0)
-        acc_norm = float(np.linalg.norm(acc_mean))
-        up_axis = int(np.argmax(np.abs(acc_mean)))
-        up_sign = float(np.sign(acc_mean[up_axis]) or 1.0)
-        if abs(acc_norm - G) > 2.5:
-            notes.append(
-                f"static |acc|={acc_norm:.3f} differs from g={G}; check units (expect m/s^2)"
-            )
-        gyro_peak = float(np.max(np.linalg.norm(imu.gyro_rad_s, axis=1)))
-        if gyro_peak > 20.0:
-            notes.append(
-                f"peak |gyro|={gyro_peak:.1f} rad/s looks extreme; check whether data is deg/s"
-            )
-        ok = abs(acc_norm - G) < 3.5 or static_ratio > 0.05
-
-    notes.append(
-        f"suggested up axis index={up_axis} sign={up_sign:+.0f} (0=x,1=y,2=z)"
-    )
-    return ImuAuditReport(
-        ok=ok,
-        gyro_bias_rad_s=np.asarray(bias, dtype=float),
-        static_acc_mean_m_s2=np.asarray(acc_mean, dtype=float),
-        static_acc_norm_m_s2=float(acc_norm),
-        suggested_up_axis=up_axis,
-        suggested_up_sign=up_sign,
-        static_ratio=static_ratio,
-        notes=tuple(notes),
-    )

imu_lidar/imu_io.py

@@ -1,72 +0,0 @@
-"""IMU adapters for the V1 standard intermediate format.
-
-Accepted inputs
----------------
-1. CSV with header:
-   t,gx,gy,gz,ax,ay,az
-   - ``t`` in seconds on the IMU clock
-   - gyro in rad/s
-   - accel in m/s^2
-
-2. NPZ with arrays:
-   t, gyro, acc
-   shapes: (N,), (N,3), (N,3)
-"""
-
-from __future__ import annotations
-
-from pathlib import Path
-
-import numpy as np
-
-from .contracts import ImuSeries
-
-
-def load_imu_samples(path: Path | str) -> ImuSeries:
-    """Load normalized IMU samples from CSV or NPZ."""
-
-    source = Path(path)
-    if not source.exists():
-        raise FileNotFoundError(source)
-    if source.suffix.lower() == ".csv":
-        return _load_imu_csv(source)
-    if source.suffix.lower() == ".npz":
-        return _load_imu_npz(source)
-    raise ValueError(f"unsupported IMU format '{source.suffix}' (use .csv or .npz)")
-
-
-def _load_imu_csv(path: Path) -> ImuSeries:
-    data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
-    if data.ndim == 0:
-        data = np.array([data])
-    names = set(data.dtype.names or ())
-    required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
-    if not required.issubset(names):
-        raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
-    t = np.asarray(data["t"], dtype=float).reshape(-1)
-    gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
-    acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
-    order = np.argsort(t)
-    return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
-
-
-def _load_imu_npz(path: Path) -> ImuSeries:
-    with np.load(path) as payload:
-        keys = set(payload.files)
-        if not {"t", "gyro", "acc"}.issubset(keys):
-            raise ValueError(f"IMU NPZ must contain t, gyro, acc; got {sorted(keys)}")
-        t = np.asarray(payload["t"], dtype=float).reshape(-1)
-        gyro = np.asarray(payload["gyro"], dtype=float).reshape(-1, 3)
-        acc = np.asarray(payload["acc"], dtype=float).reshape(-1, 3)
-    order = np.argsort(t)
-    return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
-
-
-def save_imu_csv(path: Path | str, imu: ImuSeries) -> None:
-    """Write IMU samples to the standard CSV format."""
-
-    destination = Path(path)
-    destination.parent.mkdir(parents=True, exist_ok=True)
-    array = np.column_stack([imu.t_s, imu.gyro_rad_s, imu.acc_m_s2])
-    header = "t,gx,gy,gz,ax,ay,az"
-    np.savetxt(destination, array, delimiter=",", header=header, comments="")

imu_lidar/imu_preintegration.py

@@ -1,452 +0,0 @@
-"""Frame-to-frame IMU preintegration (Phase-A rotation + Phase-C full factor).
-
-Phase-A: ``ΔR``, 3×3 ``Σ``, ``J_bg``.
-Phase-C: ``ΔR/Δv/Δp``, 9×9 ``Σ`` (with bias RW process noise), ``J_bg``/``J_ba``.
-"""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-
-from .geometry import orthonormalize_rotation, so3_exp, so3_log, skew
-
-
-@dataclass(frozen=True)
-class GyroPreintegration:
-    """Rotation-only preintegration on ``[t0, t1]`` (IMU clock)."""
-
-    delta_R: np.ndarray
-    duration_s: float
-    mean_gyro_norm: float
-    sigma_rad: float
-    weight: float
-    bias_rad_s: np.ndarray
-    cov: np.ndarray
-    J_bg: np.ndarray
-
-
-@dataclass(frozen=True)
-class ImuPreintegration:
-    """Full IMU preintegration on ``[t0, t1]`` (IMU clock).
-
-    ``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``.
-    ``delta_v`` / ``delta_p`` are body-frame increments (no gravity).
-    Error-state order in ``cov`` / Jacobians: ``[δθ, δv, δp]`` (9).
-    ``J_bg`` / ``J_ba`` are 9×3: first-order correction w.r.t. constant bias deltas.
-    """
-
-    delta_R: np.ndarray
-    delta_v: np.ndarray
-    delta_p: np.ndarray
-    duration_s: float
-    mean_gyro_norm: float
-    sigma_rad: float
-    weight: float
-    gyro_bias_rad_s: np.ndarray
-    acc_bias_m_s2: np.ndarray
-    cov: np.ndarray
-    J_bg: np.ndarray
-    J_ba: np.ndarray
-
-
-def _right_jacobian(phi: np.ndarray) -> np.ndarray:
-    """SO(3) right Jacobian ``Jr(φ)`` with ``Exp(φ+δ)≈Exp(φ)Exp(Jr δ)``."""
-
-    phi = np.asarray(phi, dtype=float).reshape(3)
-    angle = float(np.linalg.norm(phi))
-    if angle < 1e-8:
-        return np.eye(3) - 0.5 * skew(phi)
-    axis = phi / angle
-    s = skew(axis)
-    return (
-        np.eye(3)
-        - ((1.0 - np.cos(angle)) / angle) * s
-        + ((angle - np.sin(angle)) / angle) * (s @ s)
-    )
-
-
-def _interp_vec(times_s: np.ndarray, values: np.ndarray, t: float) -> np.ndarray:
-    """Linear interpolate a 3-vector series at an exact time."""
-
-    return np.array(
-        [float(np.interp(t, times_s, values[:, axis])) for axis in range(3)],
-        dtype=float,
-    )
-
-
-def _interp_gyro(times_s: np.ndarray, gyro_rad_s: np.ndarray, t: float) -> np.ndarray:
-    """Linear interpolate gyro at an exact time."""
-
-    return _interp_vec(times_s, gyro_rad_s, t)
-
-
-def _pair_weight(duration_s: float, mean_gyro_norm: float, cov_trace: float) -> float:
-    """Larger weight for short, excited, low-covariance intervals."""
-
-    duration_term = 1.0 / max(duration_s, 0.05)
-    excite_term = min(max(mean_gyro_norm, 1e-3), 1.0)
-    avg_var = max(cov_trace / 3.0, 1e-8)
-    return float(duration_term * excite_term / avg_var)
-
-
-def preintegrate_gyro(
-    times_s: np.ndarray,
-    gyro_rad_s: np.ndarray,
-    t0: float,
-    t1: float,
-    bias_rad_s: np.ndarray | None = None,
-    *,
-    sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
-) -> GyroPreintegration:
-    """Discrete mid-point gyro preintegration with exact endpoints.
-
-    ``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``
-    via right-invariant updates ``ΔR ← ΔR Exp((ω-b) dt)``.
-
-    Also returns:
-    - ``cov``: 3×3 covariance of the right tangent noise on ``ΔR``
-    - ``J_bg``: ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``
-    """
-
-    times_s = np.asarray(times_s, dtype=float).reshape(-1)
-    gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
-    bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
-    duration = float(max(t1 - t0, 0.0))
-
-    empty = GyroPreintegration(
-        delta_R=np.eye(3),
-        duration_s=0.0,
-        mean_gyro_norm=0.0,
-        sigma_rad=1e3,
-        weight=1e-6,
-        bias_rad_s=bias.copy(),
-        cov=np.eye(3) * 1e6,
-        J_bg=np.zeros((3, 3)),
-    )
-    if times_s.size < 2 or duration <= 0:
-        return empty
-
-    t0 = float(np.clip(t0, times_s[0], times_s[-1]))
-    t1 = float(np.clip(t1, times_s[0], times_s[-1]))
-    duration = float(max(t1 - t0, 0.0))
-    if duration <= 0:
-        return empty
-
-    left = int(np.searchsorted(times_s, t0, side="left") - 1)
-    right = int(np.searchsorted(times_s, t1, side="right"))
-    left = max(left, 0)
-    right = min(right, times_s.size - 1)
-    if right <= left:
-        return empty
-
-    delta_r = np.eye(3)
-    j_bg = np.zeros((3, 3))
-    cov = np.zeros((3, 3))
-    sigma2 = float(sigma_g_rad_s_sqrt_hz) ** 2
-    gyro_norms: list[float] = []
-
-    for index in range(left, right):
-        t_a = float(times_s[index])
-        t_b = float(times_s[index + 1])
-        if t_b <= t0 or t_a >= t1:
-            continue
-        seg0 = max(t_a, t0)
-        seg1 = min(t_b, t1)
-        dt = seg1 - seg0
-        if dt <= 0:
-            continue
-
-        # Exact endpoint gyro via linear interpolation inside the sample interval.
-        g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
-        g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
-        omega = 0.5 * (g_a + g_b) - bias
-        gyro_norms.append(float(np.linalg.norm(omega)))
-
-        theta = omega * dt
-        jr = _right_jacobian(theta)
-        a_mat = so3_exp(-theta)
-        j_bg = a_mat @ j_bg - jr * dt
-        cov = a_mat @ cov @ a_mat.T + jr @ (sigma2 * dt * np.eye(3)) @ jr.T
-        delta_r = delta_r @ so3_exp(theta)
-
-    delta_r = orthonormalize_rotation(delta_r)
-    mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
-    cov = 0.5 * (cov + cov.T)
-    cov = cov + np.eye(3) * 1e-12
-    if mean_gyro_norm < 0.02:
-        cov = cov * 4.0
-    cov_trace = float(np.trace(cov))
-    sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
-    weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
-    return GyroPreintegration(
-        delta_R=delta_r,
-        duration_s=duration,
-        mean_gyro_norm=mean_gyro_norm,
-        sigma_rad=sigma_rad,
-        weight=weight,
-        bias_rad_s=bias.copy(),
-        cov=cov,
-        J_bg=np.asarray(j_bg, dtype=float),
-    )
-
-
-def preintegrate_imu(
-    times_s: np.ndarray,
-    gyro_rad_s: np.ndarray,
-    acc_m_s2: np.ndarray,
-    t0: float,
-    t1: float,
-    gyro_bias_rad_s: np.ndarray | None = None,
-    acc_bias_m_s2: np.ndarray | None = None,
-    *,
-    sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
-    sigma_a_m_s2_sqrt_hz: float = 2.0e-2,
-    sigma_bg_rw_rad_s_sqrt_hz: float = 1.0e-5,
-    sigma_ba_rw_m_s2_sqrt_hz: float = 1.0e-3,
-) -> ImuPreintegration:
-    """Mid-point IMU preintegration with exact endpoints and bias-RW noise.
-
-    Discrete updates (right-invariant)::
-
-        ΔR ← ΔR Exp((ω-bg) dt)
-        Δv ← Δv + ΔR (a-ba) dt
-        Δp ← Δp + Δv_old dt + 0.5 ΔR (a-ba) dt²
-
-    Propagates a 15-DoF error state ``[δθ, δv, δp, δbg, δba]`` then returns the
-    top-left 9×9 covariance (bias RW already folded in) and 9×3 Jacobians.
-    """
-
-    times_s = np.asarray(times_s, dtype=float).reshape(-1)
-    gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
-    acc_m_s2 = np.asarray(acc_m_s2, dtype=float).reshape(-1, 3)
-    bg = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
-    ba = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float).reshape(3)
-
-    empty = ImuPreintegration(
-        delta_R=np.eye(3),
-        delta_v=np.zeros(3),
-        delta_p=np.zeros(3),
-        duration_s=0.0,
-        mean_gyro_norm=0.0,
-        sigma_rad=1e3,
-        weight=1e-6,
-        gyro_bias_rad_s=bg.copy(),
-        acc_bias_m_s2=ba.copy(),
-        cov=np.eye(9) * 1e6,
-        J_bg=np.zeros((9, 3)),
-        J_ba=np.zeros((9, 3)),
-    )
-    if times_s.size < 2 or acc_m_s2.shape != gyro_rad_s.shape:
-        return empty
-
-    t0 = float(np.clip(t0, times_s[0], times_s[-1]))
-    t1 = float(np.clip(t1, times_s[0], times_s[-1]))
-    duration = float(max(t1 - t0, 0.0))
-    if duration <= 0:
-        return empty
-
-    left = int(np.searchsorted(times_s, t0, side="left") - 1)
-    right = int(np.searchsorted(times_s, t1, side="right"))
-    left = max(left, 0)
-    right = min(right, times_s.size - 1)
-    if right <= left:
-        return empty
-
-    delta_r = np.eye(3)
-    delta_v = np.zeros(3)
-    delta_p = np.zeros(3)
-    # Jacobians of [δθ, δv, δp] w.r.t. constant bias (accumulated analytically).
-    j_bg = np.zeros((9, 3))
-    j_ba = np.zeros((9, 3))
-    # 15×15 covariance: [θ, v, p, bg, ba]
-    cov15 = np.zeros((15, 15))
-    sg2 = float(sigma_g_rad_s_sqrt_hz) ** 2
-    sa2 = float(sigma_a_m_s2_sqrt_hz) ** 2
-    sbg2 = float(sigma_bg_rw_rad_s_sqrt_hz) ** 2
-    sba2 = float(sigma_ba_rw_m_s2_sqrt_hz) ** 2
-    gyro_norms: list[float] = []
-
-    for index in range(left, right):
-        t_a = float(times_s[index])
-        t_b = float(times_s[index + 1])
-        if t_b <= t0 or t_a >= t1:
-            continue
-        seg0 = max(t_a, t0)
-        seg1 = min(t_b, t1)
-        dt = seg1 - seg0
-        if dt <= 0:
-            continue
-
-        g_a = _interp_vec(times_s, gyro_rad_s, seg0)
-        g_b = _interp_vec(times_s, gyro_rad_s, seg1)
-        a_a = _interp_vec(times_s, acc_m_s2, seg0)
-        a_b = _interp_vec(times_s, acc_m_s2, seg1)
-        omega = 0.5 * (g_a + g_b) - bg
-        acc = 0.5 * (a_a + a_b) - ba
-        gyro_norms.append(float(np.linalg.norm(omega)))
-
-        theta = omega * dt
-        jr = _right_jacobian(theta)
-        r_dt = so3_exp(theta)
-        r_mid = delta_r  # rotate body accel into i0 frame before update
-
-        # Bias Jacobians (Forster-style first-order recursion).
-        j_r_bg = j_bg[0:3]
-        j_v_bg = j_bg[3:6]
-        j_p_bg = j_bg[6:9]
-        j_r_ba = j_ba[0:3]
-        j_v_ba = j_ba[3:6]
-        j_p_ba = j_ba[6:9]
-
-        acc_skew = skew(acc)
-        j_p_bg_new = j_p_bg + j_v_bg * dt - 0.5 * r_mid @ acc_skew @ j_r_bg * (dt**2)
-        j_v_bg_new = j_v_bg - r_mid @ acc_skew @ j_r_bg * dt
-        j_r_bg_new = r_dt.T @ j_r_bg - jr * dt
-
-        j_p_ba_new = j_p_ba + j_v_ba * dt - 0.5 * r_mid * (dt**2)
-        j_v_ba_new = j_v_ba - r_mid * dt
-        j_r_ba_new = r_dt.T @ j_r_ba
-
-        j_bg = np.vstack([j_r_bg_new, j_v_bg_new, j_p_bg_new])
-        j_ba = np.vstack([j_r_ba_new, j_v_ba_new, j_p_ba_new])
-
-        # Nominal state update (use pre-update Δv in position).
-        delta_p = delta_p + delta_v * dt + 0.5 * r_mid @ acc * (dt**2)
-        delta_v = delta_v + r_mid @ acc * dt
-        delta_r = orthonormalize_rotation(delta_r @ r_dt)
-
-        # Linearized error-state transition (15×15).
-        f = np.eye(15)
-        a_mat = so3_exp(-theta)
-        f[0:3, 0:3] = a_mat
-        f[0:3, 9:12] = -jr * dt
-        f[3:6, 0:3] = -r_mid @ acc_skew * dt
-        f[3:6, 12:15] = -r_mid * dt
-        f[6:9, 0:3] = -0.5 * r_mid @ acc_skew * (dt**2)
-        f[6:9, 3:6] = np.eye(3) * dt
-        f[6:9, 12:15] = -0.5 * r_mid * (dt**2)
-
-        # Noise: continuous densities σ²; Var(∫n dt)=σ² dt. Columns: n_g, n_a, n_bg, n_ba.
-        g_mat = np.zeros((15, 12))
-        g_mat[0:3, 0:3] = jr
-        g_mat[3:6, 3:6] = r_mid
-        g_mat[6:9, 3:6] = 0.5 * r_mid * dt
-        g_mat[9:12, 6:9] = np.eye(3)
-        g_mat[12:15, 9:12] = np.eye(3)
-        q = np.zeros((12, 12))
-        q[0:3, 0:3] = sg2 * dt * np.eye(3)
-        q[3:6, 3:6] = sa2 * dt * np.eye(3)
-        q[6:9, 6:9] = sbg2 * dt * np.eye(3)
-        q[9:12, 9:12] = sba2 * dt * np.eye(3)
-        cov15 = f @ cov15 @ f.T + g_mat @ q @ g_mat.T
-
-    delta_r = orthonormalize_rotation(delta_r)
-    mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
-    cov9 = cov15[0:9, 0:9]
-    cov9 = 0.5 * (cov9 + cov9.T) + np.eye(9) * 1e-12
-    if mean_gyro_norm < 0.02:
-        cov9 = cov9.copy()
-        cov9[0:3, 0:3] = cov9[0:3, 0:3] * 4.0
-    cov_trace = float(np.trace(cov9[0:3, 0:3]))
-    sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
-    weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
-    return ImuPreintegration(
-        delta_R=delta_r,
-        delta_v=np.asarray(delta_v, dtype=float),
-        delta_p=np.asarray(delta_p, dtype=float),
-        duration_s=duration,
-        mean_gyro_norm=mean_gyro_norm,
-        sigma_rad=sigma_rad,
-        weight=weight,
-        gyro_bias_rad_s=bg.copy(),
-        acc_bias_m_s2=ba.copy(),
-        cov=np.asarray(cov9, dtype=float),
-        J_bg=np.asarray(j_bg, dtype=float),
-        J_ba=np.asarray(j_ba, dtype=float),
-    )
-
-
-def apply_bias_correction_imu(
-    preint: ImuPreintegration,
-    delta_gyro_bias: np.ndarray | None = None,
-    delta_acc_bias: np.ndarray | None = None,
-) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
-    """First-order bias correction of ``ΔR/Δv/Δp``.
-
-    Returns ``(delta_R, delta_v, delta_p)``.
-    """
-
-    dbg = np.zeros(3) if delta_gyro_bias is None else np.asarray(delta_gyro_bias, dtype=float).reshape(3)
-    dba = np.zeros(3) if delta_acc_bias is None else np.asarray(delta_acc_bias, dtype=float).reshape(3)
-    j_bg = np.asarray(preint.J_bg, dtype=float).reshape(9, 3)
-    j_ba = np.asarray(preint.J_ba, dtype=float).reshape(9, 3)
-    delta_r = orthonormalize_rotation(preint.delta_R @ so3_exp(j_bg[0:3] @ dbg))
-    delta_v = preint.delta_v + j_bg[3:6] @ dbg + j_ba[3:6] @ dba
-    delta_p = preint.delta_p + j_bg[6:9] @ dbg + j_ba[6:9] @ dba
-    return delta_r, np.asarray(delta_v, dtype=float), np.asarray(delta_p, dtype=float)
-
-
-def relative_rotation_from_lidar(R_X: np.ndarray, R_B: np.ndarray) -> np.ndarray:
-    """Map LiDAR relative rotation into IMU frame: ``R_X R_B R_X^T``."""
-
-    r_x = orthonormalize_rotation(R_X)
-    r_b = orthonormalize_rotation(R_B)
-    return orthonormalize_rotation(r_x @ r_b @ r_x.T)
-
-
-def preintegration_rotation_residual(
-    delta_R: np.ndarray,
-    R_X: np.ndarray,
-    R_B: np.ndarray,
-) -> np.ndarray:
-    """``log( delta_R^T * R_X R_B R_X^T )`` in so(3)."""
-
-    predicted = relative_rotation_from_lidar(R_X, R_B)
-    return so3_log(delta_R.T @ predicted)
-
-
-def apply_bias_jacobian_correction(
-    delta_R: np.ndarray,
-    J_bg: np.ndarray,
-    delta_bias_rad_s: np.ndarray,
-) -> np.ndarray:
-    """First-order update ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``."""
-
-    db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
-    j_bg = np.asarray(J_bg, dtype=float).reshape(3, 3)
-    return orthonormalize_rotation(delta_R @ so3_exp(j_bg @ db))
-
-
-def apply_constant_bias_correction(
-    delta_R: np.ndarray,
-    duration_s: float,
-    delta_bias_rad_s: np.ndarray,
-) -> np.ndarray:
-    """Legacy first-order correction when ``J_bg`` is unavailable.
-
-    ``ΔR(b+δb) ≈ ΔR Exp(-δb Δt)`` (identity Jacobian approximation).
-    """
-
-    db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
-    return orthonormalize_rotation(delta_R @ so3_exp(-db * float(duration_s)))
-
-
-def residual_whiten_matrix(cov: np.ndarray) -> np.ndarray:
-    """Return ``W`` such that ``W @ e`` is approximately information-whitened.
-
-    Accepts square ``n×n`` covariances (3×3 rotation or 9×9 full IMU).
-    """
-
-    matrix = np.asarray(cov, dtype=float)
-    if matrix.ndim != 2 or matrix.shape[0] != matrix.shape[1]:
-        raise ValueError("cov must be square")
-    n = matrix.shape[0]
-    matrix = 0.5 * (matrix + matrix.T) + np.eye(n) * 1e-10
-    try:
-        info = np.linalg.inv(matrix)
-        return np.linalg.cholesky(info).T
-    except np.linalg.LinAlgError:
-        scale = 1.0 / max(float(np.sqrt(np.trace(matrix) / n)), 1e-6)
-        return np.eye(n) * scale

imu_lidar/joint_optimizer.py

@@ -1,977 +0,0 @@
-"""Joint extrinsic refinement: Phase-A rotation factors + Phase-C SE(3) IMU factors."""
-
-from __future__ import annotations
-
-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
-
-from .contracts import ImuSeries, MotionPair
-from .geometry import make_transform, orthonormalize_rotation, so3_exp, so3_log
-from .imu_preintegration import (
-    apply_bias_jacobian_correction,
-    apply_constant_bias_correction,
-    preintegrate_gyro,
-    preintegration_rotation_residual,
-    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
-    translation_accepted: bool
-    residual_rms_rot_deg: float
-    residual_rms_trans_m: float
-    observability: ObservabilityReport
-    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, ...] = ()
-
-
-def _pair_weight(pair: MotionPair) -> float:
-    weight = float(pair.metadata.get("weight", 1.0))
-    if not np.isfinite(weight) or weight <= 0:
-        return 1.0
-    return weight
-
-
-def _pair_j_bg(pair: MotionPair) -> np.ndarray | None:
-    raw = pair.metadata.get("J_bg")
-    if raw is None:
-        return None
-    return np.asarray(raw, dtype=float).reshape(3, 3)
-
-
-def _pair_cov(pair: MotionPair) -> np.ndarray:
-    raw = pair.metadata.get("cov")
-    if raw is None:
-        sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2))
-        return np.eye(3) * max(sigma, 1e-4) ** 2
-    return np.asarray(raw, dtype=float).reshape(3, 3)
-
-
-def _corrected_delta_r(
-    pair: MotionPair,
-    delta_bias: np.ndarray,
-    *,
-    imu: ImuSeries | None,
-    bias0: np.ndarray,
-) -> np.ndarray:
-    j_bg = _pair_j_bg(pair)
-    if j_bg is not None:
-        return apply_bias_jacobian_correction(pair.R_A, j_bg, delta_bias)
-    if imu is not None and "t_i_imu_s" in pair.metadata and "t_j_imu_s" in pair.metadata:
-        preint = preintegrate_gyro(
-            imu.t_s,
-            imu.gyro_rad_s,
-            float(pair.metadata["t_i_imu_s"]),
-            float(pair.metadata["t_j_imu_s"]),
-            bias0 + delta_bias,
-        )
-        return preint.delta_R
-    duration = float(pair.metadata.get("duration_s", max(pair.t_j_s - pair.t_i_s, 1e-3)))
-    return apply_constant_bias_correction(pair.R_A, duration, delta_bias)
-
-
-def _gravity_basis(g0: np.ndarray) -> np.ndarray:
-    """Return 3×2 orthonormal basis spanning the plane orthogonal to ``g0``."""
-
-    g = np.asarray(g0, dtype=float).reshape(3)
-    n = np.linalg.norm(g)
-    if n < 1e-9:
-        g = np.array([0.0, 0.0, -G_NORM])
-        n = G_NORM
-    g = g / n
-    axis = np.array([1.0, 0.0, 0.0]) if abs(g[0]) < 0.9 else np.array([0.0, 1.0, 0.0])
-    e1 = np.cross(g, axis)
-    e1 /= max(np.linalg.norm(e1), 1e-12)
-    e2 = np.cross(g, e1)
-    return np.column_stack([e1, e2])
-
-
-def _gravity_from_params(xy: np.ndarray, g0: np.ndarray, basis: np.ndarray) -> np.ndarray:
-    raw = np.asarray(g0, dtype=float).reshape(3) + basis @ np.asarray(xy, dtype=float).reshape(2)
-    n = float(np.linalg.norm(raw))
-    if n < 1e-9:
-        return np.asarray(g0, dtype=float).reshape(3)
-    return raw * (G_NORM / n)
-
-
-def _lidar_to_imu_relative(r_x: np.ndarray, t_x: np.ndarray, r_b: np.ndarray, t_b: np.ndarray):
-    """Map LiDAR relative pose to IMU: ``T_A = T_X T_B T_X^{-1}``."""
-
-    r_a = orthonormalize_rotation(r_x @ r_b @ r_x.T)
-    t_a = (np.eye(3) - r_a) @ t_x + r_x @ t_b
-    return r_a, t_a
-
-
-def _corrected_preint_quantities(
-    pair: MotionPair,
-    bg_i: np.ndarray,
-    ba_i: np.ndarray,
-    bg0: np.ndarray,
-    ba0: np.ndarray,
-) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
-    """First-order correct ΔR/Δv/Δp for keyframe biases vs preintegration biases."""
-
-    dbg = np.asarray(bg_i, dtype=float).reshape(3) - np.asarray(bg0, dtype=float).reshape(3)
-    dba = np.asarray(ba_i, dtype=float).reshape(3) - np.asarray(ba0, dtype=float).reshape(3)
-    j_bg = pair.metadata.get("J_bg9")
-    j_ba = pair.metadata.get("J_ba")
-    delta_v0 = np.asarray(pair.metadata.get("delta_v", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
-    delta_p0 = (
-        np.asarray(pair.t_A_m, dtype=float).reshape(3)
-        if pair.t_A_m is not None
-        else np.asarray(pair.metadata.get("delta_p", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
-    )
-    if j_bg is None or j_ba is None:
-        delta_r = apply_bias_jacobian_correction(
-            pair.R_A,
-            _pair_j_bg(pair) if _pair_j_bg(pair) is not None else np.zeros((3, 3)),
-            dbg,
-        )
-        return delta_r, delta_v0, delta_p0
-    j_bg_m = np.asarray(j_bg, dtype=float).reshape(9, 3)
-    j_ba_m = np.asarray(j_ba, dtype=float).reshape(9, 3)
-    delta_r = orthonormalize_rotation(pair.R_A @ so3_exp(j_bg_m[0:3] @ dbg))
-    delta_v = delta_v0 + j_bg_m[3:6] @ dbg + j_ba_m[3:6] @ dba
-    delta_p = delta_p0 + j_bg_m[6:9] @ dbg + j_ba_m[6:9] @ dba
-    return delta_r, delta_v, delta_p
-
-
-def _build_nav_rotations(
-    keyframe_ids: list[int],
-    id_to_idx: dict[int, int],
-    consecutive_pairs: dict[tuple[int, int], MotionPair],
-    r_x: np.ndarray,
-    t_x: np.ndarray,
-) -> list[np.ndarray]:
-    """Chain IMU orientations; restart at session/gap boundaries (no cross-link)."""
-
-    del id_to_idx
-    rotations = [np.eye(3) for _ in keyframe_ids]
-    for k in range(len(keyframe_ids) - 1):
-        a = keyframe_ids[k]
-        b = keyframe_ids[k + 1]
-        pair = consecutive_pairs.get((a, b))
-        if pair is None:
-            # Missing link or new session: start a fresh nav chain.
-            rotations[k + 1] = np.eye(3)
-            continue
-        t_b = np.zeros(3) if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float)
-        r_meas, _ = _lidar_to_imu_relative(r_x, t_x, pair.R_B, t_b)
-        rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas)
-    return rotations
-
-
-def _solve_phase_c_se3(
-    pairs: list[MotionPair],
-    r_x: 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,
-    t_init: np.ndarray | None = None,
-    t_prior: np.ndarray | None = None,
-    t_prior_sigma_m: np.ndarray | float | None = None,
-) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]:
-    """Keyframe IMU factor optimization for full SE(3)."""
-
-    notes: list[str] = []
-    usable = [pair for pair in pairs if pair.t_B_m is not None and "delta_v" in pair.metadata]
-    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_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] = {}
-    kf_session: dict[int, str] = {}
-    for pair in usable:
-        stamp[pair.i] = float(pair.metadata.get("t_i_imu_s", pair.t_i_s))
-        stamp[pair.j] = float(pair.metadata.get("t_j_imu_s", pair.t_j_s))
-        kf_session[pair.i] = pair.session_id
-        kf_session[pair.j] = pair.session_id
-    session_ids = sorted(set(kf_session.values()))
-    keyframe_ids: list[int] = []
-    for sid in session_ids:
-        local = [kid for kid, sess in kf_session.items() if sess == sid]
-        local.sort(key=lambda kid: stamp[kid])
-        keyframe_ids.extend(local)
-    k_count = len(keyframe_ids)
-    id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)}
-
-    consecutive_pairs: dict[tuple[int, int], MotionPair] = {}
-    for pair in usable:
-        if kf_session.get(pair.i) != kf_session.get(pair.j):
-            continue
-        if id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
-            consecutive_pairs[(pair.i, pair.j)] = pair
-    notes.append(
-        f"phase-C multi-session graph: sessions={len(session_ids)}, "
-        f"keyframes={k_count}, consecutive_links={len(consecutive_pairs)}"
-    )
-
-    g0 = np.asarray(gravity_init, dtype=float).reshape(3)
-    if np.linalg.norm(g0) < 1e-6:
-        g0 = np.array([0.0, 0.0, -G_NORM])
-    g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9))
-    basis = _gravity_basis(g0)
-    ba0 = np.zeros(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
-    n_b = 3 * k_count
-    dim = 3 + 3 + 2 + n_v + n_b + n_b
-    x0 = np.zeros(dim)
-    t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
-    x0[3:6] = t0
-    t_prior_vec = None if t_prior is None else np.asarray(t_prior, dtype=float).reshape(3)
-    if t_prior_sigma_m is None:
-        t_sigma = np.array([0.05, 0.05, 0.05], dtype=float)
-    else:
-        t_sigma = np.asarray(t_prior_sigma_m, dtype=float).reshape(-1)
-        if t_sigma.size == 1:
-            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] = bg_init
-
-    whitened = []
-    for pair in usable:
-        cov9 = pair.metadata.get("cov9")
-        if cov9 is None:
-            cov = _pair_cov(pair)
-            cov9_m = np.eye(9)
-            cov9_m[0:3, 0:3] = cov
-            cov9_m[3:6, 3:6] = np.eye(3) * 0.25
-            cov9_m[6:9, 6:9] = np.eye(3) * 1.0
-        else:
-            cov9_m = np.asarray(cov9, dtype=float).reshape(9, 9)
-        whitened.append(residual_whiten_matrix(cov9_m))
-
-    def unpack(vec: np.ndarray):
-        r_opt = orthonormalize_rotation(so3_exp(vec[0:3]) @ r_x)
-        t_opt = vec[3:6]
-        g_opt = _gravity_from_params(vec[6:8], g0, basis)
-        base = 8
-        vels = vec[base : base + n_v].reshape(k_count, 3)
-        base += n_v
-        bgs = vec[base : base + n_b].reshape(k_count, 3)
-        base += n_b
-        bas = vec[base : base + n_b].reshape(k_count, 3)
-        return r_opt, t_opt, g_opt, vels, bgs, bas
-
-    def residuals(vec: np.ndarray) -> np.ndarray:
-        r_opt, t_opt, g_opt, vels, bgs, bas = unpack(vec)
-        nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
-        out: list[np.ndarray] = []
-
-        for pair, whiten in zip(usable, whitened):
-            i_idx = id_to_idx[pair.i]
-            j_idx = id_to_idx[pair.j]
-            dt = float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s))
-            dt = max(dt, 1e-3)
-            delta_r, delta_v, delta_p = _corrected_preint_quantities(
-                pair, bgs[i_idx], bas[i_idx], bg0, ba0
-            )
-            t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
-            r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
-            r_i = nav_r[i_idx]
-            v_i = vels[i_idx]
-            v_j = vels[j_idx]
-
-            err_r = so3_log(delta_r.T @ r_meas)
-            err_v = v_j - v_i - g_opt * dt - r_i @ delta_v
-            err_p = r_i @ (t_meas - delta_p) - v_i * dt - 0.5 * g_opt * (dt**2)
-            err = np.concatenate([err_r, err_v, err_p])
-            w = np.sqrt(_pair_weight(pair))
-            out.append(w * (whiten @ err))
-
-        # Bias random-walk between consecutive keyframes (same session only).
-        for k in range(k_count - 1):
-            a = keyframe_ids[k]
-            b = keyframe_ids[k + 1]
-            if kf_session.get(a) != kf_session.get(b):
-                continue
-            dt = max(stamp[b] - stamp[a], 1e-3)
-            scale_g = 1.0 / (max(sigma_bg_rw, 1e-8) * np.sqrt(dt))
-            scale_a = 1.0 / (max(sigma_ba_rw, 1e-8) * np.sqrt(dt))
-            out.append(scale_g * (bgs[k + 1] - bgs[k]))
-            out.append(scale_a * (bas[k + 1] - bas[k]))
-
-        # Weak priors: first keyframe of each session + CAD/installation translation.
-        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] - 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))
-        else:
-            out.append(0.2 * t_opt)  # soft |t|~0 prior when no CAD prior
-        return np.concatenate(out)
-
-    # Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime.
-    opt = least_squares(residuals, x0, loss="huber", f_scale=0.05, max_nfev=80)
-    r_opt, t_opt, g_opt, vels, bgs, bas = unpack(opt.x)
-
-    rot_errs = []
-    trans_errs = []
-    nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
-    for pair in usable:
-        i_idx = id_to_idx[pair.i]
-        j_idx = id_to_idx[pair.j]
-        dt = max(float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s)), 1e-3)
-        delta_r, delta_v, delta_p = _corrected_preint_quantities(
-            pair, bgs[i_idx], bas[i_idx], bg0, ba0
-        )
-        t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
-        r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
-        r_i = nav_r[i_idx]
-        err_r = so3_log(delta_r.T @ r_meas)
-        err_p = r_i @ (t_meas - delta_p) - vels[i_idx] * dt - 0.5 * g_opt * (dt**2)
-        rot_errs.append(np.degrees(np.linalg.norm(err_r)))
-        trans_errs.append(float(np.linalg.norm(err_p)))
-        del delta_v, j_idx
-
-    rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
-    trans_rms = float(np.sqrt(np.mean(np.square(trans_errs)))) if trans_errs else 1e9
-    bg_mean = np.mean(bgs, axis=0)
-    ba_mean = np.mean(bas, axis=0)
-    notes.append(
-        "phase-C SE3 (Δv/Δp + g + keyframe v/bias RW): "
-        f"keyframes={k_count}, pairs={len(usable)}, "
-        f"|t|={float(np.linalg.norm(t_opt)):.3f} m, "
-        f"|g|={float(np.linalg.norm(g_opt)):.3f}, "
-        f"trans_rms={trans_rms:.3f} m"
-    )
-    return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes
-
-
-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,
-    *,
-    force_rotation_only: bool = False,
-    imu: ImuSeries | None = None,
-    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,
-    t_prior_m: np.ndarray | None = None,
-    t_prior_sigma_m: np.ndarray | float | None = None,
-) -> JointExtrinsicResult:
-    """Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3)."""
-
-    del delta_t_s  # reserved for future SE(3) time coupling
-    if enable_phase_c is None:
-        enable_phase_c = not force_rotation_only
-
-    usable = [pair for pair in pairs if pair.t_B_m is not None]
-    observability = analyze_observability(usable, r_x)
-    notes = list(observability.notes)
-
-    r = orthonormalize_rotation(np.asarray(r_x, dtype=float))
-    bias0 = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
-    t_seed = None if t_init_m is None else np.asarray(t_init_m, dtype=float).reshape(3)
-    weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
-    whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable]
-    prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
-
-    def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray:
-        residuals = []
-        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(whiten @ err)
-        residuals.append(prior_w * delta_bias)
-        return np.concatenate(residuals) if residuals else np.zeros(0)
-
-    def residual_rot_bias(vec: np.ndarray) -> np.ndarray:
-        r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
-        return rotation_residuals(r_opt, vec[3:])
-
-    if usable:
-        opt = least_squares(
-            residual_rot_bias,
-            np.zeros(6),
-            loss="huber",
-            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 (single Σ whitening + J_bg): "
-            f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
-            f"pairs={len(usable)}"
-        )
-    else:
-        bias_out = bias0
-        delta_bias = np.zeros(3)
-        notes.append("no pairs for joint refine")
-
-    rot_errs = []
-    for pair in usable:
-        delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
-        err = preintegration_rotation_residual(delta_r, r, pair.R_B)
-        rot_errs.append(np.degrees(np.linalg.norm(err)))
-    rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
-
-    t = np.zeros(3) if t_seed is None else t_seed.copy()
-    translation_accepted = False
-    trans_rms = 1e9
-    gravity_out: np.ndarray | None = None
-    accel_bias_out: np.ndarray | None = None
-
-    if gravity_init_m_s2 is None:
-        gravity_init = np.array([0.0, 0.0, -G_NORM])
-    else:
-        gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3)
-
-    if t_prior_m is not None:
-        notes.append(
-            "using CAD/installation translation prior "
-            f"t={np.asarray(t_prior_m, dtype=float).reshape(3).tolist()}"
-        )
-
-    if (
-        enable_phase_c
-        and not force_rotation_only
-        and observability.translation_observable
-        and observability.rotation_observable
-        and len(usable) >= 5
-    ):
-        r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3(
-            usable,
-            r,
-            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,
-            t_prior_sigma_m=t_prior_sigma_m,
-        )
-        notes.extend(c_notes)
-        translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
-        if not translation_accepted:
-            # 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)
-                notes.append(
-                    "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")
-                t = np.zeros(3)
-    elif (
-        not force_rotation_only
-        and observability.translation_observable
-        and observability.rotation_observable
-        and len(usable) >= 5
-    ):
-        # Legacy hand-eye translation fallback when Phase-C metadata missing.
-        def residual_se3(vec: np.ndarray) -> np.ndarray:
-            r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
-            t_opt = vec[3:]
-            residuals = []
-            for pair, weight, whiten in zip(usable, weights, whitens):
-                delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
-                residuals.append(
-                    np.sqrt(weight) * (whiten @ preintegration_rotation_residual(delta_r, r_opt, pair.R_B))
-                )
-                pred = (pair.R_A - np.eye(3)) @ t_opt
-                meas = r_opt @ np.asarray(pair.t_B_m, dtype=float)
-                residuals.append(np.sqrt(weight) * (pred - meas))
-            if t_prior_m is not None:
-                sigma = np.asarray(t_prior_sigma_m if t_prior_sigma_m is not None else 0.05, dtype=float)
-                if sigma.size == 1:
-                    sigma = np.full(3, float(sigma), dtype=float)
-                residuals.append((t_opt - np.asarray(t_prior_m, dtype=float).reshape(3)) / np.maximum(sigma, 1e-3))
-            return np.concatenate(residuals)
-
-        x_se3 = np.zeros(6)
-        if t_seed is not None:
-            x_se3[3:] = t_seed
-        elif t_prior_m is not None:
-            x_se3[3:] = np.asarray(t_prior_m, dtype=float).reshape(3)
-        opt_t = least_squares(residual_se3, x_se3, loss="huber", f_scale=0.05, max_nfev=200)
-        r = orthonormalize_rotation(so3_exp(opt_t.x[:3]) @ r)
-        t = opt_t.x[3:]
-        rot_errs = []
-        trans_errs = []
-        for pair in usable:
-            delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
-            rot_errs.append(np.degrees(np.linalg.norm(preintegration_rotation_residual(delta_r, r, pair.R_B))))
-            pred = (pair.R_A - np.eye(3)) @ t
-            meas = r @ np.asarray(pair.t_B_m, dtype=float)
-            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
-        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 = 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)")
-
-    return JointExtrinsicResult(
-        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=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

@@ -1,50 +0,0 @@
-"""LiDAR keyframe selection."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-
-from .contracts import LidarFrame
-from .registration import register_lidar_pair
-
-
-@dataclass(frozen=True)
-class KeyframeSet:
-    indices: tuple[int, ...]
-    frames: tuple[LidarFrame, ...]
-
-
-def build_keyframes(
-    frames: list[LidarFrame],
-    *,
-    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."""
-
-    if not frames:
-        return KeyframeSet((), ())
-
-    selected = [0]
-    last = 0
-    for index in range(1, len(frames)):
-        if index - last > max_frame_gap:
-            selected.append(index)
-            last = index
-            continue
-        result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
-        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)
-            last = index
-
-    if selected[-1] != len(frames) - 1 and len(frames) > 1:
-        selected.append(len(frames) - 1)
-
-    unique = tuple(dict.fromkeys(selected))
-    return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))

imu_lidar/lidar_deskew.py

@@ -1,59 +0,0 @@
-"""Coarse LiDAR deskew using a constant body rate over the sweep."""
-
-from __future__ import annotations
-
-import numpy as np
-
-from .contracts import ImuSeries, LidarFrame
-from .geometry import so3_exp
-from .time_offset import lidar_time_to_imu_time
-
-
-def deskew_lidar_frames(
-    frames: list[LidarFrame],
-    imu: ImuSeries,
-    *,
-    delta_t_s: float,
-    R_IMU_lidar: np.ndarray | None = None,
-    gyro_bias_rad_s: np.ndarray | None = None,
-) -> list[LidarFrame]:
-    """Return deskewed copies when extrinsic is known; otherwise return originals."""
-
-    if R_IMU_lidar is None:
-        return frames
-
-    bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
-    r_li = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3).T
-    output: list[LidarFrame] = []
-
-    for frame in frames:
-        n = frame.points_xyz.shape[0]
-        if n < 10:
-            output.append(frame)
-            continue
-        t_mid_imu = lidar_time_to_imu_time(frame.t_mid_s, delta_t_s)
-        index = int(np.clip(np.searchsorted(imu.t_s, t_mid_imu), 1, imu.t_s.size - 1))
-        omega_lidar = r_li @ (imu.gyro_rad_s[index] - bias)
-        duration = max(frame.t_end_s - frame.t_start_s, 1e-3)
-        rel = np.linspace(-0.5, 0.5, n) * duration
-        deskewed = np.empty_like(frame.points_xyz)
-        # Piecewise-constant rotation over a few time bins.
-        bins = 12
-        edges = np.linspace(-0.5 * duration, 0.5 * duration, bins + 1)
-        for b in range(bins):
-            mask = (rel >= edges[b]) & (rel <= edges[b + 1] if b == bins - 1 else rel < edges[b + 1])
-            if not np.any(mask):
-                continue
-            tau = 0.5 * (edges[b] + edges[b + 1])
-            rot = so3_exp(omega_lidar * float(tau))
-            deskewed[mask] = frame.points_xyz[mask] @ rot.T
-        output.append(
-            LidarFrame(
-                frame_id=frame.frame_id,
-                t_start_s=frame.t_start_s,
-                t_end_s=frame.t_end_s,
-                points_xyz=deskewed,
-                path=frame.path,
-            )
-        )
-    return output

imu_lidar/lidar_io.py

@@ -1,127 +0,0 @@
-"""LiDAR adapters for the V1 standard intermediate format.
-
-Accepted input: a directory containing ``frames_index.csv`` and per-frame NPZ files.
-
-frames_index.csv
-----------------
-frame_id,file,t_start,t_end
-
-Each NPZ referenced by ``file`` must contain:
-- points: float array shaped (N, 3) in LiDAR Cartesian coordinates (metres)
-"""
-
-from __future__ import annotations
-
-from pathlib import Path
-
-import numpy as np
-
-from .contracts import LidarFrame
-
-
-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}")
-
-    rows = np.genfromtxt(index_path, delimiter=",", names=True, dtype=None, encoding="utf-8")
-    if rows.ndim == 0:
-        rows = np.array([rows])
-    names = set(rows.dtype.names or ())
-    # NumPy may rename reserved name ``file`` to ``file_``.
-    file_key = "filename" if "filename" in names else ("file_" if "file_" in names else "file")
-    required = {"frame_id", "t_start", "t_end"}
-    if not required.issubset(names) or file_key not in names:
-        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:
-        frame_id = str(row["frame_id"])
-        rel = str(row[file_key])
-        npz_path = root / rel
-        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[+])")
-        frames.append(
-            LidarFrame(
-                frame_id=frame_id,
-                t_start_s=float(row["t_start"]),
-                t_end_s=float(row["t_end"]),
-                points_xyz=points[:, :3],
-                path=npz_path,
-            )
-        )
-    frames.sort(key=lambda frame: frame.t_mid_s)
-    return frames
-
-
-def save_lidar_session(
-    root: Path | str,
-    frames: list[LidarFrame],
-    *,
-    points_dirname: str = "frames",
-) -> None:
-    """Write a LiDAR session directory in the standard intermediate format."""
-
-    destination = Path(root)
-    frames_dir = destination / points_dirname
-    frames_dir.mkdir(parents=True, exist_ok=True)
-    index_rows: list[str] = ["frame_id,filename,t_start,t_end"]
-    for index, frame in enumerate(frames):
-        relative = f"{points_dirname}/frame_{index:05d}.npz"
-        np.savez_compressed(destination / relative, points=np.asarray(frame.points_xyz, dtype=float))
-        index_rows.append(f"{frame.frame_id},{relative},{frame.t_start_s:.9f},{frame.t_end_s:.9f}")
-    (destination / "frames_index.csv").write_text("\n".join(index_rows) + "\n", encoding="utf-8")

imu_lidar/motion_pairs.py

@@ -1,220 +0,0 @@
-"""Build IMU/LiDAR relative-motion pairs for hand-eye calibration."""
-
-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
-
-from .contracts import ImuSeries, LidarFrame, MotionPair
-from .geometry import make_transform, rotation_angle_deg
-from .imu_preintegration import preintegrate_imu
-from .registration import register_lidar_pair
-from .time_offset import lidar_time_to_imu_time
-
-
-@dataclass(frozen=True)
-class MotionPairSet:
-    pairs: tuple[MotionPair, ...]
-    notes: tuple[str, ...] = ()
-
-
-def build_motion_pairs(
-    *,
-    session_id: str,
-    keyframes: list[LidarFrame],
-    keyframe_indices: list[int] | tuple[int, ...],
-    imu: ImuSeries,
-    delta_t_s: float,
-    gyro_bias_rad_s: np.ndarray | None = None,
-    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.
-
-    IMU side uses full Phase-C preintegration (``ΔR/Δv/Δp``, ``Σ9``, ``J_bg/J_ba``).
-    Rotation hand-eye still consumes ``R_A = ΔR`` only.
-    """
-
-    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
-
-            t_i_imu = lidar_time_to_imu_time(frame_i.t_mid_s, delta_t_s)
-            t_j_imu = lidar_time_to_imu_time(frame_j.t_mid_s, delta_t_s)
-            if t_j_imu <= t_i_imu:
-                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,
-                imu.gyro_rad_s,
-                imu.acc_m_s2,
-                t_i_imu,
-                t_j_imu,
-                bias_g,
-                bias_a,
-            )
-            r_a = preint.delta_R
-            r_b = reg.transform[:3, :3]
-            t_b = reg.transform[:3, 3]
-            rot_a = rotation_angle_deg(r_a)
-            if abs(rot_a - reg.rotation_deg) > max(15.0, 1.0 * max(rot_a, reg.rotation_deg)):
-                continue
-
-            pairs.append(
-                MotionPair(
-                    session_id=session_id,
-                    i=int(keyframe_indices[i]),
-                    j=int(keyframe_indices[j]),
-                    t_i_s=frame_i.t_mid_s,
-                    t_j_s=frame_j.t_mid_s,
-                    R_A=r_a,
-                    R_B=r_b,
-                    t_A_m=np.asarray(preint.delta_p, dtype=float),
-                    t_B_m=np.asarray(t_b, dtype=float),
-                    fitness=reg.fitness,
-                    metadata={
-                        "backend": reg.backend,
-                        "rotation_deg_B": reg.rotation_deg,
-                        "translation_m_B": reg.translation_m,
-                        "rotation_deg_A": rot_a,
-                        "weight": preint.weight,
-                        "duration_s": preint.duration_s,
-                        "mean_gyro_norm": preint.mean_gyro_norm,
-                        "preint_sigma_rad": preint.sigma_rad,
-                        "cov": preint.cov[0:3, 0:3].tolist(),
-                        "cov9": preint.cov.tolist(),
-                        "J_bg": preint.J_bg[0:3, 0:3].tolist(),
-                        "J_bg9": preint.J_bg.tolist(),
-                        "J_ba": preint.J_ba.tolist(),
-                        "delta_v": preint.delta_v.tolist(),
-                        "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))
-
-
-def pairs_to_transforms(pairs: tuple[MotionPair, ...]) -> tuple[list[np.ndarray], list[np.ndarray]]:
-    """Helper returning SE(3) lists when translations are present."""
-
-    a_list: list[np.ndarray] = []
-    b_list: list[np.ndarray] = []
-    for pair in pairs:
-        if pair.t_B_m is None:
-            continue
-        t_a = np.zeros(3) if pair.t_A_m is None else pair.t_A_m
-        a_list.append(make_transform(t_a, pair.R_A))
-        b_list.append(make_transform(pair.t_B_m, pair.R_B))
-    return a_list, b_list

imu_lidar/motion_pairs_io.py

@@ -1,154 +0,0 @@
-"""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

@@ -1,114 +0,0 @@
-"""Normalized-Jacobian observability analysis for rotation / SE(3) gates."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-
-from .contracts import MotionPair
-from .geometry import skew, so3_log
-
-
-@dataclass(frozen=True)
-class ObservabilityReport:
-    rotation_observable: bool
-    translation_observable: bool
-    condition_rotation: float
-    condition_translation: float
-    notes: tuple[str, ...] = ()
-
-
-def _rotation_jacobian(pairs: list[MotionPair], r_x: np.ndarray) -> np.ndarray:
-    rows = []
-    for pair in pairs:
-        # Residual r = log(R_x^T R_A R_x R_B^T); approximate J w.r.t. left perturbation of R_x.
-        # Use finite-difference columns for robustness in V1.
-        base = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
-        cols = []
-        eps = 1e-5
-        for axis in range(3):
-            delta = np.zeros(3)
-            delta[axis] = eps
-            r_pert = r_x @ (np.eye(3) + skew(delta))
-            # Orthonormalize lightly
-            u, _, vt = np.linalg.svd(r_pert)
-            r_pert = u @ vt
-            pert = so3_log(r_pert.T @ pair.R_A @ r_pert @ pair.R_B.T)
-            cols.append((pert - base) / eps)
-        rows.append(np.column_stack(cols))
-    return np.vstack(rows) if rows else np.zeros((0, 3))
-
-
-def analyze_observability(
-    pairs: list[MotionPair] | tuple[MotionPair, ...],
-    r_x: np.ndarray,
-    *,
-    condition_threshold: float = 100.0,
-) -> ObservabilityReport:
-    """Gate whether rotation-only or full SE(3) should be accepted."""
-
-    usable = list(pairs)
-    notes: list[str] = []
-    if len(usable) < 3:
-        return ObservabilityReport(False, False, 1e9, 1e9, ("insufficient pairs",))
-
-    j_r = _rotation_jacobian(usable, np.asarray(r_x, dtype=float))
-    if j_r.size == 0:
-        return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
-
-    singular = np.linalg.svd(j_r, compute_uv=False)
-    cond_r = float(singular[0] / max(singular[-1], 1e-12))
-    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 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 not observable: condition={cond_r:.1f}, "
-            f"min_information={rotation_information:.3e}"
-        )
-    else:
-        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: condition={cond_t:.1f}, "
-            f"min_information={translation_information:.3e}; "
-            "full SE3 will be rejected"
-        )
-    return ObservabilityReport(
-        rotation_observable=rotation_ok,
-        translation_observable=translation_ok,
-        condition_rotation=cond_r,
-        condition_translation=cond_t,
-        notes=tuple(notes),
-    )

imu_lidar/phase_a_replay.py

@@ -1,290 +0,0 @@
-"""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

@@ -1,941 +0,0 @@
-"""Executable LiDAR–IMU calibration pipeline (V1)."""
-
-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
-
-from .contracts import (
-    CalibrationMode,
-    CalibrationRequest,
-    CalibrationResult,
-    CalibrationStatus,
-    MotionPair,
-    SessionInput,
-)
-from .finalize import finalize_result
-from .imu_audit import audit_imu
-from .imu_io import load_imu_samples
-from .joint_optimizer import solve_joint_extrinsic
-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
-from .vehicle_config import load_vehicle_config, prior_enabled
-
-# Remap keyframe indices so multi-session Phase-C graphs do not collide.
-_SESSION_INDEX_OFFSET = 1_000_000
-
-
-def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult:
-    return TimeOffsetResult(
-        delta_t_s=refined.delta_t_s,
-        correlation_peak=refined.correlation_peak,
-        search_s=previous.search_s,
-        notes=tuple(list(previous.notes) + list(refined.notes)),
-        ok=True,
-    )
-
-
-@dataclass(frozen=True)
-class PipelineStage:
-    name: str
-    responsibility: str
-
-
-STAGES = (
-    PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"),
-    PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"),
-    PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"),
-    PipelineStage("time_offset", "各会话独立粗估/精修 δt"),
-    PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"),
-    PipelineStage("motion_pairs", "各会话构造运动对，再合并"),
-    PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"),
-    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."""
-
-    return STAGES
-
-
-def _build_pairs_and_handeye(
-    *,
-    session_id: str,
-    working_frames,
-    imu,
-    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),
-        keyframe_indices=keyframes.indices,
-        imu=imu,
-        delta_t_s=delta_t_s,
-        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,
-    )
-    return keyframes, pair_set, handeye
-
-
-def _translation_prior_from_config(
-    vehicle_config: dict[str, Any] | None,
-) -> tuple[np.ndarray | None, np.ndarray | float | None]:
-    if vehicle_config is None or not prior_enabled(vehicle_config, "translation_prior"):
-        return None, None
-    init_cfg = vehicle_config.get("initialization") or {}
-    tp = init_cfg.get("translation_prior") or {}
-    if tp.get("t_IMU_lidar_m") is None:
-        return None, None
-    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)}
-
-    fixed_time_offset_s = (
-        session.fixed_time_offset_s
-        if session.fixed_time_offset_s is not None
-        else request.fixed_time_offset_s
-    )
-    if fixed_time_offset_s is not None:
-        offset_source = "fixed"
-        offset = TimeOffsetResult(
-            delta_t_s=float(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) 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
-
-    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,
-                delta_t_s=offset.delta_t_s,
-                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,
-            imu=imu,
-            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",
-                "session_id": session.session_id,
-                "iteration": iteration,
-                "time_offset": asdict(offset),
-                "imu_audit": asdict(imu_report),
-                "timestamp_audit": asdict(ts),
-                "keyframes": 0 if keyframes is None else len(keyframes.indices),
-                "pair_notes": pairs_notes,
-                "handeye": asdict(handeye),
-            }
-        r_x = handeye.R_IMU_lidar
-
-        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,
-                delta_t_s=offset.delta_t_s,
-                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,
-                imu=imu,
-                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",
-                    "session_id": session.session_id,
-                    "iteration": iteration,
-                    "time_offset": asdict(offset),
-                    "imu_audit": asdict(imu_report),
-                    "timestamp_audit": asdict(ts),
-                    "keyframes": 0 if keyframes is None else len(keyframes.indices),
-                    "pair_notes": pairs_notes,
-                    "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))
-    if acc_n > 1e-6:
-        gravity_init = -acc_mean * (9.80665 / acc_n)
-    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,
-        "pairs": tuple(pair_set.pairs),
-        "gyro_bias_rad_s": np.asarray(imu_report.gyro_bias_rad_s, dtype=float).reshape(3),
-        "gravity_init_m_s2": gravity_init,
-        "timestamp_audit": asdict(ts),
-        "imu_audit": {
-            **asdict(imu_report),
-            "gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(),
-            "static_acc_mean_m_s2": imu_report.static_acc_mean_m_s2.tolist(),
-        },
-        "time_offset": asdict(offset),
-        "time_offset_s": float(offset.delta_t_s),
-        "keyframes": len(keyframes.indices),
-        "pair_count": pair_count,
-        "pair_notes": pairs_notes,
-        "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,
-            "R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
-        },
-    }
-
-
-def _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]:
-    merged: list[MotionPair] = []
-    for index, prep in enumerate(prepared):
-        id_offset = (index + 1) * _SESSION_INDEX_OFFSET
-        for pair in prep["pairs"]:
-            merged.append(
-                replace(
-                    pair,
-                    i=int(pair.i) + id_offset,
-                    j=int(pair.j) + id_offset,
-                )
-            )
-    return merged
-
-
-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 finish(
-            status=CalibrationStatus.BLOCKED,
-            message="no sessions provided",
-            details={},
-        )
-
-    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
-            _emit_progress(
-                progress_callback,
-                1,
-                "blocked",
-                reason="vehicle_config",
-                error=str(exc),
-            )
-            return finish(
-                status=CalibrationStatus.BLOCKED,
-                message=f"vehicle config failed: {exc}",
-                details={},
-            )
-    _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]] = []
-    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 finish(
-                status=CalibrationStatus.BLOCKED,
-                message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})",
-                details={"sessions": [prep]},
-            )
-        prepared.append(prep)
-
-    all_pairs = _remap_pairs_for_joint(prepared)
-    pair_counts_per_session = {
-        p["session_id"]: int(p["pair_count"]) for p in prepared
-    }
-    _emit_progress(
-        progress_callback,
-        7,
-        "joint_handeye_start",
-        session_count=len(prepared),
-        merged_pair_count=len(all_pairs),
-        pair_counts_per_session=pair_counts_per_session,
-    )
-    handeye_started_at = perf_counter()
-    handeye = solve_rotation_handeye(
-        all_pairs,
-        R_prior=r_prior,
-        prior_sigma_deg=prior_sigma_deg,
-    )
-    _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={
-                "sessions": [_public_session(p) for p in prepared],
-                "joint_handeye": asdict(handeye),
-                "merged_pair_count": len(all_pairs),
-            },
-        )
-
-    force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
-    t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
-    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,
-        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),
-                "vehicle_config_loaded": vehicle_config is not None,
-                "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"]),
-                    "R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
-                },
-                "joint": {
-                    "translation_accepted": joint.translation_accepted,
-                    "residual_rms_rot_deg": joint.residual_rms_rot_deg,
-                    "residual_rms_trans_m": joint.residual_rms_trans_m,
-                    "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 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(),
-                    "gravity_m_s2": None
-                    if joint.gravity_m_s2 is None
-                    else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
-                },
-                "translation_accepted": joint.translation_accepted,
-                "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)
-    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)"
-        else:
-            status = CalibrationStatus.FULL_SE3_REJECTED
-            message = (
-                f"rotation accepted jointly ({len(prepared)} sessions); "
-                "translation deferred until Phase-B/C session-state redesign"
-            )
-    elif joint.observability.rotation_observable:
-        status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
-        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 finish(
-        status=status,
-        message=message,
-        details={
-            "sessions": session_results,
-            "joint": {
-                "session_count": len(prepared),
-                "merged_pair_count": len(all_pairs),
-                "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=None if status == CalibrationStatus.BLOCKED else T,
-        time_offset_s=delta_t,
-        motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared),
-    )
-
-
-def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
-    payload = dict(session_result)
-    payload.pop("T_IMU_lidar", None)
-    payload.pop("pairs", None)
-    payload.pop("gyro_bias_rad_s", None)
-    payload.pop("gravity_init_m_s2", None)
-    return payload

imu_lidar/registration.py

@@ -1,159 +0,0 @@
-"""LiDAR relative-motion registration.
-
-Uses Open3D Generalized ICP when available; otherwise a NumPy point-to-point ICP.
-"""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-
-from .contracts import LidarFrame
-from .geometry import make_transform, orthonormalize_rotation, rotation_angle_deg, so3_log
-
-
-@dataclass(frozen=True)
-class RegistrationResult:
-    transform: np.ndarray
-    fitness: float
-    rotation_deg: float
-    translation_m: float
-    backend: str
-    ok: bool
-
-
-def _voxel_downsample(points: np.ndarray, voxel: float) -> np.ndarray:
-    if points.shape[0] == 0:
-        return points
-    quantized = np.floor(points / voxel).astype(np.int64)
-    _, unique_indices = np.unique(quantized, axis=0, return_index=True)
-    return points[np.sort(unique_indices)]
-
-
-def _numpy_icp(
-    source: np.ndarray,
-    target: np.ndarray,
-    *,
-    max_iterations: int = 30,
-    max_correspondence: float = 1.0,
-) -> RegistrationResult:
-    src = _voxel_downsample(source, 0.2)
-    tgt = _voxel_downsample(target, 0.2)
-    if src.shape[0] < 50 or tgt.shape[0] < 50:
-        return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "numpy_icp", False)
-
-    # Subsample for speed.
-    rng = np.random.default_rng(0)
-    if src.shape[0] > 4000:
-        src = src[rng.choice(src.shape[0], 4000, replace=False)]
-    if tgt.shape[0] > 8000:
-        tgt = tgt[rng.choice(tgt.shape[0], 8000, replace=False)]
-
-    r = np.eye(3)
-    t = np.zeros(3)
-    last_error = 1e9
-    inlier_ratio = 0.0
-    for _ in range(max_iterations):
-        transformed = src @ r.T + t
-        # Nearest neighbour in target via brute force on chunks.
-        diff = transformed[:, None, :] - tgt[None, :, :]
-        dist2 = np.sum(diff * diff, axis=2)
-        nn = np.argmin(dist2, axis=1)
-        dist = np.sqrt(dist2[np.arange(src.shape[0]), nn])
-        mask = dist < max_correspondence
-        inlier_ratio = float(np.mean(mask))
-        if np.count_nonzero(mask) < 30:
-            break
-        p = transformed[mask]
-        q = tgt[nn[mask]]
-        mu_p = p.mean(axis=0)
-        mu_q = q.mean(axis=0)
-        h = (p - mu_p).T @ (q - mu_q)
-        u, _, vt = np.linalg.svd(h)
-        r_delta = vt.T @ u.T
-        if np.linalg.det(r_delta) < 0:
-            vt[-1, :] *= -1
-            r_delta = vt.T @ u.T
-        t_delta = mu_q - r_delta @ mu_p
-        # Update global transform: x' = r_delta (r x + t) + t_delta
-        r = orthonormalize_rotation(r_delta @ r)
-        t = r_delta @ t + t_delta
-        mean_err = float(np.mean(dist[mask]))
-        if abs(last_error - mean_err) < 1e-4:
-            break
-        last_error = mean_err
-
-    transform = make_transform(t, r)
-    return RegistrationResult(
-        transform=transform,
-        fitness=inlier_ratio,
-        rotation_deg=rotation_angle_deg(r),
-        translation_m=float(np.linalg.norm(t)),
-        backend="numpy_icp",
-        ok=inlier_ratio > 0.15,
-    )
-
-
-def _open3d_gicp(source: np.ndarray, target: np.ndarray) -> RegistrationResult | None:
-    try:
-        import open3d as o3d
-    except ImportError:
-        return None
-
-    src = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(source))
-    tgt = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(target))
-    src = src.voxel_down_sample(0.2)
-    tgt = tgt.voxel_down_sample(0.2)
-    if len(src.points) < 50 or len(tgt.points) < 50:
-        return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "open3d_gicp", False)
-    src.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
-    tgt.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
-    result = o3d.pipelines.registration.registration_generalized_icp(
-        src,
-        tgt,
-        1.0,
-        np.eye(4),
-        o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),
-        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50),
-    )
-    transform = np.asarray(result.transformation, dtype=float)
-    return RegistrationResult(
-        transform=transform,
-        fitness=float(result.fitness),
-        rotation_deg=rotation_angle_deg(transform[:3, :3]),
-        translation_m=float(np.linalg.norm(transform[:3, 3])),
-        backend="open3d_gicp",
-        ok=float(result.fitness) > 0.15,
-    )
-
-
-def register_lidar_pair(source_points: np.ndarray, target_points: np.ndarray) -> RegistrationResult:
-    """Register source -> target and return ``T_target_source``."""
-
-    source = np.asarray(source_points, dtype=float).reshape(-1, 3)
-    target = np.asarray(target_points, dtype=float).reshape(-1, 3)
-    open3d_result = _open3d_gicp(source, target)
-    if open3d_result is not None:
-        return open3d_result
-    return _numpy_icp(source, target)
-
-
-def estimate_frame_rotations(
-    frames: list[LidarFrame],
-    *,
-    stride: int = 1,
-) -> tuple[list[np.ndarray], list[tuple[float, float]]]:
-    """Estimate consecutive (or strided) LiDAR relative rotations for time sync."""
-
-    rotations: list[np.ndarray] = []
-    pair_times: list[tuple[float, float]] = []
-    for index in range(0, len(frames) - stride, max(stride, 1)):
-        a = frames[index]
-        b = frames[index + stride]
-        result = register_lidar_pair(b.points_xyz, a.points_xyz)
-        if not result.ok:
-            continue
-        rotations.append(result.transform[:3, :3])
-        pair_times.append((a.t_mid_s, b.t_mid_s))
-    return rotations, pair_times

imu_lidar/rotation_handeye.py

@@ -1,217 +0,0 @@
-"""SO(3) rotation hand-eye solver for ``R_A R_X = R_X R_B``."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-from scipy.optimize import least_squares
-
-from .contracts import MotionPair
-from .geometry import orthonormalize_rotation, rotation_angle_deg, skew, so3_exp, so3_log
-
-
-@dataclass(frozen=True)
-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, ...] = ()
-
-
-def _pair_weight(pair: MotionPair) -> float:
-    weight = float(pair.metadata.get("weight", 1.0))
-    if not np.isfinite(weight) or weight <= 0:
-        return 1.0
-    return weight
-
-
-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] = []
-    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(float(pair_weight))
-        rows.append(w * skew(alpha + beta))
-        rhs.append(w * (beta - alpha))
-    if len(rows) < 2:
-        return np.eye(3)
-    a = np.vstack(rows)
-    b = np.concatenate(rhs)
-    try:
-        rotvec, *_ = np.linalg.lstsq(a, b, rcond=None)
-    except np.linalg.LinAlgError:
-        return np.eye(3)
-    return orthonormalize_rotation(so3_exp(rotvec))
-
-
-def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
-    err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
-    return float(np.degrees(np.linalg.norm(err)))
-
-
-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)))
-
-
-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",),
-        )
-
-    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(
-        "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:
-        return so3_log(r)
-
-    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)
-    r_x = unpack(opt.x)
-    errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in usable], dtype=float)
-    # 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")
-    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 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

@@ -1,332 +0,0 @@
-"""Constant IMU-to-LiDAR clock-offset estimation via angular-rate correlation."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-from scipy import signal
-
-from .contracts import ImuSeries, LidarFrame
-from .geometry import rotation_angle_deg, so3_log
-from .registration import estimate_frame_rotations
-
-
-@dataclass(frozen=True)
-class TimeOffsetResult:
-    delta_t_s: float
-    correlation_peak: float
-    search_s: float
-    notes: tuple[str, ...] = ()
-    ok: bool = True
-
-
-def _magnitude_series(times: np.ndarray, values: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
-    mag = np.linalg.norm(values, axis=1) if values.ndim == 2 else np.asarray(values, dtype=float)
-    return np.asarray(times, dtype=float), np.asarray(mag, dtype=float)
-
-
-def _correlate_offset(
-    imu_t: np.ndarray,
-    imu_mag: np.ndarray,
-    lidar_t: np.ndarray,
-    lidar_mag: np.ndarray,
-    *,
-    search_s: float,
-    sample_hz: float,
-) -> tuple[float, float]:
-    """Return ``(delta_t, peak)`` for ``t_imu = t_lidar + delta_t``.
-
-    Implementation: resample both on LiDAR-relative grid, shift IMU by candidate
-    offsets, maximize normalized correlation. This avoids ambiguous lag signs.
-    """
-
-    t_start = float(lidar_t[0])
-    t_end = float(lidar_t[-1])
-    if t_end - t_start < 0.5:
-        return 0.0, 0.0
-    dt = 1.0 / sample_hz
-    grid = np.arange(t_start, t_end, dt)
-    lidar_sig = np.interp(grid, lidar_t, lidar_mag, left=0.0, right=0.0)
-    lidar_sig = lidar_sig - np.mean(lidar_sig)
-    lidar_norm = float(np.linalg.norm(lidar_sig)) + 1e-12
-
-    best_delta = 0.0
-    best_peak = -1.0
-    for delta in np.arange(-search_s, search_s + 1e-12, dt):
-        imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
-        imu_sig = imu_sig - np.mean(imu_sig)
-        denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
-        peak = float(np.dot(imu_sig, lidar_sig) / denom)
-        if peak > best_peak:
-            best_peak = peak
-            best_delta = float(delta)
-
-    # Local parabolic refinement.
-    deltas = np.array([best_delta - dt, best_delta, best_delta + dt], dtype=float)
-    peaks = []
-    for delta in deltas:
-        imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
-        imu_sig = imu_sig - np.mean(imu_sig)
-        denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
-        peaks.append(float(np.dot(imu_sig, lidar_sig) / denom))
-    y0, y1, y2 = peaks
-    denom = y0 - 2 * y1 + y2
-    if abs(denom) > 1e-12:
-        refined = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
-        # Parabola can jump outside the searched window; keep it clamped.
-        if abs(refined) <= search_s + dt:
-            best_delta = refined
-        best_peak = float(y1)
-    return best_delta, best_peak
-
-
-def estimate_time_offset(
-    imu: ImuSeries,
-    frames: list[LidarFrame],
-    *,
-    gyro_bias_rad_s: np.ndarray | None = None,
-    search_s: float = 1.0,
-    sample_hz: float = 50.0,
-) -> TimeOffsetResult:
-    """Estimate ``t_imu = t_lidar + delta_t``.
-
-    Positive ``delta_t`` means the IMU clock reading is ahead of the LiDAR clock
-    for the same physical instant (IMU timestamps are larger).
-    """
-
-    notes: list[str] = []
-    if len(frames) < 5:
-        return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR frames",), False)
-
-    bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
-    gyro = imu.gyro_rad_s - bias
-
-    # Use short consecutive (or near-consecutive) pairs. A large stride (e.g.
-    # len//20) averages over many seconds and destroys |ω| correlation even when
-    # host/device clocks are already aligned.
-    stride = 1 if len(frames) < 80 else 2
-    rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
-    if len(rotations) < 8:
-        rotations, pair_times = estimate_frame_rotations(frames, stride=1)
-    if len(rotations) < 4:
-        return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR relative rotations",), False)
-
-    lidar_t = []
-    lidar_w = []
-    for (t_a, t_b), rotation in zip(pair_times, rotations):
-        dt_pair = max(t_b - t_a, 1e-3)
-        omega = so3_log(rotation) / dt_pair
-        lidar_t.append(0.5 * (t_a + t_b))
-        lidar_w.append(omega)
-    lidar_t_arr = np.asarray(lidar_t, dtype=float)
-    lidar_w_arr = np.asarray(lidar_w, dtype=float)
-
-    imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
-    lidar_t_mag, lidar_mag = _magnitude_series(lidar_t_arr, lidar_w_arr)
-    delta, peak = _correlate_offset(
-        imu_t,
-        imu_mag,
-        lidar_t_mag,
-        lidar_mag,
-        search_s=search_s,
-        sample_hz=sample_hz,
-    )
-
-    notes.append(
-        f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
-    )
-    notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
-    # Host-UTC-bridged sessions are already on one timeline; |ω| peak can stay
-    # weak even at the correct lag (ICP rate vs gyro scale). Accept near-zero δt.
-    near_zero = abs(float(delta)) <= min(0.05, 0.25 * float(search_s))
-    ok = peak > 0.15 or near_zero
-    if peak <= 0.15 and near_zero:
-        notes.append(
-            f"correlation peak weak ({peak:.3f}) but |delta_t|={abs(delta):.4f}s ~0; "
-            "accepting as already-aligned (e.g. host UTC bridge)"
-        )
-    elif not ok:
-        notes.append("correlation peak is weak; check overlapping motion and axis units")
-    return TimeOffsetResult(
-        delta_t_s=delta,
-        correlation_peak=peak,
-        search_s=search_s,
-        notes=tuple(notes),
-        ok=ok,
-    )
-
-
-def lidar_time_to_imu_time(t_lidar_s: float, delta_t_s: float) -> float:
-    """Convert a LiDAR timestamp to the IMU clock using ``t_imu = t_lidar + delta_t``."""
-
-    return float(t_lidar_s + delta_t_s)
-
-
-def _lidar_omega_series(
-    frames: list[LidarFrame],
-    *,
-    stride: int,
-) -> tuple[np.ndarray, np.ndarray]:
-    rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
-    if len(rotations) < 4:
-        rotations, pair_times = estimate_frame_rotations(frames, stride=1)
-    lidar_t: list[float] = []
-    lidar_w: list[np.ndarray] = []
-    for (t_a, t_b), rotation in zip(pair_times, rotations):
-        dt_pair = max(t_b - t_a, 1e-3)
-        omega = so3_log(rotation) / dt_pair
-        lidar_t.append(0.5 * (t_a + t_b))
-        lidar_w.append(omega)
-    return np.asarray(lidar_t, dtype=float), np.asarray(lidar_w, dtype=float)
-
-
-def refine_time_offset_signed(
-    imu: ImuSeries,
-    frames: list[LidarFrame],
-    *,
-    delta_t_s: float,
-    R_IMU_lidar: np.ndarray,
-    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``.
-
-    Cost: mean squared error between ``gyro_imu(t_lidar+δt)`` and
-    ``R_IMU_lidar @ omega_lidar(t_lidar)`` on a common grid around the coarse ``δt``.
-    """
-
-    notes: list[str] = [f"signed refine around coarse delta_t={delta_t_s:.6f}s"]
-    if len(frames) < 5:
-        return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR frames",), False)
-
-    bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
-    gyro = imu.gyro_rad_s - bias
-    r_x = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3)
-
-    stride = max(1, len(frames) // 20)
-    lidar_t, lidar_w = _lidar_omega_series(frames, stride=stride)
-    if lidar_t.size < 4:
-        return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR omega samples",), False)
-
-    # Predicted IMU-frame angular rate from LiDAR relative rotations.
-    pred = (r_x @ lidar_w.T).T
-    t_start = float(lidar_t[0])
-    t_end = float(lidar_t[-1])
-    if t_end - t_start < 0.5:
-        return TimeOffsetResult(delta_t_s, 0.0, search_s, ("LiDAR span too short for signed refine",), False)
-
-    dt = 1.0 / sample_hz
-    grid = np.arange(t_start, t_end, dt)
-    pred_grid = np.column_stack(
-        [np.interp(grid, lidar_t, pred[:, axis], left=np.nan, right=np.nan) for axis in range(3)]
-    )
-
-    def _cost_and_corr(delta: float) -> tuple[float, float]:
-        meas = np.column_stack(
-            [
-                np.interp(grid + delta, imu.t_s, gyro[:, axis], left=np.nan, right=np.nan)
-                for axis in range(3)
-            ]
-        )
-        mask = np.isfinite(pred_grid).all(axis=1) & np.isfinite(meas).all(axis=1)
-        if int(np.count_nonzero(mask)) < 10:
-            return float("inf"), -1.0
-        err = meas[mask] - pred_grid[mask]
-        cost = float(np.mean(np.sum(err * err, axis=1)))
-        a = meas[mask].reshape(-1)
-        b = pred_grid[mask].reshape(-1)
-        a = a - np.mean(a)
-        b = b - np.mean(b)
-        corr = float(np.dot(a, b) / ((np.linalg.norm(a) + 1e-12) * (np.linalg.norm(b) + 1e-12)))
-        return cost, corr
-
-    coarse_cost, coarse_corr = _cost_and_corr(float(delta_t_s))
-    best_delta = float(delta_t_s)
-    best_cost = coarse_cost
-    best_corr = coarse_corr
-    half = abs(float(search_s))
-    for delta in np.arange(delta_t_s - half, delta_t_s + half + 1e-12, dt):
-        cost, corr = _cost_and_corr(float(delta))
-        if cost < best_cost:
-            best_cost = cost
-            best_delta = float(delta)
-            best_corr = corr
-
-    # Parabolic refine on cost around the best discrete delta.
-    samples = []
-    for delta in (best_delta - dt, best_delta, best_delta + dt):
-        cost, _ = _cost_and_corr(float(delta))
-        samples.append(cost if np.isfinite(cost) else best_cost)
-    y0, y1, y2 = samples
-    denom = y0 - 2 * y1 + y2
-    if abs(denom) > 1e-12 and y1 <= y0 and y1 <= y2:
-        candidate = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
-        cand_cost, cand_corr = _cost_and_corr(candidate)
-        if cand_cost < best_cost:
-            best_delta = candidate
-            best_cost = cand_cost
-            best_corr = cand_corr
-
-    # Guard with magnitude correlation so ICP-biased signed minima cannot wander.
-    imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
-    lidar_t_mag, lidar_mag = _magnitude_series(lidar_t, lidar_w)
-
-    def _mag_score(delta: float) -> float:
-        t_start_l = float(lidar_t_mag[0])
-        t_end_l = float(lidar_t_mag[-1])
-        grid_m = np.arange(t_start_l, t_end_l, dt)
-        lidar_sig = np.interp(grid_m, lidar_t_mag, lidar_mag, left=0.0, right=0.0)
-        lidar_sig = lidar_sig - np.mean(lidar_sig)
-        imu_sig = np.interp(grid_m + delta, imu_t, imu_mag, left=0.0, right=0.0)
-        imu_sig = imu_sig - np.mean(imu_sig)
-        denom = (float(np.linalg.norm(lidar_sig)) + 1e-12) * (float(np.linalg.norm(imu_sig)) + 1e-12)
-        return float(np.dot(imu_sig, lidar_sig) / denom)
-
-    mag_at_coarse = _mag_score(float(delta_t_s))
-    mag_at_best = _mag_score(best_delta)
-
-    notes.append(
-        f"signed 3-axis refine: delta_t={best_delta:.6f}s, "
-        f"mse={best_cost:.4g} (coarse_mse={coarse_cost:.4g}), "
-        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.98
-        # Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
-        and mag_at_best + 1e-4 >= mag_at_coarse
-    )
-    if not improved:
-        notes.append("signed refine rejected by MSE/mag-consistency; 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,
-        )
-    return TimeOffsetResult(
-        delta_t_s=best_delta,
-        correlation_peak=mag_at_best,
-        search_s=search_s,
-        notes=tuple(notes),
-        ok=True,
-    )

imu_lidar/timestamp_audit.py

@@ -1,78 +0,0 @@
-"""Timestamp audit for IMU and LiDAR streams."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-
-import numpy as np
-
-from .contracts import ImuSeries, LidarFrame
-
-
-@dataclass(frozen=True)
-class TimestampAuditReport:
-    monotonic: bool
-    epoch_count: int
-    imu_rate_hz: float
-    lidar_rate_hz: float
-    imu_duration_s: float
-    lidar_duration_s: float
-    max_imu_gap_s: float
-    max_lidar_gap_s: float
-    notes: tuple[str, ...] = ()
-    ok: bool = True
-
-
-def _rate_and_gaps(times: np.ndarray) -> tuple[float, float]:
-    if times.size < 2:
-        return 0.0, 0.0
-    dt = np.diff(times)
-    positive = dt[dt > 0]
-    if positive.size == 0:
-        return 0.0, float("inf")
-    rate = float(1.0 / np.median(positive))
-    return rate, float(np.max(dt))
-
-
-def audit_timestamps(imu: ImuSeries, frames: list[LidarFrame]) -> TimestampAuditReport:
-    """Audit native timestamps without assuming the two clocks share an epoch."""
-
-    notes: list[str] = []
-    imu_t = imu.t_s
-    lidar_t = np.asarray([frame.t_mid_s for frame in frames], dtype=float)
-
-    imu_mono = bool(np.all(np.diff(imu_t) >= 0)) if imu_t.size > 1 else False
-    lidar_mono = bool(np.all(np.diff(lidar_t) >= 0)) if lidar_t.size > 1 else False
-    if not imu_mono:
-        notes.append("IMU timestamps are not monotonic")
-    if not lidar_mono:
-        notes.append("LiDAR timestamps are not monotonic")
-
-    imu_rate, imu_gap = _rate_and_gaps(imu_t)
-    lidar_rate, lidar_gap = _rate_and_gaps(lidar_t)
-    if imu_t.size < 50:
-        notes.append(f"IMU sample count is low ({imu_t.size})")
-    if len(frames) < 5:
-        notes.append(f"LiDAR frame count is low ({len(frames)})")
-    if imu_gap > 0.05:
-        notes.append(f"large IMU gap detected: {imu_gap:.3f}s")
-    if lidar_gap > 1.0:
-        notes.append(f"large LiDAR gap detected: {lidar_gap:.3f}s")
-
-    notes.append(
-        "IMU and LiDAR clocks are treated as independent; constant offset is estimated later."
-    )
-
-    ok = imu_mono and lidar_mono and imu_t.size >= 50 and len(frames) >= 5
-    return TimestampAuditReport(
-        monotonic=imu_mono and lidar_mono,
-        epoch_count=2,
-        imu_rate_hz=imu_rate,
-        lidar_rate_hz=lidar_rate,
-        imu_duration_s=float(imu_t[-1] - imu_t[0]) if imu_t.size else 0.0,
-        lidar_duration_s=float(lidar_t[-1] - lidar_t[0]) if lidar_t.size else 0.0,
-        max_imu_gap_s=imu_gap,
-        max_lidar_gap_s=lidar_gap,
-        notes=tuple(notes),
-        ok=ok,
-    )

imu_lidar/vehicle_config.py

@@ -1,83 +0,0 @@
-"""Vehicle-installation configuration loading and light validation."""
-
-from __future__ import annotations
-
-from collections.abc import Mapping
-from pathlib import Path
-from typing import Any
-
-REQUIRED_TOP_LEVEL_KEYS = frozenset({"schema_version", "vehicle", "installation", "sensors", "time"})
-
-
-def validate_config_shape(config: Mapping[str, object]) -> list[str]:
-    """Return missing top-level keys without inventing default values."""
-
-    return sorted(REQUIRED_TOP_LEVEL_KEYS.difference(config))
-
-
-def validate_config_semantics(config: Mapping[str, Any]) -> list[str]:
-    """Return semantic issues that block calibration interpretation."""
-
-    issues: list[str] = []
-    sensors = config.get("sensors")
-    if not isinstance(sensors, Mapping):
-        return ["sensors must be a mapping"]
-    imu = sensors.get("imu")
-    lidar = sensors.get("lidar")
-    if not isinstance(imu, Mapping):
-        issues.append("sensors.imu missing")
-    else:
-        axes = ((imu.get("raw_frame") or {}) if isinstance(imu.get("raw_frame"), Mapping) else {}).get("axes")
-        if not axes:
-            issues.append("sensors.imu.raw_frame.axes is empty (declare axis meaning even if approximate)")
-    if not isinstance(lidar, Mapping):
-        issues.append("sensors.lidar missing")
-    else:
-        axes = ((lidar.get("raw_frame") or {}) if isinstance(lidar.get("raw_frame"), Mapping) else {}).get("axes")
-        if not axes:
-            issues.append("sensors.lidar.raw_frame.axes is empty (declare axis meaning even if approximate)")
-
-    time_cfg = config.get("time")
-    if not isinstance(time_cfg, Mapping):
-        issues.append("time missing")
-    else:
-        for key in ("imu_timestamp_source", "lidar_timestamp_source", "lidar_frame_time_definition"):
-            if not time_cfg.get(key):
-                issues.append(f"time.{key} is empty")
-    return issues
-
-
-def load_vehicle_config(path: str | Path) -> dict[str, Any]:
-    """Load and lightly validate a YAML vehicle configuration."""
-
-    try:
-        import yaml
-    except ImportError as exc:  # pragma: no cover
-        raise ImportError("PyYAML is required to load vehicle configuration files") from exc
-
-    config_path = Path(path)
-    with config_path.open("r", encoding="utf-8") as handle:
-        loaded = yaml.safe_load(handle)
-    if not isinstance(loaded, dict):
-        raise ValueError(f"vehicle config must be a mapping: {config_path}")
-
-    missing = validate_config_shape(loaded)
-    if missing:
-        raise ValueError(f"vehicle config missing keys {missing}: {config_path}")
-
-    semantic = validate_config_semantics(loaded)
-    if semantic:
-        raise ValueError("vehicle config semantic issues:\n- " + "\n- ".join(semantic))
-    return loaded
-
-
-def prior_enabled(config: Mapping[str, Any], name: str) -> bool:
-    """Return whether an optional prior is enabled."""
-
-    init = config.get("initialization")
-    if not isinstance(init, Mapping):
-        return False
-    prior = init.get(name)
-    if not isinstance(prior, Mapping):
-        return False
-    return bool(prior.get("enabled", False))

imu_lidar/文件职责说明.md

@@ -1,82 +0,0 @@
-# `imu_lidar` 模块说明
-
-**用途：** 改 `imu_lidar/` 源码时查阅。对外用法见根目录 [`README.md`](../README.md)。
-
-本包实现 LiDAR–IMU 外参标定：在连续行驶数据上选取关键帧，用 IMU 预积分与雷达配准构造相对运动对，求解安装外参。
-
-```text
-A ≈ 关键帧间 IMU 相对运动（预积分：旋转 / 速度增量 / 位移增量）
-B ≈ 关键帧间雷达配准
-解 R_A R_X = R_X R_B  →  旋转外参（手眼阶段只用旋转）
-再精修旋转与陀螺零偏；在完整六自由度模式下，可观时再估计平移等
-```
-
-入口：
-
-```powershell
-python -m imu_lidar.cli plan
-python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output ...
-```
-
-整体流程由 `pipeline.py` 串联。  
-修改本目录代码时，请同步更新本说明，并在 [`CHANGELOG.md`](CHANGELOG.md) 追加「时间戳 + 原本 → 改成」。
-
----
-
-## 流水线顺序与文件
-
-
-| 顺序  | 文件                      | 作用                            |
-| --- | ----------------------- | ----------------------------- |
-| 0   | `contracts.py`          | 公共数据类型与状态枚举                   |
-| 0   | `geometry.py`           | 刚体变换与旋转工具                     |
-| 0   | `vehicle_config.py`     | 读取并校验车辆 YAML                  |
-| 1   | `imu_io.py`             | 读标准 IMU 中间格式                  |
-| 1   | `lidar_io.py`           | 读标准雷达会话目录                     |
-| 2   | `timestamp_audit.py`    | 时间单调 / 频率 / 空洞检查              |
-| 3   | `imu_audit.py`          | 静止零偏、加速度模长检查、建议竖直轴            |
-| 4   | `time_offset.py`        | 粗估时间偏置 δt，并用旋转外参精修            |
-| 5   | `registration.py`       | 帧间点云配准                        |
-| 5   | `keyframes.py`          | 按运动量抽取关键帧                     |
-| 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（含 `motion_pairs.json`） |
-| —   | `pipeline.py`           | 编排全流程                         |
-| —   | `cli.py`                | 命令行入口                         |
-| —   | `CHANGELOG.md`          | 改动记录                          |
-
-
----
-
-## 运行模式要点
-
-- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。
-- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。
-- `--mode full_se3`：当前完成 Phase-A 后明确拒绝平移；待 Phase-B/C 会话状态重构完成后再恢复完整 SE(3) 交付。
-
----
-
-## 输入格式
-
-```text
-imu.csv                         # t,gx,gy,gz,ax,ay,az（建议设备时间）
-lidar_session/
-  frames_index.csv              # frame_id,filename,t_start,t_end
-  frames/frame_XXXXX.npz        # points: (N,3) 米
-```
-
-原始 N300 `.rscap` + H32 dlog（或旧 MSOP `.rscap`）用仓库工具导出：`python tools/export_rscap_to_v1.py ...`（见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md)）。
-
----
-
-## 当前能力
-
-- 本包是仓库**唯一**标定路径：质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →（可选）完整六自由度 → 报告  
-- 点云去畸变：可选  
-- 阶段与用法见根目录 [`README.md`](../README.md)  
-- 改动史：[`CHANGELOG.md`](CHANGELOG.md)

pyproject.toml

@@ -1,28 +0,0 @@
-[project]
-name = "lidar-imu-calibration"
-version = "0.3.0"
-description = "LiDAR–IMU extrinsic calibration from continuous-motion keyframes (imu_lidar)"
-requires-python = ">=3.10"
-dependencies = [
-  "numpy>=1.26",
-  "scipy>=1.11",
-  "pyyaml>=6.0",
-]
-
-[project.optional-dependencies]
-open3d = ["open3d>=0.17"]
-dev = ["pytest>=7.4"]
-
-[project.scripts]
-lidar-imu-calibration = "imu_lidar.cli:main"
-
-[build-system]
-requires = ["setuptools>=68", "wheel"]
-build-backend = "setuptools.build_meta"
-
-[tool.setuptools]
-packages = ["imu_lidar", "tools"]
-
-[tool.pytest.ini_options]
-testpaths = ["tests"]
-pythonpath = ["."]

requirements.txt

@@ -0,0 +1,4 @@
+numpy>=1.26
+scipy>=1.11
+open3d>=0.18
+small-gicp

results/README.md

@@ -0,0 +1,15 @@
+# 当前车辆标定结果
+
+本目录只保存当前车辆、当前传感器安装条件下的最终可交付结果；不保存历史车辆数据、原始采集包、点云帧或中间配准产物。
+
+## 2026-08 车辆 / 27 个静止站点
+
+目录 [`vehicle_20260808/`](vehicle_20260808/) 对应本机运行目录 `D:\data\rtk_lidar_run\outputs_vehicle_h19165`。
+
+- 外参：`final_T_RTK_lidar.json`
+- 质量摘要：`summary.json`
+- 坐标约定：`p_RTK = T_RTK_lidar · p_lidar`，RTK 为车头向前坐标系（`HeadingOffsetDeg=-90`）。
+- RTK 参考点高度：1.9165 m（ANT1 相位中心）。
+- 质量：27 个站点、20 个共识运动对、平移 RMS 0.07116 m、旋转 RMS 0.98209°。
+
+这些文件记录的是 host 时间关联版本的现有最终结果。后续采用 RTK 测量时间重新导出后，应写入新的结果目录，不能覆盖本目录。

results/vehicle_20260808/final_T_RTK_lidar.json

@@ -0,0 +1,337 @@
+{
+  "schema_version": 1,
+  "success": true,
+  "convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
+  "equation": "A_RTK_ij X = X B_LiDAR_ij",
+  "frames": {
+    "RTK": {
+      "origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
+      "x_axis": "vehicle forward after applying the configured G90 heading offset",
+      "y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
+      "z_axis": "up",
+      "yaw_enu_deg": "90 - (rawHeadingDeg + -90)",
+      "frame_mode": "vehicle_forward_heading_offset"
+    },
+    "LiDAR": {
+      "description": "raw Helios sensor frame from points_raw polar decode",
+      "x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
+      "y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
+      "z_axis": "up",
+      "origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ"
+    }
+  },
+  "backend": "consensus",
+  "measured_lidar_extrinsic_used_as_initial": true,
+  "solver_initial_extrinsic": "D:\\First-dev-dept\\calibration-rtk-run\\run\\rtk_lidar_mechanical_initial.json",
+  "body_heading_offset_deg": -90.0,
+  "body_heading_offset_used": true,
+  "body_antenna_lever_xy_used": false,
+  "translation_m": [
+    0.21782224963960972,
+    -0.41134780227275347,
+    0.1065423366878719
+  ],
+  "rotation_rpy_deg_xyz": [
+    0.06623859235262805,
+    0.8096624730962496,
+    -0.5513220563826681
+  ],
+  "quaternion_xyzw": [
+    0.00061201333859398,
+    0.007062715262034178,
+    -0.004815137247499642,
+    0.9999632782988027
+  ],
+  "coordinate_contract_audit": {
+    "status": "no_near_180_degree_axis_conflict",
+    "requires_physical_axis_confirmation": false,
+    "mechanical_initial_path": "D:\\First-dev-dept\\calibration-rtk-run\\run\\rtk_lidar_mechanical_initial.json",
+    "mechanical_self_consistency": {
+      "baseline_points": "vehicle_right",
+      "frame_mode": "vehicle_forward_heading_offset",
+      "heading_offset_deg": -90.0,
+      "consistent": true,
+      "issues": []
+    },
+    "solution_vs_declared_baseline_side": {
+      "baseline_points": "vehicle_right",
+      "solution_yaw_deg": -0.5513220563826681,
+      "expected_yaw_deg": 0.0,
+      "yaw_error_deg": 0.5513220563826735,
+      "xy_error_m": 0.007516661287052368,
+      "z_error_m": 0.0280423356878719,
+      "mixed_translation_rotation_inheritance": false,
+      "near_expected_pose": true
+    },
+    "solution_relative_to_mechanical_initial": {
+      "translation_m": 0.029032271487700816,
+      "rotation_deg": 0.9820426535863694,
+      "delta_matrix_4x4": [
+        [
+          0.9998538650128304,
+          0.009638565806830942,
+          0.014119017957784103,
+          0.006962889639609726
+        ],
+        [
+          -0.009621275903042714,
+          0.9999528797859223,
+          -0.0012919976154994906,
+          0.002831671727246521
+        ],
+        [
+          -0.014130805670674627,
+          0.0011559658421926345,
+          0.9998994869856016,
+          0.0280423356878719
+        ],
+        [
+          0.0,
+          0.0,
+          0.0,
+          1.0
+        ]
+      ]
+    },
+    "note": "No automatic 180-degree correction was applied. Confirm static GNHPR left/right vs vehicle heading and Helios +X vs vehicle forward before deployment."
+  },
+  "matrix_4x4": [
+    [
+      0.9998538650128304,
+      0.009638565806830942,
+      0.014119017957784103,
+      0.21782224963960972
+    ],
+    [
+      -0.009621275903042714,
+      0.9999528797859223,
+      -0.0012919976154994906,
+      -0.41134780227275347
+    ],
+    [
+      -0.014130805670674627,
+      0.0011559658421926345,
+      0.9998994869856016,
+      0.1065423366878719
+    ],
+    [
+      0.0,
+      0.0,
+      0.0,
+      1.0
+    ]
+  ],
+  "quality": {
+    "stations": 27,
+    "pairs": 20,
+    "residuals": {
+      "pairs": 20,
+      "translation_m": {
+        "rms": 0.07116027693011169,
+        "median": 0.048258124379040895,
+        "p90": 0.10707349630410129,
+        "p95": 0.12100345665884242,
+        "max": 0.1859157912711514
+      },
+      "rotation_deg": {
+        "rms": 0.9820870524210346,
+        "median": 0.6015253089423158,
+        "p90": 1.626986719630708,
+        "p95": 1.7485896823567941,
+        "max": 2.6898815510329674
+      },
+      "per_pair": [
+        {
+          "pair_index": 0,
+          "translation_m": 0.10590532722733068,
+          "rotation_deg": 0.5565962122599133
+        },
+        {
+          "pair_index": 1,
+          "translation_m": 0.11758701799503664,
+          "rotation_deg": 0.36543717366192036
+        },
+        {
+          "pair_index": 2,
+          "translation_m": 0.07568813061789224,
+          "rotation_deg": 1.6990480050580474
+        },
+        {
+          "pair_index": 3,
+          "translation_m": 0.07918615709103373,
+          "rotation_deg": 0.33723851079591255
+        },
+        {
+          "pair_index": 4,
+          "translation_m": 0.04618587731787657,
+          "rotation_deg": 0.21044665580544528
+        },
+        {
+          "pair_index": 5,
+          "translation_m": 0.1859157912711514,
+          "rotation_deg": 2.6898815510329674
+        },
+        {
+          "pair_index": 6,
+          "translation_m": 0.015032558607699278,
+          "rotation_deg": 0.2283712127949818
+        },
+        {
+          "pair_index": 7,
+          "translation_m": 0.013237326674396453,
+          "rotation_deg": 1.6189799101387812
+        },
+        {
+          "pair_index": 8,
+          "translation_m": 0.07003654121179845,
+          "rotation_deg": 0.6464544056247182
+        },
+        {
+          "pair_index": 9,
+          "translation_m": 0.093450766078183,
+          "rotation_deg": 0.6476769159531274
+        },
+        {
+          "pair_index": 10,
+          "translation_m": 0.015054656507824072,
+          "rotation_deg": 1.041955302231996
+        },
+        {
+          "pair_index": 11,
+          "translation_m": 0.02338150242985638,
+          "rotation_deg": 0.9212619385214977
+        },
+        {
+          "pair_index": 12,
+          "translation_m": 0.01577916809950981,
+          "rotation_deg": 0.5021648028537498
+        },
+        {
+          "pair_index": 13,
+          "translation_m": 0.05033037144020521,
+          "rotation_deg": 0.31352985458971655
+        },
+        {
+          "pair_index": 14,
+          "translation_m": 0.059761151158357104,
+          "rotation_deg": 0.8078349560587313
+        },
+        {
+          "pair_index": 15,
+          "translation_m": 0.04160156756834453,
+          "rotation_deg": 0.32936437427192145
+        },
+        {
+          "pair_index": 16,
+          "translation_m": 0.02424387832291359,
+          "rotation_deg": 0.430799739443549
+        },
+        {
+          "pair_index": 17,
+          "translation_m": 0.05938006964566194,
+          "rotation_deg": 0.3855347569405268
+        },
+        {
+          "pair_index": 18,
+          "translation_m": 0.003922740066294881,
+          "rotation_deg": 0.8803664894801937
+        },
+        {
+          "pair_index": 19,
+          "translation_m": 0.02258390614293418,
+          "rotation_deg": 0.9489255319885953
+        }
+      ]
+    },
+    "weighted_jacobian_condition_number": 7.551077537197385,
+    "linearized_one_sigma": {
+      "translation_m": [
+        0.01109415838733884,
+        0.011043247019191056,
+        0.0049617743728656676
+      ],
+      "rotation_deg": [
+        0.08809558088476348,
+        0.08691570892756702,
+        0.13552099560800782
+      ],
+      "warning": "conditional local estimate; bootstrap is the primary stability check"
+    },
+    "bootstrap": {
+      "runs": 200,
+      "order": [
+        "x_m",
+        "y_m",
+        "z_m",
+        "roll_deg",
+        "pitch_deg",
+        "yaw_deg"
+      ],
+      "std": [
+        0.006235825930578342,
+        0.004851737742140203,
+        0.0004474159829207979,
+        0.07186455153709871,
+        0.11762318545143821,
+        0.061763211598034336
+      ],
+      "p025": [
+        0.20955504469154063,
+        -0.4174678620205485,
+        0.10568919463688603,
+        -0.06202286115194262,
+        0.5207464314096704,
+        -0.6639417037512721
+      ],
+      "p975": [
+        0.23211472114546824,
+        -0.4001091835786546,
+        0.1074353326322909,
+        0.23746760471402314,
+        1.0462704287205609,
+        -0.43455346485967655
+      ]
+    }
+  },
+  "z_constraint": {
+    "observable_from_planar_AX_XB": false,
+    "method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
+    "rtk_reference_height_above_ground_m": 1.9165,
+    "warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
+  },
+  "important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
+  "selection": {
+    "recommended": true,
+    "reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
+    "open3d_vs_small_gicp": {
+      "translation_m": 0.00312519750472982,
+      "rotation_deg": 0.11907006217018351,
+      "delta_matrix_4x4": [
+        [
+          0.9999996872332217,
+          -0.00030752729270551075,
+          -0.0007286703114418758,
+          0.000533303946557151
+        ],
+        [
+          0.00030892706731572284,
+          0.9999981058814015,
+          0.0019216653392719056,
+          -0.002909732881513971
+        ],
+        [
+          0.0007280779667146176,
+          -0.0019218898442212235,
+          0.9999978881187204,
+          0.001007919095162138
+        ],
+        [
+          0.0,
+          0.0,
+          0.0,
+          1.0
+        ]
+      ]
+    }
+  }
+}

results/vehicle_20260808/summary.json

@@ -0,0 +1,50 @@
+{
+  "final": {
+    "translation_m": [
+      0.21782224963960972,
+      -0.41134780227275347,
+      0.1065423366878719
+    ],
+    "rotation_rpy_deg_xyz": [
+      0.06623859235262805,
+      0.8096624730962496,
+      -0.5513220563826681
+    ],
+    "pairs": 20,
+    "translation_rms_m": 0.07116027693011169,
+    "rotation_rms_deg": 0.9820870524210346,
+    "condition_number": 7.551077537197385,
+    "coordinate_contract_status": "no_near_180_degree_axis_conflict",
+    "recommended_for_deployment": true
+  },
+  "backend_difference": {
+    "translation_m": 0.00312519750472982,
+    "rotation_deg": 0.11907006217018351,
+    "delta_matrix_4x4": [
+      [
+        0.9999996872332217,
+        -0.00030752729270551075,
+        -0.0007286703114418758,
+        0.000533303946557151
+      ],
+      [
+        0.00030892706731572284,
+        0.9999981058814015,
+        0.0019216653392719056,
+        -0.002909732881513971
+      ],
+      [
+        0.0007280779667146176,
+        -0.0019218898442212235,
+        0.9999978881187204,
+        0.001007919095162138
+      ],
+      [
+        0.0,
+        0.0,
+        0.0,
+        1.0
+      ]
+    ]
+  }
+}

run/README.md

@@ -0,0 +1,72 @@
+# run目录
+
+根 README 含完整复现与本次结果说明；这里只列入口职责。
+
+| 脚本 | 用途 |
+|---|---|
+| `run_full_pipeline.ps1` | 站目录导出 `combined/` 后跑到 `T_RTK_lidar` |
+| `export_multisensor_stations.ps1` | 薄封装：`tools/export_raw_to_combined.py` |
+| `prepare_multisensor_dataset.ps1` | 每站一帧 + RTK 位姿（默认含双天线 pitch/roll） |
+| `run_direct_rtk_lidar.ps1` | 从 `combined/` 标定并封装最终结果（**默认车头向前 -90**） |
+| `run_single_dataset.ps1` | 地面、双 GICP、精筛、共识、AX=XB |
+| `run_joint_rtk_lidar.ps1` | 多批共识对联合求解 |
+| `view_result.ps1` | 3D 运动对对比 |
+| `rtk_lidar_mechanical_initial.json` | 仅 AX=XB 初值；**禁止**用于 pair |
+
+## 默认参数（匹配当前约 2 m 车顶雷达 / 车头向前）
+
+| 参数 | 默认 |
+|---|---|
+| `HeadingOffsetDeg` | `-90`（车头向前；主从装反、基线朝右） |
+| `GroundZMin/Max` | `-2.5` / `-1.5` |
+| `ExpectedStations` | `27` |
+| `MinStations` | `20` |
+| `RtkReferenceHeightAboveGroundM` | **无默认，必填**（本车 1.9165） |
+
+pair 注册**不传** `--initial-extrinsic`。
+
+## 原始 → combined
+
+站目录：
+
+```powershell
+python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu-rscap ... --out ... --overwrite
+```
+
+- `-TimeBasis device_gnss`（默认）：设备时 ↔ GNSS  
+- `-TimeBasis host`：主机接收时间  
+
+G90 连续录制 + 站时间窗：
+
+```powershell
+python tools\export_g90_h32_windows_to_combined.py `
+  --segments-csv <rtk_lidar_station_segments.csv> `
+  --lidar-dlog <dump_1.zip> --lidar-dlog <dump_2.zip> `
+  --rtk-rscap <g90_1.rscap> --rtk-rscap <g90_2.rscap> `
+  --out <output_root> --expected-stations 27 --frame-stride 5
+```
+
+可加 `--reuse-export` 续跑。
+
+## 已有 combined 复现本次结果
+
+```powershell
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
+  -CombinedRoot "D:\data\rtk_lidar_run\combined" `
+  -WorkRoot "D:\data\rtk_lidar_run\prepared_vehicle_h19165" `
+  -OutputRoot "D:\data\rtk_lidar_run\outputs_vehicle_h19165" `
+  -RtkReferenceHeightAboveGroundM 1.9165 `
+  -HeadingOffsetDeg -90 `
+  -ExpectedStations 27 `
+  -GroundZMin -2.5 -GroundZMax -1.5
+```
+
+## 可视化本次结果
+
+```powershell
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
+  -Frames "D:\data\rtk_lidar_run\prepared_vehicle_h19165\frames_all" `
+  -Pairs "D:\data\rtk_lidar_run\outputs_vehicle_h19165\consensus\B_consensus.npz" `
+  -Extrinsic "D:\data\rtk_lidar_run\outputs_vehicle_h19165\final_T_RTK_lidar.json" `
+  -PairIndex 0
+```

run/export_multisensor_stations.ps1

@@ -0,0 +1,58 @@
+param(
+    [Parameter(Mandatory = $true)][string]$DataRoot,
+    [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [Parameter(Mandatory = $true)][string]$RtkCapture,
+    [Parameter(Mandatory = $true)][string]$ImuCapture,
+    [string]$LidarObject = "frontlidar",
+    [string]$LidarCaptureName = "h32.rscap",
+    [string]$Timezone = "+08:00",
+    [string[]]$StationNames = @(),
+    [int]$Stride = 1,
+    [double]$RtkMaxDtMs = 150.0,
+    [double]$ImuBeforeMs = 100.0,
+    [double]$ImuAfterMs = 100.0,
+    [ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss",
+    [switch]$SkipLidarExport,
+    [switch]$SkipSerialParsing
+)
+
+$ErrorActionPreference = "Stop"
+$RepoRoot = Split-Path -Parent $PSScriptRoot
+$Exporter = Join-Path $RepoRoot "tools\export_raw_to_combined.py"
+
+foreach ($Path in @($DataRoot, $RtkCapture, $ImuCapture)) {
+    if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
+}
+if ($Stride -lt 1) { throw "Stride must be at least 1" }
+if ($SkipLidarExport -or $SkipSerialParsing) {
+    throw "Partial skip flags are no longer supported; use tools/export_raw_to_combined.py internals or run_direct_rtk_lidar.ps1 on an existing combined/"
+}
+
+$Args = @(
+    $Exporter,
+    "--stations-root", $DataRoot,
+    "--rtk-rscap", $RtkCapture,
+    "--imu-rscap", $ImuCapture,
+    "--out", $OutputRoot,
+    "--lidar-capture-name", $LidarCaptureName,
+    "--lidar-object", $LidarObject,
+    "--timezone", $Timezone,
+    "--stride", "$Stride",
+    "--rtk-max-dt-ms", "$RtkMaxDtMs",
+    "--imu-before-ms", "$ImuBeforeMs",
+    "--imu-after-ms", "$ImuAfterMs",
+    "--time-basis", $TimeBasis,
+    "--overwrite"
+)
+foreach ($Name in $StationNames) {
+    $Args += @("--station", $Name)
+}
+
+Write-Host "[raw → combined one-shot export]"
+& python @Args
+if ($LASTEXITCODE -ne 0) {
+    throw "export_raw_to_combined failed with Python exit code $LASTEXITCODE"
+}
+
+Write-Host "Combined NPZ: $(Join-Path $OutputRoot 'combined')"
+Write-Host "Summary: $(Join-Path $OutputRoot 'export_summary.json')"

run/prepare_multisensor_dataset.ps1

@@ -0,0 +1,22 @@
+param(
+    [Parameter(Mandatory = $true)][string]$CombinedRoot,
+    [Parameter(Mandatory = $true)][string]$Output,
+    [Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
+    [Parameter(Mandatory = $true)][double[]]$AntennaLever,
+    [string]$PoseName = "rtk_gga_raw_heading",
+    [int]$MinStations = 20,
+    [int]$ExpectedStations = 0,
+    [double]$HeadingStdLimitDeg = 0.5,
+    [switch]$Overwrite
+)
+
+$ErrorActionPreference = "Stop"
+if ($AntennaLever.Count -ne 3) { throw "AntennaLever must contain X,Y,Z in body coordinates" }
+$Repo = Split-Path -Parent $PSScriptRoot
+$Args = @((Join-Path $Repo "tools\prepare_multisensor_station_dataset.py"), "--combined-root", $CombinedRoot,
+    "--output", $Output, "--pose-name", $PoseName, "--heading-offset-deg", "$HeadingOffsetDeg", "--antenna-lever") +
+    @($AntennaLever | ForEach-Object { "$_" }) + @("--min-stations", "$MinStations",
+    "--expected-stations", "$ExpectedStations", "--heading-std-limit-deg", "$HeadingStdLimitDeg")
+if ($Overwrite) { $Args += "--overwrite" }
+& python @Args
+if ($LASTEXITCODE -ne 0) { throw "Multisensor dataset preparation failed" }

run/rtk_lidar_mechanical_initial.json

@@ -0,0 +1,41 @@
+{
+  "schema_version": 3,
+  "convention": "T_RTK_lidar maps raw LiDAR points into the vehicle-forward RTK body frame (X forward, Y left, Z up) after HeadingOffsetDeg=-90",
+  "frame_mode": "vehicle_forward_heading_offset",
+  "heading_offset_deg": -90.0,
+  "baseline_points": "vehicle_right",
+  "baseline_points_note": "Field-confirmed: master/slave assignment reversed vs G90 diagram, antennas left-right symmetric about rear-axle centerline. Master/GGA on vehicle left, slave on right; rawHeading points vehicle right.",
+  "vehicle_flu_lever_master_to_lidar_m": [
+    0.210859360,
+    -0.414179474,
+    0.078500001
+  ],
+  "vehicle_flu_note": "Vehicle FLU: LiDAR origin relative to master/GGA = ahead, right, above. CAD drawing X was opposite vehicle-forward; longitudinal sign is +X in true FLU (solver also converges to +X).",
+  "antenna_symmetry_note": "Master/slave are mirrors about the rear-axle centerline; swap flips baseline 180° and the vehicle-Y sign of the master→LiDAR lever",
+  "translation_m": [
+    0.210859360,
+    -0.414179474,
+    0.078500001
+  ],
+  "rotation_rpy_deg_xyz": [
+    0.0,
+    0.0,
+    0.0
+  ],
+  "matrix_4x4": [
+    [1.0, 0.0, 0.0, 0.210859360],
+    [0.0, 1.0, 0.0, -0.414179474],
+    [0.0, 0.0, 1.0, 0.078500001],
+    [0.0, 0.0, 0.0, 1.0]
+  ],
+  "use": "Final AX=XB solver initialization only; never use for LiDAR pair registration",
+  "yaw_note": "In vehicle-forward delivery, LiDAR +X ≈ vehicle forward ⇒ mechanical yaw ≈ 0",
+  "z_note": "78.500001 mm = H_L - H_R with H_L=1994.999879 mm, H_R=1916.499878 mm",
+  "attitude_composition": "R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset); pitch/roll stay in baseline frame",
+  "baseline_frame_equivalent": {
+    "heading_offset_deg": 0.0,
+    "translation_m": [0.414179474, 0.210859360, 0.078500001],
+    "rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
+    "note": "Same physical install expressed in rawHeading baseline frame"
+  }
+}

run/run_direct_rtk_lidar.ps1

@@ -0,0 +1,71 @@
+param(
+    [Parameter(Mandatory = $true)][string]$CombinedRoot,
+    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
+    [string]$OutputRoot = "",
+    [string]$WorkRoot = "",
+    [int]$ExpectedStations = 27,
+    [int]$MinStations = 20,
+    [int]$MinPairs = 20,
+    [int]$Bootstrap = 200,
+    # Roof-mounted H32 (~2 m): ground points are near z≈-2 in the LiDAR frame (Z-up).
+    # The old [-1.4, -0.4] window fits walls on this vehicle and must not be reused.
+    [double]$GroundZMin = -2.5,
+    [double]$GroundZMax = -1.5,
+    [int]$SmallGicpMaxGap = 26,
+    [int]$Open3DMaxGap = 26,
+    [double]$MaxReferenceTranslationM = 8.0,
+    # Baseline frame: rawHeading as RTK X. Default vehicle-forward for this car: -90
+    # (master/slave swapped, baseline points vehicle-right).
+    [double]$HeadingOffsetDeg = -90.0,
+    [string]$SolverInitialExtrinsic = "",
+    [double]$RefineMinInlierRatio = 0.63,
+    [double]$RefineMaxInlierRmseM = 0.14
+)
+
+$ErrorActionPreference = "Stop"
+$Repo = Split-Path -Parent $PSScriptRoot
+if ([string]::IsNullOrWhiteSpace($OutputRoot)) { $OutputRoot = Join-Path $Repo "outputs\rtk_lidar_calibration" }
+if ([string]::IsNullOrWhiteSpace($WorkRoot)) { $WorkRoot = Join-Path $Repo "work\prepared_rtk_direct" }
+if ([string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
+    $SolverInitialExtrinsic = Join-Path $PSScriptRoot "rtk_lidar_mechanical_initial.json"
+}
+
+$PoseName = if ([math]::Abs($HeadingOffsetDeg) -le 1e-12) {
+    "rtk_gga_raw_heading"
+} else {
+    "rtk_vehicle_heading"
+}
+$ReferencePoseFile = "reference_poses_${PoseName}.csv"
+$Prepared = $WorkRoot
+
+& (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") `
+    -CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg $HeadingOffsetDeg `
+    -AntennaLever @(0.0,0.0,0.0) -PoseName $PoseName -MinStations $MinStations `
+    -ExpectedStations $ExpectedStations -Overwrite
+if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }
+
+# Pair registration intentionally has no --initial-extrinsic (B must stay X-independent).
+# SolverInitialExtrinsic is applied only in the final AX=XB calibrate stage.
+& (Join-Path $Repo "run\run_single_dataset.ps1") `
+    -Prepared $Prepared -OutputRoot $OutputRoot `
+    -ReferencePoseFile $ReferencePoseFile `
+    -ReferenceHeight $RtkReferenceHeightAboveGroundM `
+    -MinStations $MinStations -MinPairs $MinPairs -Bootstrap $Bootstrap `
+    -GroundZMin $GroundZMin -GroundZMax $GroundZMax `
+    -SmallGicpMaxGap $SmallGicpMaxGap -Open3DMaxGap $Open3DMaxGap `
+    -MaxReferenceTranslationM $MaxReferenceTranslationM `
+    -SolverInitialExtrinsic $SolverInitialExtrinsic `
+    -RefineMinInlierRatio $RefineMinInlierRatio `
+    -RefineMaxInlierRmseM $RefineMaxInlierRmseM
+if ($LASTEXITCODE -ne 0) { throw "RTK-direct calibration failed" }
+
+$Finalize = @(
+    (Join-Path $Repo "code\finalize_direct_rtk_lidar.py"),
+    "--result-root", $OutputRoot,
+    "--reference-height", "$RtkReferenceHeightAboveGroundM",
+    "--heading-offset-deg", "$HeadingOffsetDeg"
+)
+& python @Finalize
+if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
+
+Write-Host "Final T_RTK_lidar: $(Join-Path $OutputRoot 'final_T_RTK_lidar.json')"

run/run_full_pipeline.ps1

@@ -0,0 +1,43 @@
+param(
+    [Parameter(Mandatory = $true)][string]$DataRoot,
+    [Parameter(Mandatory = $true)][string]$RtkCapture,
+    [Parameter(Mandatory = $true)][string]$ImuCapture,
+    [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [string]$LidarObject = "frontlidar",
+    [string]$LidarCaptureName = "h32.rscap",
+    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
+    [string]$Timezone = "+08:00",
+    [ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss",
+    [int]$ExpectedStations = 27,
+    [int]$MinStations = 20,
+    [int]$MinPairs = 20,
+    [int]$Bootstrap = 200,
+    # Roof-mounted H32 (~2 m). Do not reuse [-1.4, -0.4] on this vehicle.
+    [double]$GroundZMin = -2.5,
+    [double]$GroundZMax = -1.5,
+    [double]$HeadingOffsetDeg = -90.0
+)
+
+$ErrorActionPreference = "Stop"
+$ExportRoot = Join-Path $OutputRoot "exported"
+$PreparedRoot = Join-Path $OutputRoot "prepared_rtk_direct"
+$CalibrationRoot = Join-Path $OutputRoot "calibration"
+
+& (Join-Path $PSScriptRoot "export_multisensor_stations.ps1") `
+    -DataRoot $DataRoot -RtkCapture $RtkCapture -ImuCapture $ImuCapture `
+    -OutputRoot $ExportRoot -LidarObject $LidarObject -LidarCaptureName $LidarCaptureName `
+    -Timezone $Timezone -TimeBasis $TimeBasis
+if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" }
+
+& (Join-Path $PSScriptRoot "run_direct_rtk_lidar.ps1") `
+    -CombinedRoot (Join-Path $ExportRoot "combined") `
+    -WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
+    -RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM `
+    -HeadingOffsetDeg $HeadingOffsetDeg `
+    -MinStations $MinStations `
+    -ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap `
+    -GroundZMin $GroundZMin -GroundZMax $GroundZMax
+if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" }
+
+Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')"
+Write-Host "Prepared frames: $(Join-Path $PreparedRoot 'frames_all')"

run/run_joint_rtk_lidar.ps1

@@ -0,0 +1,103 @@
+param(
+    [Parameter(Mandatory = $true)][string[]]$BatchNames,
+    [Parameter(Mandatory = $true)][string[]]$Pairs,
+    [Parameter(Mandatory = $true)][string[]]$GroundPlanes,
+    [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
+    [int]$Bootstrap = 200,
+    [double]$MaxBatchTranslationDifferenceM = 0.25,
+    [double]$MaxBatchRotationDifferenceDeg = 5.0
+)
+
+$ErrorActionPreference = "Stop"
+$Repo = Split-Path -Parent $PSScriptRoot
+
+if ($BatchNames.Count -lt 2) {
+    throw "At least two independent batches are required"
+}
+if (($Pairs.Count -ne $BatchNames.Count) -or ($GroundPlanes.Count -ne $BatchNames.Count)) {
+    throw "BatchNames, Pairs, and GroundPlanes must have the same number of entries"
+}
+if ($RtkReferenceHeightAboveGroundM -le 0.0) {
+    throw "RtkReferenceHeightAboveGroundM must be greater than zero"
+}
+foreach ($Path in @($Pairs + $GroundPlanes)) {
+    if (-not (Test-Path -LiteralPath $Path)) {
+        throw "Input does not exist: $Path"
+    }
+}
+
+$PreflightRoot = Join-Path $OutputRoot "preflight"
+New-Item -ItemType Directory -Force -Path $PreflightRoot | Out-Null
+$BatchExtrinsics = @()
+for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
+    $SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
+    $BatchExtrinsic = Join-Path $PreflightRoot "$SafeName.json"
+    Write-Host "[preflight batch: $($BatchNames[$Index])]"
+    & python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
+        --pairs $Pairs[$Index] `
+        --ground-planes $GroundPlanes[$Index] `
+        --reference-height $RtkReferenceHeightAboveGroundM `
+        --bootstrap 0 `
+        --output $BatchExtrinsic | Out-Null
+    if ($LASTEXITCODE -ne 0) { throw "Batch preflight failed: $($BatchNames[$Index])" }
+    $BatchExtrinsics += $BatchExtrinsic
+}
+
+for ($Index = 1; $Index -lt $BatchNames.Count; $Index++) {
+    $SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
+    $ComparisonPath = Join-Path $PreflightRoot "$SafeName-vs-batch0.json"
+    & python (Join-Path $Repo "code\compare_extrinsics.py") `
+        --reference $BatchExtrinsics[0] `
+        --candidate $BatchExtrinsics[$Index] `
+        --output $ComparisonPath | Out-Null
+    if ($LASTEXITCODE -ne 0) { throw "Batch comparison failed: $($BatchNames[$Index])" }
+
+    $Comparison = Get-Content -LiteralPath $ComparisonPath -Raw | ConvertFrom-Json
+    $TranslationDifference = [double]$Comparison.relative_translation_norm_m
+    $RotationDifference = [double]$Comparison.relative_rotation_deg
+    Write-Host ("[preflight consistency] {0} vs {1}: {2:F4} m / {3:F3} deg" -f `
+        $BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference)
+    if (($TranslationDifference -gt $MaxBatchTranslationDifferenceM) -or
+        ($RotationDifference -gt $MaxBatchRotationDifferenceDeg)) {
+        throw ("Batch extrinsics are inconsistent: {0} vs {1} = {2:F4} m / {3:F3} deg; " +
+            "check RTK heading/frame convention and sensor installation") -f `
+            $BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference
+    }
+}
+
+$InputRoot = Join-Path $OutputRoot "inputs"
+$JointPairs = Join-Path $InputRoot "joint_consensus_pairs.npz"
+$JointGroundPlanes = Join-Path $InputRoot "joint_ground_planes.csv"
+$InputSummary = Join-Path $InputRoot "joint_input_summary.json"
+$Extrinsic = Join-Path $OutputRoot "shared_T_RTK_lidar.json"
+
+$BuildArgs = @((Join-Path $Repo "code\build_joint_rtk_lidar_inputs.py"))
+for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
+    $BuildArgs += @(
+        "--batch-name", $BatchNames[$Index],
+        "--pairs", $Pairs[$Index],
+        "--ground-planes", $GroundPlanes[$Index]
+    )
+}
+$BuildArgs += @(
+    "--output-pairs", $JointPairs,
+    "--output-ground-planes", $JointGroundPlanes,
+    "--summary", $InputSummary
+)
+
+Write-Host "[combine independent batches]"
+& python @BuildArgs
+if ($LASTEXITCODE -ne 0) { throw "Combining independent batches failed" }
+
+Write-Host "[solve shared T_RTK_lidar]"
+& python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
+    --pairs $JointPairs `
+    --ground-planes $JointGroundPlanes `
+    --reference-height $RtkReferenceHeightAboveGroundM `
+    --bootstrap $Bootstrap `
+    --output $Extrinsic
+if ($LASTEXITCODE -ne 0) { throw "Shared RTK-LiDAR calibration failed" }
+
+Write-Host "Shared T_RTK_lidar: $Extrinsic"
+Write-Host "Joint input summary: $InputSummary"

run/run_single_dataset.ps1

@@ -0,0 +1,95 @@
+param(
+    [Parameter(Mandatory = $true)][string]$Prepared,
+    [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [Parameter(Mandatory = $true)][double]$ReferenceHeight,
+    [string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
+    [int]$MinStations = 20,
+    [int]$MinPairs = 20,
+    [int]$Bootstrap = 100,
+    # Roof-mounted H32 (~2 m): ground near z≈-2. Old [-1.4,-0.4] fits walls on this vehicle.
+    [double]$GroundZMin = -2.5,
+    [double]$GroundZMax = -1.5,
+    [int]$SmallGicpMaxGap = 26,
+    [int]$Open3DMaxGap = 26,
+    [double]$MaxReferenceTranslationM = 8.0,
+    [string]$SolverInitialExtrinsic = "",
+    [double]$RefineMinInlierRatio = 0.63,
+    [double]$RefineMaxInlierRmseM = 0.14
+)
+
+$ErrorActionPreference = "Stop"
+$Repo = Split-Path -Parent $PSScriptRoot
+$Code = Join-Path $Repo "code\rigorous_calibration.py"
+$Refine = Join-Path $Repo "code\refine_pairs.py"
+$Consensus = Join-Path $Repo "code\cross_backend_filter.py"
+$Frames = Join-Path $Prepared "frames_all"
+$ReferencePoses = Join-Path $Prepared $ReferencePoseFile
+$Common = Join-Path $OutputRoot "common"
+$Open = Join-Path $OutputRoot "open3d_gicp"
+$Small = Join-Path $OutputRoot "small_gicp"
+$ConsensusOut = Join-Path $OutputRoot "consensus"
+
+function Run-Python {
+    param([string]$Stage, [string[]]$Arguments)
+    Write-Host "[$Stage]"
+    & python @Arguments
+    if ($LASTEXITCODE -ne 0) { throw "$Stage failed with Python exit code $LASTEXITCODE" }
+}
+
+foreach ($Path in @($Frames, $ReferencePoses)) {
+    if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
+}
+New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null
+
+$Ground = Join-Path $Common "ground_planes.csv"
+Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground,
+    "--z-min", "$GroundZMin", "--z-max", "$GroundZMax")
+
+foreach ($Backend in @("small_gicp", "open3d")) {
+    $Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
+    $Raw = Join-Path $Directory "B_estimation.npz"
+    $QualityJson = Join-Path $Directory "B_quality.json"
+    $QualityCsv = Join-Path $Directory "B_quality.csv"
+    $MaxGap = if ($Backend -eq "open3d") { $Open3DMaxGap } else { $SmallGicpMaxGap }
+    $PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--reference-poses", $ReferencePoses,
+                  "--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv,
+                  "--min-stations", "$MinStations", "--min-pairs", "$MinPairs", "--max-gap", "$MaxGap")
+    if ($MaxReferenceTranslationM -gt 0) {
+        $PairArgs += @("--max-reference-translation", "$MaxReferenceTranslationM")
+    }
+    if ($Backend -eq "open3d") { $PairArgs += @("--multistart", "1", "--iterations", "40") }
+    Run-Python "$Backend pairs" $PairArgs
+    Run-Python "$Backend X-independent refinement" @(
+        $Refine, "--pairs", $Raw, "--quality-json", $QualityJson,
+        "--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs",
+        "--min-inlier-ratio", "$RefineMinInlierRatio",
+        "--max-inlier-rmse", "$RefineMaxInlierRmseM"
+    )
+    $CalibrationArgs = @(
+        $Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
+        "--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
+        "--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
+    )
+    if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
+        $CalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
+    }
+    Run-Python "$Backend calibration" $CalibrationArgs
+}
+
+$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
+Run-Python "cross-backend consensus" @(
+    $Consensus, "--open3d-pairs", (Join-Path $Open "B_refined.npz"),
+    "--small-pairs", (Join-Path $Small "B_refined.npz"),
+    "--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
+)
+$ConsensusCalibrationArgs = @(
+    $Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
+    "--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
+    "--output", (Join-Path $ConsensusOut "extrinsic.json")
+)
+if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
+    $ConsensusCalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
+}
+Run-Python "consensus calibration" $ConsensusCalibrationArgs
+
+Write-Host "Calibration results: $OutputRoot"

run/view_result.ps1

@@ -0,0 +1,19 @@
+param(
+    [Parameter(Mandatory = $true)][string]$Frames,
+    [Parameter(Mandatory = $true)][string]$Pairs,
+    [Parameter(Mandatory = $true)][string]$Extrinsic,
+    [int]$PairIndex = 0,
+    [double]$LeftRollDeg = 0.0,
+    [double]$LeftPitchDeg = 0.0,
+    [double]$LeftYawDeg = 0.0
+)
+
+$ErrorActionPreference = "Stop"
+$Repo = Split-Path -Parent $PSScriptRoot
+foreach ($Path in @($Frames, $Pairs, $Extrinsic)) {
+    if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
+}
+& python (Join-Path $Repo "code\visualize_pair_3d.py") `
+    --frames $Frames --pairs $Pairs --extrinsic $Extrinsic --pair-index $PairIndex `
+    --left-rpy-deg $LeftRollDeg $LeftPitchDeg $LeftYawDeg
+if ($LASTEXITCODE -ne 0) { throw "Visualization failed with Python exit code $LASTEXITCODE" }

tests/README.md

@@ -1,131 +0,0 @@
-# 测试说明
-
-**用途：** 说明合成 pytest 与旧车 S2 线下试验的边界（避免误读）。日常跑通请先看根目录 [README](../README.md)。
-
-| 试验 | 是否默认 pytest | 在证明什么 | 不在证明什么 |
-|---|---|---|---|
-| **合成数据** | 是 | 算法链路正确、能收回已知 yaw / δt | 实车安装精度、平移可交付 |
-| **旧车 S2** | 否（线下手工） | 主机时间旧数据上流水线能跑完；质量门会拒绝坏结果 | 外参真值；新车可用性 |
-
----
-
-## 1. 自动化测试（合成数据 / `pytest`）
-
-入口：`tests/test_v1_pipeline.py`  
-命令：
-
-```powershell
-cd <仓库根目录>
-python -m pytest -q
-```
-
-
-| 测试                                                            | 输入           | 在测什么                | 期望结果                                       |
-| ------------------------------------------------------------- | ------------ | ------------------- | ------------------------------------------ |
-| `test_rotation_handeye_recovers_yaw`                          | 合成运动对（无点云）   | 旋转手眼能否收回已知 yaw      | 旋转误差 < 1°                                  |
-| `test_time_offset_on_synthetic`                               | 合成会话（故意加 δt） | 模长相关粗估时间偏置          | |δt 误差| < 0.05 s                           |
-| `test_signed_time_offset_refine_improves_or_keeps`            | 同上 + 真值 R    | 有符号三轴 δt 精修         | 不比粗估明显更差                                   |
-| `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_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）。  
-一键复现见根目录 README：`tools/reproduce_synthetic.py`。
-
----
-
-
-
-## 2. 旧车 S2 线下试验（不在默认 pytest 里）
-
-
-
-### 用了什么数据
-
-
-| 项       | 内容                                                                         |
-| ------- | -------------------------------------------------------------------------- |
-| 车辆 / 批次 | 旧 **S2** 验证集（`S2_scheme1_validation`）数据在网盘的“室外车数据\IMU-雷达标定数据”              |
-| 典型路径    | `D:\IMU_calibration\work\S2_scheme1_validation\`（历史目录名；含 `imu.csv` + 雷达会话） |
-| IMU 时间  | **主机 UTC 接收时间**（串口块到达时刻），不是 IMU 设备时间                                       |
-| 雷达时间    | dlog 导出的主机侧 `unix_time_ns`，不是 MSOP 设备时间                                    |
-| 帧率特征    | 雷达约 **1 Hz** 量级，关键帧间隔偏长                                                    |
-| 配置烟测    | `config/s2_old_smoke.yaml`（仅声明为旧数据烟测，不当交付）                                 |
-
-
-这些数据**只能用来验证流水线能否跑通**，不能当作新车外参真值来源。  
-`blocked` 是质量门的**预期结果**，不是「算法突然坏了」。
-
-### 做了什么测试
-
-对同一批 S2 中间格式多次跑 `cli run`，例如：
-
-- 预积分加强后的输出目录（本机历史名如 `out_scheme2_preint`）
-- 有符号 δt / 联合精修后的输出（本机历史名如 `out_scheme2_phaseA`）
-- 用 `tools/compare_s2_runs.py` 对比两次 `summary.json`
-
-命令形态（路径按本机实际修改；雷达会话目录若仍叫 `scheme2_session` 为历史命名）：
-
-```powershell
-python -m imu_lidar.cli run `
-  --vehicle-config config\s2_old_smoke.yaml `
-  --imu D:\IMU_calibration\work\S2_scheme1_validation\imu.csv `
-  --lidar D:\IMU_calibration\work\S2_scheme1_validation\scheme2_session `
-  --output path\to\out_s2 `
-  --mode rotation_only `
-  --time-offset-search-s 2.0
-
-python tools\compare_s2_runs.py path\to\out_old\summary.json path\to\out_new\summary.json
-```
-
-
-
-### 得到什么结果（记录摘要）
-
-
-| 指标       | 预积分加强一轮       | 有符号 δt / 精修一轮      |
-| -------- | ------------- | ------------------ |
-| `status` | `blocked`（预期） | `blocked`（预期）      |
-| 手眼 RMS   | 约 **15.0°**   | 约 **14.5°**        |
-| 手眼中位数    | —             | 约 **6.8°**         |
-| δt       | 约 **−2.0 s**  | 约 **−1.75 s**（有修正） |
-| 相关峰      | 很弱（约 0.18）    | 仍弱（约 0.13）         |
-| 结论       | 链路可跑          | 残差略降，但 **不当交付外参**  |
-
-
-原因归纳：
-
-1. 时间戳是**主机时间**，相关峰弱，δt / yaw / 零偏互相耦合；
-2. 雷达约 1 Hz，运动对间隔长，IMU 侧更易漂；
-3. 质量门主动 `blocked`，避免把坏结果当成安装参数。
-
-**正式标定**必须改用设备时间（IMU `device_timestamp`、雷达 MSOP 设备时）重新采集后再跑。
-
----
-
-
-
-## 3. 配准结果怎么目视检查
-
-标定跑完后（合成或实车）：
-
-```powershell
-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 `
-  --save-png examples\synthetic_session\out\pair0_overlay.png
-```
-
-交互窗口快捷键：`1`–`4` 切换叠点模式；`N`/`]` 下一运动对，`P`/`[` 上一运动对。  
-无显示器时加 `--no-gui --save-png ...` 只出俯视图 PNG。

tests/test_export_rscap_helpers.py

@@ -1,104 +0,0 @@
-"""Unit tests for rscap → V1 export helpers (no large real captures)."""
-
-from __future__ import annotations
-
-import struct
-
-import numpy as np
-
-from tools.rscap_v2.h32_msop import (
-    CHANNELS,
-    PACKET_LENGTH,
-    decode_packet_points,
-    default_vertical_deg,
-    default_horizontal_deg,
-    device_timestamp_ms,
-    normalize_azimuth_deg,
-)
-from tools.rscap_v2.n300_imu import crc8_fdilink, crc16_fdilink, iter_n300_imu_samples
-from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
-
-
-def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
-    packet = bytearray(PACKET_LENGTH)
-    packet[17] = 1  # 2.5 mm unit
-    sec = seconds.to_bytes(6, "big")
-    packet[20:26] = sec
-    packet[26:30] = int(microseconds).to_bytes(4, "big")
-    az_raw = int(round(az_deg * 100))
-    for block in range(12):
-        offset = 42 + block * 100
-        packet[offset] = 255
-        packet[offset + 1] = 238
-        packet[offset + 2] = (az_raw >> 8) & 0xFF
-        packet[offset + 3] = az_raw & 0xFF
-        idx = offset + 4
-        for _ch in range(CHANNELS):
-            # 4.0 m at 2.5 mm/unit => raw = 1600
-            packet[idx] = (1600 >> 8) & 0xFF
-            packet[idx + 1] = 1600 & 0xFF
-            packet[idx + 2] = 10
-            idx += 3
-    return bytes(packet)
-
-
-def test_h32_device_timestamp_and_points():
-    packet = _make_msop_packet(seconds=1700000000, microseconds=123000)
-    assert device_timestamp_ms(packet) == 1700000000 * 1000 + 123
-    az_list, pts = decode_packet_points(
-        packet,
-        default_vertical_deg(),
-        default_horizontal_deg(),
-        min_range_m=0.1,
-        max_range_m=50.0,
-    )
-    assert len(az_list) == 12
-    assert pts.shape[0] == 12 * CHANNELS
-    assert np.allclose(np.linalg.norm(pts, axis=1), 4.0, atol=1e-3)
-
-
-def test_normalize_azimuth():
-    assert abs(normalize_azimuth_deg(190.0) + 170.0) < 1e-9
-
-
-def _n300_imu_frame(device_us: int = 123456) -> bytes:
-    payload = bytearray(56)
-    struct.pack_into("<3f", payload, 0, 0.1, -0.2, 0.3)
-    struct.pack_into("<3f", payload, 12, 0.0, 0.0, 9.81)
-    struct.pack_into("<q", payload, 48, device_us)
-    header = bytearray([0xFC, 0x40, 56, 7])
-    header.append(crc8_fdilink(header))
-    crc = crc16_fdilink(payload)
-    frame = bytes(header) + crc.to_bytes(2, "big") + bytes(payload) + b"\xFD"
-    return frame
-
-
-def test_n300_imu_sample_from_capture_chunks():
-    frame = _n300_imu_frame(654321)
-    header = CaptureHeader(
-        sensor_kind="wheeltec-n300",
-        session_id="test",
-        session_start_utc_ticks=0,
-        session_start_monotonic_ticks=0,
-        monotonic_frequency=10_000_000,
-        port="COM1",
-        baud=921600,
-        file_start_utc_ticks=0,
-    )
-    chunk = RawChunk(
-        sequence=1,
-        receive_utc_ticks=100,
-        receive_monotonic_ticks=1,
-        raw=frame,
-        record_file_offset=0,
-        raw_file_offset=0,
-        record_crc32=0,
-        crc_valid=True,
-    )
-    capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
-    samples = iter_n300_imu_samples(capture)
-    assert len(samples) == 1
-    assert samples[0].device_timestamp_us == 654321
-    assert abs(samples[0].t_s - 654321e-6) < 1e-12
-    assert abs(samples[0].gyro_rad_s[0] - 0.1) < 1e-6
-    assert abs(samples[0].accel_m_s2[2] - 9.81) < 1e-5

tests/test_g90_host_association.py

@@ -0,0 +1,116 @@
+"""Regression tests for G90 GNHPR parsing and host-time LiDAR association."""
+
+from __future__ import annotations
+
+import json
+import sys
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+TOOLS = ROOT / "tools"
+CODE = ROOT / "code"
+sys.path.insert(0, str(TOOLS))
+sys.path.insert(0, str(TOOLS / "rscap_v2"))
+sys.path.insert(0, str(CODE))
+
+from build_multisensor_npz import build_combined  # noqa: E402
+from pipeline_common import parse_gnhpr  # noqa: E402
+from rigorous_calibration import load_npz_xyz  # noqa: E402
+
+
+def test_parse_gnhpr_fixed_heading():
+    row = parse_gnhpr("$GNHPR,070411.40,354.7437,000.2518,000.0000,4,26,0.00,0999*58")
+    assert row["type"] == "GNHPR"
+    assert row["raw_heading_deg"] == 354.7437
+    assert row["pitch_deg"] == 0.2518
+    assert row["roll_deg"] == 0.0
+    assert row["heading_quality"] == 4
+    assert row["satellites"] == 26
+    assert row["heading_valid"] is True
+
+
+def test_host_time_uses_lidar_receive_time_and_preserves_device_time(tmp_path: Path):
+    host_ns = 1_786_240_000_000_000_000
+    device_ns = 1_500_000_000_000_000_000
+    frame_dir = tmp_path / "lidar"
+    frame_dir.mkdir()
+    np.savez_compressed(
+        frame_dir / "frame.npz",
+        points=np.zeros((4, 4), dtype=np.float32),
+        unix_time_ns=np.asarray([device_ns], dtype=np.int64),
+        host_receive_utc_ns=np.asarray([host_ns], dtype=np.int64),
+    )
+
+    rtk = tmp_path / "rtk.jsonl"
+    rows = [
+        {
+            "type": "GGA",
+            "checksum_valid": True,
+            "host_receive_utc_ns": host_ns + 20_000_000,
+            "lat_deg": 31.0,
+            "lon_deg": 121.0,
+            "altitude_m": 10.0,
+            "fix_quality": 4,
+            "satellites": 20,
+            "raw_line": "$GNGGA,...",
+        },
+        {
+            "type": "GNHPR",
+            "checksum_valid": True,
+            "host_receive_utc_ns": host_ns - 10_000_000,
+            "raw_heading_deg": 90.0,
+            "pitch_deg": 1.0,
+            "roll_deg": 0.0,
+            "heading_quality": 4,
+            "heading_solution": "GNHPR_QUALITY_4",
+            "heading_valid": True,
+            "satellites": 22,
+            "raw_line": "$GNHPR,...",
+        },
+    ]
+    rtk.write_text("".join(json.dumps(row) + "\n" for row in rows), encoding="utf-8")
+    imu = tmp_path / "imu.jsonl"
+    imu.write_text("", encoding="utf-8")
+
+    out = tmp_path / "combined"
+    summary = build_combined(
+        [("STATION-01", frame_dir)],
+        [rtk],
+        [imu],
+        out,
+        time_basis="host",
+        rtk_max_dt_ms=100.0,
+    )
+
+    assert summary["frames"] == 1
+    assert summary["rtk_valid"] == 1
+    assert summary["heading_valid"] == 1
+    assert summary["rtk_fixed"] == 1
+    with np.load(next((out / "frames").glob("*.npz")), allow_pickle=False) as frame:
+        assert int(frame["lidar_association_time_ns"][0]) == host_ns
+        assert int(frame["unix_time_ns"][0]) == device_ns
+        assert int(frame["rtk_gga_dt_ns"][0]) == 20_000_000
+        assert int(frame["rtk_heading_dt_ns"][0]) == -10_000_000
+
+
+def test_registration_prefers_lidar_association_time(tmp_path: Path):
+    host_ns = 1_786_240_000_000_000_000
+    device_ns = 1_500_000_000_000_000_000
+    source = tmp_path / "frame.npz"
+    np.savez_compressed(
+        source,
+        points_raw=np.asarray(
+            [[1000.0, 0.0, 0.0, 1.0], [2000.0, 90.0, 0.0, 1.0]],
+            dtype=np.float32,
+        ),
+        unix_time_ns=np.asarray([device_ns], dtype=np.int64),
+        lidar_association_time_ns=np.asarray([host_ns], dtype=np.int64),
+        frame_counter=np.asarray([7], dtype=np.int64),
+    )
+
+    timestamp, counter, xyz = load_npz_xyz(source)
+    assert timestamp == host_ns / 1e9
+    assert counter == 7
+    assert xyz.shape == (2, 3)

tests/test_h32_dlog_export.py

@@ -1,24 +1,30 @@
-"""Unit tests for H32 Medulla dlog → V1 export helpers."""
+"""Unit tests for H32 Medulla raw dlog → station frame export helpers."""
 
 from __future__ import annotations
 
 import struct
+import sys
 from pathlib import Path
 
 import numpy as np
 
-from tools.h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles
+ROOT = Path(__file__).resolve().parents[1]
-from tools.h32_dlog.dobject import RECORD_RE, discover_records, iter_payloads, resolve_dlog_root
+TOOLS = ROOT / "tools"
-from tools.h32_dlog.timeutil import local_wall_to_dotnet_ticks
+sys.path.insert(0, str(TOOLS))
-from tools.h32_dlog.load_session import load_h32_dlog_lidar
+sys.path.insert(0, str(TOOLS / "rscap_v2"))
-from tools.h32_dlog.payload_v1 import (
+
+from export_h32_rscap_station import export_station_h32_dlog, is_h32_raw_dlog_station  # noqa: E402
+from h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles  # noqa: E402
+from h32_dlog.dobject import discover_records, iter_payloads, resolve_dlog_root  # noqa: E402
+from h32_dlog.load_session import load_h32_dlog_lidar  # noqa: E402
+from h32_dlog.payload_v1 import (  # noqa: E402
     MsopPacketItem,
     build_difop_payload,
     build_msop_batch_payload,
     parse_difop_payload,
     parse_msop_batch_payload,
 )
-from tools.rscap_v2.h32_msop import PACKET_LENGTH, iter_h32_frames_from_packets
+from h32_msop import PACKET_LENGTH, iter_h32_frames_polar_from_packets  # noqa: E402
 
 
 def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
@@ -67,10 +73,7 @@ def _write_dorec_record(
     ticks: int,
     record_id: str,
     payload: bytes,
-    offset: int = 0,
 ) -> int:
-    """Append one DObject record; return file offset of the record start."""
-
     path.parent.mkdir(parents=True, exist_ok=True)
     name_b = object_name.encode("ascii")
     id_b = record_id.encode("ascii")
@@ -84,26 +87,11 @@ def _write_dorec_record(
         + payload
     )
     with path.open("ab" if path.exists() else "wb") as handle:
-        if offset:
-            handle.seek(offset)
         start = handle.tell()
         handle.write(blob)
     return start
 
 
-def test_recovered_index_line_and_local_ticks():
-    line = (
-        ">DObject `frontlidar-msop-raw` post len=15532B, id:9CF1, "
-        "tic:639218060782100466, @data.bin:0"
-    )
-    match = RECORD_RE.search(line)
-    assert match is not None
-    assert match.group("name") == "frontlidar-msop-raw"
-    assert match.group("file") == "data.bin"
-    assert int(match.group("offset")) == 0
-    assert local_wall_to_dotnet_ticks("2026-08-08T17:14:38") == 639218060780000000
-
-
 def test_parse_msop_and_difop_payload_roundtrip():
     packet = _make_msop_packet(seconds=1700000000, microseconds=123456)
     item = MsopPacketItem(
@@ -118,33 +106,21 @@ def test_parse_msop_and_difop_payload_roundtrip():
     batch = parse_msop_batch_payload(msop_payload)
     assert batch.session_id == "sess-a"
     assert len(batch.packets) == 1
-    assert batch.packets[0].sequence == 7
-    assert batch.packets[0].device_timestamp_us == item.device_timestamp_us
-    assert batch.packets[0].device_timestamp_valid is True
     assert batch.packets[0].raw == packet
 
     vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
     difop_raw = _make_difop_packet(vertical=vertical, horizontal=[0.05] * CHANNELS)
-    difop_payload = build_difop_payload(raw=difop_raw, sequence=3)
+    difop = parse_difop_payload(build_difop_payload(raw=difop_raw, sequence=3))
-    difop = parse_difop_payload(difop_payload)
-    assert difop.sequence == 3
     angles = parse_difop_angles(difop.raw)
-    assert angles.vertical_deg.shape == (CHANNELS,)
     assert np.allclose(angles.vertical_deg, vertical, atol=1e-2)
     assert np.allclose(angles.horizontal_deg, 0.05, atol=1e-2)
 
 
-def test_difop_signed_angle_negative():
+def test_export_station_h32_dlog_mini(tmp_path: Path):
-    packet = _make_difop_packet(vertical=[-5.25] + [0.0] * 31)
+    station = tmp_path / "001"
-    angles = parse_difop_angles(packet)
-    assert abs(angles.vertical_deg[0] + 5.25) < 1e-9
-
-
-def test_load_h32_dlog_lidar_mini_session(tmp_path: Path):
-    dlog = tmp_path / "session" / "dlog"
     dorec_name = "raw.dorec"
-    dorec_path = dlog / "dobject_recording" / dorec_name
+    dorec_path = station / "dobject_recording" / dorec_name
-    log_path = dlog / "dobject" / "rec.log"
+    log_path = station / "dobject" / "rec.log"
 
     vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
     difop_payload = build_difop_payload(raw=_make_difop_packet(vertical=vertical), sequence=1)
@@ -155,13 +131,12 @@ def test_load_h32_dlog_lidar_mini_session(tmp_path: Path):
                 sequence=1,
                 device_timestamp_us=100_000_000,
                 device_timestamp_valid=True,
-                host_receive_utc_ticks=1,
+                host_receive_utc_ticks=621355968000000000 + 10_000_000,
                 host_receive_monotonic_ticks=2,
                 raw=msop_packet,
             )
         ]
     )
-
     off_difop = _write_dorec_record(
         dorec_path,
         object_name="frontlidar-difop-raw",
@@ -176,7 +151,6 @@ def test_load_h32_dlog_lidar_mini_session(tmp_path: Path):
         record_id="BB",
         payload=msop_payload,
     )
-
     log_path.parent.mkdir(parents=True, exist_ok=True)
     log_path.write_text(
         "\n".join(
@@ -191,22 +165,26 @@ def test_load_h32_dlog_lidar_mini_session(tmp_path: Path):
         encoding="utf-8",
     )
 
-    root = resolve_dlog_root(tmp_path / "session")
+    assert resolve_dlog_root(station) == station.resolve()
-    assert root == dlog
+    assert is_h32_raw_dlog_station(station)
-    assert len(discover_records(root, "frontlidar-msop-raw")) == 1
+    assert len(discover_records(station, "frontlidar-msop-raw")) == 1
-    payloads = list(iter_payloads(root, "frontlidar-msop-raw"))
+    assert len(list(iter_payloads(station, "frontlidar-msop-raw"))) == 1
-    assert len(payloads) == 1
 
-    session = load_h32_dlog_lidar(tmp_path / "session", require_difop=True)
+    session = load_h32_dlog_lidar(station, require_difop=True)
     assert session.angle_source == "difop_channel_angles"
-    assert len(session.msop_packets) == 1
+    frames = iter_h32_frames_polar_from_packets(
-    assert np.allclose(session.vertical_deg, vertical, atol=1e-2)
-
-    frames = iter_h32_frames_from_packets(
         session.msop_packets,
+        host_utc_ticks=session.msop_host_utc_ticks,
         min_frame_points=1,
         vertical_deg=session.vertical_deg,
         horizontal_deg=session.horizontal_deg,
     )
     assert len(frames) == 1
-    assert frames[0].points_xyz.shape[0] > 0
+    assert frames[0].points_raw.shape[1] == 5
+
+    out = tmp_path / "export"
+    meta = export_station_h32_dlog(station, out, require_difop=True, min_frame_points=1)
+    assert meta["kind"] == "h32_dlog_raw"
+    assert meta["angle_source"] == "difop_channel_angles"
+    assert meta["frames_written"] >= 1
+    assert any((out / "frames").glob("*.npz"))

tests/test_hi13_imu.py

@@ -1,79 +0,0 @@
-"""Unit tests for HI13 / HI91 IMU decoding."""
-
-from __future__ import annotations
-
-import struct
-
-from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
-from tools.rscap_v2.hi13_imu import crc16_hi13, iter_hi13_imu_samples, parse_hi91_frame
-
-
-def _hi91_frame(
-    *,
-    device_ms: int = 123456,
-    accel_g=(0.0, 0.0, 1.0),
-    gyro_dps=(1.0, -2.0, 3.0),
-) -> bytes:
-    payload = bytearray(76)
-    payload[0] = 0x91
-    struct.pack_into("<H", payload, 1, 0)  # pps
-    payload[3] = 25  # temp
-    struct.pack_into("<f", payload, 4, 101325.0)
-    struct.pack_into("<I", payload, 8, device_ms)
-    struct.pack_into("<fff", payload, 12, *accel_g)
-    struct.pack_into("<fff", payload, 24, *gyro_dps)
-    # remaining mag/rpy/quat left zero
-    payload_length = len(payload)
-    header = bytearray(6)
-    header[0] = 0x5A
-    header[1] = 0xA5
-    header[2] = payload_length & 0xFF
-    header[3] = (payload_length >> 8) & 0xFF
-    frame_wo_crc = bytes(header[:4]) + bytes(payload)
-    # crc over header[0:4] + payload
-    tmp = bytearray(6 + payload_length)
-    tmp[0:4] = header[0:4]
-    tmp[6:] = payload
-    crc = crc16_hi13(tmp, payload_length)
-    header[4] = crc & 0xFF
-    header[5] = (crc >> 8) & 0xFF
-    return bytes(header) + bytes(payload)
-
-
-def test_parse_hi91_units():
-    frame = _hi91_frame(device_ms=5000, accel_g=(0.0, 0.0, 1.0), gyro_dps=(57.2957795, 0.0, 0.0))
-    parsed = parse_hi91_frame(frame)
-    assert parsed is not None
-    gyro, accel, device_ms = parsed
-    assert device_ms == 5000
-    assert abs(accel[2] - 9.80665) < 1e-4
-    assert abs(gyro[0] - 1.0) < 1e-5
-
-
-def test_iter_hi13_from_capture():
-    frame = _hi91_frame(device_ms=42)
-    header = CaptureHeader(
-        sensor_kind="hi13r4-imu",
-        session_id="t",
-        session_start_utc_ticks=0,
-        session_start_monotonic_ticks=0,
-        monotonic_frequency=10_000_000,
-        port="COM1",
-        baud=115200,
-        file_start_utc_ticks=0,
-    )
-    chunk = RawChunk(
-        sequence=1,
-        receive_utc_ticks=100,
-        receive_monotonic_ticks=1,
-        raw=frame,
-        record_file_offset=0,
-        raw_file_offset=0,
-        record_crc32=0,
-        crc_valid=True,
-    )
-    capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
-    samples = iter_hi13_imu_samples(capture)
-    assert len(samples) == 1
-    assert samples[0].device_timestamp_us == 42_000
-    assert abs(samples[0].t_s - 0.042) < 1e-12

tests/test_motion_pairs_io.py

@@ -1,78 +0,0 @@
-"""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

@@ -1,194 +0,0 @@
-"""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

@@ -1,260 +0,0 @@
-"""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_rtk_lidar_coordinate_contract.py

@@ -0,0 +1,153 @@
+"""Regression tests for the RTK–LiDAR coordinate and initialization contract."""
+
+from __future__ import annotations
+
+import math
+import sys
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / "tools"))
+sys.path.insert(0, str(ROOT / "code"))
+
+from finalize_direct_rtk_lidar import (  # noqa: E402
+    coordinate_contract_audit,
+    mechanical_self_consistency,
+)
+from prepare_multisensor_station_dataset import heading_to_enu_yaw  # noqa: E402
+from rtk_attitude import attitude_rotation, rtk_body_rotation  # noqa: E402
+from rigorous_calibration import (  # noqa: E402
+    build_parser,
+    load_extrinsic_matrix,
+    params_transform,
+    transform_params,
+)
+
+
+def test_left_baseline_heading_plus_90_points_vehicle_forward() -> None:
+    corrected, yaw = heading_to_enu_yaw(270.0, 90.0)
+    assert corrected == 0.0
+    assert math.degrees(yaw) == 90.0
+
+
+def test_east_vehicle_heading_maps_to_zero_enu_yaw() -> None:
+    corrected, yaw = heading_to_enu_yaw(0.0, 90.0)
+    assert corrected == 90.0
+    assert math.degrees(yaw) == 0.0
+
+
+def test_attitude_rotation_applies_baseline_pitch_elevation() -> None:
+    _, yaw = heading_to_enu_yaw(0.0, 0.0)  # heading north → body X = +North
+    rotation = attitude_rotation(yaw, pitch_deg=10.0, roll_deg=0.0)
+    body_x = rotation @ np.array([1.0, 0.0, 0.0])
+    np.testing.assert_allclose(
+        body_x,
+        [0.0, math.cos(math.radians(10.0)), math.sin(math.radians(10.0))],
+        atol=1e-12,
+    )
+
+
+def test_vehicle_forward_offset_keeps_pitch_about_baseline() -> None:
+    # Baseline points east (vehicle right if nose north); pitch elevates baseline X.
+    # Vehicle-forward offset -90 must not simply Ry after vehicle yaw.
+    raw_heading = 90.0
+    pitch = 10.0
+    r_correct = rtk_body_rotation(raw_heading, -90.0, pitch_deg=pitch, roll_deg=0.0)
+    _, yaw_raw = heading_to_enu_yaw(raw_heading, 0.0)
+    r_baseline = attitude_rotation(yaw_raw, pitch_deg=pitch, roll_deg=0.0)
+    rz90 = np.array([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
+    np.testing.assert_allclose(r_correct, r_baseline @ rz90, atol=1e-12)
+    # Level vehicle-forward X should point north.
+    r_level = rtk_body_rotation(raw_heading, -90.0, pitch_deg=0.0, roll_deg=0.0)
+    np.testing.assert_allclose(r_level @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-12)
+
+
+def test_pair_registration_has_no_extrinsic_argument() -> None:
+    parser = build_parser()
+    pair_options = {
+        option
+        for action in parser._subparsers._group_actions[0].choices["pairs"]._actions
+        for option in action.option_strings
+    }
+    assert "--initial-extrinsic" not in pair_options
+    assert "--global-voxel" in pair_options
+
+
+def test_mechanical_initial_is_vehicle_forward_swapped_master() -> None:
+    path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
+    document = __import__("json").loads(path.read_text(encoding="utf-8-sig"))
+    transform = load_extrinsic_matrix(path)
+    np.testing.assert_allclose(transform[:3, 3], [0.210859360, -0.414179474, 0.078500001])
+    np.testing.assert_allclose(transform[:3, :3], np.eye(3))
+    np.testing.assert_allclose(params_transform(transform_params(transform)), transform, atol=1e-12)
+    assert document["baseline_points"] == "vehicle_right"
+    assert document["frame_mode"] == "vehicle_forward_heading_offset"
+    assert document["heading_offset_deg"] == -90.0
+    assert document["rotation_rpy_deg_xyz"][2] == 0.0
+    check = mechanical_self_consistency(document)
+    assert check["consistent"] is True
+
+
+def test_mixed_left_xy_plus_right_yaw_mechanical_is_rejected() -> None:
+    mixed = {
+        "baseline_points": "vehicle_left",
+        "translation_m": [0.414179474, 0.210859360, 0.078500001],
+        "rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
+        "matrix_4x4": [
+            [0.0, -1.0, 0.0, 0.414179474],
+            [1.0, 0.0, 0.0, 0.210859360],
+            [0.0, 0.0, 1.0, 0.078500001],
+            [0.0, 0.0, 0.0, 1.0],
+        ],
+    }
+    check = mechanical_self_consistency(mixed)
+    assert check["consistent"] is False
+
+
+def test_deprecated_minus_xy_right_baseline_is_rejected_for_swapped_master() -> None:
+    deprecated = {
+        "baseline_points": "vehicle_right",
+        "translation_m": [-0.414179474, -0.210859360, 0.078500001],
+        "rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
+    }
+    check = mechanical_self_consistency(deprecated)
+    assert check["consistent"] is False
+
+
+def test_near_180_degree_solution_is_flagged_for_physical_axis_check() -> None:
+    initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
+    initial = load_extrinsic_matrix(initial_path)
+    solution = np.eye(4)
+    solution[:3, :3] = initial[:3, :3] @ np.diag([-1.0, -1.0, 1.0])
+    solution[:3, 3] = initial[:3, 3]
+    audit = coordinate_contract_audit({
+        "solver_initial_extrinsic": str(initial_path),
+        "matrix_4x4": solution.tolist(),
+    })
+    assert audit["status"] == "near_180_degree_axis_conflict"
+    assert audit["requires_physical_axis_confirmation"] is True
+
+
+def test_previous_mixed_result_branch_is_not_recommended() -> None:
+    """Old baseline-frame mixed solution disagrees with vehicle-forward mechanical initial."""
+    initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
+    solution = np.array(
+        [
+            [0.00942353438668686, -0.9999215926659111, 0.00824654595155475, 0.4123055815579212],
+            [0.9998714355322929, 0.009529416150714898, 0.012895837871912157, 0.2173092104098051],
+            [-0.012973411511822136, 0.008123961367132958, 0.9998828390593821, 0.10405760639434848],
+            [0.0, 0.0, 0.0, 1.0],
+        ],
+        float,
+    )
+    audit = coordinate_contract_audit({
+        "solver_initial_extrinsic": str(initial_path),
+        "matrix_4x4": solution.tolist(),
+    })
+    assert audit["requires_physical_axis_confirmation"] is True
+    assert audit["status"] in {
+        "near_180_degree_axis_conflict",
+        "solution_disagrees_with_mechanical_baseline_side",
+    }

tests/test_v1_pipeline.py

@@ -1,221 +0,0 @@
-"""Automated tests for imu_lidar (synthetic data).
-
-See ``tests/README.md`` for:
-- what each pytest covers;
-- offline S2 host-time experiments (not run in default pytest) and recorded outcomes.
-"""
-
-from __future__ import annotations
-
-from pathlib import Path
-
-import numpy as np
-
-from imu_lidar.contracts import CalibrationMode, CalibrationRequest, MotionPair, SessionInput
-from imu_lidar.geometry import so3_exp
-from imu_lidar.pipeline import run_calibration
-from imu_lidar.rotation_handeye import solve_rotation_handeye
-from imu_lidar.time_offset import estimate_time_offset
-from imu_lidar.imu_io import load_imu_samples
-from imu_lidar.lidar_io import load_lidar_frames
-from tools.generate_synthetic_session import generate_synthetic_session
-
-
-def test_rotation_handeye_recovers_yaw():
-    r_true = so3_exp(np.deg2rad(np.array([1.0, -2.0, 30.0])))
-    pairs = []
-    rng = np.random.default_rng(1)
-    for _ in range(20):
-        axis = rng.normal(size=3)
-        axis /= np.linalg.norm(axis)
-        angle = np.deg2rad(rng.uniform(8.0, 35.0))
-        r_b = so3_exp(axis * angle)
-        r_a = r_true @ r_b @ r_true.T
-        pairs.append(
-            MotionPair(
-                session_id="s",
-                i=0,
-                j=1,
-                t_i_s=0.0,
-                t_j_s=1.0,
-                R_A=r_a,
-                R_B=r_b,
-            )
-        )
-    result = solve_rotation_handeye(pairs)
-    assert result.ok
-    err = np.linalg.norm(_log(r_true.T @ result.R_IMU_lidar))
-    assert np.degrees(err) < 1.0
-
-
-def _log(rotation: np.ndarray) -> np.ndarray:
-    from imu_lidar.geometry import so3_log
-
-    return so3_log(rotation)
-
-
-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"
-    request = CalibrationRequest(
-        vehicle_config=config,
-        sessions=(
-            SessionInput(
-                session_id="synth",
-                imu_source=tmp_path / "imu.csv",
-                lidar_source=tmp_path / "lidar",
-            ),
-        ),
-        requested_mode=CalibrationMode.ROTATION_ONLY,
-        output_directory=out,
-        max_iterations=1,
-        time_offset_search_s=0.5,
-        min_pair_rotation_deg=2.0,
-        min_pair_translation_m=0.05,
-    )
-    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 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):
-    meta = generate_synthetic_session(tmp_path, delta_t_s=0.21, yaw_extrinsic_deg=15.0)
-    imu = load_imu_samples(tmp_path / "imu.csv")
-    frames = load_lidar_frames(tmp_path / "lidar")
-    offset = estimate_time_offset(imu, frames, search_s=0.5)
-    assert offset.ok
-    assert abs(offset.delta_t_s - meta["delta_t_s"]) < 0.05
-
-
-def test_preintegration_bias_jacobian_matches_finite_difference():
-    from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
-    from imu_lidar.geometry import so3_log
-
-    rng = np.random.default_rng(0)
-    t = np.linspace(0.0, 1.0, 200)
-    gyro = rng.normal(scale=0.2, size=(t.size, 3))
-    bias0 = np.array([0.01, -0.02, 0.005])
-    preint = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0)
-    db = np.array([1e-3, -2e-3, 5e-4])
-    approx = apply_bias_jacobian_correction(preint.delta_R, preint.J_bg, db)
-    exact = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0 + db).delta_R
-    err = np.linalg.norm(so3_log(approx.T @ exact))
-    assert err < 2e-3
-
-
-def test_imu_preintegration_recovers_constant_accel_translation():
-    from imu_lidar.imu_preintegration import preintegrate_imu
-    from imu_lidar.geometry import so3_log
-
-    # Constant body accel (no gravity in preint body increments), zero gyro.
-    dt = 0.01
-    t = np.arange(0.0, 1.0 + 1e-9, dt)
-    gyro = np.zeros((t.size, 3))
-    acc = np.tile(np.array([0.5, -0.2, 0.1]), (t.size, 1))
-    preint = preintegrate_imu(t, gyro, acc, 0.0, 1.0, np.zeros(3), np.zeros(3))
-    assert np.linalg.norm(so3_log(preint.delta_R)) < 1e-9
-    # Δv ≈ a Δt, Δp ≈ 0.5 a Δt²
-    assert np.linalg.norm(preint.delta_v - acc[0] * 1.0) < 5e-3
-    assert np.linalg.norm(preint.delta_p - 0.5 * acc[0] * 1.0) < 1e-2
-    assert preint.cov.shape == (9, 9)
-    assert preint.J_bg.shape == (9, 3) and preint.J_ba.shape == (9, 3)
-
-
-def test_imu_preintegration_bias_jacobian_finite_difference():
-    from imu_lidar.imu_preintegration import apply_bias_correction_imu, preintegrate_imu
-
-    rng = np.random.default_rng(2)
-    t = np.linspace(0.0, 0.8, 160)
-    gyro = rng.normal(scale=0.15, size=(t.size, 3))
-    acc = rng.normal(scale=0.5, size=(t.size, 3)) + np.array([0.0, 0.0, 9.8])
-    bg0 = np.array([0.01, -0.01, 0.0])
-    ba0 = np.array([0.02, 0.0, -0.01])
-    base = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0, ba0)
-    dbg = np.array([5e-4, -3e-4, 2e-4])
-    dba = np.array([1e-3, -5e-4, 0.0])
-    r_a, v_a, p_a = apply_bias_correction_imu(base, dbg, dba)
-    exact = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0 + dbg, ba0 + dba)
-    from imu_lidar.geometry import so3_log
-
-    assert np.linalg.norm(so3_log(r_a.T @ exact.delta_R)) < 5e-3
-    assert np.linalg.norm(v_a - exact.delta_v) < 3e-2
-    assert np.linalg.norm(p_a - exact.delta_p) < 2e-2
-
-
-def test_synthetic_pipeline_full_se3_smoke(tmp_path: Path):
-    generate_synthetic_session(tmp_path, delta_t_s=0.12, yaw_extrinsic_deg=18.0)
-    config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
-    out = tmp_path / "out_se3"
-    request = CalibrationRequest(
-        vehicle_config=config,
-        sessions=(
-            SessionInput(
-                session_id="synth",
-                imu_source=tmp_path / "imu.csv",
-                lidar_source=tmp_path / "lidar",
-            ),
-        ),
-        requested_mode=CalibrationMode.FULL_SE3,
-        output_directory=out,
-        max_iterations=1,
-        time_offset_search_s=0.5,
-        min_pair_rotation_deg=2.0,
-        min_pair_translation_m=0.05,
-    )
-    result = run_calibration(request)
-    assert result.status.value in {
-        "full_se3_accepted",
-        "full_se3_rejected_due_to_observability",
-        "rotation_only_accepted",
-        "blocked",
-    }
-    session0 = result.details["sessions"][0]
-    assert "delta_v" in session0.get("pair_notes", []) or session0.get("pair_count", 0) >= 0
-    # 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):
-    from imu_lidar.geometry import so3_exp
-    from imu_lidar.time_offset import refine_time_offset_signed
-
-    meta = generate_synthetic_session(tmp_path, delta_t_s=0.18, yaw_extrinsic_deg=20.0)
-    imu = load_imu_samples(tmp_path / "imu.csv")
-    frames = load_lidar_frames(tmp_path / "lidar")
-    coarse = estimate_time_offset(imu, frames, search_s=0.5)
-    r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
-    refined = refine_time_offset_signed(
-        imu,
-        frames,
-        delta_t_s=coarse.delta_t_s,
-        R_IMU_lidar=r_true,
-        search_s=0.08,
-    )
-    assert refined.ok
-    # Must not drift farther from truth than the coarse estimate by a large margin.
-    assert abs(refined.delta_t_s - meta["delta_t_s"]) <= abs(coarse.delta_t_s - meta["delta_t_s"]) + 0.01

tools/README.md

@@ -0,0 +1,30 @@
+# tools目录
+
+| 文件 | 输入→输出 |
+|---|---|
+| **`export_raw_to_combined.py`** | **一步导出**：逐站 H32（dlog MSOP+DIFOP 或旧 `.rscap`）+ 全程 G90/N300 `.rscap` → `combined/` |
+| **`export_g90_h32_windows_to_combined.py`** | **本次 27 站**：G90 连续 rscap + H32 DLog ZIP，按站时间窗 → `combined/`（host UTC 关联） |
+| `export_h32_rscap_station.py` | 内部零件：单站 H32 → 雷达帧 NPZ（一般不必单独跑） |
+| `h32_dlog/` | 新 H32 DLogCapture：dobject 索引、MSOP/DIFOP payload、DIFOP 通道角 |
+| `frontlidar_dlog_export.py` | **旧数据** 已解码点云 dlog → 逐帧 NPZ；无 raw MSOP 时由一步导出回退调用 |
+| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU（调试用）；一步导出已内嵌同等逻辑 |
+| `rscap_v2/h32_msop.py` | H32 MSOP 解码（XYZ / 极坐标 `points_raw`） |
+| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
+| `rscap_v2/audit_capture_v2.py` | 检查rscap结构、时间范围和记录统计 |
+| `build_multisensor_npz.py` | 关联雷达帧与 RTK/IMU → combined；一步导出内部调用 |
+| `prepare_multisensor_station_dataset.py` | combined NPZ → 每站一帧`frames_all`和`reference_poses_*.csv` |
+
+推荐用法：
+
+```powershell
+python tools\export_raw_to_combined.py `
+  --stations-root path\to\stations `
+  --rtk-rscap path\to\rtk.rscap `
+  --imu-rscap path\to\imu.rscap `
+  --out path\to\exported `
+  --overwrite
+```
+
+当前标定只使用LiDAR和RTK；IMU保持原始传感器坐标，不参与点云去畸变或外参求解。prepared阶段对站内有效RTK取平均、对heading取圆均值，并选择有效帧序列的中间LiDAR帧。
+
+G90 `#PVTSLNA` 没有 NMEA `fix_quality` 字段时，解析会写入合成值 `4`，以便沿用 prepare 的固定解筛选（`{4,5}`）。

tools/__init__.py

@@ -1 +0,0 @@
-# Tools package for local scripts and tests.

tools/build_multisensor_npz.py

@@ -0,0 +1,415 @@
+#!/usr/bin/env python3
+"""Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window.
+
+Inputs are LiDAR frame NPZ files from ``export_h32_rscap_station.py`` (or legacy
+``frontlidar_dlog_export.py``) and parsed RTK/IMU JSONL from
+``parse_rtk_imu_v2.py``.  Raw ``.rscap`` files remain the traceability source;
+this script never modifies them.
+
+Position rows may be NMEA ``GGA`` or G90 ``PVTSLNA`` (both expose ``lat_deg`` /
+``lon_deg`` / ``altitude_m``).  Default time basis is LiDAR device time vs GNSS
+week/TOW; ``--time-basis host`` keeps the legacy host-receive nearest-neighbour
+association for old dlog datasets.
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+
+GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
+POSITION_TYPES = {"GGA", "PVTSLNA"}
+HEADING_TYPES = {"UNIHEADINGA", "GNHPR"}
+
+
+def parse_named_path(text: str) -> tuple[str, Path]:
+    if "=" not in text:
+        raise argparse.ArgumentTypeError("expected NAME=PATH")
+    name, raw_path = text.split("=", 1)
+    if not name.strip():
+        raise argparse.ArgumentTypeError("segment name is empty")
+    return name.strip(), Path(raw_path)
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--lidar",
+        type=parse_named_path,
+        action="append",
+        required=True,
+        metavar="NAME=FRAMES_DIR",
+        help="Repeat for each LiDAR segment; directory contains exported *.npz frames.",
+    )
+    parser.add_argument("--rtk", type=Path, action="append", required=True, help="Parsed rtk.jsonl; repeat per session.")
+    parser.add_argument("--imu", type=Path, action="append", required=True, help="Parsed imu.jsonl; repeat per session.")
+    parser.add_argument("--out", type=Path, required=True)
+    parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
+    parser.add_argument("--imu-before-ms", type=float, default=100.0)
+    parser.add_argument("--imu-after-ms", type=float, default=100.0)
+    parser.add_argument("--gps-utc-leap-seconds", type=int, default=18)
+    parser.add_argument(
+        "--time-basis",
+        choices=("device_gnss", "host"),
+        default="device_gnss",
+        help="device_gnss: LiDAR unix_time_ns ↔ GNSS week/TOW; host: legacy host-receive association.",
+    )
+    parser.add_argument("--overwrite", action="store_true")
+    return parser.parse_args()
+
+
+def load_jsonl(paths: list[Path]) -> list[dict[str, Any]]:
+    rows: list[dict[str, Any]] = []
+    for source_index, path in enumerate(paths):
+        source_file = str(path.resolve())
+        with path.open("r", encoding="utf-8") as stream:
+            for line_number, line in enumerate(stream, start=1):
+                if not line.strip():
+                    continue
+                row = json.loads(line)
+                row["_source_file"] = source_file
+                row["_source_index"] = source_index
+                row["_source_line"] = line_number
+                rows.append(row)
+    return rows
+
+
+def utf8_array(value: Any) -> np.ndarray:
+    return np.frombuffer(str(value if value is not None else "").encode("utf-8"), dtype=np.uint8)
+
+
+def scalar(array: np.ndarray) -> Any:
+    return array.reshape(-1)[0].item()
+
+
+def nearest_index(times: np.ndarray, target: int) -> int:
+    if not len(times):
+        return -1
+    right = int(np.searchsorted(times, target, side="left"))
+    candidates = [index for index in (right - 1, right) if 0 <= index < len(times)]
+    return min(candidates, key=lambda index: abs(int(times[index]) - target))
+
+
+def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
+    """Recover timing inside each serial chunk from device timestamps.
+
+    A capture chunk has one host receive timestamp but may contain several IMU
+    frames.  The last frame is anchored to the chunk receive time and earlier
+    frames are moved backwards by their device timestamp difference.
+    """
+    groups: dict[tuple[int, int], list[dict[str, Any]]] = {}
+    for row in rows:
+        if not row.get("crc_valid") or row.get("device_timestamp_ms") is None:
+            continue
+        key = (int(row["_source_index"]), int(row.get("source_chunk_sequence_last", -1)))
+        groups.setdefault(key, []).append(row)
+    result: list[dict[str, Any]] = []
+    for group in groups.values():
+        group.sort(key=lambda row: (int(row["device_timestamp_ms"]), int(row["_source_line"])))
+        last_device = int(group[-1]["device_timestamp_ms"])
+        host_ns = int(group[-1]["host_receive_utc_ns"])
+        for row in group:
+            delta_ms = (last_device - int(row["device_timestamp_ms"])) & 0xFFFFFFFF
+            if delta_ms > 60_000:
+                delta_ms = 0
+            copied = dict(row)
+            copied["estimated_time_ns"] = host_ns - delta_ms * 1_000_000
+            result.append(copied)
+    result.sort(key=lambda row: int(row["estimated_time_ns"]))
+    return result
+
+
+def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
+    week, tow_ms = row.get("gnss_week"), row.get("gnss_tow_ms")
+    if week is None or tow_ms is None:
+        return None
+    seconds = int(week) * 604800 + float(tow_ms) / 1000.0 - leap_seconds
+    return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
+
+
+def association_time_ns(row: dict[str, Any], time_basis: str, leap_seconds: int) -> int | None:
+    if time_basis == "host":
+        host = row.get("host_receive_utc_ns")
+        return int(host) if host is not None else None
+    device = gnss_utc_ns(row, leap_seconds)
+    if device is not None:
+        return device
+    host = row.get("host_receive_utc_ns")
+    return int(host) if host is not None else None
+
+
+def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray:
+    return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype)
+
+
+def raw_frame_matrix(rows: list[dict[str, Any]]) -> tuple[np.ndarray, np.ndarray]:
+    frames = [bytes.fromhex(str(row.get("raw_frame_hex", ""))) for row in rows]
+    lengths = np.asarray([len(frame) for frame in frames], dtype=np.int32)
+    width = max(lengths, default=0)
+    matrix = np.zeros((len(frames), width), dtype=np.uint8)
+    for index, frame in enumerate(frames):
+        matrix[index, : len(frame)] = np.frombuffer(frame, dtype=np.uint8)
+    return matrix, lengths
+
+
+def add_rtk(values: dict[str, np.ndarray], prefix: str, row: dict[str, Any] | None, dt_ns: int | None) -> None:
+    values[f"{prefix}_valid"] = np.asarray([row is not None], dtype=np.uint8)
+    values[f"{prefix}_dt_ns"] = np.asarray([dt_ns or 0], dtype=np.int64)
+    values[f"{prefix}_host_receive_utc_ns"] = np.asarray([0], dtype=np.int64)
+    values[f"{prefix}_raw_utf8"] = utf8_array("")
+    values[f"{prefix}_source_file_utf8"] = utf8_array("")
+    values[f"{prefix}_source_raw_file_offset"] = np.asarray([-1], dtype=np.int64)
+    values[f"{prefix}_source_raw_byte_length"] = np.asarray([0], dtype=np.int32)
+    if row is None:
+        return
+    values[f"{prefix}_host_receive_utc_ns"] = np.asarray([row.get("host_receive_utc_ns", 0)], dtype=np.int64)
+    values[f"{prefix}_raw_utf8"] = utf8_array(row.get("raw_line", ""))
+    values[f"{prefix}_source_file_utf8"] = utf8_array(row.get("_source_file", ""))
+    values[f"{prefix}_source_raw_file_offset"] = np.asarray([row.get("source_raw_file_offset", -1)], dtype=np.int64)
+    values[f"{prefix}_source_raw_byte_length"] = np.asarray([row.get("source_raw_byte_length", 0)], dtype=np.int32)
+
+
+def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
+    for key, dtype, default in (
+        ("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
+        ("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
+        ("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
+        ("differential_age_s", np.float64, np.nan),
+        ("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
+        ("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
+        ("pitch_deg", np.float64, np.nan), ("roll_deg", np.float64, np.nan),
+        ("heading_stddev_deg", np.float64, np.nan),
+        ("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
+        ("solution_satellites", np.int32, -1),
+    ):
+        values[f"rtk_{key}"] = np.asarray([default], dtype=dtype)
+    values["rtk_fixed"] = np.asarray([0], dtype=np.uint8)
+    values["rtk_heading_solution_utf8"] = utf8_array("")
+    values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64)
+    values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64)
+
+
+def build_combined(
+    lidar_segments: list[tuple[str, Path]],
+    rtk_paths: list[Path],
+    imu_paths: list[Path],
+    out: Path,
+    *,
+    rtk_max_dt_ms: float = 150.0,
+    imu_before_ms: float = 100.0,
+    imu_after_ms: float = 100.0,
+    gps_utc_leap_seconds: int = 18,
+    time_basis: str = "device_gnss",
+    overwrite: bool = False,
+) -> dict[str, Any]:
+    """Associate LiDAR frames with RTK/IMU and write ``out/`` combined package."""
+
+    if out.exists() and any(out.iterdir()) and not overwrite:
+        raise FileExistsError(f"{out} is non-empty; pass overwrite=True")
+    frames_out = out / "frames"
+    frames_out.mkdir(parents=True, exist_ok=True)
+
+    rtk_rows = load_jsonl(rtk_paths)
+    positions = []
+    for row in rtk_rows:
+        if row.get("type") not in POSITION_TYPES or not row.get("checksum_valid"):
+            continue
+        if row.get("lat_deg") is None or row.get("lon_deg") is None:
+            continue
+        assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
+        if assoc is None:
+            continue
+        copied = dict(row)
+        copied["_assoc_time_ns"] = assoc
+        positions.append(copied)
+    positions.sort(key=lambda row: int(row["_assoc_time_ns"]))
+
+    heading = []
+    for row in rtk_rows:
+        if row.get("type") not in HEADING_TYPES or not row.get("checksum_valid") or not row.get("heading_valid"):
+            continue
+        assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
+        if assoc is None:
+            continue
+        copied = dict(row)
+        copied["_assoc_time_ns"] = assoc
+        heading.append(copied)
+    heading.sort(key=lambda row: int(row["_assoc_time_ns"]))
+
+    imu = estimate_imu_times(load_jsonl(imu_paths))
+    position_times = np.asarray([int(row["_assoc_time_ns"]) for row in positions], dtype=np.int64)
+    heading_times = np.asarray([int(row["_assoc_time_ns"]) for row in heading], dtype=np.int64)
+    imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64)
+
+    manifest: list[dict[str, Any]] = []
+    global_index = 0
+    max_rtk_ns = int(rtk_max_dt_ms * 1_000_000)
+    before_ns = int(imu_before_ms * 1_000_000)
+    after_ns = int(imu_after_ms * 1_000_000)
+
+    for segment_name, frame_dir in lidar_segments:
+        frame_paths = sorted(frame_dir.glob("*.npz"))
+        if not frame_paths:
+            raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
+        for segment_index, source in enumerate(frame_paths):
+            with np.load(source, allow_pickle=False) as frame:
+                values = {key: np.asarray(frame[key]) for key in frame.files}
+            lidar_device_time_ns = int(scalar(values["unix_time_ns"]))
+            if time_basis == "host":
+                lidar_time_ns = int(scalar(values["host_receive_utc_ns"]))
+                if lidar_time_ns <= 0:
+                    raise ValueError(f"host time requested but missing in {source}")
+            else:
+                lidar_time_ns = lidar_device_time_ns
+            values["lidar_association_time_ns"] = np.asarray([lidar_time_ns], dtype=np.int64)
+
+            position_index = nearest_index(position_times, lidar_time_ns)
+            heading_index = nearest_index(heading_times, lidar_time_ns)
+            position_row = positions[position_index] if position_index >= 0 else None
+            heading_row = heading[heading_index] if heading_index >= 0 else None
+            position_dt = int(position_times[position_index]) - lidar_time_ns if position_index >= 0 else None
+            heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None
+            position_ok = position_row is not None and abs(position_dt or 0) <= max_rtk_ns
+            heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns
+            add_rtk(values, "rtk_gga", position_row if position_ok else None, position_dt)
+            add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt)
+            initialize_rtk_measurements(values)
+
+            if position_ok and position_row:
+                for key, dtype, default in (
+                    ("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
+                    ("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
+                    ("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
+                    ("differential_age_s", np.float64, np.nan),
+                ):
+                    values[f"rtk_{key}"] = np.asarray([position_row.get(key, default)], dtype=dtype)
+                values["rtk_gga_satellites"] = np.asarray([position_row.get("satellites", -1)], dtype=np.int32)
+                if position_row.get("gnss_week") is not None:
+                    values["rtk_gnss_week"] = np.asarray([position_row.get("gnss_week", -1)], dtype=np.int32)
+                    values["rtk_gnss_tow_ms"] = np.asarray([position_row.get("gnss_tow_ms", -1)], dtype=np.int64)
+                values["rtk_fixed"] = np.asarray([int(position_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
+            if heading_ok and heading_row:
+                for key, dtype, default in (
+                    ("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
+                    ("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
+                    ("pitch_deg", np.float64, np.nan), ("roll_deg", np.float64, np.nan),
+                    ("heading_stddev_deg", np.float64, np.nan),
+                    ("pitch_stddev_deg", np.float64, np.nan),
+                    ("solution_satellites", np.int32, -1),
+                ):
+                    value = heading_row.get(key, default)
+                    if key == "roll_deg" and value is None:
+                        value = 0.0
+                    values[f"rtk_{key}"] = np.asarray([value], dtype=dtype)
+                values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
+                values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
+                device_ns = gnss_utc_ns(heading_row, gps_utc_leap_seconds)
+                values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64)
+                values["rtk_heading_host_minus_gnss_ns"] = np.asarray(
+                    [int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64
+                )
+
+            left = int(np.searchsorted(imu_times, lidar_time_ns - before_ns, side="left"))
+            right = int(np.searchsorted(imu_times, lidar_time_ns + after_ns, side="right"))
+            window = imu[left:right]
+            values["imu_window_count"] = np.asarray([len(window)], dtype=np.int32)
+            values["imu_valid"] = np.asarray([bool(window)], dtype=np.uint8)
+            values["imu_time_ns"] = numeric_array(window, "estimated_time_ns", np.int64, 0)
+            values["imu_host_receive_utc_ns"] = numeric_array(window, "host_receive_utc_ns", np.int64, 0)
+            for key in ("device_timestamp_ms", "pps_sync_stamp_ms", "tag"):
+                values[f"imu_{key}"] = numeric_array(window, key, np.int64, -1)
+            for key in (
+                "temperature_c", "air_pressure_pa", "accel_x_mps2", "accel_y_mps2", "accel_z_mps2",
+                "gyro_x_radps", "gyro_y_radps", "gyro_z_radps", "mag_x_ut", "mag_y_ut", "mag_z_ut",
+                "roll_deg", "pitch_deg", "yaw_deg", "quaternion_w", "quaternion_x", "quaternion_y", "quaternion_z",
+            ):
+                values[f"imu_{key}"] = numeric_array(window, key, np.float64, np.nan)
+            values["imu_source_index"] = numeric_array(window, "_source_index", np.int32, -1)
+            values["imu_source_raw_file_offset"] = numeric_array(window, "source_raw_file_offset", np.int64, -1)
+            raw_matrix, raw_lengths = raw_frame_matrix(window)
+            values["imu_raw_frame_bytes"] = raw_matrix
+            values["imu_raw_frame_length"] = raw_lengths
+            values["imu_source_files_json_utf8"] = utf8_array(
+                json.dumps([str(path.resolve()) for path in imu_paths], ensure_ascii=False)
+            )
+            values["source_lidar_file_utf8"] = utf8_array(source.resolve())
+            values["segment_name_utf8"] = utf8_array(segment_name)
+
+            output = frames_out / f"{segment_name}_{segment_index:06d}.npz"
+            np.savez_compressed(output, **values)
+            manifest.append({
+                "global_index": global_index,
+                "segment": segment_name,
+                "segment_index": segment_index,
+                "output": str(output.relative_to(out)),
+                "source_lidar": str(source.resolve()),
+                "lidar_time_ns": lidar_time_ns,
+                "lidar_device_time_ns": lidar_device_time_ns,
+                "rtk_gga_dt_ns": position_dt,
+                "rtk_heading_dt_ns": heading_dt,
+                "rtk_valid": position_ok,
+                "heading_valid": heading_ok,
+                "rtk_fix_quality": position_row.get("fix_quality") if position_ok and position_row else None,
+                "rtk_fixed": bool(position_ok and position_row and int(position_row.get("fix_quality", -1)) in {4, 5}),
+                "imu_window_count": len(window),
+            })
+            global_index += 1
+
+    fields = sorted({key for row in manifest for key in row})
+    with (out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
+        writer = csv.DictWriter(stream, fieldnames=fields)
+        writer.writeheader()
+        writer.writerows(manifest)
+    if time_basis == "device_gnss":
+        time_basis_text = (
+            "LiDAR MSOP/device unix_time_ns ↔ RTK GNSS week/TOW (fallback host receive); "
+            "IMU still windowed on host-anchored device deltas"
+        )
+    else:
+        time_basis_text = (
+            "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement"
+        )
+    summary = {
+        "frames": len(manifest),
+        "segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in lidar_segments},
+        "rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest),
+        "heading_valid": sum(bool(row["heading_valid"]) for row in manifest),
+        "rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest),
+        "imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest),
+        "rtk_max_dt_ms": rtk_max_dt_ms,
+        "imu_window_ms": [-imu_before_ms, imu_after_ms],
+        "time_basis": time_basis_text,
+        "time_basis_mode": time_basis,
+        "position_message_types": sorted(POSITION_TYPES),
+        "imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied",
+    }
+    (out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
+    return summary
+
+
+def main() -> int:
+    args = parse_args()
+    summary = build_combined(
+        args.lidar,
+        args.rtk,
+        args.imu,
+        args.out,
+        rtk_max_dt_ms=args.rtk_max_dt_ms,
+        imu_before_ms=args.imu_before_ms,
+        imu_after_ms=args.imu_after_ms,
+        gps_utc_leap_seconds=args.gps_utc_leap_seconds,
+        time_basis=args.time_basis,
+        overwrite=args.overwrite,
+    )
+    print(json.dumps(summary, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/build_slam_delivery.py

@@ -0,0 +1,295 @@
+#!/usr/bin/env python3
+"""Build LiDAR GT/quality tables for a continuous LiDAR + dual-RTK + IMU run."""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import datetime as dt
+import json
+import math
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from rtk_attitude import heading_to_enu_yaw, rotation_to_quat_xyzw, rtk_body_rotation
+
+
+def args() -> argparse.Namespace:
+    p = argparse.ArgumentParser(description=__doc__)
+    p.add_argument("--lidar-manifest", type=Path, required=True)
+    p.add_argument("--rtk-jsonl", type=Path, required=True)
+    p.add_argument("--imu-jsonl", type=Path, required=True)
+    p.add_argument("--extrinsic", type=Path, required=True)
+    p.add_argument("--out", type=Path, required=True)
+    p.add_argument("--max-bracket-ms", type=float, default=150.0)
+    p.add_argument("--heading-std-limit-deg", type=float, default=0.5)
+    p.add_argument(
+        "--heading-offset-deg",
+        type=float,
+        default=None,
+        help="Added to rawHeading before ENU yaw. Default: body_heading_offset_deg from extrinsic JSON, else 0.",
+    )
+    p.add_argument(
+        "--orientation-model",
+        choices=("heading_pitch_roll", "yaw_only"),
+        default="heading_pitch_roll",
+        help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll in T_W_RTK; yaw_only forces pitch=roll=0",
+    )
+    return p.parse_args()
+
+
+POSITION_TYPES = {"GGA", "PVTSLNA"}
+HEADING_TYPES = {"UNIHEADINGA", "GNHPR"}
+
+
+def heading_row_valid(row: dict[str, Any]) -> bool:
+    if row.get("type") == "UNIHEADINGA":
+        return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
+    if row.get("type") == "GNHPR":
+        return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
+    return False
+
+
+def heading_quality_ok(row: dict[str, Any], std_limit_deg: float) -> list[str]:
+    reasons: list[str] = []
+    if row.get("type") == "UNIHEADINGA":
+        if str(row.get("heading_solution", "")) != "NARROW_INT":
+            reasons.append("HEADING_NOT_NARROW_INT")
+        std = float(row.get("heading_stddev_deg") or math.inf)
+        if std > std_limit_deg:
+            reasons.append("HEADING_STD_EXCEEDED")
+    elif row.get("type") == "GNHPR":
+        quality = int(row.get("heading_quality", -1) or -1)
+        if quality not in {4, 5} and not row.get("heading_valid"):
+            reasons.append("HEADING_QUALITY_NOT_FIXED")
+    return reasons
+
+
+def read_jsonl(path: Path) -> list[dict[str, Any]]:
+    with path.open(encoding="utf-8") as f:
+        return [json.loads(line) for line in f if line.strip()]
+
+
+def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray:
+    a, e2 = 6378137.0, 6.69437999014e-3
+    lat, lon = math.radians(lat_deg), math.radians(lon_deg)
+    slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
+    n = a / math.sqrt(1.0 - e2 * slat * slat)
+    return np.array([(n + height_m) * clat * clon,
+                     (n + height_m) * clat * slon,
+                     (n * (1.0 - e2) + height_m) * slat], dtype=float)
+
+
+def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: float) -> np.ndarray:
+    lat, lon = math.radians(lat_deg), math.radians(lon_deg)
+    slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
+    r = np.array([[-slon, clon, 0.0],
+                  [-slat * clon, -slat * slon, clat],
+                  [clat * clon, clat * slon, slat]], dtype=float)
+    return r @ (ecef - origin)
+
+
+def bracket(rows: list[dict[str, Any]], times: np.ndarray, t: int,
+            max_ns: int) -> tuple[dict[str, Any], dict[str, Any], float] | None:
+    right = int(np.searchsorted(times, t, side="left"))
+    if right == 0 or right >= len(times):
+        return None
+    left = right - 1
+    t0, t1 = int(times[left]), int(times[right])
+    if t1 <= t0 or t - t0 > max_ns or t1 - t > max_ns:
+        return None
+    return rows[left], rows[right], (t - t0) / (t1 - t0)
+
+
+def circular_lerp_deg(a: float, b: float, u: float) -> float:
+    delta = (b - a + 180.0) % 360.0 - 180.0
+    return (a + u * delta) % 360.0
+
+
+def linear_lerp(a: float, b: float, u: float) -> float:
+    return (1.0 - u) * a + u * b
+
+
+def iso_utc(ns: int) -> str:
+    return dt.datetime.fromtimestamp(ns / 1e9, dt.timezone.utc).isoformat(timespec="microseconds")
+
+
+def write_imu_csv(rows: list[dict[str, Any]], path: Path) -> None:
+    fields = [
+        "host_receive_utc_ns", "device_timestamp_ms", "pps_sync_stamp_ms", "crc_valid",
+        "accel_x_mps2", "accel_y_mps2", "accel_z_mps2",
+        "gyro_x_radps", "gyro_y_radps", "gyro_z_radps",
+        "mag_x_ut", "mag_y_ut", "mag_z_ut", "temperature_c", "air_pressure_pa",
+        "roll_deg", "pitch_deg", "yaw_deg",
+        "quaternion_x", "quaternion_y", "quaternion_z", "quaternion_w",
+        "source_chunk_sequence_first", "source_raw_file_offset",
+    ]
+    with path.open("w", encoding="utf-8", newline="") as f:
+        w = csv.DictWriter(f, fieldnames=fields)
+        w.writeheader()
+        for row in rows:
+            w.writerow({key: row.get(key) for key in fields})
+
+
+def main() -> int:
+    a = args()
+    a.out.mkdir(parents=True, exist_ok=True)
+    with a.lidar_manifest.open(encoding="utf-8-sig", newline="") as f:
+        lidar = [row for row in csv.DictReader(f) if not row.get("error")]
+    rtk = read_jsonl(a.rtk_jsonl)
+    imu = [row for row in read_jsonl(a.imu_jsonl) if row.get("crc_valid")]
+    positions = sorted(
+        [
+            r for r in rtk
+            if r.get("type") in POSITION_TYPES
+            and r.get("checksum_valid")
+            and r.get("lat_deg") is not None
+        ],
+        key=lambda r: int(r["host_receive_utc_ns"]),
+    )
+    heading = sorted(
+        [r for r in rtk if r.get("type") in HEADING_TYPES and heading_row_valid(r)],
+        key=lambda r: int(r["host_receive_utc_ns"]),
+    )
+    if not lidar or len(positions) < 2 or len(heading) < 2:
+        raise RuntimeError("insufficient LiDAR/GGA|PVTSLNA/heading(GNHPR|UNIHEADINGA) data")
+
+    ext = json.loads(a.extrinsic.read_text(encoding="utf-8"))
+    t_r_l = np.asarray(ext["matrix_4x4"], dtype=float)
+    if t_r_l.shape != (4, 4):
+        raise ValueError("extrinsic matrix_4x4 must be 4x4")
+    heading_offset_deg = (
+        float(a.heading_offset_deg)
+        if a.heading_offset_deg is not None
+        else float(ext.get("body_heading_offset_deg", 0.0) or 0.0)
+    )
+
+    position_times = np.asarray([int(r["host_receive_utc_ns"]) for r in positions], dtype=np.int64)
+    heading_times = np.asarray([int(r["host_receive_utc_ns"]) for r in heading], dtype=np.int64)
+    origin_row = next(
+        (r for r in positions if int(r.get("fix_quality", -1)) in {4, 5}),
+        positions[0],
+    )
+    origin_lat, origin_lon, origin_alt = (float(origin_row[k]) for k in ("lat_deg", "lon_deg", "altitude_m"))
+    origin_ecef = geodetic_to_ecef(origin_lat, origin_lon, origin_alt)
+    max_ns = int(a.max_bracket_ms * 1_000_000)
+    pose_rows: list[dict[str, Any]] = []
+
+    for index, frame in enumerate(lidar):
+        t = int(frame["unix_time_ns"])
+        gb = bracket(positions, position_times, t, max_ns)
+        hb = bracket(heading, heading_times, t, max_ns)
+        reasons: list[str] = []
+        available = gb is not None and hb is not None
+        row: dict[str, Any] = {
+            "frame_index": index, "lidar_time_ns": t, "lidar_time_utc": iso_utc(t),
+            "lidar_file": frame["output_file"], "point_count": frame["point_count"],
+            "pose_available": int(available), "gt_valid": 0, "invalid_reason": "",
+        }
+        if not available:
+            if gb is None: reasons.append("POSITION_NOT_BRACKETED")
+            if hb is None: reasons.append("HEADING_NOT_BRACKETED")
+            row.update({k: "" for k in ("x_m", "y_m", "z_m", "qx", "qy", "qz", "qw",
+                                                 "rtk_x_m", "rtk_y_m", "rtk_z_m", "raw_heading_deg")})
+            row["invalid_reason"] = ";".join(reasons)
+            pose_rows.append(row)
+            continue
+
+        g0, g1, gu = gb
+        h0, h1, hu = hb
+        p0 = geodetic_to_ecef(float(g0["lat_deg"]), float(g0["lon_deg"]), float(g0["altitude_m"]))
+        p1 = geodetic_to_ecef(float(g1["lat_deg"]), float(g1["lon_deg"]), float(g1["altitude_m"]))
+        p_rtk = ecef_to_enu((1.0 - gu) * p0 + gu * p1, origin_ecef, origin_lat, origin_lon)
+        raw_heading = circular_lerp_deg(float(h0["raw_heading_deg"]), float(h1["raw_heading_deg"]), hu)
+        corrected_heading, yaw = heading_to_enu_yaw(raw_heading, heading_offset_deg)
+        if a.orientation_model == "heading_pitch_roll":
+            pitch = linear_lerp(float(h0.get("pitch_deg") or 0.0), float(h1.get("pitch_deg") or 0.0), hu)
+            roll = linear_lerp(float(h0.get("roll_deg") or 0.0), float(h1.get("roll_deg") or 0.0), hu)
+        else:
+            pitch = 0.0
+            roll = 0.0
+        t_w_r = np.eye(4)
+        t_w_r[:3, :3] = rtk_body_rotation(
+            raw_heading, heading_offset_deg, pitch_deg=pitch, roll_deg=roll
+        )
+        t_w_r[:3, 3] = p_rtk
+        t_w_l = t_w_r @ t_r_l
+        q = rotation_to_quat_xyzw(t_w_l[:3, :3])
+
+        fix0, fix1 = int(g0.get("fix_quality", -1)), int(g1.get("fix_quality", -1))
+        if fix0 not in {4, 5} or fix1 not in {4, 5}:
+            reasons.append("RTK_POSITION_NOT_FIXED")
+        reasons.extend(heading_quality_ok(h0, a.heading_std_limit_deg))
+        reasons.extend(heading_quality_ok(h1, a.heading_std_limit_deg))
+        # Deduplicate while preserving order
+        reasons = list(dict.fromkeys(reasons))
+        row.update({
+            "gt_valid": int(not reasons), "invalid_reason": ";".join(reasons),
+            "x_m": t_w_l[0, 3], "y_m": t_w_l[1, 3], "z_m": t_w_l[2, 3],
+            "qx": q[0], "qy": q[1], "qz": q[2], "qw": q[3],
+            "rtk_x_m": p_rtk[0], "rtk_y_m": p_rtk[1], "rtk_z_m": p_rtk[2],
+            "raw_heading_deg": raw_heading,
+            "corrected_heading_deg": corrected_heading,
+            "heading_offset_deg": heading_offset_deg,
+            "yaw_enu_deg": math.degrees(yaw),
+            "pitch_deg": pitch,
+            "roll_deg": roll,
+            "position_fix_before": fix0, "position_fix_after": fix1,
+            "heading_type_before": h0.get("type"), "heading_type_after": h1.get("type"),
+            "heading_solution_before": h0.get("heading_solution"),
+            "heading_solution_after": h1.get("heading_solution"),
+            "position_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6,
+            "position_after_dt_ms": (int(g1["host_receive_utc_ns"]) - t) / 1e6,
+            "heading_before_dt_ms": (t - int(h0["host_receive_utc_ns"])) / 1e6,
+            "heading_after_dt_ms": (int(h1["host_receive_utc_ns"]) - t) / 1e6,
+        })
+        pose_rows.append(row)
+
+    fields = list(dict.fromkeys(k for row in pose_rows for k in row))
+    pose_path = a.out / "lidar_gt_pose_enu.csv"
+    with pose_path.open("w", encoding="utf-8", newline="") as f:
+        w = csv.DictWriter(f, fieldnames=fields)
+        w.writeheader(); w.writerows(pose_rows)
+    write_imu_csv(imu, a.out / "imu_parsed.csv")
+
+    summary = {
+        "coordinate_convention": "T_W_L maps raw LiDAR points to local ENU; T_W_L = T_W_RTK @ T_RTK_lidar",
+        "world_frame": "local ENU, origin is the first RTK FIX position sample",
+        "rtk_frame": (
+            "delivered body X follows rawHeading after heading_offset_deg; "
+            "pitch/roll applied in baseline frame before the fixed offset"
+        ),
+        "heading_offset_deg": heading_offset_deg,
+        "heading_sources_accepted": sorted(HEADING_TYPES),
+        "position_sources_accepted": sorted(POSITION_TYPES),
+        "orientation_model": a.orientation_model,
+        "orientation_composition": (
+            "R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset)"
+        ),
+        "orientation_note": "Uses dual-antenna GNHPR/UNIHEADINGA pitch/roll; IMU orientation is not fused",
+        "time_basis": "LiDAR and serial host UTC; no jointly estimated clock offset/drift",
+        "lidar_frames": len(pose_rows),
+        "pose_available_frames": sum(int(r["pose_available"]) for r in pose_rows),
+        "gt_valid_frames": sum(int(r["gt_valid"]) for r in pose_rows),
+        "gt_invalid_frames": sum(not int(r["gt_valid"]) for r in pose_rows),
+        "imu_frames": len(imu),
+        "enu_origin": {"lat_deg": origin_lat, "lon_deg": origin_lon, "altitude_m": origin_alt},
+        "quality_rule": (
+            "position endpoints fix_quality in {4,5}; UNIHEADINGA endpoints NARROW_INT with std gate; "
+            "GNHPR endpoints heading_valid/quality 4|5; both streams bracket LiDAR time"
+        ),
+        "heading_std_limit_deg": a.heading_std_limit_deg,
+        "max_bracket_ms": a.max_bracket_ms,
+        "warning": "gt_valid is a quality gate, not independent proof of +/-3 cm absolute accuracy",
+    }
+    (a.out / "delivery_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
+    print(json.dumps(summary, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
+
+

tools/compare_s2_runs.py

@@ -1,75 +0,0 @@
-#!/usr/bin/env python3
-"""Compare two S2 offline summary.json runs (real artifacts only)."""
-
-from __future__ import annotations
-
-import json
-import sys
-from pathlib import Path
-
-import numpy as np
-
-from imu_lidar.geometry import rpy_deg_xyz, so3_log
-
-
-def dig(path: Path):
-    data = json.loads(path.read_text(encoding="utf-8"))
-    session = data["details"]["sessions"][0]
-    return data, session, session.get("handeye", {}), session.get("time_offset", {})
-
-
-def main() -> int:
-    old_path = Path(sys.argv[1])
-    new_path = Path(sys.argv[2])
-    old, so, heo, too = dig(old_path)
-    new, sn, hen, ton = dig(new_path)
-
-    print("==== COMPARISON (real summary.json artifacts) ====")
-    print(f"{'metric':28s} {'run_a':28s} {'run_b':28s}")
-    rows = [
-        ("status", old.get("status"), new.get("status")),
-        ("stage", so.get("stage"), sn.get("stage")),
-        ("delta_t_s", f"{too.get('delta_t_s'):.6f}", f"{ton.get('delta_t_s'):.6f}"),
-        (
-            "corr/mag_peak",
-            f"{too.get('correlation_peak'):.6f}",
-            f"{ton.get('correlation_peak'):.6f}",
-        ),
-        ("keyframes", so.get("keyframes"), sn.get("keyframes")),
-        ("handeye_pairs", heo.get("pair_count"), hen.get("pair_count")),
-        ("handeye_ok", heo.get("ok"), hen.get("ok")),
-        ("rms_deg", f"{heo.get('residual_rms_deg'):.4f}", f"{hen.get('residual_rms_deg'):.4f}"),
-        (
-            "median_deg",
-            f"{heo.get('residual_median_deg'):.4f}",
-            f"{hen.get('residual_median_deg'):.4f}",
-        ),
-    ]
-    for key, a, b in rows:
-        print(f"{key:28s} {str(a):28s} {str(b):28s}")
-
-    print("run_a pair_notes:", so.get("pair_notes"))
-    print("run_b pair_notes:", sn.get("pair_notes"))
-    print("run_a time notes:", too.get("notes"))
-    print("run_b time notes:", ton.get("notes"))
-    print("run_a handeye notes:", heo.get("notes"))
-    print("run_b handeye notes:", hen.get("notes"))
-
-    r_old = np.asarray(heo["R_IMU_lidar"], dtype=float)
-    r_new = np.asarray(hen["R_IMU_lidar"], dtype=float)
-    print("R relative change deg:", float(np.degrees(np.linalg.norm(so3_log(r_old.T @ r_new)))))
-    print("RPY run_a deg:", rpy_deg_xyz(r_old))
-    print("RPY run_b deg:", rpy_deg_xyz(r_new))
-    print("delta rms (b-a):", hen.get("residual_rms_deg") - heo.get("residual_rms_deg"))
-    print(
-        "delta median (b-a):",
-        hen.get("residual_median_deg") - heo.get("residual_median_deg"),
-    )
-    print("artifacts:")
-    print(" run_a:", old_path)
-    print(" run_b:", new_path)
-    return 0
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

tools/convert_lidar_npz_to_xyz.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+"""Convert exported LiDAR polar NPZ frames to portable XYZ-in-metres NPZ frames."""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+
+
+def main() -> int:
+    p = argparse.ArgumentParser(description=__doc__)
+    p.add_argument("--input", type=Path, required=True)
+    p.add_argument("--output", type=Path, required=True)
+    p.add_argument("--overwrite", action="store_true")
+    a = p.parse_args()
+    sources = sorted(a.input.glob("*.npz"))
+    if not sources:
+        raise FileNotFoundError(f"no NPZ frames in {a.input}")
+    a.output.mkdir(parents=True, exist_ok=True)
+    written = skipped = 0
+    for index, source in enumerate(sources, 1):
+        target = a.output / source.name
+        if target.exists() and not a.overwrite:
+            skipped += 1
+            continue
+        with np.load(source, allow_pickle=False) as f:
+            raw = np.asarray(f["points_raw"], dtype=np.float32)
+            time_ns = np.asarray(f["unix_time_ns"], dtype=np.int64)
+            counter = np.asarray(f["frame_counter"], dtype=np.int32)
+        distance_m = raw[:, 0] * np.float32(0.001)
+        azimuth = np.deg2rad(raw[:, 1])
+        altitude = np.deg2rad(raw[:, 2])
+        cos_alt = np.cos(altitude)
+        xyz = np.column_stack((distance_m * cos_alt * np.cos(azimuth),
+                               distance_m * cos_alt * np.sin(azimuth),
+                               distance_m * np.sin(altitude))).astype(np.float32, copy=False)
+        np.savez_compressed(target, xyz_m=xyz, intensity=raw[:, 3].astype(np.float32, copy=False),
+                            progression=raw[:, 4].astype(np.float32, copy=False),
+                            unix_time_ns=time_ns, frame_counter=counter)
+        written += 1
+        if index % 100 == 0 or index == len(sources):
+            print(f"[{index}/{len(sources)}] written={written} skipped={skipped}", flush=True)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/export_g90_h32_windows_to_combined.py

@@ -0,0 +1,381 @@
+#!/usr/bin/env python3
+"""Export audited H32/G90 static windows directly to RTK--LiDAR combined data.
+
+This adapter is for captures where multiple static stations live inside large
+DLog archives instead of one directory per station.  It uses the H32 packet
+host-receive UTC ticks as the common software clock, parses G90 ``$GNGGA`` and
+``$GNHPR`` from one or more V2 captures, and deliberately does not require IMU.
+Raw inputs are opened read-only.
+"""
+
+from __future__ import annotations
+
+import argparse
+import bisect
+import csv
+import json
+import shutil
+import struct
+import sys
+import zipfile
+from dataclasses import dataclass
+from datetime import datetime, timedelta, timezone
+from pathlib import Path
+from typing import BinaryIO, Iterator
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parent
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "rscap_v2"))
+
+from build_multisensor_npz import build_combined  # noqa: E402
+from h32_dlog.difop import DifopAngles, parse_difop_angles  # noqa: E402
+from h32_dlog.dotnet_bin import read_dotnet_string  # noqa: E402
+from h32_dlog.payload_v1 import parse_difop_payload, parse_msop_batch_payload  # noqa: E402
+from capture_format_v2 import read_capture  # noqa: E402
+from h32_msop import iter_h32_frames_polar_from_packets  # noqa: E402
+from pipeline_common_corrected import parse_rtk_capture, write_jsonl  # noqa: E402
+
+DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
+TICKS_PER_SECOND = 10_000_000
+LOCAL_TZ = timezone(timedelta(hours=8))
+MSOP_OBJECT = "frontlidar-msop-raw"
+DIFOP_OBJECT = "frontlidar-difop-raw"
+
+
+@dataclass(frozen=True)
+class Window:
+    station_id: str
+    start_ticks: int
+    end_ticks: int
+
+
+def local_text_to_utc_ticks(text: str) -> int:
+    value = datetime.strptime(text.strip(), "%Y-%m-%d %H:%M:%S.%f").replace(tzinfo=LOCAL_TZ)
+    return int(round(value.timestamp() * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS
+
+
+def read_exact(stream: BinaryIO, length: int) -> bytes:
+    value = stream.read(length)
+    if len(value) != length:
+        raise EOFError(f"expected {length} bytes, got {len(value)}")
+    return value
+
+
+def iter_zip_dobject_payloads(path: Path) -> Iterator[tuple[str, bytes]]:
+    """Sequentially read DObject records from a standard Medulla DLog ZIP."""
+
+    with zipfile.ZipFile(path) as archive:
+        candidates = [
+            name for name in archive.namelist()
+            if name.replace("\\", "/").endswith("dobject_recording/data.bin")
+        ]
+        if len(candidates) != 1:
+            raise ValueError(f"{path}: expected one dobject_recording/data.bin, got {candidates}")
+        with archive.open(candidates[0], "r") as stream:
+            while True:
+                try:
+                    name = read_dotnet_string(stream)
+                except EOFError:
+                    break
+                read_exact(stream, 8)  # outer DObject tick
+                read_dotnet_string(stream)  # record id
+                length = struct.unpack("<i", read_exact(stream, 4))[0]
+                if length < 0 or length > 128 * 1024 * 1024:
+                    raise ValueError(f"{path}: invalid DObject payload length {length}")
+                yield name, read_exact(stream, length)
+
+
+def load_windows(path: Path) -> list[Window]:
+    grouped: dict[str, list[tuple[int, int]]] = {}
+    with path.open("r", encoding="utf-8-sig", newline="") as stream:
+        for row in csv.DictReader(stream):
+            station = row["station_id"].strip()
+            grouped.setdefault(station, []).append(
+                (local_text_to_utc_ticks(row["local_start"]), local_text_to_utc_ticks(row["local_end"]))
+            )
+    merged: list[Window] = []
+    for station, ranges in grouped.items():
+        current: list[list[int]] = []
+        for start, end in sorted(ranges):
+            if current and start <= current[-1][1]:
+                current[-1][1] = max(current[-1][1], end)
+            else:
+                current.append([start, end])
+        merged.extend(Window(station, start, end) for start, end in current)
+    merged.sort(key=lambda item: item.start_ticks)
+    for previous, current in zip(merged, merged[1:]):
+        if current.start_ticks <= previous.end_ticks and current.station_id != previous.station_id:
+            raise ValueError(f"overlapping stations: {previous} and {current}")
+    return merged
+
+
+def station_lookup(windows: list[Window]):
+    starts = [item.start_ticks for item in windows]
+
+    def lookup(ticks: int) -> str | None:
+        index = bisect.bisect_right(starts, ticks) - 1
+        if index >= 0 and ticks <= windows[index].end_ticks:
+            return windows[index].station_id
+        return None
+
+    return lookup
+
+
+def save_frames(
+    station: str,
+    packet_items: dict[tuple[str, int], tuple[int, int, bytes]],
+    export_root: Path,
+    angles: DifopAngles,
+    source: Path,
+    *,
+    frame_stride: int,
+    seen_frame_keys: set[tuple[str, int, int]],
+) -> int:
+    if not packet_items:
+        return 0
+    ordered = sorted(packet_items.values(), key=lambda item: (item[0], item[1]))
+    frames = iter_h32_frames_polar_from_packets(
+        (item[2] for item in ordered),
+        host_utc_ticks=[item[0] for item in ordered],
+        frame_stride=frame_stride,
+        min_frame_points=100,
+        min_range_m=0.3,
+        max_range_m=120.0,
+        vertical_deg=angles.vertical_deg,
+        horizontal_deg=angles.horizontal_deg,
+    )
+    frames_dir = export_root / station / "frames"
+    frames_dir.mkdir(parents=True, exist_ok=True)
+    written = 0
+    for frame in frames:
+        # Overlapping archives contain identical revolutions.  The host stamp
+        # and 0.1 s device bucket make the key stable without comparing points.
+        key = (station, int(round(frame.host_receive_utc_ns / 10_000_000)), int(round(frame.t_start_s * 10)))
+        if key in seen_frame_keys:
+            continue
+        seen_frame_keys.add(key)
+        device_ns = int(round(frame.t_start_s * 1_000_000_000))
+        destination = frames_dir / f"h32_{frame.host_receive_utc_ns}_{device_ns}.npz"
+        np.savez_compressed(
+            destination,
+            points_raw=np.asarray(frame.points_raw, dtype=np.float32),
+            frame_counter=np.asarray([len(seen_frame_keys)], dtype=np.int32),
+            point_count=np.asarray([len(frame.points_raw)], dtype=np.int32),
+            unix_time_ns=np.asarray([device_ns], dtype=np.int64),
+            device_time_s=np.asarray([frame.t_start_s], dtype=np.float64),
+            device_time_end_s=np.asarray([frame.t_end_s], dtype=np.float64),
+            host_receive_utc_ns=np.asarray([frame.host_receive_utc_ns], dtype=np.int64),
+            source_file_utf8=np.frombuffer(str(source.resolve()).encode("utf-8"), dtype=np.uint8),
+        )
+        written += 1
+    return written
+
+
+def scan_dlog_sources(
+    sources: list[Path],
+    windows: list[Window],
+    export_root: Path,
+    *,
+    frame_stride: int,
+) -> dict[str, object]:
+    lookup = station_lookup(windows)
+    angles: DifopAngles | None = None
+    seen_packets: dict[str, set[tuple[str, int]]] = {}
+    seen_frames: set[tuple[str, int, int]] = set()
+    frame_counts: dict[str, int] = {}
+    source_stats: list[dict[str, object]] = []
+
+    for source_index, source in enumerate(sources, 1):
+        print(f"[dlog {source_index}/{len(sources)}] {source}", flush=True)
+        packets: dict[str, dict[tuple[str, int], tuple[int, int, bytes]]] = {}
+        msop_batches = difop_records = selected_packets = duplicates = 0
+        for object_name, payload in iter_zip_dobject_payloads(source):
+            if object_name == DIFOP_OBJECT:
+                difop_records += 1
+                if angles is None:
+                    try:
+                        angles = parse_difop_angles(parse_difop_payload(payload).raw)
+                    except (EOFError, ValueError):
+                        pass
+                continue
+            if object_name != MSOP_OBJECT:
+                continue
+            batch = parse_msop_batch_payload(payload)
+            msop_batches += 1
+            for item in batch.packets:
+                station = lookup(item.host_receive_utc_ticks)
+                if station is None:
+                    continue
+                packet_key = (batch.session_id, item.sequence)
+                station_seen = seen_packets.setdefault(station, set())
+                if packet_key in station_seen:
+                    duplicates += 1
+                    continue
+                station_seen.add(packet_key)
+                packets.setdefault(station, {})[packet_key] = (
+                    item.host_receive_utc_ticks,
+                    item.sequence,
+                    item.raw,
+                )
+                selected_packets += 1
+        if angles is None:
+            raise RuntimeError(f"no valid H32 DIFOP angles found before decoding {source}")
+        written = 0
+        for station, items in packets.items():
+            count = save_frames(
+                station,
+                items,
+                export_root,
+                angles,
+                source,
+                frame_stride=frame_stride,
+                seen_frame_keys=seen_frames,
+            )
+            frame_counts[station] = frame_counts.get(station, 0) + count
+            written += count
+        source_stats.append(
+            {
+                "source": str(source.resolve()),
+                "msop_batches": msop_batches,
+                "difop_records": difop_records,
+                "selected_packets": selected_packets,
+                "duplicate_packets": duplicates,
+                "frames_written": written,
+            }
+        )
+        print(f"  selected_packets={selected_packets} frames={written} duplicates={duplicates}", flush=True)
+    return {"frame_counts": frame_counts, "sources": source_stats}
+
+
+def parse_rtk_sources(paths: list[Path], parsed_root: Path) -> dict[str, object]:
+    rows = []
+    source_stats = []
+    for path in paths:
+        capture_rows = parse_rtk_capture(
+            read_capture(path),
+            accepted_prefixes=("$GNGGA", "$GPGGA", "$GNHPR"),
+        )
+        for row in capture_rows:
+            row["capture_source"] = str(path.resolve())
+        rows.extend(capture_rows)
+        source_stats.append(
+            {
+                "source": str(path.resolve()),
+                "rows": len(capture_rows),
+                "gga_valid": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in capture_rows),
+                "gnhpr_valid": sum(
+                    row.get("type") == "GNHPR" and row.get("checksum_valid") and row.get("heading_valid")
+                    for row in capture_rows
+                ),
+            }
+        )
+    parsed_root.mkdir(parents=True, exist_ok=True)
+    write_jsonl(parsed_root / "rtk.jsonl", rows)
+    write_jsonl(parsed_root / "imu.jsonl", [])
+    return {"rows": len(rows), "sources": source_stats}
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--segments-csv", type=Path, required=True)
+    parser.add_argument("--lidar-dlog", type=Path, action="append", default=[])
+    parser.add_argument("--rtk-rscap", type=Path, action="append", required=True)
+    parser.add_argument("--out", type=Path, required=True)
+    parser.add_argument("--expected-stations", type=int, default=0)
+    parser.add_argument("--frame-stride", type=int, default=5)
+    parser.add_argument("--rtk-max-dt-ms", type=float, default=200.0)
+    parser.add_argument("--reuse-export", action="store_true", help="Keep existing export/ and resume parsed/combined stages.")
+    parser.add_argument("--overwrite", action="store_true")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    if args.frame_stride < 1:
+        raise SystemExit("--frame-stride must be >= 1")
+    if not args.reuse_export and not args.lidar_dlog:
+        raise SystemExit("at least one --lidar-dlog is required unless --reuse-export is used")
+    for source in [args.segments_csv, *args.lidar_dlog, *args.rtk_rscap]:
+        if not source.is_file():
+            raise FileNotFoundError(source)
+    if args.reuse_export:
+        export_root = args.out / "export"
+        if not export_root.is_dir():
+            raise FileNotFoundError(f"--reuse-export requested but missing {export_root}")
+        for name in ("parsed", "combined", "export_summary.json"):
+            target = args.out / name
+            if target.is_dir():
+                shutil.rmtree(target)
+            elif target.exists():
+                target.unlink()
+    elif args.out.exists() and any(args.out.iterdir()):
+        if not args.overwrite:
+            raise FileExistsError(f"{args.out} is non-empty; pass --overwrite")
+        for name in ("export", "parsed", "combined", "export_summary.json"):
+            target = args.out / name
+            if target.is_dir():
+                shutil.rmtree(target)
+            elif target.exists():
+                target.unlink()
+    args.out.mkdir(parents=True, exist_ok=True)
+    windows = load_windows(args.segments_csv)
+    expected_ids = sorted({item.station_id for item in windows})
+    if args.reuse_export:
+        frame_counts = {
+            station.name: len(list((station / "frames").glob("*.npz")))
+            for station in (args.out / "export").iterdir()
+            if station.is_dir()
+        }
+        lidar_summary = {"frame_counts": frame_counts, "sources": [], "reused_export": True}
+    else:
+        lidar_summary = scan_dlog_sources(
+            args.lidar_dlog,
+            windows,
+            args.out / "export",
+            frame_stride=args.frame_stride,
+        )
+        frame_counts = lidar_summary["frame_counts"]
+    exported_ids = sorted(station for station, count in frame_counts.items() if count)
+    missing = sorted(set(expected_ids) - set(exported_ids))
+    if missing:
+        raise RuntimeError(f"stations without decoded H32 frames: {missing}")
+    if args.expected_stations and len(exported_ids) != args.expected_stations:
+        raise RuntimeError(f"expected {args.expected_stations} stations, exported {len(exported_ids)}")
+    rtk_summary = parse_rtk_sources(args.rtk_rscap, args.out / "parsed")
+    lidar_segments = [(station, args.out / "export" / station / "frames") for station in exported_ids]
+    combined_summary = build_combined(
+        lidar_segments,
+        [args.out / "parsed" / "rtk.jsonl"],
+        [],
+        args.out / "combined",
+        rtk_max_dt_ms=args.rtk_max_dt_ms,
+        time_basis="host",
+        overwrite=True,
+    )
+    summary = {
+        "role": "G90 GNGGA/GNHPR + H32 DLog static-window export",
+        "segments_csv": str(args.segments_csv.resolve()),
+        "time_basis": "H32 MSOP host_receive_utc_ticks <-> G90 rscap host_receive_utc_ns",
+        "imu_used": False,
+        "expected_station_ids": expected_ids,
+        "station_count": len(exported_ids),
+        "lidar": lidar_summary,
+        "rtk": rtk_summary,
+        "combined": combined_summary,
+        "outputs": {
+            "combined": str((args.out / "combined").resolve()),
+            "manifest": str((args.out / "combined" / "manifest.csv").resolve()),
+        },
+    }
+    (args.out / "export_summary.json").write_text(
+        json.dumps(summary, ensure_ascii=False, indent=2) + "\n",
+        encoding="utf-8",
+    )
+    print(json.dumps({"stations": len(exported_ids), "combined": combined_summary}, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/export_h32_rscap_station.py

@@ -0,0 +1,338 @@
+#!/usr/bin/env python3
+"""Export one static-station H32 capture into LiDAR frame NPZs.
+
+Supports:
+
+- V2 ``.rscap`` (legacy MSOP-only RawCapture)
+- Medulla dlog from ``RSLidarH32_3D_DLogCaptureNet48`` (raw MSOP + DIFOP)
+
+This is an **internal** helper used by ``export_raw_to_combined.py``.
+For RTK–LiDAR calibration, prefer the one-shot exporter that writes ``combined/``.
+
+Output frame contract (consumed by ``build_multisensor_npz.py``):
+
+- ``points_raw``: (N, 5) polar ``d_mm, azimuth_deg, altitude_deg, intensity, progression``
+- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns)
+- ``frame_counter``, ``point_count``, optional host receive stamp
+
+Raw ``.rscap`` / dlog files are never modified.
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import sys
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parent
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "rscap_v2"))
+
+from capture_format_v2 import file_summary, read_capture  # noqa: E402
+from h32_dlog.dobject import discover_records, resolve_dlog_root  # noqa: E402
+from h32_dlog.load_session import load_h32_dlog_lidar  # noqa: E402
+from h32_msop import (  # noqa: E402
+    iter_h32_frames_polar,
+    iter_h32_frames_polar_from_packets,
+)
+
+
+def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> Path:
+    candidates = [
+        station_dir / capture_name,
+        station_dir / "h32.rscap",
+        station_dir / "lidar.rscap",
+    ]
+    for path in candidates:
+        if path.is_file():
+            return path
+    raise FileNotFoundError(
+        f"no LiDAR .rscap under {station_dir}; tried {[str(p.name) for p in candidates]}"
+    )
+
+
+def try_resolve_dlog_root(station_dir: Path) -> Path | None:
+    try:
+        return resolve_dlog_root(station_dir)
+    except FileNotFoundError:
+        return None
+
+
+def is_h32_raw_dlog_station(
+    station_dir: Path,
+    *,
+    msop_object: str = "frontlidar-msop-raw",
+) -> bool:
+    root = try_resolve_dlog_root(station_dir)
+    if root is None:
+        return False
+    return len(discover_records(root, msop_object)) > 0
+
+
+def is_legacy_pointcloud_dlog_station(
+    station_dir: Path,
+    *,
+    msop_object: str = "frontlidar-msop-raw",
+) -> bool:
+    root = try_resolve_dlog_root(station_dir)
+    if root is None:
+        return False
+    return not is_h32_raw_dlog_station(station_dir, msop_object=msop_object)
+
+
+def _write_polar_frames(
+    *,
+    out: Path,
+    frames,
+    source_label: str,
+    compress: bool,
+    write_reports: bool,
+    resume: bool,
+    metadata_extra: dict[str, Any],
+) -> dict[str, Any]:
+    frames_dir = out / "frames"
+    frames_dir.mkdir(parents=True, exist_ok=True)
+    saver = np.savez_compressed if compress else np.savez
+    manifest_rows: list[dict[str, Any]] = []
+    written = 0
+    for index, frame in enumerate(frames):
+        unix_time_ns = int(round(frame.t_start_s * 1_000_000_000))
+        name = f"h32_{index:06d}_{unix_time_ns}_frame{index}.npz"
+        destination = frames_dir / name
+        if resume and destination.exists():
+            continue
+        points = np.asarray(frame.points_raw, dtype=np.float32)
+        payload = {
+            "points_raw": points,
+            "frame_counter": np.asarray([index], dtype=np.int32),
+            "point_count": np.asarray([points.shape[0]], dtype=np.int32),
+            "unix_time_ns": np.asarray([unix_time_ns], dtype=np.int64),
+            "device_time_s": np.asarray([frame.t_start_s], dtype=np.float64),
+            "device_time_end_s": np.asarray([frame.t_end_s], dtype=np.float64),
+            "host_receive_utc_ns": np.asarray([frame.host_receive_utc_ns], dtype=np.int64),
+            "source_file_utf8": np.frombuffer(source_label.encode("utf-8"), dtype=np.uint8),
+        }
+        saver(destination, **payload)
+        written += 1
+        manifest_rows.append(
+            {
+                "index": index,
+                "output": name,
+                "unix_time_ns": unix_time_ns,
+                "point_count": int(points.shape[0]),
+                "host_receive_utc_ns": int(frame.host_receive_utc_ns),
+            }
+        )
+
+    metadata: dict[str, Any] = {
+        "frames_decoded": len(frames),
+        "frames_written": written,
+        "frames_dir": str(frames_dir.resolve()),
+        "time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
+        "points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
+        **metadata_extra,
+    }
+    (out / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
+    (out / "README.md").write_text(
+        "# H32 station export (internal)\n\n"
+        f"- source: `{source_label}`\n"
+        f"- frames: `{frames_dir}`\n"
+        "- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
+        encoding="utf-8",
+    )
+    if write_reports:
+        reports = out / "reports"
+        reports.mkdir(parents=True, exist_ok=True)
+        with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
+            writer = csv.DictWriter(
+                stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"]
+            )
+            writer.writeheader()
+            writer.writerows(manifest_rows)
+        (reports / "export_summary.json").write_text(
+            json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8"
+        )
+    return metadata
+
+
+def export_station_h32(
+    station: Path,
+    out: Path,
+    *,
+    capture_name: str = "h32.rscap",
+    stride: int = 1,
+    min_frame_points: int = 100,
+    min_range_m: float = 0.3,
+    max_range_m: float = 120.0,
+    compress: bool = True,
+    write_reports: bool = False,
+    resume: bool = False,
+) -> dict[str, Any]:
+    """Decode one station H32 ``.rscap`` into ``out/frames/*.npz``."""
+
+    rscap = (
+        station
+        if station.is_file() and station.suffix.lower() == ".rscap"
+        else resolve_lidar_rscap(station, capture_name)
+    )
+    capture = read_capture(rscap)
+    frames = iter_h32_frames_polar(
+        capture,
+        min_frame_points=min_frame_points,
+        frame_stride=max(1, stride),
+        min_range_m=min_range_m,
+        max_range_m=max_range_m,
+    )
+    if not frames:
+        raise RuntimeError(f"no H32 frames decoded from {rscap}")
+    return _write_polar_frames(
+        out=out,
+        frames=frames,
+        source_label=str(rscap.resolve()),
+        compress=compress,
+        write_reports=write_reports,
+        resume=resume,
+        metadata_extra={
+            "kind": "h32_rscap",
+            "source_rscap": str(rscap.resolve()),
+            "capture": file_summary(capture),
+            "angle_source": "default_msop_only_vertical_-16_to_16_deg",
+        },
+    )
+
+
+def export_station_h32_dlog(
+    station: Path,
+    out: Path,
+    *,
+    msop_object: str = "frontlidar-msop-raw",
+    difop_object: str = "frontlidar-difop-raw",
+    require_difop: bool = True,
+    stride: int = 1,
+    min_frame_points: int = 100,
+    min_range_m: float = 0.3,
+    max_range_m: float = 120.0,
+    compress: bool = True,
+    write_reports: bool = False,
+    resume: bool = False,
+) -> dict[str, Any]:
+    """Decode one station H32 raw-MSOP/DIFOP dlog into ``out/frames/*.npz``."""
+
+    session = load_h32_dlog_lidar(
+        station,
+        msop_object=msop_object,
+        difop_object=difop_object,
+        require_difop=require_difop,
+    )
+    frames = iter_h32_frames_polar_from_packets(
+        session.msop_packets,
+        host_utc_ticks=session.msop_host_utc_ticks,
+        min_frame_points=min_frame_points,
+        frame_stride=max(1, stride),
+        min_range_m=min_range_m,
+        max_range_m=max_range_m,
+        vertical_deg=session.vertical_deg,
+        horizontal_deg=session.horizontal_deg,
+    )
+    if not frames:
+        raise RuntimeError(f"no H32 frames decoded from dlog {session.dlog_root}")
+    return _write_polar_frames(
+        out=out,
+        frames=frames,
+        source_label=str(session.dlog_root.resolve()),
+        compress=compress,
+        write_reports=write_reports,
+        resume=resume,
+        metadata_extra={
+            "kind": "h32_dlog_raw",
+            "source_dlog": str(session.dlog_root.resolve()),
+            "msop_object": session.msop_object,
+            "difop_object": session.difop_object,
+            "msop_packets": len(session.msop_packets),
+            "msop_batches": session.msop_batch_count,
+            "difop_records": session.difop_record_count,
+            "session_id": session.session_id,
+            "lidar_ip": session.lidar_ip,
+            "angle_source": session.angle_source,
+        },
+    )
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--station", type=Path, required=True, help="Station directory, .rscap, or dlog root")
+    parser.add_argument("--out", type=Path, required=True)
+    parser.add_argument("--capture-name", default="h32.rscap")
+    parser.add_argument("--msop-object", default="frontlidar-msop-raw")
+    parser.add_argument("--difop-object", default="frontlidar-difop-raw")
+    parser.add_argument(
+        "--require-difop",
+        action=argparse.BooleanOptionalAction,
+        default=True,
+        help="For dlog stations, require valid DIFOP angles (default: true)",
+    )
+    parser.add_argument("--stride", type=int, default=1)
+    parser.add_argument("--min-frame-points", type=int, default=100)
+    parser.add_argument("--min-range-m", type=float, default=0.3)
+    parser.add_argument("--max-range-m", type=float, default=120.0)
+    parser.add_argument("--compress", action="store_true", default=True)
+    parser.add_argument("--write-reports", action="store_true")
+    parser.add_argument("--resume", action="store_true", help="Skip frames that already exist")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    station = args.station
+    if station.is_file() and station.suffix.lower() == ".rscap":
+        metadata = export_station_h32(
+            station,
+            args.out,
+            capture_name=args.capture_name,
+            stride=args.stride,
+            min_frame_points=args.min_frame_points,
+            min_range_m=args.min_range_m,
+            max_range_m=args.max_range_m,
+            compress=args.compress,
+            write_reports=args.write_reports,
+            resume=args.resume,
+        )
+    elif is_h32_raw_dlog_station(station, msop_object=args.msop_object):
+        metadata = export_station_h32_dlog(
+            station,
+            args.out,
+            msop_object=args.msop_object,
+            difop_object=args.difop_object,
+            require_difop=args.require_difop,
+            stride=args.stride,
+            min_frame_points=args.min_frame_points,
+            min_range_m=args.min_range_m,
+            max_range_m=args.max_range_m,
+            compress=args.compress,
+            write_reports=args.write_reports,
+            resume=args.resume,
+        )
+    else:
+        metadata = export_station_h32(
+            station,
+            args.out,
+            capture_name=args.capture_name,
+            stride=args.stride,
+            min_frame_points=args.min_frame_points,
+            min_range_m=args.min_range_m,
+            max_range_m=args.max_range_m,
+            compress=args.compress,
+            write_reports=args.write_reports,
+            resume=args.resume,
+        )
+    print(json.dumps(metadata, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/export_motion_pairs_for_viz.py

@@ -1,111 +0,0 @@
-#!/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/export_raw_to_combined.py

@@ -0,0 +1,373 @@
+#!/usr/bin/env python3
+"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package.
+
+Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw captures in,
+calibration-ready intermediate out. Downstream prepare/solve consume ``combined/``
+only (``manifest.csv`` + associated frame NPZs).
+
+Expected raw layout (new H32 DLogCapture):
+
+    stations/
+      001/                 # dobject/ + dobject_recording/  (or 001/dlog/...)
+      002/
+      ...
+    captures/
+      rtk.rscap            # G90: #PVTSLNA + #UNIHEADINGA
+      imu.rscap            # N300 (associated only; not used in AX=XB)
+
+Also accepts legacy per-station ``h32.rscap``, and older decoded-point-cloud dlog
+stations (prefer ``--time-basis host`` for those).
+
+Output under ``--out``:
+
+    export/<station>/frames/*.npz   # internal LiDAR frames
+    parsed/rtk.jsonl, imu.jsonl
+    combined/frames/*.npz + manifest.csv + dataset_summary.json
+    export_summary.json
+
+Raw ``.rscap`` / dlog files are never modified.
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import shutil
+import subprocess
+import sys
+from pathlib import Path
+from typing import Any
+
+ROOT = Path(__file__).resolve().parent
+REPO = ROOT.parent
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "rscap_v2"))
+
+from build_multisensor_npz import build_combined  # noqa: E402
+from capture_format_v2 import file_summary, read_capture  # noqa: E402
+from export_h32_rscap_station import (  # noqa: E402
+    export_station_h32,
+    export_station_h32_dlog,
+    is_h32_raw_dlog_station,
+    is_legacy_pointcloud_dlog_station,
+    resolve_lidar_rscap,
+    try_resolve_dlog_root,
+)
+from pipeline_common_corrected import (  # noqa: E402
+    parse_imu_capture,
+    parse_rtk_capture,
+    write_json,
+    write_jsonl,
+)
+
+
+def is_h32_station(station: Path, capture_name: str) -> bool:
+    try:
+        resolve_lidar_rscap(station, capture_name)
+    except FileNotFoundError:
+        return False
+    return True
+
+
+def is_dlog_station(station: Path) -> bool:
+    return try_resolve_dlog_root(station) is not None
+
+
+def discover_stations(
+    stations_root: Path,
+    names: list[str],
+    capture_name: str,
+    msop_object: str,
+) -> list[Path]:
+    if names:
+        stations = [stations_root / name for name in names]
+        missing = [str(path) for path in stations if not path.is_dir()]
+        if missing:
+            raise FileNotFoundError(f"station directories missing: {missing}")
+        return stations
+    stations = sorted(
+        [
+            path
+            for path in stations_root.iterdir()
+            if path.is_dir()
+            and (
+                is_h32_station(path, capture_name)
+                or is_h32_raw_dlog_station(path, msop_object=msop_object)
+                or is_dlog_station(path)
+            )
+        ],
+        key=lambda path: path.name,
+    )
+    if not stations:
+        raise FileNotFoundError(
+            f"no station with H32 dlog/MSOP, {capture_name}/lidar.rscap, or "
+            f"dobject+dobject_recording under {stations_root}"
+        )
+    return stations
+
+
+def export_legacy_dlog_station(
+    station: Path,
+    out: Path,
+    *,
+    lidar_object: str,
+    timezone: str,
+    stride: int,
+) -> None:
+    exporter = ROOT / "frontlidar_dlog_export.py"
+    command = [
+        sys.executable,
+        str(exporter),
+        "--dlog",
+        str(try_resolve_dlog_root(station) or station),
+        "--out",
+        str(out),
+        "--object",
+        lidar_object,
+        "--format",
+        "npz",
+        "--timezone",
+        timezone,
+        "--stride",
+        str(stride),
+        "--compress",
+        "--skip-rtk",
+        "--write-reports",
+        "--resume",
+    ]
+    completed = subprocess.run(command, check=False)
+    if completed.returncode != 0:
+        raise RuntimeError(f"legacy dlog export failed for {station} (exit {completed.returncode})")
+
+
+def parse_serial(rtk_rscap: Path, imu_rscap: Path, parsed_root: Path) -> dict[str, Any]:
+    parsed_root.mkdir(parents=True, exist_ok=True)
+    rtk_capture = read_capture(rtk_rscap)
+    imu_capture = read_capture(imu_rscap)
+    rtk_rows = parse_rtk_capture(rtk_capture)
+    imu_rows = parse_imu_capture(imu_capture)
+    write_jsonl(parsed_root / "rtk.jsonl", rtk_rows)
+    write_jsonl(parsed_root / "imu.jsonl", imu_rows)
+    summary = {
+        "rtk_capture": file_summary(rtk_capture),
+        "imu_capture": file_summary(imu_capture),
+        "rtk_records": len(rtk_rows),
+        "rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows),
+        "rtk_pvtslna": sum(row.get("type") == "PVTSLNA" and row.get("checksum_valid") for row in rtk_rows),
+        "rtk_gga": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in rtk_rows),
+        "rtk_heading_valid": sum(row.get("type") == "UNIHEADINGA" and row.get("heading_valid") for row in rtk_rows),
+        "imu_frames": len(imu_rows),
+        "imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows),
+        "imu_types": sorted({str(row.get("type")) for row in imu_rows}),
+    }
+    write_json(parsed_root / "parse_summary.json", summary)
+    return summary
+
+
+def export_raw_to_combined(
+    *,
+    stations_root: Path,
+    rtk_rscap: Path,
+    imu_rscap: Path,
+    out: Path,
+    station_names: list[str] | None = None,
+    lidar_capture_name: str = "h32.rscap",
+    msop_object: str = "frontlidar-msop-raw",
+    difop_object: str = "frontlidar-difop-raw",
+    require_difop: bool = True,
+    lidar_object: str = "frontlidar",
+    timezone: str = "+08:00",
+    stride: int = 1,
+    rtk_max_dt_ms: float = 150.0,
+    imu_before_ms: float = 100.0,
+    imu_after_ms: float = 100.0,
+    time_basis: str = "device_gnss",
+    overwrite: bool = False,
+) -> dict[str, Any]:
+    """Full raw → combined export. Returns ``export_summary`` dict."""
+
+    if not stations_root.is_dir():
+        raise FileNotFoundError(f"stations root does not exist: {stations_root}")
+    if not rtk_rscap.is_file():
+        raise FileNotFoundError(f"RTK capture missing: {rtk_rscap}")
+    if not imu_rscap.is_file():
+        raise FileNotFoundError(f"IMU capture missing: {imu_rscap}")
+    if out.exists() and any(out.iterdir()) and not overwrite:
+        raise FileExistsError(f"{out} is non-empty; pass --overwrite")
+    if overwrite and out.exists():
+        for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
+            target = out / child
+            if target.is_dir():
+                shutil.rmtree(target)
+            elif target.is_file():
+                target.unlink()
+
+    out.mkdir(parents=True, exist_ok=True)
+    export_root = out / "export"
+    parsed_root = out / "parsed"
+    combined_root = out / "combined"
+
+    stations = discover_stations(
+        stations_root, station_names or [], lidar_capture_name, msop_object
+    )
+    parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root)
+
+    station_meta: list[dict[str, Any]] = []
+    lidar_segments: list[tuple[str, Path]] = []
+    saw_legacy_dlog = False
+    for station in stations:
+        station_out = export_root / station.name
+        if is_h32_raw_dlog_station(station, msop_object=msop_object):
+            meta = export_station_h32_dlog(
+                station,
+                station_out,
+                msop_object=msop_object,
+                difop_object=difop_object,
+                require_difop=require_difop,
+                stride=stride,
+                write_reports=True,
+                resume=False,
+            )
+            kind = "h32_dlog_raw"
+        elif is_h32_station(station, lidar_capture_name):
+            meta = export_station_h32(
+                station,
+                station_out,
+                capture_name=lidar_capture_name,
+                stride=stride,
+                write_reports=True,
+                resume=False,
+            )
+            kind = "h32_rscap"
+        elif is_legacy_pointcloud_dlog_station(station, msop_object=msop_object) or is_dlog_station(
+            station
+        ):
+            saw_legacy_dlog = True
+            export_legacy_dlog_station(
+                station,
+                station_out,
+                lidar_object=lidar_object,
+                timezone=timezone,
+                stride=stride,
+            )
+            meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
+            kind = "legacy_dlog"
+        else:
+            raise RuntimeError(
+                f"station {station.name} has neither H32 raw dlog, .rscap, nor legacy dlog layout"
+            )
+        frames_dir = station_out / "frames"
+        if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")):
+            raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}")
+        lidar_segments.append((station.name, frames_dir))
+        station_meta.append({"station": station.name, "kind": kind, "frames_dir": str(frames_dir), **meta})
+
+    if saw_legacy_dlog and time_basis == "device_gnss":
+        print(
+            "[warn] legacy point-cloud dlog stations use host/DObject time; prefer --time-basis host",
+            file=sys.stderr,
+        )
+
+    combined_summary = build_combined(
+        lidar_segments,
+        [parsed_root / "rtk.jsonl"],
+        [parsed_root / "imu.jsonl"],
+        combined_root,
+        rtk_max_dt_ms=rtk_max_dt_ms,
+        imu_before_ms=imu_before_ms,
+        imu_after_ms=imu_after_ms,
+        time_basis=time_basis,
+        overwrite=True,
+    )
+
+    summary = {
+        "role": "RTK-LiDAR one-shot raw export (like Lidar-IMU export_rscap_to_v1)",
+        "stations_root": str(stations_root.resolve()),
+        "rtk_rscap": str(rtk_rscap.resolve()),
+        "imu_rscap": str(imu_rscap.resolve()),
+        "out": str(out.resolve()),
+        "station_count": len(stations),
+        "stations": station_meta,
+        "parsed": parse_summary,
+        "combined": combined_summary,
+        "outputs": {
+            "combined": str(combined_root.resolve()),
+            "manifest": str((combined_root / "manifest.csv").resolve()),
+            "parsed": str(parsed_root.resolve()),
+            "export": str(export_root.resolve()),
+        },
+        "timestamp_policy": {
+            "default_time_basis": time_basis,
+            "lidar_h32_dlog": "MSOP device timestamp → unix_time_ns; DIFOP channel angles for XYZ",
+            "lidar_h32_rscap": "MSOP device timestamp → unix_time_ns; default vertical angles",
+            "lidar_legacy_dlog": "DObject/host time; use time_basis=host",
+            "rtk": "GNSS week/TOW when time_basis=device_gnss; else host_receive_utc_ns",
+            "imu": "associated only; host-anchored device deltas in combined window",
+            "host_utc": "kept for audit; not the default calibration timeline for new captures",
+        },
+        "next_step": "run/run_direct_rtk_lidar.ps1 -CombinedRoot <out>/combined ...",
+    }
+    (out / "export_summary.json").write_text(
+        json.dumps(summary, ensure_ascii=False, indent=2) + "\n",
+        encoding="utf-8",
+    )
+    return summary
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    parser.add_argument("--stations-root", type=Path, required=True, help="Directory of per-station folders")
+    parser.add_argument("--rtk-rscap", type=Path, required=True, help="Continuous G90/RTK V2 .rscap")
+    parser.add_argument("--imu-rscap", type=Path, required=True, help="Continuous N300/IMU V2 .rscap")
+    parser.add_argument("--out", type=Path, required=True, help="Output package root (contains combined/)")
+    parser.add_argument("--station", action="append", default=[], help="Optional station name filter; repeatable")
+    parser.add_argument("--lidar-capture-name", default="h32.rscap", help="Legacy H32 .rscap filename")
+    parser.add_argument("--msop-object", default="frontlidar-msop-raw", help="Raw MSOP DObject name")
+    parser.add_argument("--difop-object", default="frontlidar-difop-raw", help="Raw DIFOP DObject name")
+    parser.add_argument(
+        "--require-difop",
+        action=argparse.BooleanOptionalAction,
+        default=True,
+        help="Require DIFOP channel angles for H32 raw dlog stations (default: true)",
+    )
+    parser.add_argument("--lidar-object", default="frontlidar", help="Legacy decoded point-cloud DObject name")
+    parser.add_argument("--timezone", default="+08:00", help="Legacy dlog tick timezone")
+    parser.add_argument("--stride", type=int, default=1)
+    parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
+    parser.add_argument("--imu-before-ms", type=float, default=100.0)
+    parser.add_argument("--imu-after-ms", type=float, default=100.0)
+    parser.add_argument("--time-basis", choices=("device_gnss", "host"), default="device_gnss")
+    parser.add_argument("--overwrite", action="store_true")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    if args.stride < 1:
+        raise SystemExit("stride must be >= 1")
+    summary = export_raw_to_combined(
+        stations_root=args.stations_root,
+        rtk_rscap=args.rtk_rscap,
+        imu_rscap=args.imu_rscap,
+        out=args.out,
+        station_names=args.station,
+        lidar_capture_name=args.lidar_capture_name,
+        msop_object=args.msop_object,
+        difop_object=args.difop_object,
+        require_difop=args.require_difop,
+        lidar_object=args.lidar_object,
+        timezone=args.timezone,
+        stride=args.stride,
+        rtk_max_dt_ms=args.rtk_max_dt_ms,
+        imu_before_ms=args.imu_before_ms,
+        imu_after_ms=args.imu_after_ms,
+        time_basis=args.time_basis,
+        overwrite=args.overwrite,
+    )
+    print(json.dumps(summary, ensure_ascii=False, indent=2))
+    print(f"\nCombined package ready: {summary['outputs']['combined']}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/export_rscap_to_v1.py

@@ -1,430 +0,0 @@
-#!/usr/bin/env python3
-"""Export IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
-
-IMU sources:
-- ``--imu-kind hi13`` (HI13R4 / HI91) or ``n300`` or ``auto``
-- one or more ``--imu-rscap`` files (concatenated)
-
-LiDAR sources (exactly one):
-- ``--lidar-dlog``: Medulla dlog dir **or recovered zip** (MSOP + DIFOP)
-- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap``
-
-Optional host-time window (local wall clock, DateTime.Now.Ticks convention):
-- ``--host-start`` / ``--host-end`` e.g. ``2026-08-08T17:40:05``
-
-Output under ``--out``:
-
-    imu.csv
-    lidar/
-      frames_index.csv
-      frames/frame_XXXXX.npz
-    export_summary.json
-
-Device times stay in ``t`` / ``t_start``/``t_end``. Host UTC receive times are
-also written so LiDAR–IMU alignment can bridge clocks without forcing first-frame
-device coincidence.
-"""
-
-from __future__ import annotations
-
-import argparse
-import csv
-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 tools.h32_dlog.load_session import load_h32_dlog_lidar
-from tools.h32_dlog.timeutil import (
-    local_wall_to_dotnet_ticks,
-    local_wall_to_utc_dotnet_ticks,
-    utc_dotnet_ticks_to_unix_s,
-)
-from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
-from tools.rscap_v2.h32_msop import iter_h32_frames, iter_h32_frames_from_packets
-from tools.rscap_v2.hi13_imu import iter_hi13_imu_samples
-from tools.rscap_v2.n300_imu import ImuSample, iter_n300_imu_samples, samples_to_arrays
-
-
-def write_imu_csv(path: Path, samples: list[ImuSample]) -> None:
-    path.parent.mkdir(parents=True, exist_ok=True)
-    with path.open("w", newline="", encoding="utf-8") as handle:
-        writer = csv.writer(handle)
-        writer.writerow(
-            [
-                "t",
-                "gx",
-                "gy",
-                "gz",
-                "ax",
-                "ay",
-                "az",
-                "t_host_utc_s",
-                "receive_utc_ticks",
-            ]
-        )
-        for sample in samples:
-            ticks = int(sample.host_receive_utc_ticks)
-            t_host = utc_dotnet_ticks_to_unix_s(ticks) if ticks > 0 else float("nan")
-            writer.writerow(
-                [
-                    f"{sample.t_s:.9f}",
-                    f"{sample.gyro_rad_s[0]:.12g}",
-                    f"{sample.gyro_rad_s[1]:.12g}",
-                    f"{sample.gyro_rad_s[2]:.12g}",
-                    f"{sample.accel_m_s2[0]:.12g}",
-                    f"{sample.accel_m_s2[1]:.12g}",
-                    f"{sample.accel_m_s2[2]:.12g}",
-                    f"{t_host:.9f}" if ticks > 0 else "",
-                    ticks,
-                ]
-            )
-
-
-def write_lidar_session(root: Path, frames) -> dict:
-    frames_dir = root / "frames"
-    frames_dir.mkdir(parents=True, exist_ok=True)
-    index_path = root / "frames_index.csv"
-    with index_path.open("w", newline="", encoding="utf-8") as handle:
-        writer = csv.writer(handle)
-        writer.writerow(
-            [
-                "frame_id",
-                "filename",
-                "t_start",
-                "t_end",
-                "host_receive_utc_ticks",
-                "t_host_utc_s",
-                "host_receive_utc_end_ticks",
-                "t_host_utc_end_s",
-            ]
-        )
-        point_counts = []
-        host_ok = 0
-        for index, frame in enumerate(frames):
-            rel = f"frames/frame_{index:05d}.npz"
-            np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32))
-            h0 = int(getattr(frame, "host_receive_utc_ticks_start", 0) or 0)
-            h1 = int(getattr(frame, "host_receive_utc_ticks_end", 0) or 0)
-            t_host0 = utc_dotnet_ticks_to_unix_s(h0) if h0 > 0 else float("nan")
-            t_host1 = utc_dotnet_ticks_to_unix_s(h1) if h1 > 0 else float("nan")
-            if h0 > 0:
-                host_ok += 1
-            writer.writerow(
-                [
-                    index,
-                    rel,
-                    f"{frame.t_start_s:.9f}",
-                    f"{frame.t_end_s:.9f}",
-                    h0,
-                    f"{t_host0:.9f}" if h0 > 0 else "",
-                    h1,
-                    f"{t_host1:.9f}" if h1 > 0 else "",
-                ]
-            )
-            point_counts.append(int(frame.points_xyz.shape[0]))
-    return {
-        "frames": len(frames),
-        "frames_with_host_utc": host_ok,
-        "points_min": int(min(point_counts)) if point_counts else 0,
-        "points_max": int(max(point_counts)) if point_counts else 0,
-        "points_mean": float(np.mean(point_counts)) if point_counts else 0.0,
-        "t_start": float(frames[0].t_start_s) if frames else None,
-        "t_end": float(frames[-1].t_end_s) if frames else None,
-    }
-
-
-def detect_imu_kind(paths: list[Path], explicit: str) -> str:
-    if explicit != "auto":
-        return explicit
-    joined = " ".join(path.name.lower() for path in paths)
-    if "hi13" in joined or "hipnuc" in joined:
-        return "hi13"
-    if "n300" in joined or "wheeltec" in joined:
-        return "n300"
-    return "hi13"
-
-
-def load_imu_samples(
-    paths: list[Path],
-    *,
-    kind: str,
-    host_ticks_min: int | None,
-    host_ticks_max: int | None,
-) -> tuple[list[ImuSample], list[dict], str]:
-    samples: list[ImuSample] = []
-    captures_meta: list[dict] = []
-    for path in paths:
-        capture = read_capture(path)
-        captures_meta.append(file_summary(capture))
-        if kind == "hi13":
-            part = iter_hi13_imu_samples(
-                capture,
-                host_utc_ticks_min=host_ticks_min,
-                host_utc_ticks_max=host_ticks_max,
-            )
-        elif kind == "n300":
-            part = iter_n300_imu_samples(capture)
-            if host_ticks_min is not None or host_ticks_max is not None:
-                part = [
-                    sample
-                    for sample in part
-                    if (host_ticks_min is None or sample.host_receive_utc_ticks >= host_ticks_min)
-                    and (host_ticks_max is None or sample.host_receive_utc_ticks <= host_ticks_max)
-                ]
-        else:
-            raise ValueError(f"unsupported imu kind: {kind}")
-        samples.extend(part)
-    samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
-    return samples, captures_meta, kind
-
-
-def export_session(
-    *,
-    imu_rscap: list[Path] | Path,
-    out: Path,
-    lidar_rscap: Path | None = None,
-    lidar_dlog: Path | None = None,
-    imu_kind: str = "auto",
-    msop_object: str = "frontlidar-msop-raw",
-    difop_object: str = "frontlidar-difop-raw",
-    require_difop: bool = False,
-    host_start: str | None = None,
-    host_end: str | None = None,
-    frame_stride: int = 1,
-    max_points_per_frame: int | None = 80000,
-    min_range_m: float = 0.3,
-    max_range_m: float = 120.0,
-    min_frame_points: int = 100,
-) -> dict:
-    if (lidar_rscap is None) == (lidar_dlog is None):
-        raise ValueError("provide exactly one of lidar_rscap or lidar_dlog")
-
-    imu_paths = [imu_rscap] if isinstance(imu_rscap, Path) else list(imu_rscap)
-    if not imu_paths:
-        raise ValueError("at least one --imu-rscap is required")
-
-    # LiDAR DObject tic uses DateTime.Now; IMU/MSOP host fields use UTC.
-    lidar_ticks_min = local_wall_to_dotnet_ticks(host_start) if host_start else None
-    lidar_ticks_max = local_wall_to_dotnet_ticks(host_end) if host_end else None
-    imu_ticks_min = local_wall_to_utc_dotnet_ticks(host_start) if host_start else None
-    imu_ticks_max = local_wall_to_utc_dotnet_ticks(host_end) if host_end else None
-    kind = detect_imu_kind(imu_paths, imu_kind)
-
-    out.mkdir(parents=True, exist_ok=True)
-    samples, imu_captures, kind = load_imu_samples(
-        imu_paths,
-        kind=kind,
-        host_ticks_min=imu_ticks_min,
-        host_ticks_max=imu_ticks_max,
-    )
-    t, _gyro, _accel = samples_to_arrays(samples)
-    imu_csv = out / "imu.csv"
-    write_imu_csv(imu_csv, samples)
-    imu_host_ok = sum(1 for sample in samples if sample.host_receive_utc_ticks > 0)
-
-    if lidar_dlog is not None:
-        session = load_h32_dlog_lidar(
-            lidar_dlog,
-            msop_object=msop_object,
-            difop_object=difop_object,
-            require_difop=require_difop,
-            host_ticks_min=lidar_ticks_min,
-            host_ticks_max=lidar_ticks_max,
-        )
-        frames = iter_h32_frames_from_packets(
-            session.msop_packets,
-            host_utc_ticks=session.msop_host_utc_ticks,
-            min_frame_points=min_frame_points,
-            frame_stride=frame_stride,
-            min_range_m=min_range_m,
-            max_range_m=max_range_m,
-            max_points_per_frame=max_points_per_frame,
-            vertical_deg=session.vertical_deg,
-            horizontal_deg=session.horizontal_deg,
-        )
-        lidar_meta = {
-            "source": "dlog",
-            "lidar_dlog": session.dlog_root,
-            "msop_object": session.msop_object,
-            "difop_object": session.difop_object,
-            "msop_packets": len(session.msop_packets),
-            "msop_packets_with_host_utc": sum(1 for ticks in session.msop_host_utc_ticks if ticks > 0),
-            "msop_batches": session.msop_batch_count,
-            "difop_records": session.difop_record_count,
-            "session_id": session.session_id,
-            "lidar_ip": session.lidar_ip,
-            "angle_source": session.angle_source,
-            "timestamp_note": (
-                "device: h32_msop_device_timestamp -> seconds; "
-                "host: MSOP HostReceiveUtcTicks -> unix seconds"
-            ),
-        }
-    else:
-        assert lidar_rscap is not None
-        lidar_capture = read_capture(lidar_rscap)
-        frames = iter_h32_frames(
-            lidar_capture,
-            min_frame_points=min_frame_points,
-            frame_stride=frame_stride,
-            min_range_m=min_range_m,
-            max_range_m=max_range_m,
-            max_points_per_frame=max_points_per_frame,
-        )
-        lidar_meta = {
-            "source": "rscap_v2",
-            "lidar_rscap": str(lidar_rscap),
-            "capture": file_summary(lidar_capture),
-            "angle_source": "default_msop_only_vertical_-16_to_16_deg",
-            "timestamp_note": (
-                "device: h32_msop_device_timestamp_ms -> seconds; "
-                "host: rscap receive_utc_ticks -> unix seconds"
-            ),
-        }
-
-    lidar_dir = out / "lidar"
-    lidar_stats = write_lidar_session(lidar_dir, frames)
-    imu_time_note = (
-        "hi13_device_timestamp_ms -> seconds"
-        if kind == "hi13"
-        else "n300_device_timestamp_us -> seconds"
-    )
-
-    summary = {
-        "imu_rscap": [str(path) for path in imu_paths],
-        "imu_kind": kind,
-        "out": str(out),
-        "host_window": {
-            "host_start": host_start,
-            "host_end": host_end,
-            "lidar_ticks_min": lidar_ticks_min,
-            "lidar_ticks_max": lidar_ticks_max,
-            "imu_ticks_min": imu_ticks_min,
-            "imu_ticks_max": imu_ticks_max,
-            "note": "local wall cut; lidar DObject tic=DateTime.Now, IMU/MSOP host=UTC",
-        },
-        "timestamp_policy": {
-            "imu_device": imu_time_note,
-            "imu_host": "rscap receive_utc_ticks -> t_host_utc_s",
-            "lidar_device": "MSOP device timestamp -> t_start/t_end",
-            "lidar_host": "MSOP HostReceiveUtcTicks -> t_host_utc_s",
-            "calibration_align": "bridge via host UTC; do not force first device samples to coincide",
-        },
-        "imu": {
-            "samples": int(t.shape[0]),
-            "samples_with_host_utc": imu_host_ok,
-            "t_start": float(t[0]) if t.size else None,
-            "t_end": float(t[-1]) if t.size else None,
-            "captures": imu_captures,
-        },
-        "lidar": {
-            **lidar_stats,
-            "frame_stride": int(frame_stride),
-            "max_points_per_frame": max_points_per_frame,
-            **lidar_meta,
-        },
-        "outputs": {
-            "imu_csv": str(imu_csv),
-            "lidar_session": str(lidar_dir),
-        },
-    }
-    (out / "export_summary.json").write_text(
-        json.dumps(summary, indent=2, ensure_ascii=False) + "\n",
-        encoding="utf-8",
-    )
-    return summary
-
-
-def main() -> int:
-    parser = argparse.ArgumentParser(description=__doc__)
-    parser.add_argument(
-        "--imu-rscap",
-        type=Path,
-        action="append",
-        required=True,
-        help="IMU V2 .rscap (repeatable)",
-    )
-    parser.add_argument(
-        "--imu-kind",
-        choices=("auto", "hi13", "n300"),
-        default="auto",
-        help="IMU decoder (default: auto from filename)",
-    )
-    lidar = parser.add_mutually_exclusive_group(required=True)
-    lidar.add_argument(
-        "--lidar-dlog",
-        type=Path,
-        help="H32 dlog directory or recovered zip (indices.log + data.bin)",
-    )
-    lidar.add_argument(
-        "--lidar-rscap",
-        type=Path,
-        help="Legacy H32 MSOP V2 .rscap",
-    )
-    parser.add_argument("--msop-object", default="frontlidar-msop-raw")
-    parser.add_argument("--difop-object", default="frontlidar-difop-raw")
-    parser.add_argument("--require-difop", action="store_true")
-    parser.add_argument("--host-start", type=str, default=None, help="Local wall start, e.g. 2026-08-08T17:40:05")
-    parser.add_argument("--host-end", type=str, default=None, help="Local wall end, e.g. 2026-08-08T17:45:15")
-    parser.add_argument("--out", type=Path, required=True)
-    parser.add_argument("--frame-stride", type=int, default=1)
-    parser.add_argument("--max-points-per-frame", type=int, default=80000)
-    parser.add_argument("--min-range-m", type=float, default=0.3)
-    parser.add_argument("--max-range-m", type=float, default=120.0)
-    parser.add_argument("--min-frame-points", type=int, default=100)
-    args = parser.parse_args()
-    max_points = None if args.max_points_per_frame <= 0 else args.max_points_per_frame
-    summary = export_session(
-        imu_rscap=args.imu_rscap,
-        lidar_rscap=args.lidar_rscap,
-        lidar_dlog=args.lidar_dlog,
-        imu_kind=args.imu_kind,
-        msop_object=args.msop_object,
-        difop_object=args.difop_object,
-        require_difop=args.require_difop,
-        host_start=args.host_start,
-        host_end=args.host_end,
-        out=args.out,
-        frame_stride=args.frame_stride,
-        max_points_per_frame=max_points,
-        min_range_m=args.min_range_m,
-        max_range_m=args.max_range_m,
-        min_frame_points=args.min_frame_points,
-    )
-    print(
-        json.dumps(
-            {
-                "imu_kind": summary["imu_kind"],
-                "imu_samples": summary["imu"]["samples"],
-                "imu_host_utc": summary["imu"]["samples_with_host_utc"],
-                "lidar_frames": summary["lidar"]["frames"],
-                "lidar_host_utc": summary["lidar"]["frames_with_host_utc"],
-                "lidar_source": summary["lidar"]["source"],
-                "angle_source": summary["lidar"]["angle_source"],
-                "host_window": summary["host_window"],
-                "imu_csv": summary["outputs"]["imu_csv"],
-                "lidar_session": summary["outputs"]["lidar_session"],
-                "export_summary": str(Path(args.out) / "export_summary.json"),
-            },
-            ensure_ascii=False,
-            indent=2,
-        )
-    )
-    if summary["imu"]["samples"] == 0:
-        raise SystemExit("no valid IMU samples decoded in window")
-    if summary["lidar"]["frames"] == 0:
-        raise SystemExit("no valid H32 frames decoded in window")
-    if summary["lidar"]["frames_with_host_utc"] == 0:
-        raise SystemExit("no LiDAR frames with MSOP HostReceiveUtcTicks; cannot host-bridge align")
-    if summary["imu"]["samples_with_host_utc"] == 0:
-        raise SystemExit("no IMU samples with host receive UTC; cannot host-bridge align")
-    return 0
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

tools/export_usable_20260808_windows.py

@@ -1,125 +0,0 @@
-#!/usr/bin/env python3
-"""Export priority LiDAR–IMU windows from calibration_usable_20260808.
-
-Does not push anything; writes local V1 sessions under --out-root.
-"""
-
-from __future__ import annotations
-
-import argparse
-import json
-import sys
-from pathlib import Path
-
-ROOT = Path(__file__).resolve().parents[1]
-if str(ROOT) not in sys.path:
-    sys.path.insert(0, str(ROOT))
-
-from tools.export_rscap_to_v1 import export_session
-
-DEFAULT_DATA = Path(r"D:\data\calibration_usable_20260808")
-LIDAR_ZIP = "lidar_dlog/dorec_recovered_20260808_171438_181422.zip"
-IMU_MAIN = "imu_rscap/hi13r4-imu_20260808-092827.638_39783edb-e46e-4b28-a5e8-427b981c2fce.rscap"
-IMU_TAIL = "imu_rscap/hi13r4-imu_20260808-101022.036_87ea5edc-cd3d-4192-809a-469fbc8cac01.rscap"
-
-# From usable-segment chart (local wall clock).
-WINDOWS = [
-    {
-        "name": "priority_174005_174515",
-        "host_start": "2026-08-08T17:40:05",
-        "host_end": "2026-08-08T17:45:15",
-        "imu": [IMU_MAIN],
-        "priority": True,
-    },
-    {
-        "name": "priority_174905_175450",
-        "host_start": "2026-08-08T17:49:05",
-        "host_end": "2026-08-08T17:54:50",
-        "imu": [IMU_MAIN],
-        "priority": True,
-    },
-    {
-        "name": "priority_175910_180530",
-        "host_start": "2026-08-08T17:59:10",
-        "host_end": "2026-08-08T18:05:30",
-        "imu": [IMU_MAIN],
-        "priority": True,
-    },
-    {
-        "name": "usable_181035_181050",
-        "host_start": "2026-08-08T18:10:35",
-        "host_end": "2026-08-08T18:10:50",
-        "imu": [IMU_TAIL],
-        "priority": False,
-    },
-    {
-        "name": "usable_181225_181300",
-        "host_start": "2026-08-08T18:12:25",
-        "host_end": "2026-08-08T18:13:00",
-        "imu": [IMU_TAIL],
-        "priority": False,
-    },
-    {
-        "name": "usable_181350_181410",
-        "host_start": "2026-08-08T18:13:50",
-        "host_end": "2026-08-08T18:14:10",
-        "imu": [IMU_TAIL],
-        "priority": False,
-    },
-]
-
-
-def main() -> int:
-    parser = argparse.ArgumentParser(description=__doc__)
-    parser.add_argument("--data-root", type=Path, default=DEFAULT_DATA)
-    parser.add_argument(
-        "--out-root",
-        type=Path,
-        default=DEFAULT_DATA / "sessions_v1",
-    )
-    parser.add_argument("--priority-only", action="store_true", default=True)
-    parser.add_argument("--all-windows", action="store_true")
-    parser.add_argument("--frame-stride", type=int, default=5)
-    parser.add_argument("--max-points-per-frame", type=int, default=40000)
-    args = parser.parse_args()
-    priority_only = not args.all_windows
-    lidar = args.data_root / LIDAR_ZIP
-    if not lidar.is_file():
-        raise SystemExit(f"missing lidar zip: {lidar}")
-
-    selected = [w for w in WINDOWS if (not priority_only) or w["priority"]]
-    results = []
-    for window in selected:
-        out = args.out_root / window["name"]
-        imu_paths = [args.data_root / rel for rel in window["imu"]]
-        print(f"=== exporting {window['name']} ===", flush=True)
-        summary = export_session(
-            imu_rscap=imu_paths,
-            lidar_dlog=lidar,
-            imu_kind="hi13",
-            require_difop=True,
-            host_start=window["host_start"],
-            host_end=window["host_end"],
-            out=out,
-            frame_stride=args.frame_stride,
-            max_points_per_frame=args.max_points_per_frame,
-        )
-        brief = {
-            "name": window["name"],
-            "imu_samples": summary["imu"]["samples"],
-            "lidar_frames": summary["lidar"]["frames"],
-            "angle_source": summary["lidar"]["angle_source"],
-            "out": str(out),
-        }
-        results.append(brief)
-        print(json.dumps(brief, ensure_ascii=False, indent=2), flush=True)
-
-    manifest = args.out_root / "export_windows_manifest.json"
-    args.out_root.mkdir(parents=True, exist_ok=True)
-    manifest.write_text(json.dumps(results, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
-    print(f"manifest: {manifest}")
-    return 0
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

tools/generate_synthetic_session.py

@@ -1,114 +0,0 @@
-"""Generate a tiny synthetic session for V1 smoke tests."""
-
-from __future__ import annotations
-
-from pathlib import Path
-
-import numpy as np
-
-from imu_lidar.contracts import ImuSeries, LidarFrame
-from imu_lidar.geometry import so3_exp
-from imu_lidar.imu_io import save_imu_csv
-from imu_lidar.lidar_io import save_lidar_session
-
-
-def _wall_cloud(rng: np.random.Generator, n: int = 800) -> np.ndarray:
-    yz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
-    wall_x = np.column_stack([np.full(n // 3, 8.0), yz[:, 0], yz[:, 1]])
-    xz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
-    wall_y = np.column_stack([xz[:, 0], np.full(n // 3, 6.0), xz[:, 1]])
-    xy = rng.uniform([-5, -5], [5, 5], size=(n - 2 * (n // 3), 2))
-    ground = np.column_stack([xy[:, 0], xy[:, 1], np.full(xy.shape[0], -1.0)])
-    return np.vstack([wall_x, wall_y, ground])
-
-
-def generate_synthetic_session(
-    output_root: Path,
-    *,
-    delta_t_s: float = 0.17,
-    yaw_extrinsic_deg: float = 25.0,
-    seed: int = 0,
-) -> dict[str, float]:
-    """Write IMU CSV + LiDAR frames with known extrinsic rotation and time offset."""
-
-    rng = np.random.default_rng(seed)
-    output_root = Path(output_root)
-    output_root.mkdir(parents=True, exist_ok=True)
-
-    r_x = so3_exp(np.deg2rad(np.array([2.0, -1.5, yaw_extrinsic_deg])))
-    map_points = _wall_cloud(rng)
-
-    lidar_hz = 10.0
-    duration = 8.0
-    lidar_times = np.arange(0.0, duration, 1.0 / lidar_hz)
-    # Non-yaw excitation is required for unique SO(3) hand-eye observability.
-    yaw = 0.5 * np.sin(0.8 * lidar_times) + 0.12 * lidar_times
-    pitch = 0.18 * np.sin(1.3 * lidar_times + 0.4)
-    roll = 0.12 * np.sin(1.7 * lidar_times + 1.0)
-    yaw_rate = np.gradient(yaw, lidar_times)
-    pitch_rate = np.gradient(pitch, lidar_times)
-    roll_rate = np.gradient(roll, lidar_times)
-
-    frames: list[LidarFrame] = []
-    for index, (t, yaw_i, pitch_i, roll_i) in enumerate(zip(lidar_times, yaw, pitch, roll)):
-        r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
-        t_wl = np.array([0.4 * t, 0.05 * np.sin(0.5 * t), 0.0])
-        points = (map_points - t_wl) @ r_wl
-        points = points + rng.normal(0.0, 0.01, size=points.shape)
-        frames.append(
-            LidarFrame(
-                frame_id=str(index),
-                t_start_s=float(t),
-                t_end_s=float(t + 0.08),
-                points_xyz=points.astype(float),
-            )
-        )
-    save_lidar_session(output_root / "lidar", frames)
-
-    imu_hz = 100.0
-    t_lidar_grid = np.arange(0.0, duration, 1.0 / imu_hz)
-    omega_lidar = np.column_stack(
-        [
-            np.interp(t_lidar_grid, lidar_times, roll_rate),
-            np.interp(t_lidar_grid, lidar_times, pitch_rate),
-            np.interp(t_lidar_grid, lidar_times, yaw_rate),
-        ]
-    )
-    omega_imu = omega_lidar @ r_x.T
-
-    g_world = np.array([0.0, 0.0, 9.80665])
-    acc_rows = []
-    for yaw_i, pitch_i, roll_i in zip(
-        np.interp(t_lidar_grid, lidar_times, yaw),
-        np.interp(t_lidar_grid, lidar_times, pitch),
-        np.interp(t_lidar_grid, lidar_times, roll),
-    ):
-        r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
-        g_in_lidar = r_wl.T @ g_world
-        acc_rows.append(r_x @ g_in_lidar)
-    acc = np.asarray(acc_rows, dtype=float)
-
-    static_t = np.arange(-1.0, 0.0, 1.0 / imu_hz)
-    static_gyro = np.zeros((static_t.size, 3))
-    static_acc = np.tile(r_x @ g_world, (static_t.size, 1))
-
-    t_imu = np.concatenate([static_t + delta_t_s, t_lidar_grid + delta_t_s])
-    gyro = np.vstack([static_gyro, omega_imu]) + rng.normal(0.0, 0.001, size=(t_imu.size, 3))
-    acc_all = np.vstack([static_acc, acc]) + rng.normal(0.0, 0.01, size=(t_imu.size, 3))
-    imu = ImuSeries(t_s=t_imu, gyro_rad_s=gyro, acc_m_s2=acc_all)
-    save_imu_csv(output_root / "imu.csv", imu)
-
-    return {
-        "delta_t_s": float(delta_t_s),
-        "yaw_extrinsic_deg": float(yaw_extrinsic_deg),
-    }
-
-
-if __name__ == "__main__":
-    import json
-
-    out = Path("examples/synthetic_session")
-    meta = generate_synthetic_session(out)
-    (out / "meta.json").write_text(json.dumps(meta, indent=2), encoding="utf-8")
-    print(f"wrote {out}")
-    print(meta)

tools/h32_dlog/__init__.py

@@ -1,18 +1,15 @@
 """Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP."""
 
 from .difop import parse_difop_angles
-from .dobject import discover_records, iter_payloads, open_dlog_source, resolve_dlog_root
+from .dobject import discover_records, iter_payloads, resolve_dlog_root
 from .load_session import H32DlogLidarSession, load_h32_dlog_lidar
 from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
-from .timeutil import local_wall_to_dotnet_ticks
 
 __all__ = [
     "H32DlogLidarSession",
     "discover_records",
     "iter_payloads",
     "load_h32_dlog_lidar",
-    "local_wall_to_dotnet_ticks",
-    "open_dlog_source",
     "parse_difop_angles",
     "parse_difop_payload",
     "parse_msop_batch_payload",

tools/h32_dlog/dobject.py

@@ -1,24 +1,16 @@
-"""Index and read Medulla DObject recordings.
+"""Index and read Medulla DObject recordings (dobject/ + dobject_recording/)."""
-
-Supports:
-
-- standard layout: ``dobject/**/*.log`` + ``dobject_recording/**/*.dorec``
-- recovered layout: ``dobject/all/indices.log`` + ``dobject_recording/data.bin``
-- either as an extracted directory or a zip containing those paths
-"""
 
 from __future__ import annotations
 
 import re
 import struct
-import zipfile
 from dataclasses import dataclass
 from pathlib import Path
 from typing import BinaryIO, Iterator
 
 
 RECORD_RE = re.compile(
-    r"^(?:\[(?P<log_time>[^]]+)\])?>?\s*DObject `(?P<name>[^`]+)` post "
+    r"^\[(?P<log_time>[^]]+)\].*?DObject `(?P<name>[^`]+)` post "
     r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), "
     r"@(?P<file>[^:]+):(?P<offset>\d+)"
 )
@@ -37,225 +29,37 @@ class RecordRef:
     dotnet_ticks: int
 
 
-class _ZipStoredMemberIO:
-    """Random-access reader for a ZIP_STORED member via the underlying zip file.
-
-    ``ZipExtFile.seek`` on multi-GB members is far too slow for per-record reads.
-    """
-
-    def __init__(self, zip_path: Path, member_name: str, data_offset: int, data_size: int):
-        self._path = zip_path
-        self._member_name = member_name
-        self._data_offset = data_offset
-        self._data_size = data_size
-        self._fh = zip_path.open("rb")
-        self._pos = 0
-
-    def seek(self, offset: int, whence: int = 0) -> int:
-        if whence == 0:
-            self._pos = offset
-        elif whence == 1:
-            self._pos += offset
-        elif whence == 2:
-            self._pos = self._data_size + offset
-        else:
-            raise ValueError(f"invalid whence: {whence}")
-        if self._pos < 0:
-            raise ValueError("negative seek")
-        return self._pos
-
-    def read(self, size: int = -1) -> bytes:
-        if size is None or size < 0:
-            size = self._data_size - self._pos
-        if size <= 0 or self._pos >= self._data_size:
-            return b""
-        size = min(size, self._data_size - self._pos)
-        self._fh.seek(self._data_offset + self._pos)
-        data = self._fh.read(size)
-        self._pos += len(data)
-        return data
-
-    def close(self) -> None:
-        self._fh.close()
-
-
-def _zip_stored_member_offset(zip_path: Path, info: zipfile.ZipInfo) -> int:
-    if info.compress_type != zipfile.ZIP_STORED:
-        raise RuntimeError(
-            f"member {info.filename!r} is compressed (type={info.compress_type}); "
-            "extract it first or store uncompressed"
-        )
-    with zip_path.open("rb") as handle:
-        handle.seek(info.header_offset)
-        header = handle.read(30)
-        if len(header) != 30 or header[:4] != b"PK\x03\x04":
-            raise RuntimeError(f"bad local zip header for {info.filename!r}")
-        name_len, extra_len = struct.unpack("<HH", header[26:30])
-        return info.header_offset + 30 + name_len + extra_len
-
-
-@dataclass
-class DlogSource:
-    """Opened dlog directory or recovered zip."""
-
-    label: str
-    directory: Path | None = None
-    zip_path: Path | None = None
-    _zip: zipfile.ZipFile | None = None
-    _log_cache: dict[str, str] | None = None
-    _member_offsets: dict[str, tuple[int, int]] | None = None
-
-    def close(self) -> None:
-        if self._zip is not None:
-            self._zip.close()
-            self._zip = None
-
-    def __enter__(self) -> "DlogSource":
-        return self
-
-    def __exit__(self, exc_type, exc, tb) -> None:
-        self.close()
-
-    def iter_log_texts(self) -> Iterator[tuple[str, str]]:
-        if self.zip_path is not None:
-            assert self._zip is not None
-            if self._log_cache is None:
-                self._log_cache = {}
-                names = sorted(
-                    name
-                    for name in self._zip.namelist()
-                    if name.replace("\\", "/").startswith("dobject/")
-                    and name.replace("\\", "/").endswith(".log")
-                )
-                for name in names:
-                    key = name.replace("\\", "/")
-                    self._log_cache[key] = self._zip.read(name).decode("utf-8", errors="replace")
-            for name, text in self._log_cache.items():
-                yield name, text
-            return
-        assert self.directory is not None
-        for log_path in sorted((self.directory / "dobject").rglob("*.log")):
-            relative = log_path.relative_to(self.directory).as_posix()
-            yield relative, log_path.read_text(encoding="utf-8", errors="replace")
-
-    def open_recording(self, name: str) -> tuple[object, BinaryIO]:
-        """Return (owner, binary stream) supporting seek/read of one recording member."""
-
-        base = Path(name).name
-        if self.zip_path is not None:
-            assert self._zip is not None
-            candidates = [
-                n
-                for n in self._zip.namelist()
-                if Path(n.replace("\\", "/")).name.casefold() == base.casefold()
-                and "dobject_recording/" in n.replace("\\", "/")
-            ]
-            if not candidates:
-                alt = name.replace("\\", "/")
-                if alt in self._zip.namelist():
-                    candidates = [alt]
-                elif f"dobject_recording/{base}" in self._zip.namelist():
-                    candidates = [f"dobject_recording/{base}"]
-            if not candidates:
-                raise FileNotFoundError(f"missing recording in zip: {name}")
-            if len(candidates) > 1:
-                raise RuntimeError(f"ambiguous recording in zip {name}: {candidates}")
-            member = candidates[0].replace("\\", "/")
-            if self._member_offsets is None:
-                self._member_offsets = {}
-            if member not in self._member_offsets:
-                info = self._zip.getinfo(member)
-                self._member_offsets[member] = (
-                    _zip_stored_member_offset(self.zip_path, info),
-                    info.file_size,
-                )
-            data_offset, data_size = self._member_offsets[member]
-            stream = _ZipStoredMemberIO(self.zip_path, member, data_offset, data_size)
-            return stream, stream
-
-        assert self.directory is not None
-        index = index_dorec_files(self.directory)
-        if base.casefold() == "data.bin":
-            path = self.directory / "dobject_recording" / "data.bin"
-            if not path.is_file():
-                matches = list((self.directory / "dobject_recording").rglob("data.bin"))
-                if not matches:
-                    raise FileNotFoundError(f"missing recording file: {name}")
-                path = matches[0]
-            stream = path.open("rb")
-            return stream, stream
-        path = choose_dorec(index, name)
-        stream = path.open("rb")
-        return stream, stream
-
-
-def open_dlog_source(value: Path | str) -> DlogSource:
-    path = Path(value).expanduser().resolve()
-    if path.is_file() and path.suffix.lower() == ".zip":
-        zf = zipfile.ZipFile(path, "r")
-        names = {n.replace("\\", "/") for n in zf.namelist()}
-        has_log = any(n.startswith("dobject/") and n.endswith(".log") for n in names)
-        has_rec = any(n.startswith("dobject_recording/") for n in names)
-        if not (has_log and has_rec):
-            zf.close()
-            raise FileNotFoundError(f"{path} is not a recovered/standard dlog zip")
-        return DlogSource(label=str(path), zip_path=path, _zip=zf)
-
-    root = path
-    if not ((root / "dobject").is_dir() and (root / "dobject_recording").is_dir()):
-        child = root / "dlog"
-        if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
-            root = child
-        else:
-            raise FileNotFoundError(f"{path} does not contain dobject and dobject_recording")
-    return DlogSource(label=str(root), directory=root)
-
-
 def resolve_dlog_root(value: Path | str) -> Path:
-    """Backward-compatible helper: directory roots only (not zip)."""
+    root = Path(value).expanduser().resolve()
-
+    if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir():
-    source = open_dlog_source(value)
+        return root
-    try:
+    child = root / "dlog"
-        if source.directory is None:
+    if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
-            raise FileNotFoundError(
+        return child
-                f"{value} is a zip; use open_dlog_source()/iter_payloads_from_source()"
+    raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording")
-            )
-        return source.directory
-    finally:
-        source.close()
 
 
-def discover_records_from_source(
+def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
-    source: DlogSource,
-    object_name: str,
-    *,
-    host_ticks_min: int | None = None,
-    host_ticks_max: int | None = None,
-) -> list[RecordRef]:
     pending: list[tuple[str, str, str, int, int, str, int, str]] = []
-    name_key = object_name.casefold()
+    for log_path in sorted((dlog_root / "dobject").rglob("*.log")):
-    for relative_log, text in source.iter_log_texts():
+        relative_log = log_path.relative_to(dlog_root).as_posix()
-        for line in text.splitlines():
+        with log_path.open("r", encoding="utf-8", errors="replace") as stream:
-            match = RECORD_RE.search(line.strip())
+            for line in stream:
-            if not match or match.group("name").casefold() != name_key:
+                match = RECORD_RE.search(line)
-                continue
+                if not match or match.group("name").casefold() != object_name.casefold():
-            ticks = int(match.group("tic"))
+                    continue
-            if host_ticks_min is not None and ticks < host_ticks_min:
+                pending.append(
-                continue
+                    (
-            if host_ticks_max is not None and ticks > host_ticks_max:
+                        match.group("name"),
-                continue
+                        match.group("log_time"),
-            pending.append(
+                        relative_log,
-                (
+                        int(match.group("offset")),
-                    match.group("name"),
+                        int(match.group("len")),
-                    match.group("log_time") or "",
+                        match.group("id").upper(),
-                    relative_log,
+                        int(match.group("tic")),
-                    int(match.group("offset")),
+                        match.group("file"),
-                    int(match.group("len")),
+                    )
-                    match.group("id").upper(),
-                    ticks,
-                    match.group("file"),
                 )
-            )
     pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3]))
     seen: set[tuple[str, int, int]] = set()
     records: list[RecordRef] = []
@@ -280,19 +84,10 @@ def discover_records_from_source(
     return records
 
 
-def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
-    with open_dlog_source(dlog_root) as source:
-        return discover_records_from_source(source, object_name)
-
-
 def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
     result: dict[str, list[Path]] = {}
-    recording = dlog_root / "dobject_recording"
+    for path in (dlog_root / "dobject_recording").rglob("*.dorec"):
-    if not recording.is_dir():
+        result.setdefault(path.name.casefold(), []).append(path)
-        return result
-    for path in recording.rglob("*"):
-        if path.is_file() and path.suffix.lower() in {".dorec", ".bin"}:
-            result.setdefault(path.name.casefold(), []).append(path)
     return result
 
 
@@ -312,15 +107,17 @@ def read_exact(stream: BinaryIO, size: int) -> bytes:
     return data
 
 
-def _read_payload_at(stream: BinaryIO, record: RecordRef) -> bytes:
+def read_record_payload(path: Path, record: RecordRef) -> bytes:
-    stream.seek(record.source_offset)
+    with path.open("rb") as stream:
-    name_length = read_exact(stream, 1)[0]
+        stream.seek(record.source_offset)
-    name = read_exact(stream, name_length).decode("ascii")
+        name_length = read_exact(stream, 1)[0]
-    ticks = struct.unpack("<q", read_exact(stream, 8))[0]
+        name = read_exact(stream, name_length).decode("ascii")
-    id_length = read_exact(stream, 1)[0]
+        ticks = struct.unpack("<q", read_exact(stream, 8))[0]
-    id_bytes = read_exact(stream, id_length)
+        id_length = read_exact(stream, 1)[0]
-    payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
+        id_bytes = read_exact(stream, id_length)
-    payload = read_exact(stream, payload_length)
+        payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
+        payload = read_exact(stream, payload_length)
+
     try:
         record_id = id_bytes.decode("ascii")
     except UnicodeDecodeError:
@@ -336,46 +133,47 @@ def _read_payload_at(stream: BinaryIO, record: RecordRef) -> bytes:
     return payload
 
 
-def iter_payloads_from_source(
+def iter_payloads(dlog_root: Path, object_name: str) -> Iterator[tuple[RecordRef, bytes]]:
-    source: DlogSource,
+    root = resolve_dlog_root(dlog_root)
-    object_name: str,
+    records = discover_records(root, object_name)
-    *,
-    host_ticks_min: int | None = None,
-    host_ticks_max: int | None = None,
-) -> Iterator[tuple[RecordRef, bytes]]:
-    records = discover_records_from_source(
-        source,
-        object_name,
-        host_ticks_min=host_ticks_min,
-        host_ticks_max=host_ticks_max,
-    )
     if not records:
         return
-    open_files: dict[str, BinaryIO] = {}
+    dorec_index = index_dorec_files(root)
+    open_files: dict[str, tuple[Path, BinaryIO]] = {}
     try:
         for record in records:
-            key = Path(record.source_dorec).name.casefold()
+            key = record.source_dorec.casefold()
-            stream = open_files.get(key)
+            handle = open_files.get(key)
-            if stream is None:
+            if handle is None:
-                _owner, stream = source.open_recording(record.source_dorec)
+                path = choose_dorec(dorec_index, record.source_dorec)
-                open_files[key] = stream
+                handle = (path, path.open("rb"))
-            yield record, _read_payload_at(stream, record)
+                open_files[key] = handle
+            path, stream = handle
+            stream.seek(record.source_offset)
+            name_length = read_exact(stream, 1)[0]
+            name = read_exact(stream, name_length).decode("ascii")
+            ticks = struct.unpack("<q", read_exact(stream, 8))[0]
+            id_length = read_exact(stream, 1)[0]
+            id_bytes = read_exact(stream, id_length)
+            payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
+            payload = read_exact(stream, payload_length)
+            try:
+                record_id = id_bytes.decode("ascii")
+            except UnicodeDecodeError:
+                record_id = id_bytes.hex().upper()
+            if name != record.object_name:
+                raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
+            if ticks != record.dotnet_ticks:
+                raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
+            if payload_length != record.payload_length:
+                raise ValueError(
+                    f"payload mismatch: log={record.payload_length}, dorec={payload_length}"
+                )
+            if record_id.upper() != record.log_record_id.upper():
+                raise ValueError(
+                    f"record id mismatch: log={record.log_record_id}, dorec={record_id}"
+                )
+            yield record, payload
     finally:
-        for stream in open_files.values():
+        for _path, stream in open_files.values():
             stream.close()
-
-
-def iter_payloads(
-    dlog_root: Path | str,
-    object_name: str,
-    *,
-    host_ticks_min: int | None = None,
-    host_ticks_max: int | None = None,
-) -> Iterator[tuple[RecordRef, bytes]]:
-    with open_dlog_source(dlog_root) as source:
-        yield from iter_payloads_from_source(
-            source,
-            object_name,
-            host_ticks_min=host_ticks_min,
-            host_ticks_max=host_ticks_max,
-        )

tools/h32_dlog/load_session.py

@@ -2,25 +2,27 @@
 
 from __future__ import annotations
 
+import sys
 from dataclasses import dataclass
 from pathlib import Path
 
 import numpy as np
 
-from tools.rscap_v2.h32_msop import default_horizontal_deg, default_vertical_deg
+_TOOLS = Path(__file__).resolve().parents[1]
+_RSCAP_V2 = _TOOLS / "rscap_v2"
+if str(_RSCAP_V2) not in sys.path:
+    sys.path.insert(0, str(_RSCAP_V2))
+
+from h32_msop import default_horizontal_deg, default_vertical_deg  # noqa: E402
 
 from .difop import DifopAngles, parse_difop_angles
-from .dobject import (
+from .dobject import discover_records, iter_payloads, resolve_dlog_root
-    discover_records_from_source,
-    iter_payloads_from_source,
-    open_dlog_source,
-)
 from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
 
 
 @dataclass
 class H32DlogLidarSession:
-    dlog_root: str
+    dlog_root: Path
     msop_object: str
     difop_object: str
     msop_packets: list[bytes]
@@ -32,8 +34,6 @@ class H32DlogLidarSession:
     horizontal_deg: np.ndarray
     session_id: str | None = None
     lidar_ip: str | None = None
-    host_ticks_min: int | None = None
-    host_ticks_max: int | None = None
 
 
 def load_h32_dlog_lidar(
@@ -42,99 +42,68 @@ def load_h32_dlog_lidar(
     msop_object: str = "frontlidar-msop-raw",
     difop_object: str = "frontlidar-difop-raw",
     require_difop: bool = False,
-    host_ticks_min: int | None = None,
-    host_ticks_max: int | None = None,
 ) -> H32DlogLidarSession:
-    with open_dlog_source(dlog_root) as source:
+    root = resolve_dlog_root(dlog_root)
-        # DIFOP angles: prefer packets inside the window, else any in the capture.
+    msop_packets: list[bytes] = []
-        angles: DifopAngles | None = None
+    msop_host_utc_ticks: list[int] = []
-        difop_count = 0
+    batch_count = 0
-        session_id: str | None = None
+    session_id: str | None = None
-        lidar_ip: str | None = None
+    lidar_ip: str | None = None
-        for _record, payload in iter_payloads_from_source(
-            source,
-            difop_object,
-            host_ticks_min=host_ticks_min,
-            host_ticks_max=host_ticks_max,
-        ):
-            difop = parse_difop_payload(payload)
-            difop_count += 1
-            try:
-                angles = parse_difop_angles(difop.raw)
-            except ValueError:
-                continue
-            if session_id is None:
-                session_id = difop.session_id
-                lidar_ip = difop.lidar_ip
 
-        if angles is None:
+    for _record, payload in iter_payloads(root, msop_object):
-            for _record, payload in iter_payloads_from_source(source, difop_object):
+        batch = parse_msop_batch_payload(payload)
-                difop = parse_difop_payload(payload)
+        batch_count += 1
-                difop_count += 1
+        if session_id is None:
-                try:
+            session_id = batch.session_id
-                    angles = parse_difop_angles(difop.raw)
+            lidar_ip = batch.lidar_ip
-                except ValueError:
+        for item in batch.packets:
-                    continue
+            msop_packets.append(item.raw)
-                if session_id is None:
+            msop_host_utc_ticks.append(int(item.host_receive_utc_ticks))
-                    session_id = difop.session_id
-                    lidar_ip = difop.lidar_ip
-                if angles is not None:
-                    break
 
-        msop_packets: list[bytes] = []
+    angles: DifopAngles | None = None
-        msop_host_utc_ticks: list[int] = []
+    difop_count = 0
-        batch_count = 0
+    for _record, payload in iter_payloads(root, difop_object):
-        for record, payload in iter_payloads_from_source(
+        difop = parse_difop_payload(payload)
-            source,
+        difop_count += 1
-            msop_object,
+        try:
-            host_ticks_min=host_ticks_min,
+            angles = parse_difop_angles(difop.raw)
-            host_ticks_max=host_ticks_max,
+        except ValueError:
-        ):
+            continue
-            batch = parse_msop_batch_payload(payload)
+        if session_id is None:
-            batch_count += 1
+            session_id = difop.session_id
-            if session_id is None:
+            lidar_ip = difop.lidar_ip
-                session_id = batch.session_id
-                lidar_ip = batch.lidar_ip
-            for item in batch.packets:
-                msop_packets.append(item.raw)
-                # Per-packet UTC host receive from MSOP DLog payload only.
-                # Do NOT fall back to DObject tic (DateTime.Now / local).
-                msop_host_utc_ticks.append(int(item.host_receive_utc_ticks))
 
-        if not msop_packets:
+    if not msop_packets:
-            msop_records = discover_records_from_source(source, msop_object)
+        msop_records = discover_records(root, msop_object)
-            raise RuntimeError(
+        raise RuntimeError(
-                f"no MSOP packets from DObject {msop_object!r} under {source.label} "
+            f"no MSOP packets from DObject {msop_object!r} under {root} "
-                f"(log records={len(msop_records)}, "
+            f"(log records={len(msop_records)})"
-                f"host_ticks=[{host_ticks_min}, {host_ticks_max}])"
-            )
-
-        if angles is None:
-            if require_difop:
-                raise RuntimeError(
-                    f"no valid DIFOP calibration from DObject {difop_object!r} under {source.label}"
-                )
-            vertical = default_vertical_deg()
-            horizontal = default_horizontal_deg()
-            angle_source = "default_msop_only_vertical_-16_to_16_deg"
-        else:
-            vertical = angles.vertical_deg
-            horizontal = angles.horizontal_deg
-            angle_source = "difop_channel_angles"
-
-        return H32DlogLidarSession(
-            dlog_root=source.label,
-            msop_object=msop_object,
-            difop_object=difop_object,
-            msop_packets=msop_packets,
-            msop_host_utc_ticks=msop_host_utc_ticks,
-            msop_batch_count=batch_count,
-            difop_record_count=difop_count,
-            angle_source=angle_source,
-            vertical_deg=vertical,
-            horizontal_deg=horizontal,
-            session_id=session_id,
-            lidar_ip=lidar_ip,
-            host_ticks_min=host_ticks_min,
-            host_ticks_max=host_ticks_max,
         )
+
+    if angles is None:
+        if require_difop:
+            raise RuntimeError(
+                f"no valid DIFOP calibration from DObject {difop_object!r} under {root}"
+            )
+        vertical = default_vertical_deg()
+        horizontal = default_horizontal_deg()
+        angle_source = "default_msop_only_vertical_-16_to_16_deg"
+    else:
+        vertical = angles.vertical_deg
+        horizontal = angles.horizontal_deg
+        angle_source = "difop_channel_angles"
+
+    return H32DlogLidarSession(
+        dlog_root=root,
+        msop_object=msop_object,
+        difop_object=difop_object,
+        msop_packets=msop_packets,
+        msop_host_utc_ticks=msop_host_utc_ticks,
+        msop_batch_count=batch_count,
+        difop_record_count=difop_count,
+        angle_source=angle_source,
+        vertical_deg=vertical,
+        horizontal_deg=horizontal,
+        session_id=session_id,
+        lidar_ip=lidar_ip,
+    )

tools/h32_dlog/timeutil.py

@@ -1,56 +0,0 @@
-"""Wall-clock helpers for Medulla tick filtering.
-
-Two tick conventions appear in this dataset:
-
-- LiDAR DObject ``tic`` / recovered ``indices.log``: ``DateTime.Now.Ticks`` (local)
-- IMU / MSOP payload host receive fields: UTC ``DateTime.UtcNow.Ticks``
-"""
-
-from __future__ import annotations
-
-from datetime import datetime, timedelta, timezone
-
-TICKS_PER_SECOND = 10_000_000
-DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
-
-
-def _parse_local_wall(text: str) -> datetime:
-    normalized = text.strip().replace(" ", "T")
-    if normalized.endswith("Z"):
-        raise ValueError("expected local wall time without Z; got UTC marker")
-    if "+" in normalized[10:]:
-        idx = normalized.find("+", 10)
-        normalized = normalized[:idx]
-    elif normalized.count("-") > 2:
-        # timezone like -08:00 after the date
-        idx = normalized.find("-", 10)
-        if idx > 0 and ":" in normalized[idx + 1 :]:
-            normalized = normalized[:idx]
-    return datetime.fromisoformat(normalized).replace(tzinfo=None)
-
-
-def local_wall_to_dotnet_ticks(text: str) -> int:
-    """Local wall time → ``DateTime.Now.Ticks`` (LiDAR DObject tic)."""
-
-    dt = _parse_local_wall(text)
-    delta = dt - datetime(1, 1, 1)
-    return int(delta.total_seconds() * TICKS_PER_SECOND)
-
-
-def local_wall_to_utc_dotnet_ticks(text: str, *, tz_hours: float = 8.0) -> int:
-    """Local wall time in ``tz_hours`` → UTC ``DateTime.UtcNow.Ticks`` (IMU host)."""
-
-    dt = _parse_local_wall(text).replace(tzinfo=timezone(timedelta(hours=tz_hours)))
-    unix = dt.timestamp()
-    return int(round(unix * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS
-
-
-def dotnet_ticks_to_local_iso(ticks: int) -> str:
-    dt = datetime(1, 1, 1) + timedelta(microseconds=ticks / 10.0)
-    return dt.isoformat(timespec="milliseconds")
-
-
-def utc_dotnet_ticks_to_unix_s(ticks: int) -> float:
-    """UTC ``DateTime.UtcNow.Ticks`` → Unix seconds."""
-
-    return (float(ticks) - float(DOTNET_UNIX_EPOCH_TICKS)) / float(TICKS_PER_SECOND)

tools/prepare_multisensor_station_dataset.py

@@ -0,0 +1,233 @@
+#!/usr/bin/env python3
+"""Prepare one static LiDAR frame and one RTK reference pose per NPZ segment."""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import math
+import re
+import shutil
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from rtk_attitude import (
+    heading_to_enu_yaw,
+    parse_pitch_roll_from_heading_raw,
+    rotation_to_quat_xyzw,
+    rtk_body_rotation,
+)
+
+POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
+
+
+def natural_key(value: str) -> list[Any]:
+    return [int(part) if part.isdigit() else part.lower() for part in re.split(r"(\d+)", value)]
+
+
+def truth(value: Any) -> bool:
+    return str(value).strip().lower() in {"1", "true", "yes", "y"}
+
+
+def circular_mean_deg(values: np.ndarray) -> float:
+    radians = np.deg2rad(values)
+    return float(np.rad2deg(math.atan2(np.mean(np.sin(radians)), np.mean(np.cos(radians)))) % 360.0)
+
+
+def circular_std_deg(values: np.ndarray) -> float:
+    radians = np.deg2rad(values)
+    resultant = max(math.hypot(np.mean(np.cos(radians)), np.mean(np.sin(radians))), 1e-12)
+    return float(np.rad2deg(math.sqrt(-2.0 * math.log(resultant))))
+
+
+def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray:
+    a, e2 = 6378137.0, 6.69437999014e-3
+    lat, lon = math.radians(lat_deg), math.radians(lon_deg)
+    sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
+    n = a / math.sqrt(1.0 - e2 * sin_lat * sin_lat)
+    return np.array([(n + height_m) * cos_lat * cos_lon, (n + height_m) * cos_lat * sin_lon,
+                     (n * (1.0 - e2) + height_m) * sin_lat], dtype=float)
+
+
+def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: float) -> np.ndarray:
+    lat, lon = math.radians(lat_deg), math.radians(lon_deg)
+    slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
+    rotation = np.array([[-slon, clon, 0.0], [-slat * clon, -slat * slon, clat],
+                         [clat * clon, clat * slon, slat]], dtype=float)
+    return rotation @ (ecef - origin)
+
+
+def scalar(data: np.lib.npyio.NpzFile, name: str, default: float | None = None) -> float:
+    if name not in data.files:
+        if default is None:
+            raise KeyError(name)
+        return float(default)
+    return float(np.asarray(data[name]).reshape(-1)[0])
+
+
+def frame_pitch_roll(data: np.lib.npyio.NpzFile) -> tuple[float, float]:
+    pitch = scalar(data, "rtk_pitch_deg", math.nan)
+    roll = scalar(data, "rtk_roll_deg", math.nan)
+    if math.isfinite(pitch) and math.isfinite(roll):
+        return pitch, roll
+    raw = None
+    if "rtk_heading_raw_utf8" in data.files:
+        raw = bytes(np.asarray(data["rtk_heading_raw_utf8"]).reshape(-1))
+    parsed_pitch, parsed_roll = parse_pitch_roll_from_heading_raw(raw)
+    if not math.isfinite(pitch):
+        pitch = float(parsed_pitch) if parsed_pitch is not None else 0.0
+    if not math.isfinite(roll):
+        roll = float(parsed_roll) if parsed_roll is not None else 0.0
+    return pitch, roll
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--combined-root", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    parser.add_argument("--pose-name", default="rtk_gga_raw_heading")
+    parser.add_argument("--heading-offset-deg", type=float, required=True)
+    parser.add_argument("--antenna-lever", type=float, nargs=3, required=True, metavar=("X", "Y", "Z"))
+    parser.add_argument("--accepted-fixes", type=int, nargs="+", default=[4, 5])
+    parser.add_argument("--heading-std-limit-deg", type=float, default=0.5)
+    parser.add_argument("--min-stations", type=int, default=30)
+    parser.add_argument("--expected-stations", type=int, default=0)
+    parser.add_argument(
+        "--orientation-model",
+        choices=("heading_pitch_roll", "yaw_only"),
+        default="heading_pitch_roll",
+        help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll; yaw_only forces roll=pitch=0",
+    )
+    parser.add_argument("--overwrite", action="store_true")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    manifest_path = args.combined_root / "manifest.csv"
+    with manifest_path.open("r", encoding="utf-8-sig", newline="") as stream:
+        rows = list(csv.DictReader(stream))
+    required = {"segment", "output", "lidar_time_ns", "rtk_valid", "heading_valid", "rtk_fix_quality"}
+    if not rows or not required.issubset(rows[0]):
+        raise ValueError(f"{manifest_path} is empty or lacks {sorted(required)}")
+    groups: dict[str, list[dict[str, str]]] = {}
+    for row in rows:
+        groups.setdefault(row["segment"], []).append(row)
+
+    selected, summaries, rejected = [], [], []
+    accepted_fixes = set(args.accepted_fixes)
+    for segment in sorted(groups, key=natural_key):
+        group = sorted(groups[segment], key=lambda row: int(row["lidar_time_ns"]))
+        good = [row for row in group if truth(row["rtk_valid"]) and truth(row["heading_valid"])
+                and int(row["rtk_fix_quality"]) in accepted_fixes]
+        if not good:
+            rejected.append({"station": segment, "reason": "no associated fixed RTK position and valid heading"})
+            continue
+        samples = []
+        for row in good:
+            path = args.combined_root / Path(row["output"])
+            with np.load(path, allow_pickle=False) as data:
+                pitch, roll = frame_pitch_roll(data)
+                samples.append((scalar(data, "rtk_lat_deg"), scalar(data, "rtk_lon_deg"),
+                                scalar(data, "rtk_altitude_m"), scalar(data, "rtk_raw_heading_deg"),
+                                pitch, roll, scalar(data, "rtk_heading_stddev_deg", math.nan)))
+        values = np.asarray(samples, dtype=float)
+        heading_std = circular_std_deg(values[:, 3])
+        if heading_std > args.heading_std_limit_deg:
+            rejected.append({"station": segment, "reason": f"heading std {heading_std:.4f} deg exceeds limit"})
+            continue
+        frame = good[len(good) // 2]
+        source = args.combined_root / Path(frame["output"])
+        reported_std = values[:, 6]
+        reported_std_mean = float(np.nanmean(reported_std)) if np.isfinite(reported_std).any() else None
+        selected.append({
+            "station": segment, "source": source, "time": int(frame["lidar_time_ns"]) / 1e9,
+            "lat": float(np.mean(values[:, 0])), "lon": float(np.mean(values[:, 1])),
+            "alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3]),
+            "pitch": float(np.mean(values[:, 4])), "roll": float(np.mean(values[:, 5])),
+        })
+        summaries.append({
+            "station": segment, "frames": len(group), "valid_fixed_frames": len(good),
+            "heading_mean_deg": circular_mean_deg(values[:, 3]),
+            "heading_circular_std_deg": heading_std,
+            "rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
+            "rtk_roll_mean_deg": float(np.mean(values[:, 5])),
+            "reported_heading_std_mean_deg": reported_std_mean,
+            "altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source),
+        })
+
+    if args.expected_stations and len(selected) != args.expected_stations:
+        raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}")
+    if len(selected) < args.min_stations:
+        raise RuntimeError(f"need at least {args.min_stations} usable stations, got {len(selected)}; rejected={rejected}")
+    if args.output.exists() and any(args.output.iterdir()) and not args.overwrite:
+        raise FileExistsError(f"{args.output} is non-empty; pass --overwrite")
+    frames = args.output / "frames_all"
+    frames.mkdir(parents=True, exist_ok=True)
+    origin = selected[0]
+    origin_ecef = geodetic_to_ecef(origin["lat"], origin["lon"], origin["alt"])
+    lever = np.asarray(args.antenna_lever, dtype=float)
+    use_attitude = args.orientation_model == "heading_pitch_roll"
+    pose_rows = []
+    for index, item in enumerate(selected, 1):
+        destination = frames / f"station_{index:02d}.npz"
+        shutil.copy2(item["source"], destination)
+        antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
+                              origin["lat"], origin["lon"])
+        corrected_heading, yaw = heading_to_enu_yaw(item["heading"], args.heading_offset_deg)
+        pitch = float(item["pitch"]) if use_attitude else 0.0
+        roll = float(item["roll"]) if use_attitude else 0.0
+        rotation = rtk_body_rotation(
+            item["heading"], args.heading_offset_deg, pitch_deg=pitch, roll_deg=roll
+        )
+        reference_position = antenna - rotation @ lever
+        quat = rotation_to_quat_xyzw(rotation)
+        pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, *quat])))
+        summaries[index - 1].update({
+            "sequence": index, "prepared_frame": destination.name,
+            "corrected_heading_deg": corrected_heading,
+            "pose_yaw_enu_deg": math.degrees(yaw),
+            "pose_pitch_deg": pitch, "pose_roll_deg": roll,
+        })
+    pose_path = args.output / f"reference_poses_{args.pose_name}.csv"
+    with pose_path.open("w", encoding="utf-8", newline="") as stream:
+        writer = csv.DictWriter(stream, fieldnames=POSE_FIELDS); writer.writeheader(); writer.writerows(pose_rows)
+    with (args.output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
+        fields = sorted({key for row in summaries for key in row})
+        writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
+    document = {
+        "source_combined_root": str(args.combined_root.resolve()), "station_count": len(selected),
+        "rejected": rejected, "pose_csv": pose_path.name,
+        "selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
+        "reference_pose_configuration": {
+            "raw_heading_offset_deg": args.heading_offset_deg,
+            "antenna_lever_body_m": args.antenna_lever,
+            "heading_offset_semantics": (
+                "added to clockwise-from-north GNHPR heading before ENU yaw conversion"
+            ),
+            "orientation_model": args.orientation_model,
+            "orientation_composition": (
+                "R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset); "
+                "yaw_raw from rawHeading, pitch/roll stay in baseline frame"
+            ),
+            "pitch_roll_note": (
+                "pitch/roll come from dual-antenna GNHPR/UNIHEADINGA (baseline elevation / reported roll). "
+                "This is not a fused IMU vehicle attitude; G90 roll is often ~0."
+            ),
+        },
+        "stations": [{"sequence": i + 1, "source_station": item["station"],
+                      "source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
+                     for i, item in enumerate(selected)],
+    }
+    (args.output / "manifest.json").write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
+    print(json.dumps({"prepared": str(args.output.resolve()), "stations": len(selected),
+                      "rejected": rejected, "pose_csv": pose_path.name,
+                      "orientation_model": args.orientation_model}, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/reproduce_synthetic.ps1

@@ -1,23 +0,0 @@
-# 一键复现合成标定（Windows）
-# 用法：在仓库根目录执行
-#   powershell -File tools\reproduce_synthetic.ps1
-#   powershell -File tools\reproduce_synthetic.ps1 -SkipPytest
-#   powershell -File tools\reproduce_synthetic.ps1 -Mode full_se3
-
-param(
-    [ValidateSet("rotation_only", "full_se3")]
-    [string]$Mode = "rotation_only",
-    [switch]$SkipPytest
-)
-
-$ErrorActionPreference = "Stop"
-$Root = Split-Path -Parent $PSScriptRoot
-Set-Location $Root
-
-$args = @("tools\reproduce_synthetic.py", "--mode", $Mode)
-if ($SkipPytest) {
-    $args += "--skip-pytest"
-}
-
-python @args
-exit $LASTEXITCODE

tools/reproduce_synthetic.py

@@ -1,109 +0,0 @@
-"""One-click synthetic reproduce: generate → calibrate → report → pytest."""
-
-from __future__ import annotations
-
-import argparse
-import subprocess
-import sys
-from pathlib import Path
-
-
-def _repo_root() -> Path:
-    return Path(__file__).resolve().parents[1]
-
-
-def _run(cmd: list[str], cwd: Path) -> None:
-    print("+", " ".join(cmd), flush=True)
-    completed = subprocess.run(cmd, cwd=str(cwd), check=False)
-    if completed.returncode != 0:
-        raise SystemExit(completed.returncode)
-
-
-def main(argv: list[str] | None = None) -> int:
-    parser = argparse.ArgumentParser(
-        description="Generate synthetic LiDAR–IMU data, run calibration, show report, run tests."
-    )
-    parser.add_argument(
-        "--mode",
-        choices=["rotation_only", "full_se3"],
-        default="rotation_only",
-    )
-    parser.add_argument("--skip-pytest", action="store_true")
-    args = parser.parse_args(argv)
-
-    root = _repo_root()
-    session = root / "examples" / "synthetic_session"
-    imu = session / "imu.csv"
-    lidar = session / "lidar"
-    calib_out = session / "out"
-    config = root / "config" / "vehicle_installation.template.yaml"
-
-    print("=== 1/4 generate synthetic session ===", flush=True)
-    _run([sys.executable, str(root / "tools" / "generate_synthetic_session.py")], cwd=root)
-
-    print("=== 2/4 run calibration ===", flush=True)
-    _run(
-        [
-            sys.executable,
-            "-m",
-            "imu_lidar.cli",
-            "run",
-            "--vehicle-config",
-            str(config),
-            "--imu",
-            str(imu),
-            "--lidar",
-            str(lidar),
-            "--output",
-            str(calib_out),
-            "--mode",
-            args.mode,
-            "--time-offset-search-s",
-            "0.5",
-            "--min-pair-rotation-deg",
-            "2.0",
-            "--min-pair-translation-m",
-            "0.05",
-            "--max-iterations",
-            "1",
-        ],
-        cwd=root,
-    )
-
-    print("=== 3/4 show report ===", flush=True)
-    _run(
-        [
-            sys.executable,
-            str(root / "tools" / "show_calibration_report.py"),
-            "--summary",
-            str(calib_out / "summary.json"),
-            "--truth-meta",
-            str(session / "meta.json"),
-            "--plot",
-            str(calib_out / "report_preview.png"),
-        ],
-        cwd=root,
-    )
-
-    if args.skip_pytest:
-        print("=== 4/4 pytest skipped ===", flush=True)
-    else:
-        print("=== 4/4 pytest ===", flush=True)
-        _run([sys.executable, "-m", "pytest", "-q"], cwd=root)
-
-    print("\nDone.")
-    print("  INPUT")
-    print(f"    IMU CSV     : {imu}")
-    print(f"    LiDAR dir   : {lidar}")
-    print(f"    vehicle YAML: {config}")
-    print("  OUTPUT")
-    print(f"    directory   : {calib_out}")
-    print(f"    T           : {calib_out / 'T_IMU_lidar.json'}")
-    print(f"    δt          : {calib_out / 'time_offset.json'}")
-    print(f"    summary     : {calib_out / 'summary.json'}")
-    print(f"    preview PNG : {calib_out / 'report_preview.png'}")
-    return 0
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

tools/rscap_v2/__init__.py

@@ -1 +0,0 @@
-"""V2 .rscap readers and sensor decoders for export to V1 intermediate format."""

tools/rscap_v2/audit_capture_v2.py

@@ -0,0 +1,22 @@
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+from capture_format_v2 import file_summary, read_capture
+from pipeline_common import write_json
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("captures", nargs="+", type=Path)
+    parser.add_argument("--out", type=Path, required=True)
+    args = parser.parse_args()
+    summaries = [file_summary(read_capture(path)) for path in args.captures]
+    write_json(args.out, {"captures": summaries})
+    for summary in summaries:
+        print(summary)
+
+
+if __name__ == "__main__":
+    main()

tools/rscap_v2/h32_msop.py

@@ -1,4 +1,4 @@
-"""Decode RoboSense H32 MSOP packets into Cartesian frames (metres).
+"""Decode RoboSense H32 MSOP packets into Cartesian / polar frames (metres).
 
 Angle / distance conventions follow the H32 Medulla plugins:
 azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
@@ -19,7 +19,7 @@ from typing import Iterable, Sequence
 
 import numpy as np
 
-from .capture_format_v2 import CaptureFile
+from capture_format_v2 import CaptureFile
 
 PACKET_LENGTH = 1248
 DATA_START = 42
@@ -27,6 +27,11 @@ BLOCKS = 12
 BLOCK_LENGTH = 100
 CHANNELS = 32
 MIN_FRAME_POINTS_DEFAULT = 100
+DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
+
+
+def ticks_to_unix_ns(ticks: int) -> int:
+    return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100
 
 
 def default_vertical_deg() -> np.ndarray:
@@ -66,8 +71,21 @@ class LidarFrameExport:
     t_start_s: float
     t_end_s: float
     points_xyz: np.ndarray  # (N, 3) metres
-    host_receive_utc_ticks_start: int = 0
+
-    host_receive_utc_ticks_end: int = 0
+
+@dataclass
+class LidarFramePolarExport:
+    """One H32 frame in the calibration ``points_raw`` polar contract.
+
+    Columns: ``d_mm, azimuth_deg, altitude_deg, intensity, progression``.
+    Azimuth already includes the H32 channel horizontal offset and sign flip so
+    ``rigorous_calibration.load_npz_xyz`` reproduces the same Cartesian points.
+    """
+
+    t_start_s: float
+    t_end_s: float
+    points_raw: np.ndarray  # (N, 5) float32
+    host_receive_utc_ns: int
 
 
 def decode_packet_points(
@@ -143,7 +161,39 @@ def _block_points(
     return np.column_stack([xs, ys, zs]).astype(np.float64, copy=False)
 
 
-def iter_h32_frames_from_packets(
+def _block_points_raw(
+    packet: bytes,
+    block_offset: int,
+    az_deg: float,
+    unit_mm: float,
+    vertical_deg: np.ndarray,
+    horizontal_deg: np.ndarray,
+    *,
+    min_range_m: float,
+    max_range_m: float,
+) -> np.ndarray:
+    """Return polar ``points_raw`` rows compatible with ``load_npz_xyz``."""
+
+    rows: list[list[float]] = []
+    idx = block_offset + 4
+    for ch in range(CHANNELS):
+        raw = read_u16_be(packet, idx)
+        intensity = float(packet[idx + 2])
+        idx += 3
+        if raw == 0:
+            continue
+        d_mm = float(raw) * unit_mm
+        d_m = d_mm * 0.001
+        if d_m < min_range_m or d_m > max_range_m:
+            continue
+        az_ch = normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch])))
+        rows.append([d_mm, az_ch, float(vertical_deg[ch]), intensity, float(ch)])
+    if not rows:
+        return np.zeros((0, 5), dtype=np.float32)
+    return np.asarray(rows, dtype=np.float32)
+
+
+def iter_h32_frames_polar_from_packets(
     packets: Iterable[bytes],
     *,
     host_utc_ticks: Sequence[int] | None = None,
@@ -154,50 +204,35 @@ def iter_h32_frames_from_packets(
     max_points_per_frame: int | None = None,
     vertical_deg: np.ndarray | None = None,
     horizontal_deg: np.ndarray | None = None,
-) -> list[LidarFrameExport]:
+) -> list[LidarFramePolarExport]:
-    """Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap.
+    """Assemble raw MSOP packets into polar frames for the combined contract."""
-
-    ``host_utc_ticks`` is optional per-packet ``HostReceiveUtcTicks`` from the
-    MSOP DLog payload (UTC DateTime ticks). When provided, each emitted frame
-    carries host receive start/end ticks from the first/last contributing packet.
-    """
 
     vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
     horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
     if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
         raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
 
-    packet_list = list(packets)
+    frames: list[LidarFramePolarExport] = []
-    host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
-    if host_list is not None and len(host_list) != len(packet_list):
-        raise ValueError(
-            f"host_utc_ticks length {len(host_list)} != packets length {len(packet_list)}"
-        )
-
-    frames: list[LidarFrameExport] = []
     point_chunks: list[np.ndarray] = []
     t_start: float | None = None
     t_end: float | None = None
-    host_start: int | None = None
+    host_ns = 0
-    host_end: int | None = None
     prev_az: float | None = None
     kept = 0
     stride = max(1, int(frame_stride))
+    host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
 
     def emit() -> None:
-        nonlocal point_chunks, t_start, t_end, host_start, host_end, kept
+        nonlocal point_chunks, t_start, t_end, host_ns, kept
         if not point_chunks or t_start is None or t_end is None:
             point_chunks = []
             t_start = t_end = None
-            host_start = host_end = None
             return
         points = np.vstack(point_chunks)
         point_chunks = []
         start_s, end_s = t_start, t_end
-        h0 = int(host_start or 0)
+        frame_host = host_ns
-        h1 = int(host_end or 0)
         t_start = t_end = None
-        host_start = host_end = None
         if points.shape[0] < min_frame_points:
             return
         if kept % stride != 0:
@@ -210,20 +245,135 @@ def iter_h32_frames_from_packets(
         if end_s <= start_s:
             end_s = start_s + 0.1
         frames.append(
-            LidarFrameExport(
+            LidarFramePolarExport(
                 t_start_s=start_s,
                 t_end_s=end_s,
-                points_xyz=points,
+                points_raw=points.astype(np.float32, copy=False),
-                host_receive_utc_ticks_start=h0,
+                host_receive_utc_ns=int(frame_host),
-                host_receive_utc_ticks_end=h1,
             )
         )
 
-    for index, packet in enumerate(packet_list):
+    for index, packet in enumerate(packets):
+        if len(packet) != PACKET_LENGTH:
+            continue
+        packet_t = device_timestamp_ms(packet) * 1e-3
+        unit = distance_unit_mm(packet)
+        if host_list is not None and index < len(host_list):
+            chunk_host = ticks_to_unix_ns(int(host_list[index]))
+        else:
+            chunk_host = 0
+        idx = DATA_START
+        for _block in range(BLOCKS):
+            if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
+                break
+            az = read_u16_be(packet, idx + 2) * 0.01
+            if prev_az is not None and prev_az > 270.0 and az < 90.0:
+                emit()
+            prev_az = az
+            pts = _block_points_raw(
+                packet,
+                idx,
+                az,
+                unit,
+                vertical,
+                horizontal,
+                min_range_m=min_range_m,
+                max_range_m=max_range_m,
+            )
+            if pts.shape[0]:
+                if t_start is None:
+                    t_start = packet_t
+                t_end = packet_t
+                host_ns = chunk_host
+                point_chunks.append(pts)
+            idx += BLOCK_LENGTH
+
+    emit()
+    return frames
+
+
+def iter_h32_frames_polar(
+    capture: CaptureFile,
+    *,
+    min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
+    frame_stride: int = 1,
+    min_range_m: float = 0.3,
+    max_range_m: float = 120.0,
+    max_points_per_frame: int | None = None,
+    vertical_deg: np.ndarray | None = None,
+    horizontal_deg: np.ndarray | None = None,
+) -> list[LidarFramePolarExport]:
+    """Assemble MSOP packets from a V2 .rscap into polar frames."""
+
+    packets = [chunk.raw for chunk in capture.chunks]
+    host_ticks = [chunk.receive_utc_ticks for chunk in capture.chunks]
+    return iter_h32_frames_polar_from_packets(
+        packets,
+        host_utc_ticks=host_ticks,
+        min_frame_points=min_frame_points,
+        frame_stride=frame_stride,
+        min_range_m=min_range_m,
+        max_range_m=max_range_m,
+        max_points_per_frame=max_points_per_frame,
+        vertical_deg=vertical_deg,
+        horizontal_deg=horizontal_deg,
+    )
+
+
+def iter_h32_frames(
+    capture: CaptureFile,
+    *,
+    min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
+    frame_stride: int = 1,
+    min_range_m: float = 0.3,
+    max_range_m: float = 120.0,
+    max_points_per_frame: int | None = None,
+    vertical_deg: np.ndarray | None = None,
+    horizontal_deg: np.ndarray | None = None,
+) -> list[LidarFrameExport]:
+    """Assemble MSOP packets into frames using the 270°→90° azimuth wrap."""
+
+    vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
+    horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
+    if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
+        raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
+
+    frames: list[LidarFrameExport] = []
+    point_chunks: list[np.ndarray] = []
+    t_start: float | None = None
+    t_end: float | None = None
+    prev_az: float | None = None
+    kept = 0
+    stride = max(1, int(frame_stride))
+
+    def emit() -> None:
+        nonlocal point_chunks, t_start, t_end, kept
+        if not point_chunks or t_start is None or t_end is None:
+            point_chunks = []
+            t_start = t_end = None
+            return
+        points = np.vstack(point_chunks)
+        point_chunks = []
+        start_s, end_s = t_start, t_end
+        t_start = t_end = None
+        if points.shape[0] < min_frame_points:
+            return
+        if kept % stride != 0:
+            kept += 1
+            return
+        kept += 1
+        if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
+            select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
+            points = points[select]
+        if end_s <= start_s:
+            end_s = start_s + 0.1
+        frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points))
+
+    for chunk in capture.chunks:
+        packet = chunk.raw
         if len(packet) != PACKET_LENGTH:
             continue
         packet_t = device_timestamp_ms(packet) * 1e-3
-        packet_host = int(host_list[index]) if host_list is not None else 0
         unit = distance_unit_mm(packet)
         idx = DATA_START
         for _block in range(BLOCKS):
@@ -246,39 +396,9 @@ def iter_h32_frames_from_packets(
             if pts.shape[0]:
                 if t_start is None:
                     t_start = packet_t
-                    host_start = packet_host
                 t_end = packet_t
-                host_end = packet_host
                 point_chunks.append(pts)
             idx += BLOCK_LENGTH
 
     emit()
     return frames
-
-
-def iter_h32_frames(
-    capture: CaptureFile,
-    *,
-    min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
-    frame_stride: int = 1,
-    min_range_m: float = 0.3,
-    max_range_m: float = 120.0,
-    max_points_per_frame: int | None = None,
-    vertical_deg: np.ndarray | None = None,
-    horizontal_deg: np.ndarray | None = None,
-) -> list[LidarFrameExport]:
-    """Assemble MSOP packets from a V2 .rscap capture into frames."""
-
-    packets = [chunk.raw for chunk in capture.chunks]
-    host_ticks = [chunk.receive_utc_ticks for chunk in capture.chunks]
-    return iter_h32_frames_from_packets(
-        packets,
-        host_utc_ticks=host_ticks,
-        min_frame_points=min_frame_points,
-        frame_stride=frame_stride,
-        min_range_m=min_range_m,
-        max_range_m=max_range_m,
-        max_points_per_frame=max_points_per_frame,
-        vertical_deg=vertical_deg,
-        horizontal_deg=horizontal_deg,
-    )