添加 LiDAR-IMU 外参标定流水线与说明文档
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1,149 @@
|
||||
# `imu_lidar` 改动记录
|
||||
|
||||
本文件专门记录 `imu_lidar` 目录内的实现改动。
|
||||
每条包含：**时间戳**、**改动内容**（以「原本怎么做 → 改成怎么做」书写）。
|
||||
|
||||
---
|
||||
|
||||
## 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)
|
||||
|
||||
### 标题
|
||||
|
||||
- **原本**：...
|
||||
- **改成**：...
|
||||
```
|
||||
Reference in New Issue
Block a user