diff --git a/README.md b/README.md index e751b79..2c38e52 100644 --- a/README.md +++ b/README.md @@ -1,184 +1,65 @@ -# LiDAR–IMU 外参标定 +# 车辆多传感器外参标定 -用连续行驶中的 LiDAR 与 IMU 相对运动，估计安装外参与时间偏置： +本仓库同时维护两条独立的外参标定链路：LiDAR–IMU 与 RTK–IMU。两条链路共享通用的几何和 IMU 预积分基础能力，但各自的观测模型、求解器、工具入口和验收结论独立管理。 + +## 当前应如何使用 + +| 目标 | 推荐链路 | 当前可交付状态 | +| --- | --- | --- | +| LiDAR 与 IMU 的旋转、时间对齐 | LiDAR–IMU | 旋转与主机桥接时间对齐可冻结；完整平移不可交付。 | +| RTK 天线相位中心与 IMU 的工程外参 | RTK–IMU | 机械杆臂经动态数据一致性验证；data-only 平移及正式 engineering 放行尚未通过。 | +| 需要完整 RTK–IMU 工程候选和复现方式 | RTK–IMU | 先阅读 [README_RTK_IMU.md](README_RTK_IMU.md)。 | + +不要把任何一条链路未通过的平移结果当作已标定的 6DoF 外参。结果的接受状态以对应 JSON 中的 gate 为准。 + +## 为什么同时保留两条链路 + +LiDAR–IMU 链路通过点云相对运动与 IMU 预积分求手眼外参，适合旋转与时间对齐；但当前实车数据的加速度预积分位移，尤其 Z 分量，不足以支持可靠平移求解，且主要采集运动缺乏竖直激励。因此它的 `full_se3` 门禁未通过。 + +RTK–IMU 链路直接利用双天线基线、GNSS 位置和 Doppler 速度，并以机械测量给出天线相位中心相对 IMU 的绝对杆臂。它不是“用 RTK 替代 LiDAR”，而是针对 LiDAR–IMU 平移不可观的问题，采用更直接的天线位置/速度观测做工程外参验证。 + +完整理由、两条模型的差异及当前结果见 [LiDAR→RTK–IMU 决策说明](docs/lidar_imu_to_rtk_imu.md)。 + +## 仓库结构 ```text -p_IMU = T_IMU_lidar · p_lidar +calibration/ +├─ imu_lidar/ LiDAR–IMU 专用流程，以及双方共用基础模块 +├─ rtk_imu/ RTK–IMU 专用算法包 +├─ tools/ 两条链路的命令入口、导出和审计工具 +├─ tests/ 自动化测试 +├─ docs/ 详细方法、状态、采集与决策说明 +├─ artifacts/ 受审核的结果快照；大过程文件仅保留本地 +├─ config/ 车辆安装与流程配置 +├─ README.md 本页：仓库总览 +└─ README_RTK_IMU.md RTK–IMU 当前工程流程、结果与复现 ``` -**当前阶段：** 算法与合成自检已闭环；已提供 `tools/export_rscap_to_v1.py`（N300 `.rscap` + H32 dlog/MSOP → 中间格式）；**合格实车验收尚未完成**，故正式外参尚未对实车落盘交付。 +`rtk_imu/` 只依赖下列 `imu_lidar/` 通用模块： ---- +- `contracts.py`：数据契约。 +- `geometry.py`：SO(3)、变换和坐标运算。 +- `geodesy.py`：地理坐标到 ENU。 +- `imu_io.py`：IMU 读取。 +- `imu_preintegration.py`：IMU 预积分、bias 修正与 whitening。 +- `rotation_handeye.py`：通用旋转手眼初值器。 -## 先看什么（对外三份就够） +RTK–IMU 不得依赖 LiDAR 专用的 `lidar_io.py`、`lidar_deskew.py`、`registration.py`、`pipeline.py`、`phase_a.py` 或 `joint_optimizer.py`。 +## 文档入口 -| 顺序 | 文档 | 用途 | -| --- | -------------------------------------- | -------------------- | -| 1 | **本 README** | 做什么、怎么跑、结果怎么判 | -| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + 原始数据导出命令 | -| 3 | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采（合格数据要求） | +| 文档 | 内容 | +| --- | --- | +| [README_RTK_IMU.md](README_RTK_IMU.md) | RTK–IMU 当前候选、状态、流程、审核证据和复现命令。 | +| [docs/lidar_imu_to_rtk_imu.md](docs/lidar_imu_to_rtk_imu.md) | 为什么 LiDAR–IMU 暂不能交平移、为什么引入 RTK–IMU、两者边界与共享代码。 | +| [rtk_imu/README.md](rtk_imu/README.md) | RTK–IMU 包的代码职责与 import 边界。 | +| [docs/rtk_imu_multisource_v3.md](docs/rtk_imu_multisource_v3.md) | G90/HI13 统一导出及 R1b/R2V/R2G/R3 旋转链路。 | +| [docs/rtk_imu_engineering_6dof.md](docs/rtk_imu_engineering_6dof.md) | node-state factor graph、机械先验、验收门禁。 | +| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | LiDAR–IMU 方法与采集要求。 | +| [docs/20260808_LiDAR-IMU标定现状与问题.md](docs/20260808_LiDAR-IMU标定现状与问题.md) | LiDAR–IMU 当前问题、证据和历史处置记录。 | +## 结果与过程文件 -其余（方法细述、测试说明、源码职责、CHANGELOG）给深入阅读 / 改代码时用，见文末。 +`artifacts/rtk_imu_calibration_v3/` 中只提交审核结论及其关键证据。原始 `.rscap`、统一导出数据、节点状态、checkpoint、调试 JSON 与旧版本过程产物均通过 `.gitignore` 保持本地，不应随提交或推送传播。 ---- - - - -## 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 -config/ 车辆配置模板 -docs/ 采集清单、数据格式、方法细述 -tools/ 导出、合成复现、可视化 -tests/ 自动化测试 -``` - - -| 文档 | 何时看 | -| ------------------------------------------------ | ---------------- | -| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 要看方法约定与实现状态表 | -| [tests/README.md](tests/README.md) | 要看合成用例 / S2 烟测记录 | -| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 要改源码 | -| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 要查改动史 | - - -改算法请同步职责说明与 CHANGELOG；改对外用法请更新本 README。 \ No newline at end of file +每次改动前先确认使用哪一条链路；每次交付前先读取相应 release JSON 中的 accepted gate，而不是仅凭优化器收敛、残差下降或一个看似合理的 4×4 矩阵下结论。 diff --git a/docs/lidar_imu_to_rtk_imu.md b/docs/lidar_imu_to_rtk_imu.md new file mode 100644 index 0000000..736fc20 --- /dev/null +++ b/docs/lidar_imu_to_rtk_imu.md @@ -0,0 +1,92 @@ +# 从 LiDAR–IMU 平移困难到 RTK–IMU 工程外参 + +本文解释为什么仓库同时保留 LiDAR–IMU 与 RTK–IMU 两条链路、为什么本轮将天线–IMU 外参的工程验证重心转向 RTK–IMU，以及两条链路如何避免互相混淆。 + +## 决策结论 + +保留两条链路，不删除 LiDAR–IMU。LiDAR–IMU 仍然是雷达与 IMU 旋转/时间关系的有效工具，也记录了平移失败的具体证据；RTK–IMU 是针对“天线相位中心到 IMU 的杆臂”采用的独立观测模型。它们共享基础数学与 IMU 预积分，但不共享求解器或验收结论。 + +当前 RTK–IMU 结果应描述为：**机械测量锚定的杆臂，经 RTK+IMU 动态数据一致性验证的工程候选**。它不是 data-only 平移标定，也不是已正式 engineering 放行的结果。 + +## LiDAR–IMU 为什么暂时不能交平移 + +LiDAR–IMU 链路将相邻关键帧的点云配准相对运动，与同一区间 IMU 预积分运动做手眼/联合优化。当前实车数据中旋转和时间关系可得到稳定结果，但平移受两个独立问题限制： + +1. **IMU 位移预积分不可靠。** 加速度积分对重力与加速度零偏敏感；当前 `Δp` 尤其 Z 分量会发散。已有证据显示点云相对旋转残差中位约 `0.16°`，而 IMU 位移增量模长常大于 `1 m`，说明这不是单纯旋转误差。 +2. **运动激励不足。** 现有重点窗口以水平转弯为主，缺少连续坡道/俯仰激励，竖直杆臂弱可观。即使加入很强的平移先验，求解也可能只是先验回显，而非数据观测。 + +因此 LiDAR–IMU 的状态是：旋转与时间对齐可用，`full_se3`/平移不可作为安装参数交付。详细证据在 [LiDAR–IMU 现状与问题](20260808_LiDAR-IMU标定现状与问题.md) 和 [问题清单](问题清单.md)。 + +## 为什么 RTK–IMU 更适合验证天线杆臂 + +RTK 直接给出天线相位中心的 GNSS 位置与 Doppler 速度；双天线还给出 ANT1→ANT2 基线方向。对固定旋转 `R_WI` 和杆臂 `l_I = p_ANT1^I`，观测模型为： + +```text +p_ANT1^W = p_IMU^W + R_WI * l_I +v_ANT1^W = v_IMU^W + R_WI * ((omega - b_g) × l_I) +``` + +转弯时的 `omega × l_I`、加减速和坡道中的姿态变化，都会直接增强杆臂可观性；这比把加速度二次积分当作唯一位移来源更适合做天线–IMU 一致性检查。 + +双天线基线本身只有 2DoF 方向信息，不能凭空构造完整三轴姿态。本项目采用： + +1. R1b：以基线和 IMU 动态估计 ANT1→ANT2 在 IMU 中的 2DoF 方向。 +2. R2V：以基线和高质量 Doppler velocity 做独立诊断。 +3. R2G：在确认水平的静止场地，以基线 + 重力 + 地面水平先验补齐完整旋转。 +4. R3：跨会话、block-out、协方差和可观性复核。 + +正式固定旋转来源是 `R2G_gravity_level_prior`；旧式 GNHPR 三轴姿态手眼只保留为诊断，不作为正式外参。 + +## RTK–IMU 平移如何求解和验证 + +每个 GNSS node 使用 15DoF 状态 `(R,p,v,bg,ba)`，相邻 node 由 covariance-whitened IMU preintegration 与 bias random-walk 连接。因子包括： + +- BESTNAVA：天线相位中心 XYZ； +- GGA：仅在 BEST 缺失时约束 XY，不混用 MSL 高度到 ENU Z； +- Doppler：天线速度； +- HPR：可选基线/姿态观测，direct 与短 bridge 使用不同协方差； +- 静止段：gravity candidate 与 ZUPT 分离，禁止将匀速直线误作 ZUPT。 + +杆臂的判断流程不是“优化器有数值就算成功”： + +1. 先用无机械先验的 free solve 评估 Schur 边缘化后的 3×3 杆臂信息、协方差、最弱方向和多初值稳定性。 +2. 若完整 XYZ 不可观，冻结 free 结果为诊断基线，不继续无限增加非线性 free solve。 +3. 以机械测量作为绝对杆臂来源，比较 fixed-mechanical、soft-prior 与 free 的代价和物理残差。 +4. 在 circle、left-right、slope 高动态窗口，以及与标定窗口不重叠的 held-out 数据上固定杆臂验证。 +5. 做旋转扰动敏感性、独立 innovation 与 propagation root-cause audit；这些审计不重新优化杆臂。 + +## 本次结果应如何理解 + +- 当前固定工程候选：`l_I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。 +- 机械测量决定杆臂绝对值；RTK+IMU 动态数据用于独立一致性和稳定性验证。 +- 47 个非重叠标定窗口表明机械先验与数据拟合相容，但没有足够的数据驱动信息去有意义地细化机械值。 +- held-out 物理验证和旋转敏感性通过；杆臂敏感的高动态数据没有发现机械外参明显冲突。 +- 低速、低角速度独立传播仍有约 `0.20 m/s²` 的公共加速度偏差；这更像 propagation nuisance，而不是由 `omega × l` 消失后的杆臂错误造成。该问题尚未关闭，故 `engineering_translation_accepted=false`。 + +这解释了两个同时成立的事实：机械外参没有被动态数据否定，但整个传播模型尚未达到正式放行标准。机器可读结果见 [工程 release 决策](../artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。 + +## 代码边界和共享模块 + +```text +imu_lidar/ + lidar_io.py, lidar_deskew.py, registration.py, keyframes.py + motion_pairs.py, phase_a.py, pipeline.py, joint_optimizer.py + -> LiDAR–IMU 专用 + +rtk_imu/ + rtk_io.py, rtk_attitude.py, rtk_imu_rotation.py + rtk_imu_multisource.py, rtk_imu_engineering.py, rtk_imu_node_graph.py + -> RTK–IMU 专用 + +imu_lidar/contracts.py, geometry.py, geodesy.py, imu_io.py, +imu_preintegration.py, rotation_handeye.py + -> 两条链路可复用的基础能力 +``` + +RTK–IMU 只能 import 这 6 个共享模块，不应 import LiDAR 专用求解文件。命令入口都在 `tools/`，并按 `run_rtk_imu_*`、`audit_rtk_imu_*` 与 LiDAR 工具名称区分。 + +## 进一步工作 + +在宣称正式工程放行前，应优先关闭低速传播公共加速度偏差：区分图优化 nuisance accel bias 与可迁移物理 bias，复核静态 bias、重力泄漏和短时传播。不要用调机械先验、调 covariance 或扩大 free solve 来掩盖该问题。 + +RTK–IMU 的完整工程流程、当前变换和复现命令见 [README_RTK_IMU.md](../README_RTK_IMU.md)。