添加 LiDAR-IMU 外参标定流水线与说明文档
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1,194 @@
|
||||
# LiDAR–IMU 外参标定说明
|
||||
|
||||
本文说明本仓库采用的标定目标、约定、流水线与实现状态。
|
||||
代码位于本仓库的 `imu_lidar/` 包。场地采集细则可另见项目侧的采集需求文档。
|
||||
|
||||
---
|
||||
|
||||
## 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`，不当交付 |
|
||||
| 设备时间新车数据正式验收 | **待做** |
|
||||
| 原生存储一键导出为中间格式 | **待完善** |
|
||||
|
||||
细项见 [`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) |
|
||||
@@ -0,0 +1,69 @@
|
||||
# V1 标准中间数据格式
|
||||
|
||||
第一版流水线**不直接读** dlog / rscap。请先把数据整理成下列格式。
|
||||
|
||||
## IMU
|
||||
|
||||
文件：`imu.csv` 或 `imu.npz`
|
||||
|
||||
### CSV
|
||||
|
||||
```text
|
||||
t,gx,gy,gz,ax,ay,az
|
||||
0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81
|
||||
...
|
||||
```
|
||||
|
||||
| 列 | 含义 | 单位 |
|
||||
|---|---|---|
|
||||
| t | IMU 时钟时间 | s |
|
||||
| gx,gy,gz | 角速度 | rad/s |
|
||||
| ax,ay,az | 比力/加速度 | m/s² |
|
||||
|
||||
### 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
|
||||
0,frames/frame_00000.npz,10.000,10.100
|
||||
1,frames/frame_00001.npz,10.100,10.200
|
||||
```
|
||||
|
||||
也兼容旧列名 `file`（NumPy 读取时可能变成 `file_`）。
|
||||
|
||||
### 每帧 NPZ
|
||||
|
||||
- `points`: `float64/float32`，形状 `(N, 3)`，LiDAR 直角坐标系，单位米
|
||||
|
||||
## 时间不同步能不能用？
|
||||
|
||||
可以，前提是：
|
||||
|
||||
1. 两边都覆盖同一段**有角速度激励**的物理运动（尤其是转弯）；
|
||||
2. 偏置近似为**常数**（短会话）；
|
||||
3. `--time-offset-search-s` 足够覆盖可能的偏移（默认 ±1 s，可加大）。
|
||||
|
||||
若两段数据完全不是同一趟行驶，或只有静止 IMU、没有重叠运动，则无法估 δt，标定会被 `blocked`。
|
||||
|
||||
## 最小可用会话
|
||||
|
||||
- IMU：建议含静止段 + 运动段，采样率稳定
|
||||
- LiDAR：建议 ≥ 20 帧，场景有墙/柱等结构，含转弯
|
||||
@@ -0,0 +1,156 @@
|
||||
# 纯 LiDAR–IMU：标定流程与采集清单
|
||||
|
||||
仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐流程。
|
||||
目标外参：`p_IMU = T_IMU_lidar · p_lidar`（`T_A_B` 表示把 B 系点变到 A 系）。
|
||||
|
||||
算法实现与命令见根目录 `[README.md](../README.md)`；改动史见 `[imu_lidar/CHANGELOG.md](../imu_lidar/CHANGELOG.md)`。
|
||||
|
||||
---
|
||||
|
||||
## 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 → 验证后交付
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user