350 lines
14 KiB
Markdown
350 lines
14 KiB
Markdown
# LiDAR–IMU 外参标定
|
||
|
||
用车上的**激光雷达（LiDAR）**与**惯性测量单元（IMU）**在连续行驶中的相对运动，估计二者之间的安装关系（外参），并估计两路传感器的时间差。
|
||
|
||
```text
|
||
p_IMU = T_IMU_lidar · p_lidar
|
||
```
|
||
|
||
实现包：`[imu_lidar/](imu_lidar/)`。文件职责说明：`[imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md)`。
|
||
|
||
本仓库**只有这一条标定路径**（连续运动关键帧 + IMU 预积分）。不直接解析原始 `.rscap` / dlog，需先导出为中间格式。
|
||
|
||
---
|
||
|
||
## 0. 现状一览（先读）
|
||
|
||
|
||
| 项 | 说明 |
|
||
| ------------- | ------------------------------------------------------------------------ |
|
||
| **仓库做什么** | 输入标准中间格式（IMU CSV + 雷达会话），输出 LiDAR–IMU **正式安装外参** `T_IMU_lidar` 与时间偏置 `δt`。合格数据且过质量门 → 可交付（常规先交旋转 + δt；激励够再交可观平移）；数据不合格或未过门 → `blocked`，不交外参 |
|
||
| **当前阶段** | 算法链路已闭环，合成自检通过；**尚缺合格实车数据验收**，故正式外参尚未对实车落盘交付（生产级原始录制→中间格式导出仍待完善） |
|
||
| **合成数据测什么** | 证明链路能跑通，并能收回**已知**旋转（yaw）与 δt；**不证明**实车安装精度或平移交付 |
|
||
| **旧车 S2 测什么** | 仅线下烟测：主机时间旧数据上流水线能否跑完；结果预期为 `blocked`，**不当作外参真值** |
|
||
| **合格数据** | 见下方定义；拿到后即可按正式标定流程跑通，期望得到可交付外参（先旋转 + δt） |
|
||
|
||
|
||
|
||
|
||
### 什么叫合格数据
|
||
|
||
同时满足下列条件，才视为可用于正式标定（与旧车主机时间烟测数据区分）：
|
||
|
||
|
||
| 类别 | 要求 |
|
||
| ------------ | ----------------------------------------------------------------------------- |
|
||
| **时间戳** | IMU、雷达均用**设备时间**写入中间格式的 `t` / `t_start`/`t_end`（可另留主机接收时间作排查，但标定主轴不用主机时） |
|
||
| **会话结构** | 单趟：`静止 20–30 s` → `低速连续运动 3–8 min` → `再静止 10–20 s`；运动含明显左右转（如「8」字 / 圆）及适量加减速 |
|
||
| **场景** | 有墙、柱、路缘等结构，避免空旷无特征；雷达帧率与点数正常、无明显大面积丢帧 |
|
||
| **覆盖** | 两路传感器覆盖**同一段**物理行驶；安装全程不得改动 |
|
||
| **格式** | 已导出本仓库中间格式：`imu.csv` + `lidar_session/`（见 [docs/V1_数据格式.md](docs/V1_数据格式.md)） |
|
||
| **验证会话（建议）** | 另录一段独立路线 B，只做叠点/一致性检查，不参与求外参 |
|
||
|
||
|
||
**不算合格（举例）：** 仅主机接收时间；只有静止、无转弯；IMU 与雷达不是同一趟；未导出中间格式；时间相关极弱仍强行当安装参数。
|
||
|
||
完整现场清单见 [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md)。
|
||
|
||
### 合格数据拿到后 → 正式外参与预期效果
|
||
|
||
仓库目标产物就是**可安装使用的外参**（再加 δt）。合格数据过门后：
|
||
|
||
|
||
| 模式 | 跑法 | 正式交付内容 | 较现实预期 | 成功标志 |
|
||
| ----------------------- | ---------------------- | ----------- | --------------------------------- | ----------------------------------------- |
|
||
| `rotation_only`（第一步，必做） | `--mode rotation_only` | 旋转外参 + δt | 旋转约 **0.5°–2°**；δt 随同步与激励质量变化 | `summary.json` → `rotation_only_accepted` |
|
||
| `full_se3`（激励够再试） | `--mode full_se3` | 上一项 + 可观的平移 | 水平平移约数厘米～十几厘米；竖直常更差或不可观（需缓坡或垂直先验） | `full_se3_accepted`；否则旋转仍可交付、平移拒绝 |
|
||
|
||
|
||
交付前须叠点云 / 跨会话独立验证；`blocked` 一律不可用作安装参数。
|
||
**当前尚未完成的是「用合格实车数据跑出并验收这份外参」**，不是「合格数据也交不出外参」。
|
||
|
||
### 两类试验对照
|
||
|
||
|
||
| | 合成数据（默认 pytest / 一键复现） | 旧车 S2（线下，非默认测试） |
|
||
| ------- | -------------------------------------- | ---------------- |
|
||
| 目的 | 回归算法正确性 | 验证实车 IO/链路可跑 |
|
||
| 时间戳 | 可控、带已知 δt | 主机接收时间（非设备时间） |
|
||
| 是否合格数据 | 否（合成，仅对照） | **否**（主机时间，预期拒绝） |
|
||
| 期望结果 | `rotation_only_accepted`；δt / yaw 接近真值 | `blocked`（质量门拒绝） |
|
||
| 能否当安装参数 | 否 | **否** |
|
||
|
||
|
||
细节见 [tests/README.md](tests/README.md)。实现状态细表见 [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) §8。
|
||
|
||
---
|
||
|
||
|
||
|
||
## 目录
|
||
|
||
1. [现状一览（先读）](#0-现状一览先读)
|
||
2. [系统输入 / 输出](#1-系统输入--输出)
|
||
3. [一键复现（合成数据）](#2-一键复现合成数据)
|
||
4. [能求出什么](#3-能求出什么)
|
||
5. [方法概要](#4-方法概要)
|
||
6. [分步求值](#5-分步求值)
|
||
7. [程序流程](#6-程序流程)
|
||
8. [真实数据怎么跑](#7-真实数据怎么跑)
|
||
9. [结果判读](#8-结果判读)
|
||
10. [仓库结构与文档](#9-仓库结构与文档)
|
||
|
||
---
|
||
|
||
|
||
|
||
## 1. 系统输入 / 输出
|
||
|
||
|
||
|
||
### 输入
|
||
|
||
|
||
| 输入 | 说明 |
|
||
| --------- | --------------------------------------------------------------------- |
|
||
| `imu.csv` | 列：`t,gx,gy,gz,ax,ay,az`（秒；陀螺 rad/s；加速度 m/s²） |
|
||
| 雷达点云 | `frames_index.csv` + `frames/frame_XXXXX.npz`（点云米制） |
|
||
| 车辆配置 YAML | 轴向与时间语义；**不必事先填外参真值**（模板：`config/vehicle_installation.template.yaml`） |
|
||
|
||
|
||
中间格式细节见 [docs/V1_数据格式.md](docs/V1_数据格式.md)。原始 `.rscap` 等需先导出。
|
||
|
||
### 输出（`--output` 目录）
|
||
|
||
|
||
| 文件 | 内容 |
|
||
| -------------------- | ---------------------------- |
|
||
| `T_IMU_lidar.json` | 外参矩阵、平移、四元数、欧拉角 |
|
||
| `time_offset.json` | δt，定义 `t_imu = t_lidar + δt` |
|
||
| `summary.json` | 状态、残差、可观性、各阶段细节 |
|
||
| `report_preview.png` | 一键复现时生成的简易指标图（可选） |
|
||
|
||
|
||
---
|
||
|
||
|
||
|
||
## 2. 一键复现（合成数据）
|
||
|
||
不需要实车数据。脚本会：**生成合成 IMU/雷达 → 跑标定 → 打印报告并出预览图 → 跑 pytest**。
|
||
|
||
**这一趟证明什么：** 全流程可复现；能收回合成真值中的旋转（yaw）与时间偏置。
|
||
**不证明什么：** 实车安装精度、平移外参可交付、主机时间旧数据可用。
|
||
|
||
### 推荐（Windows）
|
||
|
||
```powershell
|
||
cd <本仓库根目录>
|
||
python -m pip install -e ".[dev]"
|
||
python -m pip install -e ".[open3d]" # 可选，配准更稳；未装则用内置 ICP
|
||
# 预览图需要 matplotlib（没有则跳过绑图，不影响标定）
|
||
python -m pip install matplotlib
|
||
|
||
powershell -File tools\reproduce_synthetic.ps1
|
||
```
|
||
|
||
只要标定、跳过测试：
|
||
|
||
```powershell
|
||
powershell -File tools\reproduce_synthetic.ps1 -SkipPytest
|
||
```
|
||
|
||
尝试完整六自由度模式：
|
||
|
||
```powershell
|
||
powershell -File tools\reproduce_synthetic.ps1 -Mode full_se3 -SkipPytest
|
||
```
|
||
|
||
|
||
|
||
### 等价 Python 入口
|
||
|
||
```powershell
|
||
python tools\reproduce_synthetic.py
|
||
python tools\reproduce_synthetic.py --mode full_se3 --skip-pytest
|
||
```
|
||
|
||
|
||
|
||
### 这一趟用了什么、产出什么
|
||
|
||
```text
|
||
INPUT
|
||
examples/synthetic_session/imu.csv
|
||
examples/synthetic_session/lidar/
|
||
config/vehicle_installation.template.yaml
|
||
examples/synthetic_session/meta.json # 合成真值（δt、yaw），仅用于对照
|
||
|
||
OUTPUT
|
||
examples/synthetic_session/out/T_IMU_lidar.json
|
||
examples/synthetic_session/out/time_offset.json
|
||
examples/synthetic_session/out/summary.json
|
||
examples/synthetic_session/out/report_preview.png
|
||
```
|
||
|
||
单独查看已有结果：
|
||
|
||
```powershell
|
||
python tools\show_calibration_report.py `
|
||
--summary examples\synthetic_session\out\summary.json `
|
||
--truth-meta examples\synthetic_session\meta.json `
|
||
--plot examples\synthetic_session\out\report_preview.png
|
||
```
|
||
|
||
|
||
|
||
### 配准结果 3D / 俯视可视化
|
||
|
||
蓝=目标帧，橙=源帧。
|
||
|
||
|
||
| 按键 | 作用 |
|
||
| --------------------- | ------------------------------------- |
|
||
| `1` / `2` / `3` / `4` | 原始 / IMU(X=I) / 雷达配准 B / 标定预测 `X⁻¹AX` |
|
||
| `N` 或 `]` | 下一运动对 |
|
||
| `P` 或 `[` | 上一运动对 |
|
||
|
||
|
||
```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
|
||
```
|
||
|
||
`--pair-index` 只决定起始对；窗口内可继续按 `N`/`P` 切换。无 GUI 时加 `--no-gui`。封装：`tools\view_pair.ps1`。
|
||
|
||
---
|
||
|
||
|
||
|
||
## 3. 能求出什么
|
||
|
||
|
||
| 结果 | 含义 | 说明 |
|
||
| ------- | ------------- | ------------------ |
|
||
| 旋转外参 | 雷达相对 IMU 的朝向差 | 常规主交付 |
|
||
| 时间偏置 δt | 两路时钟差（秒） | 与旋转一并给出 |
|
||
| 平移外参 | 安装位置差（米） | 激励足够且质量门通过才接受，否则置零 |
|
||
| 零偏等 | 辅助量 | 改善积分/优化，一般不当安装外参交付 |
|
||
|
||
|
||
---
|
||
|
||
|
||
|
||
## 4. 方法概要
|
||
|
||
1. 低速多转弯行驶（如「8」字），首尾各留静止段。
|
||
2. 雷达关键帧配准得相对运动 **B**。
|
||
3. 同一时段 IMU 预积分得 **A**（先用旋转）。
|
||
4. 手眼关系：`R_A · R_X ≈ R_X · R_B`。
|
||
5. 时间与旋转稳定后，若 `--mode full_se3` 且可观，再估计平移（用加速度预积分、重力、速度与时变零偏）。
|
||
|
||
---
|
||
|
||
|
||
|
||
## 5. 分步求值
|
||
|
||
|
||
| 步骤 | 交付 | 典型要求 |
|
||
| --- | ------- | ---------------------- |
|
||
| 第一步 | 旋转 + δt | 设备时间戳；静止 + 低速转弯；有结构的场景 |
|
||
| 第二步 | + 水平平移 | 更多转弯半径与加减速 |
|
||
| 第三步 | 完整六自由度 | 缓坡等俯仰激励，或垂直安装尺寸先验 |
|
||
|
||
|
||
- `--mode rotation_only`：第一步
|
||
- `--mode full_se3`：尝试完整刚体；平移仍可能被拒绝
|
||
|
||
质量门未通过时，不能当正式安装参数。
|
||
|
||
---
|
||
|
||
|
||
|
||
## 6. 程序流程
|
||
|
||
```text
|
||
读入 → 时间/IMU 质检 → 粗估 δt
|
||
→ 关键帧配准 B + IMU 预积分 A → 旋转手眼 R
|
||
→ 用 R 精修 δt（可交替组对）
|
||
→ 联合精修 R / 常值陀螺零偏
|
||
→（full_se3 且可观）重力、速度、时变零偏、平移 t
|
||
→ 写出 JSON
|
||
```
|
||
|
||
「可观」：程序检查转轴是否够多样、相对位移是否够丰富；不够则只保留旋转。
|
||
|
||
---
|
||
|
||
|
||
|
||
## 7. 真实数据怎么跑
|
||
|
||
```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
|
||
```
|
||
|
||
采集与现场清单：[docs/标定流程与采集清单.md](docs/标定流程与采集清单.md)。
|
||
|
||
旧车 S2 主机时间烟测（预期 `blocked`，不当交付）：配置见 `config/s2_old_smoke.yaml`，步骤见 `[tests/README.md](tests/README.md)`。
|
||
|
||
---
|
||
|
||
|
||
|
||
## 8. 结果判读
|
||
|
||
|
||
| `summary.json` 状态 | 含义 |
|
||
| ---------------------------------------- | ----------------- |
|
||
| `rotation_only_accepted` | 旋转可用（该模式下平移置零） |
|
||
| `full_se3_accepted` | 含平移的完整外参通过 |
|
||
| `full_se3_rejected_due_to_observability` | 旋转可用，平移未接受 |
|
||
| `blocked` | 前序失败，**不可用作安装参数** |
|
||
|
||
|
||
请同时看手眼残差、时间相关、可观性说明，并做点云叠加等独立验证。
|
||
|
||
---
|
||
|
||
|
||
|
||
## 9. 仓库结构与文档
|
||
|
||
```text
|
||
imu_lidar/ 标定算法与 CLI
|
||
config/ 车辆配置模板
|
||
docs/ 流程、数据格式、方法说明
|
||
tools/ 合成数据、一键复现、报告查看
|
||
tests/ 自动化测试
|
||
```
|
||
|
||
|
||
| 文档 | 内容 |
|
||
| ------------------------------------------------ | ---------------------- |
|
||
| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 各源文件职责 |
|
||
| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 算法改动记录 |
|
||
| [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场采集与分步流程（与现行实现对齐） |
|
||
| [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间数据格式 |
|
||
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 方法约定与实现状态 |
|
||
| [tests/README.md](tests/README.md) | 自动化测试说明 + S2 旧数据线下试验结果 |
|
||
|
||
|
||
改算法请同步更新 `imu_lidar/文件职责说明.md` 与 `CHANGELOG.md`；改安装/运行/对外说明时更新本文件。 |