From d1fa69c89b8b2e80e331ea5fbfa373985fc64b32 Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Tue, 25 Aug 2026 15:35:05 +0800 Subject: [PATCH] =?UTF-8?q?=E5=90=88=E5=B9=B6=E5=B9=B6=E5=AE=8C=E5=96=84?= =?UTF-8?q?=E5=8F=8C=E9=93=BE=E8=B7=AF=E6=A0=87=E5=AE=9A=E6=96=87=E6=A1=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 79 +++---- README_Lidar_IMU.md | 184 ---------------- README_RTK_IMU.md | 177 --------------- artifacts/rtk_imu_calibration_v1/README.md | 2 +- artifacts/rtk_imu_calibration_v3/README.md | 36 +-- docs/20260808_LiDAR-IMU标定现状与问题.md | 209 ------------------ docs/IMU-LiDAR标定.md | 195 ----------------- docs/RTK-IMU标定.md | 241 +++++++++++++++++++++ docs/V1_数据格式.md | 109 ---------- docs/lidar_imu_to_rtk_imu.md | 92 -------- docs/rtk_imu_calibration.md | 168 -------------- docs/rtk_imu_engineering_6dof.md | 108 --------- docs/rtk_imu_multisource_v3.md | 175 --------------- docs/标定流程与采集清单.md | 157 -------------- docs/问题清单.md | 209 ------------------ docs/雷达-IMU标定.md | 166 ++++++++++++++ imu_lidar/CHANGELOG.md | 8 +- imu_lidar/文件职责说明.md | 82 ------- rtk_imu/README.md | 13 -- 19 files changed, 462 insertions(+), 1948 deletions(-) delete mode 100644 README_Lidar_IMU.md delete mode 100644 README_RTK_IMU.md delete mode 100644 docs/20260808_LiDAR-IMU标定现状与问题.md delete mode 100644 docs/IMU-LiDAR标定.md create mode 100644 docs/RTK-IMU标定.md delete mode 100644 docs/V1_数据格式.md delete mode 100644 docs/lidar_imu_to_rtk_imu.md delete mode 100644 docs/rtk_imu_calibration.md delete mode 100644 docs/rtk_imu_engineering_6dof.md delete mode 100644 docs/rtk_imu_multisource_v3.md delete mode 100644 docs/标定流程与采集清单.md delete mode 100644 docs/问题清单.md create mode 100644 docs/雷达-IMU标定.md delete mode 100644 imu_lidar/文件职责说明.md delete mode 100644 rtk_imu/README.md diff --git a/README.md b/README.md index 02f7cc2..892b64b 100644 --- a/README.md +++ b/README.md @@ -1,69 +1,54 @@ # 车辆多传感器外参标定 -本仓库同时维护两条独立的外参标定链路：LiDAR–IMU 与 RTK–IMU。两条链路共享通用的几何和 IMU 预积分基础能力，但各自的观测模型、求解器、工具入口和验收结论独立管理。 +本仓库维护两条彼此独立的外参标定流程：**雷达–IMU** 和 **RTK–IMU**。两条流程复用少量通用的几何、IMU 读取和预积分代码，但不共用观测模型、求解器或验收结论。 -## 当前应如何使用 +## 从这里开始 -| 目标 | 推荐链路 | 当前可交付状态 | +| 目标 | 阅读文档 | 当前可交付范围 | | --- | --- | --- | -| LiDAR 与 IMU 的旋转、时间对齐 | LiDAR–IMU | 旋转与主机桥接时间对齐可冻结；完整平移不可交付。 | -| RTK 天线相位中心与 IMU 的工程外参 | RTK–IMU | 旋转外参已求出，平移外参利用机械先验经动态数据一致性验证确定（不是纯靠数据求出）。 | -| 需要完整 RTK–IMU 工程候选和复现方式 | RTK–IMU | 先阅读 [README_RTK_IMU.md](README_RTK_IMU.md)。 | +| 标定雷达与 IMU | [雷达–IMU 标定](docs/雷达-IMU标定.md) | 旋转与时间关系已有实车候选；平移尚不可交付。 | +| 标定双天线 RTK 与 IMU | [RTK–IMU 标定](docs/RTK-IMU标定.md) | 旋转已固定；杆臂以机械测量为基准，并完成动态一致性验证，尚未获得正式工程放行。 | -不要把任何一条链路未通过的平移结果当作已标定的 6DoF 外参。结果的接受状态以对应 JSON 中的 gate 为准。 +不要将任何一条流程中被拒绝的平移结果当作完整六自由度外参。是否接受结果，以相应结果 JSON 的 `accepted` 字段和门禁结论为准，而不是仅看优化是否收敛。 -## 为什么同时保留两条链路 +## 两条流程的关系 -LiDAR–IMU 链路通过点云相对运动与 IMU 预积分求手眼外参，适合旋转与时间对齐；但当前实车数据的加速度预积分位移，尤其 Z 分量，不足以支持可靠平移求解，且主要采集运动缺乏竖直激励。因此它的 `full_se3` 门禁未通过。 +```text +雷达–IMU + 点云相对运动 + IMU 预积分 + → 旋转与时间关系；平移受 IMU 位移误差和激励不足限制 -RTK–IMU 链路直接利用双天线基线、GNSS 位置和 Doppler 速度，并以机械测量给出天线相位中心相对 IMU 的绝对杆臂。它不是“用 RTK 替代 LiDAR”，而是针对 LiDAR–IMU 平移不可观的问题，采用更直接的天线位置/速度观测做工程外参验证。 +RTK–IMU + 双天线基线 + GNSS 位置/速度 + IMU + → 天线相位中心相对 IMU 的旋转和杆臂一致性验证 +``` -完整理由、两条模型的差异及当前结果见 [LiDAR→RTK–IMU 决策说明](docs/lidar_imu_to_rtk_imu.md)。 +选择 RTK–IMU 不是用 RTK 替换雷达。它用于解决“雷达–IMU 的平移在当前实车数据上不可可靠交付”这一独立问题；RTK–IMU 的外参也不能自动转换为雷达–IMU 外参。 ## 仓库结构 ```text calibration/ -├─ imu_lidar/ LiDAR–IMU 专用流程，以及双方共用基础模块 -├─ rtk_imu/ RTK–IMU 专用算法包 -├─ tools/ 两条链路的命令入口、导出和审计工具 -├─ tests/ 自动化测试 -├─ docs/ 详细方法、状态、采集与决策说明 -├─ artifacts/ 受审核的结果快照；大过程文件仅保留本地 -├─ config/ 车辆安装与流程配置 -├─ README.md 本页：仓库总览 -└─ README_RTK_IMU.md RTK–IMU 当前工程流程、结果与复现 +├─ imu_lidar/ 雷达–IMU 专用算法和命令行入口 +├─ rtk_imu/ RTK–IMU 专用算法 +├─ tools/ 原始数据导出、求解、审计和结果汇总入口 +├─ docs/ 两份规范说明：雷达–IMU、RTK–IMU +├─ artifacts/ 已提交的结果摘要与审核证据 +├─ config/ 车辆安装与流程配置 +└─ tests/ 自动化测试 ``` -`rtk_imu/` 只依赖下列 `imu_lidar/` 通用模块： +`rtk_imu/` 只允许复用 `imu_lidar/` 中的通用模块：`contracts.py`、`geometry.py`、`geodesy.py`、`imu_io.py`、`imu_preintegration.py` 与 `rotation_handeye.py`。它不依赖雷达专用的点云读取、去畸变、配准、雷达流程编排或雷达联合优化器。 -- `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`。 +原始 `.rscap`、统一导出数据、节点状态、检查点和调试中间结果只保留本地；仓库只提交可复核的代码、工具、配置、说明和结果摘要。RTK–IMU 的正式结果索引在 [artifacts/rtk_imu_calibration_v3/README.md](artifacts/rtk_imu_calibration_v3/README.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 当前问题、证据和历史处置记录。 | +```powershell +python -m pip install -e ".[dev]" +python -m pytest -q +``` -## 结果与过程文件 - -`artifacts/rtk_imu_calibration_v3/` 中只提交审核结论及其关键证据。原始 `.rscap`、统一导出数据、节点状态、checkpoint、调试 JSON 与旧版本过程产物均通过 `.gitignore` 保持本地，不应随提交或推送传播。 - -每次改动前先确认使用哪一条链路；每次交付前先读取相应 release JSON 中的 accepted gate，而不是仅凭优化器收敛、残差下降或一个看似合理的 4×4 矩阵下结论。 - -## LiDAR–IMU 专用运行指南 - -[README_Lidar_IMU.md](README_Lidar_IMU.md) 保留 LiDAR–IMU 的独立运行命令、合成复现与输入输出说明；它是远端原 README 的专用化重命名，不再承担仓库总导航职责。 +雷达点云处理需要时再安装可选依赖：`python -m pip install -e ".[open3d]"`。 \ No newline at end of file diff --git a/README_Lidar_IMU.md b/README_Lidar_IMU.md deleted file mode 100644 index e751b79..0000000 --- a/README_Lidar_IMU.md +++ /dev/null @@ -1,184 +0,0 @@ -# LiDAR–IMU 外参标定 - -用连续行驶中的 LiDAR 与 IMU 相对运动，估计安装外参与时间偏置： - -```text -p_IMU = T_IMU_lidar · p_lidar -``` - -**当前阶段：** 算法与合成自检已闭环；已提供 `tools/export_rscap_to_v1.py`（N300 `.rscap` + H32 dlog/MSOP → 中间格式）；**合格实车验收尚未完成**，故正式外参尚未对实车落盘交付。 - ---- - -## 先看什么（对外三份就够） - - -| 顺序 | 文档 | 用途 | -| --- | -------------------------------------- | -------------------- | -| 1 | **本 README** | 做什么、怎么跑、结果怎么判 | -| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + 原始数据导出命令 | -| 3 | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采（合格数据要求） | - - -其余（方法细述、测试说明、源码职责、CHANGELOG）给深入阅读 / 改代码时用，见文末。 - ---- - - - -## 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 diff --git a/README_RTK_IMU.md b/README_RTK_IMU.md deleted file mode 100644 index a1f099a..0000000 --- a/README_RTK_IMU.md +++ /dev/null @@ -1,177 +0,0 @@ -# RTK-IMU 标定：流程、结果与复现 - -本文是本次 G90 双天线 RTK 与 HI13 IMU 标定的工程入口。算法代码在 `rtk_imu/`，命令入口在 `tools/`，正式审核结果在 `artifacts/rtk_imu_calibration_v3/`。原始 `.rscap`、统一导出数据、状态快照与 checkpoint 均只保留本地，不提交仓库。 - -## 一句话结论 - -RTK-IMU 旋转外参由双天线基线方向与水平静止重力约束获得；平移外参以机械测量为绝对基准，再通过 RTK+IMU 联合状态图、高动态转弯/坡道数据、独立 held-out 数据和旋转扰动测试进行一致性验证。 - -当前动态数据不足以独立高精度求出完整 XYZ 杆臂，因此它不是 data-only translation calibration。机械测量给出了杆臂的绝对值；在未参与标定的数据、转弯等杆臂敏感运动以及旋转外参扰动测试中，均未发现该机械外参存在明显矛盾或不稳定性。当前结果的正确表述是：**机械杆臂锚定，并经动态数据一致性验证的工程候选**。 - -## 当前工程候选与状态 - -坐标约定：`l_I = p_ANT1^I`，即 ANT1（主天线、左侧）相位中心在 IMU 坐标系中的位置。车体/RTK 安装坐标为：+X 为 ANT1（左）到 ANT2（右），+Y 为车辆前进方向且与 IMU +Y 同向，+Z 向上；GGA 参考点为 ANT1 相位中心，离地 `1.916499878 m`。 - -- 固定旋转来源：`R2G_gravity_level_prior`。 -- 固定旋转近似 RPY（X/Y/Z）：`[0.454°, -0.003°, 0.012°]`。 -- 当前工程候选杆臂：`l_I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。 -- 变换约定：`p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU`。 -- 对应候选 `T_RTK_IMU`（四舍五入到 6 位小数）： - -```text -[[ 1.000000, -0.000214, -0.000044, 0.451777], - [ 0.000214, 0.999969, -0.007929, 0.270363], - [ 0.000046, 0.007929, 0.999969, -0.729325], - [ 0.000000, 0.000000, 0.000000, 1.000000]] -``` - -当前门禁状态必须同时保留： - -- `data_only_translation_accepted=false`：数据本身没有提供足够稳定的完整平移可观性。 -- `engineering_translation_accepted=false`：独立传播验证仍存在公共加速度偏差，不能声称正式工程放行。 -- `independent_extrinsic_sensitive_validation_passed=true`：杆臂敏感的高动态验证未发现机械杆臂冲突。 -- `heldout_physical_validation_passed=true`、`rotation_sensitivity_passed=true`：固定候选在 held-out 物理残差和旋转扰动中保持一致。 - -因此不得描述为“data-only 标定平移”或“动态数据已精确细化机械杆臂”。完整、机器可读的结论见 [engineering_release_decision.json](artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。 - -## 求解流程 - -1. **统一原始数据导出。** 使用 G90/HI13 设备时间作为主时间轴；host receive time 仅用于诊断。G90 保留 GGA 质量、GNHPR 基线及质量、BESTNAVA Doppler velocity，必要时 PVTSLNA；HI13 保留 system time、gyro、accel、姿态/四元数和 host receive time。 -2. **R0 连续性与质量控制。** 校验 checksum、RTK Fixed、设备时间单调性、IMU 覆盖、baseline jump 与测量间隔。轨迹连续性由 IMU 设备时间和预积分覆盖决定；孤立 HPR 缺失/Q5 只禁用或降权 HPR factor，不切断 IMU 轨迹。 -3. **旋转外参。** R1b 从基线与 IMU 动态估计 ANT1→ANT2 在 IMU 中的 2DoF 方向；R2V 用基线与高质量 Doppler 速度作独立诊断；R2G 使用水平静止场地中的基线＋重力＋地面水平先验补齐完整旋转。R2G 是本次正式固定旋转来源，旧 GNHPR 三轴手眼只作诊断。 -4. **节点状态图。** 每个 GNSS node 包含 `R,p,v,bg,ba` 的 15DoF 状态；相邻 node 由 covariance-whitened IMU preintegration 和 bias random walk 连接。BEST 约束 XYZ，GGA 仅在 BEST 缺失时约束 XY，Doppler 约束速度，HPR 是可选姿态/基线因子。 -5. **机械锚定的平移验证。** 杆臂以机械值为基准，分别比较无先验 free、固定机械杆臂和软先验解。free 解只用于观测性诊断，不能因为数值收敛就替代机械值。 -6. **独立验证。** 使用 circle、left-right、slope 的高动态非重叠窗口；再使用剩余 held-out 窗口、旋转 ±扰动敏感性、独立 innovation 和 propagation-bias root-cause audit 复核。 - -## 证据与限制 - -47 个非重叠标定窗口上的 free/fixed/prior 对比表明机械先验与数据拟合相容，但 posterior/prior 方差比没有显示足够的数据驱动细化，因此 `translation_refined_by_data=false`。剩余 267 个 frozen held-out 窗口的物理验证通过；然而独立传播创新在低速、低角速度区间同时出现位置和 Doppler 的同向偏差，等效为约 `0.20 m/s²` 的公共传播加速度误差。 - -该误差在 `|omega|` 很小时不能优先归因于杆臂速度项 `R*(omega × l)`，因此它不单独否决高动态杆臂敏感验证；但在传播模型根因关闭前，也不能把候选杆臂标为正式工程已放行。 - -主要审核证据： - -- [47 窗口机械分支](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json) -- [held-out 验证](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json) -- [独立创新审计](artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json) -- [传播偏差根因审计](artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json) -- [旋转敏感性](artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json) - -## 如何复现 - -### 1. 准备环境和原始数据 - -```powershell -cd -python -m pip install -e ".[dev]" -``` - -将三批 G90/HI13 原始 `.rscap` 会话放到本机的数据位置。数据位置不写入仓库；`tools/export_rtk_imu_unified.py` 中的会话配对清单必须与实际采集文件一致。 - -```powershell -$OUT = "artifacts\rtk_imu_calibration_v3\reproduce" -python tools\export_rtk_imu_unified.py --output-root "$OUT\unified" --overwrite -$MANIFEST = "$OUT\unified\manifest.json" -``` - -导出后应检查每个会话目录中的 `export_summary.json`，并确认 `manifest.json` 中记录的传感器时间轴没有被 host receive time 替换。 - -### 2. 复现固定旋转 - -`` 必须是已确认地面水平、车辆静止的会话；可多次传入 `--level-static`。R2V 只作独立诊断，不替代 R2G。 - -```powershell -python tools\run_rtk_imu_multisource.py ` - --manifest $MANIFEST ` - --level-static ` - --output "$OUT\r2g_multisource.json" -``` - -检查输出的 R2G 旋转与本 README 的近似 RPY 一致后，将该旋转固定为后续 node graph 的 `--rotation-rpy-deg 0.454 -0.003 0.012`。若 R2G 不一致，应停止，先复核天线方向、场地水平和时间/轴定义，不应继续求杆臂。 - -### 3. 复现窗口选择与无先验诊断 - -从 circle、left-right、slope 三类会话中选择高质量窗口；窗口不可共享 IMU/GNSS/HPR 样本。仓库提交的 `lever_information_window_selection*.json` 是本次审核所用选择结果，可用于对照。 - -```powershell -python tools\select_rtk_imu_windows_by_lever_information.py ` - --manifest $MANIFEST ` - --circle-session ` - --left-right-session ` - --slope-session ` - --output "$OUT\selection.json" ` - --rotation-rpy-deg 0.454 -0.003 0.012 - -python tools\run_rtk_imu_node_graph_free_selected.py ` - --manifest $MANIFEST ` - --selection "$OUT\selection.json" ` - --output "$OUT\free_baseline.json" ` - --start-name all ` - --rotation-rpy-deg 0.454 -0.003 0.012 -``` - -free solve 的作用是输出边缘化杆臂信息、协方差、最弱方向和多初值稳定性；本次数据若仍未达到完整 XYZ 可观性，不得扩大无先验求解规模来强行放行。 - -### 4. 复现机械杆臂分支和 held-out 验证 - -固定工程候选杆臂并使用同一批非重叠标定窗口。`states.npz` 和 checkpoint-dir 是本地过程产物，应保持被 `.gitignore` 排除。 - -```powershell -python tools\run_rtk_imu_mechanical_prior_branch.py ` - --manifest $MANIFEST ` - --selection "$OUT\selection.json" ` - --free-baseline "$OUT\free_baseline.json" ` - --output "$OUT\mechanical_47_window.json" ` - --state-output "$OUT\states.npz" ` - --rotation-rpy-deg 0.454 -0.003 0.012 - -python tools\run_rtk_imu_mechanical_prior_heldout.py ` - --manifest $MANIFEST ` - --calibration-selection "$OUT\selection.json" ` - --all-selection ` - --engineering-result "$OUT\mechanical_47_window.json" ` - --output "$OUT\heldout.json" ` - --checkpoint-dir "$OUT\heldout_checkpoints" ` - --rotation-rpy-deg 0.454 -0.003 0.012 -``` - -随后运行独立 innovation、传播根因审计和旋转敏感性。它们不重新优化杆臂，不应被用于调 covariance、R2G 或机械先验。 - -```powershell -python tools\audit_rtk_imu_heldout_innovation.py ` - --manifest $MANIFEST ` - --calibration-selection "$OUT\selection.json" ` - --all-selection ` - --engineering-result "$OUT\mechanical_47_window.json" ` - --output "$OUT\heldout_innovation.json" ` - --rotation-rpy-deg 0.454 -0.003 0.012 - -python tools\audit_rtk_imu_propagation_bias_root_cause.py ` - --manifest $MANIFEST ` - --calibration-selection "$OUT\selection.json" ` - --all-selection ` - --engineering-result "$OUT\mechanical_47_window.json" ` - --output "$OUT\propagation_bias_root_cause.json" ` - --rotation-rpy-deg 0.454 -0.003 0.012 -``` - -最终仅汇总已生成的结果，不在 release 阶段重新拟合： - -```powershell -python tools\finalize_rtk_imu_engineering_release.py ` - --calibration "$OUT\mechanical_47_window.json" ` - --heldout-postfit "$OUT\heldout.json" ` - --innovation "$OUT\heldout_innovation.json" ` - --sensitivity ` - --convergence-retry ` - --propagation-root-cause "$OUT\propagation_bias_root_cause.json" ` - --output "$OUT\engineering_release_decision.json" -``` - -## 相关文件 - -- [RTK-IMU 代码包](rtk_imu/README.md) -- [多源导出与旋转 V3 说明](docs/rtk_imu_multisource_v3.md) -- [engineering 6DoF 说明](docs/rtk_imu_engineering_6dof.md) -- [历史链路审计](docs/rtk_imu_calibration.md) -- [正式结果索引](artifacts/rtk_imu_calibration_v3/README.md) diff --git a/artifacts/rtk_imu_calibration_v1/README.md b/artifacts/rtk_imu_calibration_v1/README.md index f18542a..040823d 100644 --- a/artifacts/rtk_imu_calibration_v1/README.md +++ b/artifacts/rtk_imu_calibration_v1/README.md @@ -14,4 +14,4 @@ - `translation_result.json`：杆臂、平移、齐次矩阵、协方差/秩、位置/速度残差、偏置和 LOO。 - `summary.json`：供程序读取的最终状态和候选矩阵。 -当前 `all_sessions/summary.json` 是 `diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性，禁止用于车辆配置。完整解释见 `docs/rtk_imu_calibration.md`。 +当前 `all_sessions/summary.json` 是 `diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性，禁止用于车辆配置。完整解释见 [RTK–IMU 标定说明](../../docs/RTK-IMU标定.md)。 diff --git a/artifacts/rtk_imu_calibration_v3/README.md b/artifacts/rtk_imu_calibration_v3/README.md index 973168c..cd70670 100644 --- a/artifacts/rtk_imu_calibration_v3/README.md +++ b/artifacts/rtk_imu_calibration_v3/README.md @@ -1,24 +1,24 @@ -# RTK-IMU V3 正式结果集 +# RTK–IMU V3 结果与审核证据 -本目录只提交复现工程结论所需的审核结果。运行过程中的调试输出、状态快照、逐窗口 checkpoint 和旧版结果保留在本机，由仓库根目录 `.gitignore` 排除。 +本目录保存**已提交的审核快照**。它不是原始数据目录，也不是日常运行的输出目录；原始 `.rscap`、统一导出、节点状态、checkpoint 和调试文件均应保留在本地工作目录。 -## 当前结论 +## 先读哪个文件 -- 固定旋转来源：`R2G_gravity_level_prior`。 -- 工程候选杆臂：`l_I = p_ANT1^I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。 -- `data_only_translation_accepted=false`。 -- `engineering_translation_accepted=false`，原因是独立传播验证仍受公共加速度偏差影响。 -- `independent_extrinsic_sensitive_validation_passed=true`；当前结果可描述为机械杆臂经动态数据一致性验证的工程候选，不得描述为 data-only 平移标定结果。 +1. `engineering_release_decision.json`：唯一的最终结论，包含候选杆臂、两个互逆变换、验收状态、禁止的表述和全部证据路径。 +2. `mechanical_prior_engineering_47_window.json`：47 个非重叠标定窗口中，无先验、固定机械杆臂和机械软约束结果的对比。 +3. `mechanical_prior_engineering_heldout.json`：267 个独立留出窗口的物理残差与收敛情况。 +4. `heldout_independent_innovation.json` 与 `propagation_bias_root_cause_audit.json`：解释最终未放行的独立传播误差。 -## 文件说明 +## 文件分组 -- `engineering_release_decision.json`：最终状态、变换和放行判定。 -- `mechanical_prior_engineering_47_window.json`：相同 47 个非重叠窗口上的 free/fixed/prior 对比。 -- `node_graph_free_information_selected_mechanical.json`：冻结的无先验 free-solve 基线。 -- `lever_information_window_selection*.json`：窗口选择及实际边缘信息复核。 -- `mechanical_prior_engineering_heldout.json`：未参与标定窗口的 held-out 验证。 -- `heldout_independent_innovation.json`、`heldout_nonconverged_retry.json`：独立创新与失败窗口重试结果。 -- `mechanical_prior_rotation_sensitivity.json`：固定候选杆臂的旋转扰动敏感性结果。 -- `propagation_bias_root_cause_audit.json`：独立传播公共加速度误差根因审计。 +| 分组 | 文件 | 说明 | +| --- | --- | --- | +| 发布结论 | `engineering_release_decision.json` | 当前唯一的放行判断。 | +| 旋转与数据质量 | `multisource_result.json`、`bestnava_doppler_factor_yield_*.json`、`hpr_dropout_bridge_*.json` | 原始数据、旋转和双天线观测可用性的审核。 | +| 杆臂与选窗 | `lever_information_window_selection*.json`、`node_graph_free_information_selected_mechanical.json`、`mechanical_prior_engineering_47_window.json` | 无先验可观性、固定窗口及机械杆臂一致性。 | +| 独立验证 | `mechanical_prior_engineering_heldout.json`、`heldout_independent_innovation.json`、`heldout_nonconverged_retry.json`、`mechanical_prior_rotation_sensitivity.json` | 留出数据、创新、重试和旋转扰动检查。 | +| 根因和历史审计 | `propagation_bias_root_cause_audit.json`、`motion_excitation_*.json`、`factor_consistency_audit.json` 等 | 帮助理解过程和未解决问题；不替代发布结论。 | -结果 JSON 是审核快照；需要重新生成时应通过 `tools/` 中相应入口运行，且不得提交运行产生的 checkpoint 或 `.npz` 状态文件。 +`lever_information_window_selection.json` 记录 47 个标定窗口；`lever_information_window_selection_refined.json` 记录 314 个全部非重叠窗口，可从中扣除前者得到 267 个留出窗口。`mechanical_prior_engineering_47_window_states.npz` 是历史缓存，不是必需输入，也不应重新提交。 + +当前结论、术语和可执行复现步骤见 [RTK–IMU 标定](../../docs/RTK-IMU标定.md)。 \ No newline at end of file diff --git a/docs/20260808_LiDAR-IMU标定现状与问题.md b/docs/20260808_LiDAR-IMU标定现状与问题.md deleted file mode 100644 index 08b23a0..0000000 --- a/docs/20260808_LiDAR-IMU标定现状与问题.md +++ /dev/null @@ -1,209 +0,0 @@ -# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题 - -> 数据：`D:\data\calibration_usable_20260808` -> 可用会话：`sessions_v1_host_aligned`（三优先窗） -> 当前结果目录：各窗 `out_fixed_dt0/` -> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json` -> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml` -> 约定外参：`p_IMU = T_IMU_lidar · p_lidar` - ---- - -## 1. 一句话结论 - -**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。** -三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。 - ---- - -## 2. 当前可用结果（`out_fixed_dt0`） - -约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。 - -| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior | -|----|------|----|---------------------|--------------|----------|--------------| -| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° | -| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° | -| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° | - -- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。 -- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。 -- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。 - -### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar` - -- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json` -- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]` -- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]` - -```text -R = -[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ], - [ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ], - [-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]] -t = [0, 0, 0] -``` - -### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar` - -- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json` -- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]` -- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]` - -```text -R = -[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ], - [ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ], - [ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]] -t = [0, 0, 0] -``` - -### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar` - -- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json` -- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]` -- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]` - -```text -R = -[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ], - [ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ], - [ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]] -t = [0, 0, 0] -``` - -### 相对历史失败轮次 - -| 轮次 | 问题 | 结果 | -|------|------|------| -| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° | -| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） | -| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 | - ---- - -## 3. 已澄清并写入配置的坐标系 / 先验 - -### 3.1 车体与传感器 - -- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。 -- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。 -- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。 -- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。 - -### 3.2 安装量（`translation_m`） - -| 传感器 | X / Y（后轮轴中心） | Z（离地） | -|--------|---------------------|-----------| -| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** | -| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** | - -- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。 -- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。 - -### 3.3 导出外参先验 - -- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。 -- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。 -- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。 - ---- - -## 4. 现存问题清单 - -### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移） - -- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。 -- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。 -- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。 -- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。 -- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。 - -### P2. 平面运动导致竖直平移弱可观 - -- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。 -- 流水线门控已给出 `translation_accepted=false`。 -- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。 - -### P3. 时间偏移若再自由估计会被带偏（已规避，需保持） - -- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。 -- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。 -- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。 - -### P4. 单窗低残差 ≠ 外参正确（历史教训） - -- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。 -- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。 -- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。 - -### P5. 旋转软先验尚未做无先验对照 - -- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。 -- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。 - -### P6. 文档与操作约定未完全同步（工程） - -- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。 -- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。 - ---- - -## 5. 不该做 / 可以做 - -| 动作 | 建议 | -|------|------| -| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） | -| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** | -| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 | -| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** | -| 无先验 / 大 σ 旋转对照 | **建议做** | -| 冻结交付 `R` + `δt=0` 说明 | **建议做** | -| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 | - ---- - -## 6. 建议下一步顺序 - -1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。 -2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。 -3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。 -4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。 - ---- - -## 7. 常用路径与命令 - -```text -数据根： - D:\data\calibration_usable_20260808\sessions_v1_host_aligned\ - -结果： - ...\priority_XXXX\out_fixed_dt0\summary.json - ...\priority_XXXX\out_fixed_dt0\motion_pairs.json - ...\calibration_manifest_fixed_dt0.json -``` - -```powershell -# 可视化（rotation_only 默认模式4用 GICP 平移） -python tools\visualize_pair_3d.py ` - --lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar ` - --summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json ` - --pair-index 0 - -# 若要看「坏 Δp」导致的错层效果： -# --mode4-translation imu -``` - ---- - -## 8. 问题优先级（跟踪用） - -| ID | 严重度 | 状态 | 标题 | -|----|--------|------|------| -| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 | -| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 | -| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） | -| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 | -| P5 | 中 | 待做 | 无旋转先验对照 | -| P6 | 低 | 待做 | README/交付物同步 | diff --git a/docs/IMU-LiDAR标定.md b/docs/IMU-LiDAR标定.md deleted file mode 100644 index 9f527a6..0000000 --- a/docs/IMU-LiDAR标定.md +++ /dev/null @@ -1,195 +0,0 @@ -# LiDAR–IMU 外参标定说明 - -**用途：** 方法约定与实现状态（深入阅读）。日常使用请先看根目录 [`README.md`](../README.md)。 - -本文说明标定目标、约定、流水线与实现状态。代码在 `imu_lidar/`。场地采集见 [`标定流程与采集清单.md`](标定流程与采集清单.md)。 - ---- - -## 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`，不当交付 | -| 设备时间新车数据正式验收 | **待做** | -| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`（N300 rscap + H32 dlog/MSOP → V1；dlog 用 DIFOP 通道角） | - -细项见 [`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) | 改代码 | diff --git a/docs/RTK-IMU标定.md b/docs/RTK-IMU标定.md new file mode 100644 index 0000000..972402b --- /dev/null +++ b/docs/RTK-IMU标定.md @@ -0,0 +1,241 @@ +# RTK–IMU 标定 + +本文是本仓库 RTK–IMU 标定的唯一规范说明，覆盖数据、方法、当前结果、限制和复现。它不使用项目内部阶段代号作为前提。 + +## 结论与适用范围 + +本次标定的正确表述是：**双天线方向和水平静止时的重力约束给出了固定旋转；天线相位中心到 IMU 的平移以机械测量为绝对基准，RTK 与 IMU 动态数据对该安装关系完成了独立一致性与稳定性验证。** 动态数据未能独立、精确地估计完整三维杆臂，但没有发现机械测量值与真实运动矛盾。 + +| 项目 | 当前值或状态 | +| --- | --- | +| 天线相位中心相对 IMU 的杆臂 `l_I` | `[-0.4518015159, -0.2644749820, 0.7314656115] m` | +| 固定旋转来源 | 水平静止时的双天线基线与重力约束 | +| 固定旋转近似 RPY（X/Y/Z） | `[0.454°, -0.003°, 0.012°]` | +| 仅由数据估计三维平移 | 未通过：`data_only_translation_accepted=false` | +| 正式工程平移验收 | 未通过：`engineering_translation_accepted=false` | +| 杆臂敏感的高动态独立验证 | 通过：未发现机械杆臂冲突 | + +该候选可用于算法联调和工程验证，但不能称为“数据独立完成的完整六自由度标定”或“正式工程放行的外参”。低速、低角速度留出数据仍显示约 `0.20 m/s²` 的共同加速度传播误差；其根因未关闭前，工程放行保持为否。机器可读结论见 [工程发布决定](../artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。 + +## 安装与坐标定义 + +- 主天线为 ANT1，位于车辆前进方向左侧；从天线为 ANT2，位于右侧；两者平行安装。 +- 双天线报文的 heading 是**主天线到从天线**的方向。 +- IMU 的 `+Y` 指向车辆前进方向；天线基线因此指向车辆右侧。 +- GGA 的参考点为 ANT1 相位中心，离地 `1.916499878 m`。 +- `l_I = p_ANT1^I` 表示从 IMU 原点指向 ANT1 相位中心、并在 IMU 坐标系表达的向量。 + +输出变换的定义为： + +```text +p_RTK = R_RTK_IMU · p_IMU + t_RTK_IMU +p_IMU = R_RTK_IMUᵀ · p_RTK + l_I +t_RTK_IMU = -R_RTK_IMU · l_I +``` + +RTK 原点就是 ANT1，所以 `T_IMU_RTK` 的平移等于 `l_I`，两个变换必须互逆。下游实现必须按这些公式检查方向和符号。 + +## 原始数据与统一导出 + +使用三个日期的 G90 双天线 RTK 与 HI13 IMU 原始 `.rscap`。按捕获开始时间在 1.5 秒内配对，共导出 55 组会话；0815 的一组无匹配 HI13，已记录为 unmatched，不参与联合求解。 + +| 批次 | 配对会话 | GNHPR | BESTNAVA | PVTSLNA | Q4 双天线方向 | Fixed Doppler | +| --- | ---: | ---: | ---: | ---: | ---: | ---: | +| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 | +| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 | +| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 | + +旧导出以 GGA 为中心，将最近的 GNHPR 合并到同一行，且没有保留 BESTNAVA/PVTSLNA；这会把异步报文伪装成同时观测，也不能独立使用 GNSS 位置和 Doppler 速度。 + +新导出保留每条异步报文： + +| 文件 | 内容 | +| --- | --- | +| `imu.npz` | HI13 `system_time`、三轴陀螺/加速度、RPY、四元数、磁场、PPS、温度、气压和主机接收时间 | +| `rtk.csv` | 各 GGA/GNHPR/BESTNAVA/PVTSLNA 的测量时间、质量、校验和和原始字段 | +| `export_summary.json` | 报文统计、四元数检查与跨时钟诊断 | +| `manifest.json` | 会话、批次、导出目录与未配对记录 | + +HI13 `system_time` 是主时间轴。G90 的 GNSS UTC 或周/TOW 时间原样保留，并映射到此主时间轴；主机接收时间只用于时钟桥接、延迟和抖动诊断，绝不能作为采样时间。 + +## 求解与验证流程 + +```text +统一导出原始报文 + → 检查校验和、固定解、时间连续性和 IMU 预积分覆盖 + → 由双天线基线和重力固定旋转 + → 以节点状态图检查无机械先验时的杆臂可观性 + → 以机械杆臂为基准进行动态一致性验证 + → 使用未参与标定的数据和高动态转弯/坡道复核 +``` + +### 旋转 + +双天线基线只提供两个方向自由度，不能单独推出完整三维姿态。因此旋转经过三类检查： + +1. 连续高质量基线与 IMU 相对转动，检查基线在 IMU 中的方向。 +2. 基线加高速近似直线 Doppler 速度，独立检查前向；因可用高速样本不足且与 IMU 航向存在不一致，它只作诊断。 +3. 明确真实水平、车辆静止的会话中，基线确定横向，HI13 重力确定竖直，叉乘得到前向。这是当前固定旋转的正式来源。 + +旧式“直接把 GNHPR 三轴姿态与 IMU 做手眼”仅作诊断，不再作为正式外参。 + +### 平移与杆臂 + +每个 GNSS 节点包含姿态、位置、速度、陀螺偏置和加速度偏置。相邻节点由带协方差的 IMU 预积分连接；BESTNAVA 约束三维位置，Doppler 约束速度，GGA 只在 BEST 缺失时补充水平位置，双天线方向是可选姿态观测。GGA 的 MSL 高程绝不用于 ENU 的 Z 轴。 + +```text +p_ANT1^W = p_IMU^W + R_WI · l_I +v_ANT1^W = v_IMU^W + R_WI · ((ω - b_g) × l_I) +``` + +无机械先验求解只用来检查可观性：边缘化其他状态后，检查三维杆臂的信息矩阵、协方差、最弱方向和不同初值稳定性。本批数据不能稳定约束完整 XYZ，所以不会用其点估计替代机械值。 + +在同一批非重叠高动态窗口中，已比较无先验诊断解、固定机械杆臂和机械软约束解。机械值没有显著恶化位置、速度或双天线残差；但数据相对机械先验的信息增益不足，不能证明数据细化了杆臂。因此绝对值仍以机械测量为准。 + +## 如何使用结果 + +可以：固定本文旋转和机械杆臂，用于定位、融合或控制算法验证；并把“机械测量 + 动态一致性验证”写入配置或报告。 + +不能：把它描述为数据独立的三维平移标定、完整六自由度正式放行，或仅凭优化收敛就宣称杆臂真实。 + +## 结果文件与本地过程文件 + +`artifacts/rtk_imu_calibration_v3/` 是**本次提交的审核快照**，不是原始数据目录，也不是每次运行的工作目录。阅读顺序如下： + +| 类别 | 主要文件 | 用途 | +| --- | --- | --- | +| 最终结论 | `engineering_release_decision.json` | 唯一的放行状态、候选杆臂、两个互逆变换和限制说明；下游首先读取它。 | +| 旋转证据 | `multisource_result.json` | 双天线方向、重力和速度诊断的旋转结果。 | +| 杆臂一致性 | `mechanical_prior_engineering_47_window.json` | 同一 47 个标定窗口上无先验、固定机械值和机械软约束三种结果的比较。 | +| 独立验证 | `mechanical_prior_engineering_heldout.json`、`heldout_independent_innovation.json` | 未参与标定的 267 个窗口的物理残差和独立传播创新。 | +| 稳定性与根因 | `mechanical_prior_rotation_sensitivity.json`、`propagation_bias_root_cause_audit.json` | 固定旋转扰动的影响，以及低速传播公共加速度误差的诊断。 | +| 固定选窗输入 | `lever_information_window_selection.json`、`lever_information_window_selection_refined.json` | 分别记录 47 个标定窗口和 314 个全部非重叠窗口；后者用于从中扣除 47 个标定窗口，得到 267 个留出窗口。 | +| 辅助审计 | `bestnava_doppler_factor_yield_*.json`、`hpr_dropout_bridge_*.json`、`motion_excitation_*.json` 等 | 记录数据保留率、双天线短缺口处理和运动激励检查；它们解释流程选择，不单独决定放行。 | + +目录中还保留少量历史诊断和调试快照，文件名含 `debug`、`fast_diagnostic`、`p0` 或旧 node-graph 阶段。这些不是当前发布结论，阅读时应以 `engineering_release_decision.json` 及其 `evidence` 字段指向的文件为准。`mechanical_prior_engineering_47_window_states.npz` 是一次历史状态缓存；它不是复现的必要输入，也不应在后续运行中再次提交。 + +原始 `.rscap`、统一导出目录、运行中的 `.npz` 状态、checkpoint 和临时 JSON 应放在仓库外或被忽略的本地工作目录。不要把它们覆盖到 `artifacts/rtk_imu_calibration_v3/`，以免把正式审核快照和个人运行过程混在一起。 + +## 复现 + +### 复现范围 + +| 目标 | 是否需要原始数据 | 推荐操作 | +| --- | --- | --- | +| 核验当前发布结论 | 否 | 读取 `engineering_release_decision.json`，并按其 `evidence` 字段查看证据 JSON。 | +| 重做数据导出和固定旋转 | 是 | 按下文步骤 1–2 运行；结果应与本文的会话数、时间规则和旋转量级一致。 | +| 重做机械杆臂一致性与留出验证 | 是 | 按步骤 3–5 使用冻结选窗文件；计算量较大，所有输出放到本地工作目录。 | +| 生成新的工程结论 | 是，且需新的审核决策 | 不要复用或覆盖当前发布快照；应新建工作目录和结果目录，并重新执行完整门禁。 | + +以下命令用于重建本次发布所依据的流程。原始数据路径不提交仓库；导出脚本目前的三批默认来源定义在 `tools/export_rtk_imu_unified.py` 的 `DEFAULT_SOURCES`。若本机原始数据不在这些位置，应先在该常量中仅替换本地路径，保持批次和文件配对规则不变。 + +### 1. 导出 55 组统一数据 + +```powershell +python -m pip install -e ".[dev]" +$WORK = "D:\data\rtk_imu_reproduce" # 本地工作目录，不提交 +$UNIFIED = "$WORK\unified" +python tools\export_rtk_imu_unified.py --output-root $UNIFIED --overwrite +$MANIFEST = "$UNIFIED\manifest.json" +``` + +检查 `manifest.json`：应有 55 个配对会话，0815 的一个单独 G90 记录在 `unmatched`；每个会话应包含 `imu.npz`、`rtk.csv` 和 `export_summary.json`。确认 HI13 `system_time` 是主时间轴，主机接收时间没有成为观测采样时间。 + +### 2. 重建固定旋转 + +本次发布使用两个明确水平、车辆静止的会话。两个 `--level-static` 参数都必须提供： + +```powershell +$ROTATION = "$WORK\rotation.json" +python tools\run_rtk_imu_multisource.py ` + --manifest $MANIFEST ` + --level-static 0819_20260819_072130 ` + --level-static 0819_20260819_073045 ` + --output $ROTATION +``` + +检查输出中“基线 + 重力 + 水平场地约束”的旋转是否接近 `[0.454°, -0.003°, 0.012°]`。如果差异明显，应停止后续步骤，先核对天线方向、会话是否真实水平和时间/坐标定义；不要直接求杆臂。 + +### 3. 选择并诊断 47 个标定窗口 + +本次种子运动分别是持续绕圈、左右转向和坡道。脚本会在不共享 IMU/GNSS/HPR 样本的前提下，按杆臂信息增益补充窗口；当前发布的固定结果为 47 个窗口。 + +```powershell +$SEL47 = "$WORK\selection_47.json" +python tools\select_rtk_imu_windows_by_lever_information.py ` + --manifest $MANIFEST ` + --circle-session 0808_20260808_092827 ` + --left-right-session 0808_20260808_082148 ` + --slope-session 0815_20260812_123424 ` + --rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 ` + --output $SEL47 + +$FREE47 = "$WORK\free_47.json" +python tools\run_rtk_imu_node_graph_free_selected.py ` + --manifest $MANIFEST ` + --selection $SEL47 ` + --rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 ` + --start-name all ` + --output $FREE47 +``` + +`$FREE47` 只用于确认无机械先验时的可观性和多初值稳定性；它不是可交付杆臂，也不应因此修改机械值。 + +### 4. 重做机械杆臂一致性比较 + +```powershell +$ENGINEERING = "$WORK\mechanical_47.json" +python tools\run_rtk_imu_mechanical_prior_branch.py ` + --manifest $MANIFEST ` + --selection $SEL47 ` + --free-baseline $FREE47 ` + --rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 ` + --state-output "$WORK\mechanical_47_states.npz" ` + --output $ENGINEERING +``` + +结果必须同时比较无先验、固定机械杆臂和机械软约束杆臂。`mechanical_47_states.npz` 仅为本地缓存，不提交。 + +### 5. 重做留出验证和发布汇总 + +本次留出验证使用 314 个冻结的全部非重叠窗口，其中 47 个是标定窗口、267 个是留出窗口。为了精确复现本次窗口划分，直接使用仓库中的 `lever_information_window_selection_refined.json`，不要重新选择或改变窗口。 + +```powershell +$ALL314 = "artifacts\rtk_imu_calibration_v3\lever_information_window_selection_refined.json" +$HELDOUT = "$WORK\heldout.json" +python tools\run_rtk_imu_mechanical_prior_heldout.py ` + --manifest $MANIFEST ` + --calibration-selection $SEL47 ` + --all-selection $ALL314 ` + --engineering-result $ENGINEERING ` + --checkpoint-dir "$WORK\heldout_checkpoints" ` + --rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 ` + --output $HELDOUT + +python tools\audit_rtk_imu_heldout_innovation.py ` + --manifest $MANIFEST ` + --calibration-selection $SEL47 ` + --all-selection $ALL314 ` + --engineering-result $ENGINEERING ` + --rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 ` + --output "$WORK\heldout_innovation.json" + +python tools\audit_rtk_imu_propagation_bias_root_cause.py ` + --manifest $MANIFEST ` + --calibration-selection $SEL47 ` + --all-selection $ALL314 ` + --engineering-result $ENGINEERING ` + --rotation-rpy-deg 0.4543066225 -0.0026392019 0.0122384129 ` + --output "$WORK\propagation_bias_root_cause.json" +``` + +旋转敏感性、未收敛窗口重试和最终汇总属于发布级验证；它们不重新拟合杆臂。若要重新生成最终发布 JSON，必须连同上述审计结果、旋转敏感性结果和重试结果一起传给 `tools/finalize_rtk_imu_engineering_release.py`。在没有完成这些验证时，不得将本地结果标为成功。 +## 代码边界与审核证据 + +RTK 专用代码位于 `rtk_imu/`，命令行和审计工具位于 `tools/`。它只复用 `imu_lidar/` 的通用几何、地理坐标、IMU 读取、预积分和旋转初始化模块；不依赖雷达点云、配准或雷达联合优化代码。 + +- [多源旋转结果](../artifacts/rtk_imu_calibration_v3/multisource_result.json) +- [47 窗口机械杆臂比较](../artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json) +- [留出数据验证](../artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json) +- [独立创新审计](../artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json) +- [传播误差根因审计](../artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json) \ No newline at end of file diff --git a/docs/V1_数据格式.md b/docs/V1_数据格式.md deleted file mode 100644 index 62289ea..0000000 --- a/docs/V1_数据格式.md +++ /dev/null @@ -1,109 +0,0 @@ -# V1 标准中间数据格式 - -**用途：** 标定程序读入的 CSV/NPZ 约定，以及新车原始数据如何导出。总览见根目录 [README](../README.md)。 - -## 从原始数据导出 - -**推荐（新 H32 + HI13）：** HI13 `.rscap` + 雷达 Medulla dlog / recovered zip（raw MSOP + DIFOP）。 - -```powershell -python tools\export_rscap_to_v1.py ` - --imu-rscap path\to\hi13r4-imu.rscap ` - --imu-kind hi13 ` - --lidar-dlog path\to\session_or_dlog_or_recovered.zip ` - --host-start 2026-08-08T17:40:05 ` - --host-end 2026-08-08T17:45:15 ` - --out path\to\session_v1 ` - --frame-stride 5 ` - --require-difop -``` - -`--lidar-dlog` 可为：标准 `dobject/`+`dobject_recording/` 目录，或 recovered zip（`indices.log` + `data.bin`）。 -`--imu-kind`：`hi13` / `n300` / `auto`（默认按文件名推断）。 -`--host-start/end`：按本地墙钟切窗（仅裁剪；标定主轴仍是设备时间）。 -默认 DObject：`frontlidar-msop-raw`、`frontlidar-difop-raw`。 - -**兼容旧 MSOP-only `.rscap`：** - -```powershell -python tools\export_rscap_to_v1.py ` - --imu-rscap path\to\n300.rscap ` - --lidar-rscap path\to\h32_msop.rscap ` - --out path\to\session_v1 ` - --frame-stride 1 -``` - -产出：`imu.csv`、`lidar/`（含 `frames_index.csv`）、`export_summary.json`。 -标定主轴仍是**设备时间**；同时写出**主机 UTC 接收时间**，用于把雷达帧桥接到 IMU 设备钟（禁止把两边设备时间第一帧强行重合）。 - -## IMU - -文件：`imu.csv` 或 `imu.npz` - -### CSV - -```text -t,gx,gy,gz,ax,ay,az,t_host_utc_s,receive_utc_ticks -0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81,1754646005.123,6389... -... -``` - -| 列 | 含义 | 单位 | -|---|---|---| -| t | IMU 设备时钟时间 | s | -| gx,gy,gz | 角速度 | rad/s | -| ax,ay,az | 比力/加速度 | m/s² | -| t_host_utc_s | 主机 UTC 接收时间（Unix） | s | -| receive_utc_ticks | 同上，.NET UTC ticks | — | - -### 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,host_receive_utc_ticks,t_host_utc_s,host_receive_utc_end_ticks,t_host_utc_end_s -0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22 -``` - -| 列 | 含义 | -|---|---| -| t_start / t_end | H32 MSOP **设备时间**（秒） | -| t_host_utc_s / t_host_utc_end_s | 帧首/末包 **HostReceiveUtcTicks** → Unix 秒 | - -也兼容旧列名 `file`。对齐脚本用主机 UTC 把 `t_*` 重写到 IMU 设备钟后再跑标定。 - -### 每帧 NPZ - -- `points`: `float64/float32`，形状 `(N, 3)`，LiDAR 直角坐标系，单位米 - -## 时间不同步能不能用？ - -可以，前提是： - -1. 两边都覆盖同一段**有角速度激励**的物理运动（尤其是转弯）； -2. 偏置近似为**常数**（短会话）； -3. `--time-offset-search-s` 足够覆盖可能的偏移（默认 ±1 s，可加大）。 - -若两段数据完全不是同一趟行驶，或只有静止 IMU、没有重叠运动，则无法估 δt，标定会被 `blocked`。 - -## 最小可用会话 - -- IMU：建议含静止段 + 运动段，采样率稳定 -- LiDAR：建议 ≥ 20 帧，场景有墙/柱等结构，含转弯 diff --git a/docs/lidar_imu_to_rtk_imu.md b/docs/lidar_imu_to_rtk_imu.md deleted file mode 100644 index 736fc20..0000000 --- a/docs/lidar_imu_to_rtk_imu.md +++ /dev/null @@ -1,92 +0,0 @@ -# 从 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)。 diff --git a/docs/rtk_imu_calibration.md b/docs/rtk_imu_calibration.md deleted file mode 100644 index 32326a0..0000000 --- a/docs/rtk_imu_calibration.md +++ /dev/null @@ -1,168 +0,0 @@ -# RTK–IMU 标定链路审计与当前方案 - -> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果， -> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式 -> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。 - -## 结论 - -当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。 - -G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。 - -双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。 - -当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。 - -## 坐标系和参考点 - -统一输出约定： - -```text -p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU -``` - -RTK 车固坐标定义： - -- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。 -- +Y：车辆前进方向，与 IMU +Y 同向。 -- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。 -- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。 - -位置参考点： - -- GGA 为 ANT1/主天线相位中心。 -- 相位中心离地高度为 1.916499878 m。 - -## 求解链路 - -### R0：协议、数据质量和时间 - -1. 校验 NMEA checksum。 -2. GGA 位置只接收 Q=4 固定解。 -3. GNHPR 姿态只接收 Q=4 固定解；Q=5 浮点解仅进入诊断统计，不参与标定。 -4. 保留 GNHPR 自己的卫星数、差分龄期和基站号，不能再用 GGA 卫星数替代 HPR 质量。 -5. 姿态使用 `hpr_measurement_utc_s` 映射后的 IMU 设备时间。 -6. 遇到 HPR 无效、浮点、时间间隔异常或基线跳变时切断连续段，运动对不得跨断点。 - -### T0：残余时间偏移 - -只使用可观测的有符号 heading 角速度与 IMU `gyro_z` 做相关扫描。角速度模长会混入不可观的绕基线旋转，不再作为时间审计依据。 - -时间偏移只用于初始化/诊断。只有相关峰足够高、与次峰分离且峰宽足够窄时才应用，否则保持 0 s。 - -### R1：双天线基线一致性 - -对每个连续固定解片段构造 0.75 s、1.5 s、3.0 s 的相对运动。以安装先验 `u_IMU=[1,0,0]` 检验： - -```text -angle(u_RTK(t0), u_RTK(t1)) - ≈ angle(u_IMU, ΔR_IMU(t0,t1) u_IMU) -``` - -该标量约束不虚构 RTK 的前向轴和上向轴。输出整体、逐会话 RMS/P95、分轴诊断及最坏时间区间。所有会话的总权重归一，避免高激励或样本更多的会话支配结果。 - -### R2：旧式完整 HPR 诊断 - -为兼容历史输出，将基线补成零 roll 的数学坐标架，再运行完整手眼。这个结果仅用于暴露符号错误、异常会话和旧结果变化，不能作为可交付外参。 - -LOO 删除一个会话后，会重新优化剩余会话的陀螺零偏，不再固定全量数据的 nuisance 参数。 - -### T1：平移 - -只有完整三自由度旋转“可观且通过”时，才允许进入杆臂和平移求解。当前条件不满足，因此： - -- 不运行平移优化； -- 不输出 `translation_result.json`； -- `t_RTK_IMU_m` 和 `T_RTK_IMU` 为 null； -- 历史巨大 Z、米级不确定度和约 1 m/s 速度残差不再消耗优化时间。 - -## 当前 8 会话结果 - -结果目录：`artifacts/rtk_imu_calibration_v2/all_sessions`。 - -```text -可观测基线一致性： -RMS / median / P95 = 0.593665 / 0.146040 / 1.212972 deg -priority_175910 RMS / P95 = 1.316292 / 2.472570 deg -slope RMS / P95 = 1.264514 / 2.475899 deg -最坏区间 = priority_175910，约 5.084 deg -baseline gate = passed - -时间偏移： -全局候选 = +0.005 s -峰值相关 = 0.909019 -近峰宽度 = [-0.110, +0.170] s -实际应用 = 0 s - -旧式完整 HPR 诊断： -RPY = [-0.132765, -0.297433, -1.558358] deg -RMS / P95 = 1.172397 / 2.404977 deg -std = [0.615416, 0.437419, 3.687145] deg -priority_175910 re-optimized LOO = 1.698555 deg -max re-optimized LOO = 1.699427 deg（删除 slope） -legacy numeric gate = failed -full attitude observable = false - -平移： -frozen / not run -``` - -旧结果中的 `priority_175910` 条件 LOO 为 5.14°。严格剔除 Q5、会话等权、断段保护以及 LOO 重估零偏后，该会话完整诊断 LOO 降为 1.699°。它和坡道会话仍是主要异常源，但现在异常集中在基线仰角通道，而不是 heading：这更符合原始 HPR 中 Q5、低卫星数和 pitch 大幅波动的事实。 - -## 已排除或仍存在的漏洞 - -- 已修复：heading 误当车前方向。 -- 已修复：GNHPR pitch 误当纵向车体 pitch。 -- 已修复：不可观 roll 注入完整姿态。 -- 已修复：Q5 浮点 HPR 进入标定。 -- 已修复：HPR 质量字段在导出时丢失。 -- 已修复：相对运动跨越无效段或跳变。 -- 已修复：样本多的会话权重过大。 -- 已修复：LOO 固定全量会话零偏。 -- 已修复：时间相关使用三轴角速度模长。 -- 已修复：旋转未通过仍继续优化平移。 -- 仍存在：当前运动不能稳定地从 GGA 速度补全车前/车上方向。 -- 仍存在：基线绕轴自由度没有独立传感器观测。 -- 仍存在：`priority_175910` 和坡道的基线仰角存在局部异常。 - -## 如何得到可交付的完整旋转 - -按优先级建议： - -1. 采一组专用数据：空旷区域、全程 RTK fixed、较长直线加减速、左右转、坡道上下行，保留原始 GGA/GNHPR/IMU 时间和全部质量字段。 -2. 用高质量前向速度补第二根轴。只在速度足够高、航向变化平缓的区间用 GGA course，并在同一优化中建模 ANT1 杆臂、非完整车辆侧向速度约束和时间偏移。 -3. 用静止重力补上向轴时，必须把“地面水平/车辆静止”写成显式先验，并将结果标记为安装先验约束解，而不是双天线数据独立解。 -4. 若能取得 G90 内部融合后的完整 INS 姿态、第三天线、轮速/转角或可靠车体姿态源，优先作为第二独立方向。 -5. 完整旋转通过留出验证后，才恢复平移；平移应同时估计杆臂、速度、加计偏置，并检查垂向高程基准。 - -当前数据上尝试用平滑 GGA 速度直接补全姿态，结果随平滑窗口明显变化，受低速、转弯杆臂和高程差分噪声影响，不能进入正式结果。 - -## 与主流开源方法的对应 - -- Kalibr：角速度相关适合作为时间偏移初始化，不应在宽峰时强行采用候选值。 -- iKalibr：采用连续时间轨迹联合估计时空参数，并强调充分激励；适合后续专用数据。 -- MINS：异步测量插值并把传感器外参、时间和 nuisance 状态一起估计。 -- GICI-LIB：因子图中显式进行 GNSS/INS 初始化、质量控制和异常值处理。 - -本项目暂不直接引入这些大型框架，而是吸收其原则：先保证物理可观测性和数据质量，再进行联合优化；不能用自由状态吸收错误模型。 - -参考： - -- Unicore N4 Reference Commands Manual -- Unicore UM982 User Manual -- https://github.com/Unsigned-Long/iKalibr -- https://github.com/ethz-asl/kalibr -- https://github.com/rpng/MINS -- https://github.com/chichengcn/gici-open - -## 代码入口 - -- `rtk_imu/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。 -- `rtk_imu/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。 -- `rtk_imu/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。 -- `rtk_imu/rtk_imu_replay.py`：坐标定义、参考点、平移冻结和 JSON 输出。 -- `tools/rscap_v2/g90_rtk.py`：原始 GNHPR 解析及质量字段。 -- `tools/export_g90_rtk_to_sessions.py`：GNHPR 质量字段导出。 -- `tools/run_rtk_imu_calibration.py`：端到端命令行入口。 -- `tests/test_rtk_imu_calibration.py`：轴定义、Q4/Q5、时间和预积分回归测试。 diff --git a/docs/rtk_imu_engineering_6dof.md b/docs/rtk_imu_engineering_6dof.md deleted file mode 100644 index 08f587b..0000000 --- a/docs/rtk_imu_engineering_6dof.md +++ /dev/null @@ -1,108 +0,0 @@ -# RTK–IMU Engineering 6DoF 分支 - -该分支与 V3 `data_only` 严格链路并列，不改变其门禁结论： - -```text -data_only_6dof_accepted = false -``` - -工程分支固定使用真实水平静止场地得到的 R2G 旋转，默认参考值为 -`RPY=[0.4543066, -0.0026392, 0.0122384] deg`，输出始终记录： - -```text -rotation_source = R2G_gravity_level_prior -translation_conditional_on_rotation = true -``` - -## 条件模型 - -估计量是 `l_I = p_ANT1^I`。每个连续质量段使用 HI13 设备时间、raw gyro/acc 动态预积分，融合 RTK Fixed 的 GGA/BESTNAVA 位置、BESTNAVA Doppler velocity 和 Q4 GNHPR 的 ANT1→ANT2 基线。逐段 nuisance state 包含初始姿态、IMU 原点初始位置、速度、gyro bias 和 acc bias。HI13 absolute quaternion/RPY（尤其 absolute yaw）不进入机械外参因子；R2V 仅作诊断。 - -静止相关因子分为两类： - -- `gravity_candidate`：连续约 1.5 s 的低角速度、gyro/acc 方差稳定且加速度模长接近重力；它不能等价为静止。 -- `zupt_static`：在 gravity candidate 基础上，必须同时有至少约 1 s 的连续 BESTNAVA Doppler 速度接近 0。高速或匀速直线不会加入 ZUPT。 - -连续段复用 R0 质量断点：checksum 无效、定位非 Fixed、GNHPR 非 Q4、设备时间回跳、HPR 测量间隙、baseline jump、位置测量间隙或 IMU gap 都会切段，禁止跨断点预积分。 断后片段还必须至少包含 6 个求解节点且持续不少于 5 s；更短的局部欠约束微段不会跨断点拼接，而是直接不进入杆臂优化。 - -## 高程与残差口径 - -- GGA 只约束 XY；GGA MSL altitude 不定义也不参与 ENU-Z。 -- ENU-Z reference 只来自有效 Fixed BESTNAVA altitude；BESTNAVA 才约束 XYZ。 -- 输出分别为 `gga_xy_residual`、`bestnava_xyz_residual` 和 `doppler_velocity_residual`。未进入 Z factor 的 GGA 高度不进入垂向残差统计。 - -## 可观性与验收 - -杆臂可观性不再使用全状态最小奇异向量。状态分为杆臂 `l` 与 nuisance state，对优化 Hessian 计算 Schur complement： - -```text -H_l_marg = H_ll - H_ln pinv(H_nn) H_nl -``` - -只对该 3×3 marginal lever information 做 SVD，并输出： - -- `l_I_marginal_covariance_m2`、`l_I_std_m` -- `lever_information_singular_values` -- `lever_information_condition_number` -- `lever_precision_rank` -- `weakest_lever_direction_I` - -门禁分为两层： - -- `solver_health_gates` 只判断优化是否收敛、数值是否有限且残差未发散。 -- `engineering_acceptance_gates` 使用更严格的杆臂 marginal std/information/rank/condition、BESTNAVA XYZ、GGA XY、Doppler velocity、LOO、bootstrap、旋转敏感性和可选手量一致性。 - -任一核心门禁失败时，`engineering_6dof_accepted=false`。没有完整执行 bootstrap 和 18 组旋转敏感性时，两项门禁明确为 false，不会把阶段性 base/LOO 结果误标为正式放行。 - -Bootstrap 按 session 有放回抽样，并保留重复 session 的 multiplicity；重复抽中的 session 会重复贡献其全部连续段。 - -## 机械杆臂软先验与双解输出 - -`--manual-l-i-m` 仅在同时提供 `--manual-l-i-std-m` 或完整 -`--manual-l-i-covariance-m2` 时才成为白化高斯软因子。求解器始终先运行无先验 -`free_solution`，再运行 `prior_constrained_solution`；输出还包含机械参考及两者到 -参考的差值。当前活动 engineering 解为 prior-constrained 解（若启用），但 acceptance -额外要求 free-solve 也与机械参考一致，软先验不能掩盖不可观或数据矛盾。 - -示例（数值需使用实际机械测量的 1σ，不可把示例值当作默认）： - -```powershell ---manual-l-i-m -0.45072 -0.25682 0.73208 ` ---manual-l-i-std-m -``` -## 坐标转换 - -```text -T_RTK_IMU: p_RTK = R_RTK_IMU p_IMU - R_RTK_IMU l_I -T_IMU_RTK: p_IMU = R_RTK_IMU^T p_RTK + l_I -``` - -RTK 原点是 ANT1，因此 `T_IMU_RTK.translation == l_I`，两矩阵必须互逆。 - -## 分阶段运行 - -第一阶段默认只运行 base fit、marginal observability、residual audit 和 LOO： - -```powershell -python tools/run_rtk_imu_engineering_6dof.py ` - --manifest D:\data\rtk_imu_unified_v3\manifest.json ` - --output artifacts/rtk_imu_calibration_v3/engineering_6dof_base_loo.json ` - --session ` - --session ` - --session -``` - -只有 base/LOO 合理后，才显式启动全量验证： - -```powershell -python tools/run_rtk_imu_engineering_6dof.py ` - --manifest D:\data\rtk_imu_unified_v3\manifest.json ` - --output artifacts/rtk_imu_calibration_v3/engineering_6dof_full.json ` - --session ` - --session ` - --session ` - --run-bootstrap --bootstrap-repetitions 40 --bootstrap-seed 0 ` - --run-rotation-sensitivity -``` - -正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。 \ No newline at end of file diff --git a/docs/rtk_imu_multisource_v3.md b/docs/rtk_imu_multisource_v3.md deleted file mode 100644 index 1615794..0000000 --- a/docs/rtk_imu_multisource_v3.md +++ /dev/null @@ -1,175 +0,0 @@ -# RTK–IMU 多源原始数据与旋转标定 V3 - -## 可行性结论 - -重构方向正确且必要。原始捕获中确实存在旧导出链路忽略的 BESTNAVA、 -PVTSLNA 和 HI13 姿态/四元数。它们可以补充速度、质量、绝对姿态和静止 -重力约束，但本批数据尚不能通过完整旋转门禁： - -- BESTNAVA 约 0.8–1 Hz，严格高速样本很少。 -- 当前动态主要是平面 yaw，R1b 的两个倾斜自由度仍弱可观。 -- HI13 四元数与 GNSS 真北存在稳定但非机械的 yaw 偏差，可能来自磁偏角或 - 内部导航融合。 -- 两个明确水平静止会话的 R2G 很稳定，但它是水平/重力先验约束解。 - -因此，本次重构显著提高了诊断能力，也防止错误外参进入平移；它没有把信息 -不足包装成“标定成功”。 - -## 三批原始数据 - -扫描根目录： - -- 0808：D:\data\raw_serial_capture_v2，仅选 20260808。 -- 0815：D:\data\0815\raw_serial_capture_v2；目录实际包含 20260812–14。 -- 0819：D:\data\0819\raw_serial_capture_v2。 - -按文件名捕获开始时间在 1.5 s 内一一配对： - -| 批次 | G90/HI13 配对 | GNHPR | BESTNAVA | PVTSLNA | Q4 HPR | Fixed Doppler | -|---|---:|---:|---:|---:|---:|---:| -| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 | -| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 | -| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 | - -共完成 55 组统一导出。0815 有一组 G90 没有匹配 HI13，manifest 将其列为 -unmatched。发现一条 NMEA 时间为 0913A：保留原始行，设备时间留空并隔离， -没有回退到 host receive time。 - -速度不等于高速激励。增加速度 ≥1.5 m/s、水平速度标准差 ≤0.25 m/s 后， -三个批次分别只剩 44、8、0 条候选。 - -## 统一导出 - -输出根目录：D:\data\rtk_imu_unified_v3。 - -每组捕获： - -- imu.npz：HI13 system_time、gyro、accel、RPY、WXYZ quaternion、磁场、 - PPS stamp、温度、气压和 host receive UTC。 -- rtk.csv：每条原生异步 GGA/GNHPR/BESTNAVA/PVTSLNA 单独成行，不再把 - HPR/BEST/PVT 最近邻挂到 GGA。 -- export_summary.json：原始捕获摘要、报文数量、四元数范数和设备到 host - 的仿射时钟诊断。 -- manifest.json：全部会话、批次、目录和 unmatched 记录。 - -时间规则： - -1. HI13 system_time 是 IMU 主时间轴。 -2. BESTNAVA/PVTSLNA 使用 GNSS week/TOW 和 leap seconds。 -3. GGA/GNHPR 使用报文自己的 UTC time-of-day。 -4. RTK GNSS 测量时刻通过 HI13 device→host 仿射模型映射进 HI13 设备时钟。 -5. host receive time 只用于跨时钟桥接、延迟和抖动诊断，绝不替代采样时刻。 -6. checksum 无效、时间畸形或非固定解记录保留，但不进入求解。 - -## 新旋转链路 - -### R1b：基线在 IMU 中的 2DoF 方向 - -对连续 Q4 GNHPR 基线和 HI13 陀螺相对旋转使用不变量： - - angle(b_ENU(t0), b_ENU(t1)) - = angle(b_IMU, DeltaR_IMU b_IMU) - -估计 b_IMU 的两个倾斜自由度及逐会话陀螺零偏。安装信息只用于选择 +X -半球，并施加明确记录的弱 20° 先验。按会话等权，输出残差、协方差和信息 -奇异值。 - -### R2V：基线 + Doppler velocity - -筛选条件： - -- BESTNAVA position 为 SOL_COMPUTED/NARROW_INT； -- velocity 为 SOL_COMPUTED/DOPPLER_VELOCITY； -- checksum 有效； -- 水平速度 ≥1.5 m/s、速度标准差 ≤0.25 m/s； -- |IMU gyro_z| ≤3°/s； -- 速度方向与横向基线接近正交； -- GNHPR Q4，并有时间邻近的合法 HI13 quaternion。 - -基线给车右，Doppler velocity 给车前，叉积给车上。HI13 quaternion 的 -body/world 和 ENU/NED 候选全部评分，只保留残差最小者。该方法会把 HI13 -导航 yaw 偏差带入候选，因此必须和 R2G 交叉验证。 - -### R2G：基线 + 水平静止重力 - -只允许调用方明确标记的水平静止会话。本次使用： - -- 0819_20260819_072130（flat_static_hdg207） -- 0819_20260819_073045（flat_static_hdg082） - -每 10 s 分块，要求 gyro norm ≤0.35°/s，且加速度模长距标准重力不超过 -0.15 m/s²。R1b 提供车右，加速度中值提供车上，叉积得到车前。 - -### R3：稳定性与正式门禁 - -R2V/R2G 都输出： - -- 样本和会话数量； -- SO(3) RMS/P95； -- 旋转向量样本标准差和均值协方差； -- leave-one-session； -- 10 样本或 10 s block-out。 - -只有 R1b 可观、R2V 和 R2G 各自稳定、二者差异 ≤2° 时，才将 -translation_unlocked 设为 true。旧 GNHPR 三轴手眼不参与正式门禁。 - -## 首轮实测 - -R1b： - - b_IMU = [0.999999976, -0.000213959, -0.000044368] - tilt_yz = [-0.0123, -0.0025] deg - pairs = 2091 - RMS / P95 = 0.8086 / 1.7105 deg - information singular values = [1.176e-2, 8.476e-4] - std = [27.77, 7.47] deg - gate = failed - -点估计接近 +X 是安装先验与名义轴一致的结果；巨大协方差说明不能宣称 -数据独立估出了这两个小角。 - -R2V： - - qualifying samples / sessions = 41 / 2 - selected quaternion convention = HI13_q_body_to_ENU - RPY = [0.7532, 0.0708, -9.2944] deg - RMS / P95 = 1.8436 / 3.4975 deg - max leave-one-session = 3.4519 deg - gate = failed - -R2G： - - level-static blocks / sessions = 53 / 2 - RPY = [0.4543, -0.0026, 0.0122] deg - RMS / P95 = 0.2542 / 0.2732 deg - max leave-one-session = 0.2688 deg - gate = passed (level/gravity-prior constrained) - -R2V 与 R2G 的 SO(3) 差异为 9.3119°。R3 最终： - - full_rotation_accepted = false - translation_unlocked = false - -## 下一轮数据要求 - -1. BESTNAVA 改为至少 10 Hz，并确认 Doppler velocity 与 GNHPR 使用同一 - GNSS week/TOW 输出周期。 -2. 每个日期都录制多段 ≥3 m/s、持续 20–30 s 的正向直线；包含不同方位， - 避免单一磁环境和单一会话支配。 -3. 为 R1b 增加可控的 roll/pitch 激励；只有平面 yaw 无法稳定估出横向 - 基线的两个微小倾斜角。 -4. 每个日期至少录两种车头方位的明确水平静止段，检验 HI13 重力和绝对 - quaternion 的跨日期稳定性。 -5. 若 HI13 quaternion 的 yaw 来自磁融合，应获取其导航坐标定义、磁偏角 - 设置和融合状态；否则 R2V 只使用其 roll/pitch，yaw 由 GNSS 基线和速度 - 决定。 -6. 上述门禁通过前继续冻结杆臂和平移。 - -## 实现入口 - -- tools/rscap_v2/g90_rtk.py：GGA/GNHPR/BESTNAVA/PVTSLNA 和双 checksum。 -- tools/rscap_v2/hi13_imu.py：HI91 system_time、惯性、姿态和四元数。 -- tools/export_rtk_imu_unified.py：55 组配对、原生异步导出和断点续导。 -- rtk_imu/rtk_imu_multisource.py：R1b/R2V/R2G/R3。 -- tools/run_rtk_imu_multisource.py：多源旋转命令行入口。 -- artifacts/rtk_imu_calibration_v3/multisource_result.json：首轮 R3 结果。 diff --git a/docs/标定流程与采集清单.md b/docs/标定流程与采集清单.md deleted file mode 100644 index a7ff5cc..0000000 --- a/docs/标定流程与采集清单.md +++ /dev/null @@ -1,157 +0,0 @@ -# 纯 LiDAR–IMU：标定流程与采集清单 - -**用途：** 现场怎么采合格数据（看完根目录 [README](../README.md) 后再看本文即可）。 -算法命令与结果判读以 README 为准；改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。 - -仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐采集与流程要点。 -目标外参：`p_IMU = T_IMU_lidar · p_lidar`（`T_A_B` 表示把 B 系点变到 A 系）。 - ---- - -## 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 → 验证后交付 -``` - diff --git a/docs/问题清单.md b/docs/问题清单.md deleted file mode 100644 index 08b23a0..0000000 --- a/docs/问题清单.md +++ /dev/null @@ -1,209 +0,0 @@ -# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题 - -> 数据：`D:\data\calibration_usable_20260808` -> 可用会话：`sessions_v1_host_aligned`（三优先窗） -> 当前结果目录：各窗 `out_fixed_dt0/` -> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json` -> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml` -> 约定外参：`p_IMU = T_IMU_lidar · p_lidar` - ---- - -## 1. 一句话结论 - -**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。** -三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。 - ---- - -## 2. 当前可用结果（`out_fixed_dt0`） - -约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。 - -| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior | -|----|------|----|---------------------|--------------|----------|--------------| -| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° | -| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° | -| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° | - -- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。 -- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。 -- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。 - -### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar` - -- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json` -- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]` -- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]` - -```text -R = -[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ], - [ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ], - [-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]] -t = [0, 0, 0] -``` - -### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar` - -- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json` -- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]` -- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]` - -```text -R = -[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ], - [ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ], - [ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]] -t = [0, 0, 0] -``` - -### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar` - -- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json` -- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]` -- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]` - -```text -R = -[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ], - [ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ], - [ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]] -t = [0, 0, 0] -``` - -### 相对历史失败轮次 - -| 轮次 | 问题 | 结果 | -|------|------|------| -| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° | -| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） | -| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 | - ---- - -## 3. 已澄清并写入配置的坐标系 / 先验 - -### 3.1 车体与传感器 - -- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。 -- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。 -- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。 -- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。 - -### 3.2 安装量（`translation_m`） - -| 传感器 | X / Y（后轮轴中心） | Z（离地） | -|--------|---------------------|-----------| -| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** | -| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** | - -- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。 -- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。 - -### 3.3 导出外参先验 - -- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。 -- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。 -- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。 - ---- - -## 4. 现存问题清单 - -### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移） - -- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。 -- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。 -- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。 -- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。 -- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。 - -### P2. 平面运动导致竖直平移弱可观 - -- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。 -- 流水线门控已给出 `translation_accepted=false`。 -- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。 - -### P3. 时间偏移若再自由估计会被带偏（已规避，需保持） - -- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。 -- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。 -- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。 - -### P4. 单窗低残差 ≠ 外参正确（历史教训） - -- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。 -- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。 -- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。 - -### P5. 旋转软先验尚未做无先验对照 - -- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。 -- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。 - -### P6. 文档与操作约定未完全同步（工程） - -- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。 -- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。 - ---- - -## 5. 不该做 / 可以做 - -| 动作 | 建议 | -|------|------| -| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） | -| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** | -| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 | -| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** | -| 无先验 / 大 σ 旋转对照 | **建议做** | -| 冻结交付 `R` + `δt=0` 说明 | **建议做** | -| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 | - ---- - -## 6. 建议下一步顺序 - -1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。 -2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。 -3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。 -4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。 - ---- - -## 7. 常用路径与命令 - -```text -数据根： - D:\data\calibration_usable_20260808\sessions_v1_host_aligned\ - -结果： - ...\priority_XXXX\out_fixed_dt0\summary.json - ...\priority_XXXX\out_fixed_dt0\motion_pairs.json - ...\calibration_manifest_fixed_dt0.json -``` - -```powershell -# 可视化（rotation_only 默认模式4用 GICP 平移） -python tools\visualize_pair_3d.py ` - --lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar ` - --summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json ` - --pair-index 0 - -# 若要看「坏 Δp」导致的错层效果： -# --mode4-translation imu -``` - ---- - -## 8. 问题优先级（跟踪用） - -| ID | 严重度 | 状态 | 标题 | -|----|--------|------|------| -| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 | -| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 | -| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） | -| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 | -| P5 | 中 | 待做 | 无旋转先验对照 | -| P6 | 低 | 待做 | README/交付物同步 | diff --git a/docs/雷达-IMU标定.md b/docs/雷达-IMU标定.md new file mode 100644 index 0000000..6327406 --- /dev/null +++ b/docs/雷达-IMU标定.md @@ -0,0 +1,166 @@ +# 雷达–IMU 标定 + +本文是本仓库雷达–IMU 标定的唯一规范说明，覆盖目标、数据格式、运行方式、当前实车结果和采集要求。 + +## 目标与当前结论 + +目标是求雷达坐标系到 IMU 坐标系的安装关系： + +```text +p_IMU = T_IMU_lidar · p_lidar +``` + +`T_A_B` 表示把 B 坐标系中的点变换到 A 坐标系。流程利用连续行驶中的相对运动，而不是把 IMU 加速度二次积分的长时间轨迹当作绝对真值。 + +当前 2026-08-08 实车数据的结论是：**旋转和主机桥接后的时间关系可作为候选冻结；完整平移不可交付。** 不应把旋转结果和零平移拼成看似完整的 4×4 外参，更不能把当前 IMU 位移预积分用于正式平移标定。 + +| 项目 | 当前状态 | +| --- | --- | +| 连续运动雷达–IMU 流程 | 已实现并有合成测试 | +| 实车旋转候选 | 三个窗口一致，可用于进一步验证 | +| 实车时间关系 | 主机桥接后冻结为 `0 s`，不再自由精修 | +| 实车完整平移 | 不可观且受预积分位移误差影响，拒绝交付 | + +## 方法概览 + +```text +设备时间与惯性质量检查 + → 估计或确认两传感器时间关系 + → 雷达关键帧配准得到相对运动 + → 同一时间区间的 IMU 预积分得到相对运动 + → 先求旋转手眼关系 + → 只有平移可观时才进入完整六自由度优化 +``` + +对关键帧区间 `[i, j]`： + +```text +A_ij ≈ IMU 预积分相对运动 +B_ij ≈ 雷达点云配准相对运动 +A_ij · X ≈ X · B_ij +X = T_IMU_lidar +``` + +旋转子问题为 `R_A · R_X = R_X · R_B`。旋转通常比平移稳定：加速度偏置和重力方向的微小误差会在位移积分中快速累积，平面转弯又难以提供竖直杆臂信息。因此，完整六自由度只有在残差、跨会话一致性和可观性同时通过时才允许输出。 + +## 当前实车结果与限制 + +当前可用的三段优先窗口均在主机时间桥接后固定 `time_offset_s=0`，并禁用自由的有符号时间精修： + +| 会话窗口 | roll / pitch / yaw（°） | 手眼旋转 RMS（°） | +| --- | --- | ---: | +| `priority_174005_174515` | `-0.398 / 0.108 / 89.998` | 0.625 | +| `priority_174905_175450` | `-0.316 / -0.352 / 90.002` | 0.293 | +| `priority_175910_180530` | `-0.373 / -0.036 / 90.005` | 0.786 | + +三段旋转相互差约为 `0.15°–0.47°`。但以下问题阻止平移放行： + +1. IMU 预积分的位移，尤其竖直分量，会出现米级甚至更大的漂移；它不能作为可靠平移观测。 +2. 当前重点会话以平面转弯为主，缺少持续坡道或俯仰激励，竖直平移弱可观。 +3. 允许时间偏移自由精修时，时间与旋转曾发生相互补偿，产生跨窗口矛盾的航向。因此桥接后的偏移固定为零，而不是再次为降低残差调整它。 +4. 单一窗口残差小不等于外参正确；必须检查跨窗口一致性与点云叠加。 + +这也是仓库增加 RTK–IMU 流程的原因：不是替代雷达旋转标定，而是为天线相位中心到 IMU 的平移提供更直接的 GNSS 位置和速度观测。两条流程边界见仓库 [README](../README.md)。 + +## 输入、输出与配置 + +### 输入 + +```text +imu.csv + t,gx,gy,gz,ax,ay,az + 单位：秒、rad/s、m/s²；t 应为设备时间 + +lidar_session/ + frames_index.csv 帧编号、文件名、开始/结束时间 + frames/frame_XXXXX.npz 米制 XYZ 点云 + +vehicle_installation.yaml + 坐标轴、时间语义、可选机械安装信息 +``` + +原始 HI13 IMU `.rscap` 和 H32 雷达 dlog/MSOP 可转换为上述格式： + +```powershell +python tools\export_rscap_to_v1.py ` + --imu-rscap path\to\hi13-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 +``` + +### 输出 + +| 文件 | 含义 | +| --- | --- | +| `T_IMU_lidar.json` | 通过验收时的外参；仅旋转模式下不能把其中平移当交付值 | +| `time_offset.json` | 定义为 `t_imu = t_lidar + δt` 的时间关系 | +| `summary.json` | 状态、残差、可观性和拒绝原因 | +| `motion_pairs.json` | 用于复核和点云可视化的相对运动对 | + +## 运行与判读 + +安装依赖： + +```powershell +python -m pip install -e ".[dev]" +python -m pip install -e ".[open3d]" # 点云处理需要时安装 +``` + +先运行仅旋转模式： + +```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 +``` + +| `summary.json` 状态 | 含义 | +| --- | --- | +| `rotation_only_accepted` | 旋转与时间关系可进入独立验证；不代表平移可用 | +| `full_se3_accepted` | 仅当完整平移可观且门禁通过时，才可交付完整外参 | +| `full_se3_rejected_due_to_observability` | 旋转可用，平移拒绝 | +| `blocked` | 时间、质量或模型门禁失败，不能作安装参数 | + +合成复现可验证软件链路，不证明实车精度： + +```powershell +powershell -File tools\reproduce_synthetic.ps1 +python -m pytest -q +``` + +## 采集要求 + +单次有效会话建议： + +```text +静止 20–30 s → 连续低速多方向转弯 3–8 min → 静止 10–20 s +``` + +- 必须保留雷达和 IMU 的设备时间戳；不能仅依赖主机接收时间。 +- 在结构化环境采集，保证点云配准具有稳定几何约束。 +- 旋转标定优先包含左右圆或“8”字；直线加减速有助于时间和水平平移。 +- 若要尝试完整平移，增加连续缓坡、俯仰或其他竖直激励；仅平面运动通常不足。 +- 标定全过程不得改变两传感器安装关系。 + +## 代码结构 + +`imu_lidar/` 是雷达–IMU 专用实现： + +| 组件 | 作用 | +| --- | --- | +| `cli.py`、`pipeline.py` | 命令行入口与流程编排 | +| `lidar_io.py`、`registration.py`、`keyframes.py` | 点云读取、配准和关键帧 | +| `timestamp_audit.py`、`imu_audit.py`、`time_offset.py` | 时间和 IMU 质量检查、时间关系估计 | +| `imu_preintegration.py`、`motion_pairs.py` | IMU 预积分与相对运动对 | +| `rotation_handeye.py`、`joint_optimizer.py`、`observability.py` | 旋转求解、联合优化和可观性门禁 | +| `finalize.py` | 结果写出 | + +历史改动见 [imu_lidar/CHANGELOG.md](../imu_lidar/CHANGELOG.md)，自动化测试说明见 [tests/README.md](../tests/README.md)。 \ No newline at end of file diff --git a/imu_lidar/CHANGELOG.md b/imu_lidar/CHANGELOG.md index c409474..feba068 100644 --- a/imu_lidar/CHANGELOG.md +++ b/imu_lidar/CHANGELOG.md @@ -103,7 +103,7 @@ - **原本**：对外说明仍偶发「方案二」等旧称呼；根 README 缺少一眼可读的阶段 / 合成 vs 旧车 / 合格数据预期；烟测配置与对比脚本文件名带 `scheme2`。 - **改成**： - 根 `[README.md](../README.md)` 增加 §0「现状一览」；明确仓库只有一条连续运动标定路径。 - - `[tests/README.md](../tests/README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明同步边界与阶段。 + - `[tests/README.md](../tests/README.md)`、`[docs/雷达-IMU标定.md](../docs/雷达-IMU标定.md)`、本目录说明同步边界与阶段。 - `config/s2_old_smoke.yaml`、`tools/compare_s2_runs.py` 替换旧 `*scheme2*` 命名。 --- @@ -131,7 +131,7 @@ ### 文档同步 + 合成数据一键复现 -- **原本**：`docs/标定流程与采集清单.md` 仍偏旧版「待写代码 / 因子图设想」；根 README 缺少清晰的一键复现入口与输入输出总表。 +- **原本**：采集与流程说明仍偏旧版「待写代码 / 因子图设想」；根 README 缺少清晰的一键复现入口与输入输出总表。 - **改成**： - 采集清单与现行流水线对齐（完整预积分、δt↔R 交替、可观时再估平移）。 - 新增 `tools/reproduce_synthetic.py` / `.ps1`、`tools/show_calibration_report.py`；合成生成写入 `meta.json`；根 README 增加「系统输入输出 + 一键复现」。 @@ -148,8 +148,8 @@ - **原本**：根 README / `docs` / 包说明仍对照已删除的静站路径与内部阶段黑话；`pyproject` 仍声明已删除的 `static_station` 包。 - **改成**： - - 删除旧静站文档；采集清单定为 `[docs/标定流程与采集清单.md](../docs/标定流程与采集清单.md)`。 - - 根 `[README.md](../README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明改为对外可读，只保留连续运动标定路径。 + - 删除旧静站文档；采集清单并入规范说明。 + - 根 `[README.md](../README.md)`、`[docs/雷达-IMU标定.md](../docs/雷达-IMU标定.md)`、本目录说明改为对外可读，只保留连续运动标定路径。 - `pyproject.toml` 仅保留 `imu_lidar` / `tools`。 --- diff --git a/imu_lidar/文件职责说明.md b/imu_lidar/文件职责说明.md deleted file mode 100644 index b63df12..0000000 --- a/imu_lidar/文件职责说明.md +++ /dev/null @@ -1,82 +0,0 @@ -# `imu_lidar` 模块说明 - -**用途：** 改 `imu_lidar/` 源码时查阅。对外用法见根目录 [`README.md`](../README.md)。 - -本包实现 LiDAR–IMU 外参标定：在连续行驶数据上选取关键帧，用 IMU 预积分与雷达配准构造相对运动对，求解安装外参。 - -```text -A ≈ 关键帧间 IMU 相对运动（预积分：旋转 / 速度增量 / 位移增量） -B ≈ 关键帧间雷达配准 -解 R_A R_X = R_X R_B → 旋转外参（手眼阶段只用旋转） -再精修旋转与陀螺零偏；在完整六自由度模式下，可观时再估计平移等 -``` - -入口： - -```powershell -python -m imu_lidar.cli plan -python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output ... -``` - -整体流程由 `pipeline.py` 串联。 -修改本目录代码时，请同步更新本说明，并在 [`CHANGELOG.md`](CHANGELOG.md) 追加「时间戳 + 原本 → 改成」。 - ---- - -## 流水线顺序与文件 - - -| 顺序 | 文件 | 作用 | -| --- | ----------------------- | ----------------------------- | -| 0 | `contracts.py` | 公共数据类型与状态枚举 | -| 0 | `geometry.py` | 刚体变换与旋转工具 | -| 0 | `vehicle_config.py` | 读取并校验车辆 YAML | -| 1 | `imu_io.py` | 读标准 IMU 中间格式 | -| 1 | `lidar_io.py` | 读标准雷达会话目录 | -| 2 | `timestamp_audit.py` | 时间单调 / 频率 / 空洞检查 | -| 3 | `imu_audit.py` | 静止零偏、加速度模长检查、建议竖直轴 | -| 4 | `time_offset.py` | 粗估时间偏置 δt，并用旋转外参精修 | -| 5 | `registration.py` | 帧间点云配准 | -| 5 | `keyframes.py` | 按运动量抽取关键帧 | -| 5 | `lidar_deskew.py` | 可选点云去畸变（低速可关） | -| 6 | `imu_preintegration.py` | IMU 预积分（旋转及速度/位移增量、协方差、零偏雅可比） | -| 6 | `motion_pairs.py` | 构造运动对；手眼使用其中的旋转 | -| 6 | `motion_pairs_io.py` | 运动对 JSON 缓存读写（供可视化直读） | -| 7 | `rotation_handeye.py` | 加权旋转手眼 | -| 8 | `observability.py` | 旋转 / 平移可观性检查 | -| 8 | `joint_optimizer.py` | 联合精修；完整模式下可估计平移、重力、速度与时变零偏 | -| 9 | `finalize.py` | 写出结果 JSON（含 `motion_pairs.json`） | -| — | `pipeline.py` | 编排全流程 | -| — | `cli.py` | 命令行入口 | -| — | `CHANGELOG.md` | 改动记录 | - - ---- - -## 运行模式要点 - -- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。 -- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。 -- `--mode full_se3`：当前完成 Phase-A 后明确拒绝平移；待 Phase-B/C 会话状态重构完成后再恢复完整 SE(3) 交付。 - ---- - -## 输入格式 - -```text -imu.csv # t,gx,gy,gz,ax,ay,az（建议设备时间） -lidar_session/ - frames_index.csv # frame_id,filename,t_start,t_end - frames/frame_XXXXX.npz # points: (N,3) 米 -``` - -原始 N300 `.rscap` + H32 dlog（或旧 MSOP `.rscap`）用仓库工具导出：`python tools/export_rscap_to_v1.py ...`（见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md)）。 - ---- - -## 当前能力 - -- 本包是仓库**唯一**标定路径：质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →（可选）完整六自由度 → 报告 -- 点云去畸变：可选 -- 阶段与用法见根目录 [`README.md`](../README.md) -- 改动史：[`CHANGELOG.md`](CHANGELOG.md) diff --git a/rtk_imu/README.md b/rtk_imu/README.md deleted file mode 100644 index 9d8b2a9..0000000 --- a/rtk_imu/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# RTK-IMU Calibration Package - -`rtk_imu/` contains the independent RTK-IMU calibration pipeline: G90 observation I/O, antenna-baseline rotation, V1 replay, V3 multisource stages, engineering 6DoF, and the node-state factor graph. - -## Boundary - -This package may import only shared `imu_lidar` utilities: `contracts`, `geometry`, `geodesy`, `imu_io`, `imu_preintegration`, and the generic `rotation_handeye` initializer. - -It must not import LiDAR-specific modules such as `lidar_io`, `lidar_deskew`, `registration`, `pipeline`, `phase_a`, or `joint_optimizer`. The LiDAR-IMU pipeline remains in `imu_lidar/`. - -## Entrypoints -Use `tools/run_rtk_imu_*.py` and `tools/audit_rtk_imu_*.py`; these scripts import from `rtk_imu.*`. -For the engineering procedure, current result, acceptance boundary, and reproduction commands, see [`README_RTK_IMU.md`](../README_RTK_IMU.md).