# LiDAR–双天线 RTK 手眼标定

本仓库提供一套可从原始 Medulla 记录复现的静态站点标定流程，求解三维激光雷达到后轮轴中心车体系的外参

```text
X = T_body_lidar
```

约定 `T_A_B` 将 B 系坐标变换到 A 系。对任意站点 i、j：

```text
A_ij = T_W_Bi^-1 T_W_Bj       # RTK 给出的车体相对运动
B_ij = T_Li_Lj                # GICP 给出的雷达相对运动
A_ij X = X B_ij
```

当前部署建议仍采用 [results/01_previous_two_batches/final_extrinsic_deployment.json](results/01_previous_two_batches/final_extrinsic_deployment.json)。data4 是一次独立重算，结果与部署值相差约 `1.592 cm / 0.234°`，但自身 AX 残差更高，因此只作为候选和稳定性证据，不自动替换部署值。

## 标定总流程

```mermaid
flowchart LR
  raw["原始站点 dlog / RTK、IMU rscap"] --> export["分别解析并统一到时间轴"]
  export --> assoc["按每个 LiDAR 帧关联 RTK/IMU，导出 NPZ"]
  assoc --> prep["每站选一帧，构建 RTK 车体位姿 A"]
  prep --> b1["small_gicp 求 B"]
  prep --> b2["Open3D GICP 求 B"]
  b1 --> gate["与 X 无关的质量筛选及双后端一致性"]
  b2 --> gate
  gate --> solve["AX=XB + 地面约束求 X"]
  solve --> check["残差、bootstrap、条件数、跨批复核和 3D 可视化"]
```

流程有两个原始数据入口：

- 旧式数据：LiDAR 和 `GPS-POST-Z` 位于每个站点 dlog 中，使用 `export_legacy_stations.ps1`。
- 新式多传感器数据：LiDAR 位于逐站 dlog，RTK 与 IMU 是独立 `.rscap`，使用 `export_multisensor_stations.ps1`。处理顺序是统一时间轴、分别解析、按 LiDAR 帧关联、导出 NPZ。

IMU 会在新式数据中原样解析并随 LiDAR 帧关联保存，但当前 LiDAR–RTK 外参求解不使用 IMU，也不做运动畸变校正，因为每一站采集点云时车辆静止。IMU 外参应使用单独的激励数据和专用标定流程求解。

## 三批数据的角色

| 数据 | 原始格式 | 站点 | RTK 情况 | 在本仓库中的角色 |
|---|---|---:|---|---|
| 第一批 | 逐站 dlog，内嵌 GPS-POST-Z | 38 | 约 10 秒一条，部分站仅 1–11 个有效样本 | 辅助复核，不承担主要求解 |
| 第二批 | 逐站 dlog，内嵌 GPS-POST-Z | 38 | 每站约 125–412 个有效样本，航向稳定 | 现部署外参的主要求解数据 |
| data4 | 逐站 LiDAR dlog + 独立 RTK/IMU rscap | 34 | 11,678 个 LiDAR 帧均成功关联 fixed RTK、heading 和 IMU | 独立重算与跨批比较 |

原始数据体积较大，不在 Git 仓库中。复现者应从云盘取得第一批、第二批或 data4 的原始目录，并在命令行传入路径。

## 环境

- Windows PowerShell 5.1 或 PowerShell 7
- Python 3.10+
- `pip install -r requirements.txt`
- `small_gicp` 后端需要可导入 `small_gicp`；Open3D 后端需要 `open3d`

所有脚本从自身位置推导仓库根目录。数据和输出路径均由参数传入，不依赖开发者电脑上的固定路径。

## 从原始数据开始复现

以下路径只表示格式，请替换为自己的目录。

### A. 第一批、第二批旧式 dlog

```powershell
$Repo = "D:\你的代码目录\calibration"

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_legacy_stations.ps1" `
  -DataRoot "D:\你的数据目录\batch2_raw" `
  -OutputRoot "D:\你的输出目录\batch2_export"

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_legacy_dataset.ps1" `
  -ExportRoot "D:\你的输出目录\batch2_export" `
  -Output "D:\你的输出目录\batch2_prepared" `
  -HeadingOffsetDeg 21.226 `
  -AntennaLever -0.320,-0.365,0.620 `
  -ExpectedStations 38 `
  -HeadingStdLimitDeg 0.5
```

第一批采用同一导出方式，但应在导出命令显式添加 `-RtkMaxDtMs 15000`；因其 RTK 稀疏，准备阶段也不建议沿用 `0.5°` 的严格站内航向离散度阈值。上述 `21.226°` 和杆臂 `[-0.320,-0.365,0.620] m` 是本项目已有两批数据采用的配置，不是通用常数；换车或改变天线安装后必须重新确认。

### B. data4 式独立 RTK/IMU rscap

```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_multisensor_stations.ps1" `
  -DataRoot "D:\你的数据目录\data4_raw" `
  -RtkCapture "D:\你的数据目录\captures\rtk.rscap" `
  -ImuCapture "D:\你的数据目录\captures\imu.rscap" `
  -OutputRoot "D:\你的输出目录\data4_export"

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_multisensor_dataset.ps1" `
  -CombinedRoot "D:\你的输出目录\data4_export\combined" `
  -Output "D:\你的输出目录\data4_prepared" `
  -HeadingOffsetDeg 21.226 `
  -AntennaLever -0.320,-0.365,0.620 `
  -ExpectedStations 34
```

### C. 运行标定

单批数据同时运行 small_gicp、Open3D GICP 和跨后端共识：

```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_single_dataset.ps1" `
  -Prepared "D:\你的输出目录\data4_prepared" `
  -OutputRoot "D:\你的输出目录\data4_calibration" `
  -BodyHeight 0.2335
```

复现本仓库“第二批求解、第一批辅助复核”的历史流程：

```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" `
  -Batch1Prepared "D:\你的输出目录\batch1_prepared" `
  -Batch2Prepared "D:\你的输出目录\batch2_prepared" `
  -OutputRoot "D:\你的输出目录\two_batch_calibration"
```

`BodyHeight=0.2335 m` 是后轮轴中心离地高度，用于把雷达地面平面约束转换到车体原点；它不是雷达离地高度。换轮胎、胎压或车体载荷后应重新测量并评估不确定度。

## 如何判断结果

不能仅凭 `AX=XB` 残差或一张叠图宣称外参正确。至少同时检查：

1. B 的配准质量：收敛、有效对应点数/比例、RMSE、Hessian 信息矩阵特征值与条件数、small_gicp 与 Open3D 的相对运动一致性。
2. X 的可观性：加权雅可比条件数越接近 1 越均衡；极大值说明存在弱方向，但没有脱离尺度和数据分布的单一硬阈值。本次 data4 为 `7.44`，不是病态。
3. AX 残差分布：看 RMS、median、P90/P95、max 和逐对异常，而不是只看均值。
4. 重采样稳定性：bootstrap 的六自由度标准差及置信区间。
5. 跨批检查：同一 X 在独立批次的残差，以及两批独立求出的 X 之 SE(3) 差。
6. 3D 可视化：模式 3 的 B 与模式 4 的 `X^-1 A X` 是否都使相同墙面、杆件和地面重合。

当前没有把“增量小于某个值”当成绝对真值判据。工程筛查可先关注多数优质运动对是否约在厘米级到数厘米、亚度级；但阈值必须结合场景尺度、点云分辨率、RTK 质量和车辆运动幅度制定。模式 3 已错位时优先检查 B；模式 3 正常而模式 4 系统性错位时，再检查 A、坐标约定和 X。

## 结果摘要

| 结果 | 平移 xyz (m) | roll/pitch/yaw (deg) | AX 平移/旋转 RMS | 结论 |
|---|---|---|---|---|
| 部署值（第二批求解） | `[1.297760,-0.000067,0.720498]` | `[-0.785151,1.202661,-0.835510]` | `0.07985 m / 0.96118°`，39 对 | 当前建议部署 |
| data4 独立候选 | `[1.300376,-0.001707,0.704877]` | `[-0.791892,1.393823,-0.970743]` | `0.11762 m / 1.24257°`，26 对 | 独立候选，不替换部署值 |

data4 候选相对部署值变化 `1.592 cm / 0.234°`，其中 z 低 `1.562 cm`。旧部署值作用于 data4 的残差约 `0.11953 m / 1.24836°`；data4 候选作用于历史第二批的残差约 `0.07931 m / 0.98926°`。两个 X 的跨批表现接近，当前数据不足以证明 data4 的较低 z 更接近真值。

## 仓库目录

```text
code/       标定、配准筛选、共识、比较和可视化核心程序
tools/      原始 dlog/rscap 解析、时间关联、NPZ 导出和数据准备
run/        不含本机固定路径的 PowerShell 入口
results/    历史两批、data4 与跨批比较三个结果目录
```

完整复现流程和所有主要文件职责均在本 README；`run/README.md`、`tools/README.md` 和 [results/README.md](results/README.md) 只是目录内快速索引。

## 代码、工具和运行入口职责

根 README 是本仓库唯一的完整复现说明。`run/README.md`、`tools/README.md` 和 `results/README.md` 只作为进入对应目录时的快速索引，不承载另一套流程。

### code：标定核心

| 文件 | 职责 |
|---|---|
| `rigorous_calibration.py` | 核心 CLI。`ground` 拟合每站地面；`pairs` 用 small_gicp 或 Open3D GICP 求 B 和质量指标；`calibrate` 联合 AX=XB 与地面约束求 X；`validate` 计算指定 X 的逐对残差。 |
| `refine_pairs.py` | 根据收敛、RMSE、对应关系、Hessian/信息矩阵和运动覆盖筛选 B；不读取 X，避免循环挑选。 |
| `cross_backend_filter.py` | 对齐 Open3D 与 small_gicp 的同一站点对，只保留两个后端相互一致的 B。 |
| `finalize_consensus.py` | 汇总历史两批的 consensus B、外参和第一批辅助复核。 |
| `summarize_results.py` | 汇总两个后端的外参、B 质量和跨批检查，生成推荐结果。 |
| `compare_extrinsics.py` | 在 SE(3) 上计算两套外参的严格相对平移和旋转差。 |
| `visualize_pair_3d.py` | 交互显示原始点云、RTK A、GICP B、`X^-1AX`，并打印 `B^-1(X^-1AX)` 数值增量。 |
| `scan_extrinsic_sensitivity.py` | 对 X 左乘小角度 roll/pitch/yaw 扰动，检查指定运动对的局部敏感方向。 |

### tools：原始数据到 prepared

| 文件 | 职责 |
|---|---|
| `frontlidar_dlog_export.py` | 从 Medulla dlog 导出传感器坐标系 `points_raw`；旧格式可同时匹配站内 GPS-POST-Z。 |
| `prepare_station_dataset.py` | 从旧式逐站导出中每站选择一帧，计算 yaw-only RTK 后轮轴位姿并生成 prepared。 |
| `build_multisensor_npz.py` | 将独立 LiDAR、RTK、IMU 统一到 LiDAR 帧索引并生成 combined NPZ。 |
| `prepare_multisensor_station_dataset.py` | 从 combined NPZ 选择每站静止帧，生成与旧流程相同的 prepared 接口。 |
| `rscap_v2/capture_format_v2.py` | 读取 rscap v2 文件头、原始记录块和文件尾。 |
| `rscap_v2/audit_capture_v2.py` | 审计 capture 完整性、时间范围和记录统计。 |
| `rscap_v2/parse_rtk_imu_v2.py` | 分别解析 RTK 与 IMU capture，输出 JSONL。 |
| `rscap_v2/pipeline_common*.py` | rscap 解析、时间处理和采集格式兼容的共用逻辑。 |

### run：推荐 PowerShell 入口

| 文件 | 职责 |
|---|---|
| `export_legacy_stations.ps1` | 批量导出 LiDAR 与 RTK 同在逐站 dlog 中的旧格式。 |
| `export_multisensor_stations.ps1` | 解析独立 RTK/IMU，导出逐站 LiDAR，并按 LiDAR 帧建立关联。 |
| `prepare_legacy_dataset.ps1` | 旧式导出结果转换为 prepared。 |
| `prepare_multisensor_dataset.ps1` | combined 多传感器结果转换为 prepared。 |
| `run_single_dataset.ps1` | 单批数据同时完成两个 GICP 后端、B 筛选、consensus 和 X 求解。 |
| `run_all.ps1` | 历史流程：第二批求解，第一批稀疏 RTK 数据作辅助复核。 |
| `run_consensus_finish.ps1` | 在已有两个后端 B 的基础上重新生成 consensus 和汇总结果。 |
| `run_sensitivity_scan.ps1` | 对历史运动对执行外参角度灵敏度扫描。 |
| `view_result.ps1` | 传入匹配的 frames、B 和 X，运行交互式 3D 可视化。 |

### 标定核心文件的数据关系

```text
原始 dlog/rscap
  -> tools 导出和时间关联
  -> prepared/{frames_all, body_poses_*.csv}
  -> rigorous_calibration.py pairs 生成 A、B
  -> refine_pairs.py 做与 X 无关的 B 筛选
  -> cross_backend_filter.py 生成 consensus B
  -> rigorous_calibration.py calibrate 联合 AX=XB 与地面约束求 X
  -> validate / visualize_pair_3d.py 做数值和三维检查
```


## 重要限制

- RTK 车体姿态当前是双天线 heading 构造的 yaw-only 轨迹；没有用 RTK pitch/roll 构造 A。
- 新式解析器保存 IMU 与 RTK pitch 等原始字段，但当前手眼方程未融合 IMU。
- 静止站点法不估计 LiDAR–RTK 时间偏移；时间戳关联必须在导出阶段通过审计。
- 地面约束负责 roll、pitch 和 z 的补充可观性，不会独立求出另一套六自由度外参。
- 仓库归档的是结果和轻量 B 文件，不包含云盘中的原始点云数据。

## 专题说明

- [运动对诊断](PAIR_DIAGNOSTICS.md)
- [双后端共识筛选](CONSENSUS_SELECTION.md)
- [结果文件索引](results/README.md)