调整雷达到RTK标定分支为独立根目录结构

2026-07-24 08:42:16 +08:00
parent d2aae6177e
commit 6d87b6ba9c
132 changed files with 214 additions and 197832 deletions
@@ -1,226 +1,185 @@
-# LiDAR–双天线 RTK 手眼标定
+# 双天线RTK—3D LiDAR直接手眼标定

-本仓库提供一套可从原始 Medulla 记录复现的静态站点标定流程，求解三维激光雷达到后轮轴中心车体系的外参
+本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达坐标转换到RTK导航坐标系。它**不是** `base_link` 车体外参，也不会在求解阶段使用车体航向偏置或RTK到后轮轴的XY杆臂。
+
+## 1. 输出坐标约定
+
+统一约定 `T_A_B` 把B系点变换到A系：

 ```text
-X = T_body_lidar
+p_RTK = T_RTK_lidar · p_lidar
 ```

-约定 `T_A_B` 将 B 系坐标变换到 A 系。对任意站点 i、j：
+RTK导航系在本仓库中定义为：
+
+- 原点：GGA位置参考点（通常为ANT1相位中心，必须结合接收机配置确认）；
+- X轴：`rawHeading`所表示的双天线基线在水平面的投影；
+- Y轴：左；
+- Z轴：上；
+- ENU航向：`yaw = 90° - rawHeading`；
+- roll、pitch：当前轨迹中固定为0。
+
+如果下游需要 `T_body_lidar`，必须另有经过确认的 `T_body_rtk`：

 ```text
-A_ij = T_W_Bi^-1 T_W_Bj       # RTK 给出的车体相对运动
-B_ij = T_Li_Lj                # GICP 给出的雷达相对运动
-A_ij X = X B_ij
+T_body_lidar = T_body_rtk · T_RTK_lidar
 ```

-当前部署建议仍采用 [results/01_previous_two_batches/final_extrinsic_deployment.json](results/01_previous_two_batches/final_extrinsic_deployment.json)。data4 是一次独立重算，结果与部署值相差约 `1.592 cm / 0.234°`，但自身 AX 残差更高，因此只作为候选和稳定性证据，不自动替换部署值。
+## 2. 算法流程

-## 标定总流程
-
-```mermaid
-flowchart LR
-  raw["原始站点 dlog / RTK、IMU rscap"] --> export["分别解析并统一到时间轴"]
-  export --> assoc["按每个 LiDAR 帧关联 RTK/IMU，导出 NPZ"]
-  assoc --> prep["每站选一帧，构建 RTK 车体位姿 A"]
-  prep --> b1["small_gicp 求 B"]
-  prep --> b2["Open3D GICP 求 B"]
-  b1 --> gate["与 X 无关的质量筛选及双后端一致性"]
-  b2 --> gate
-  gate --> solve["AX=XB + 地面约束求 X"]
-  solve --> check["残差、bootstrap、条件数、跨批复核和 3D 可视化"]
+```text
+逐站LiDAR dlog + RTK.rscap + IMU.rscap
+  → 分别解析并保留原始字段
+  → 以LiDAR帧时间为索引关联RTK/IMU，生成combined NPZ
+  → 每站选择一帧静态点云，GGA转局部ENU，rawHeading构造yaw-only RTK pose
+  → Open3D GICP和small_gicp分别求 B_ij = T_Li_Lj
+  → 留出点、Hessian、正反向、多初值和旋转共轭不变量筛选
+  → 两后端共同认可的边形成consensus B
+  → A_ij X = X B_ij + 地面法向/高度约束求 X = T_RTK_lidar
+  → bootstrap、双后端差异、逐对残差和3D可视化检查
 ```

-流程有两个原始数据入口：
+代码实际使用：

- 旧式数据：LiDAR 和 `GPS-POST-Z` 位于每个站点 dlog 中，使用 `export_legacy_stations.ps1`。
- 新式多传感器数据：LiDAR 位于逐站 dlog，RTK 与 IMU 是独立 `.rscap`，使用 `export_multisensor_stations.ps1`。处理顺序是统一时间轴、分别解析、按 LiDAR 帧关联、导出 NPZ。
+```text
+A_ij = inv(T_W_Ri) · T_W_Rj = T_Ri_Rj
+B_ij = T_Li_Lj              # 将站点j点云变换到站点i
+A_ij · X = X · B_ij
+X = T_RTK_lidar
+```

-IMU 会在新式数据中原样解析并随 LiDAR 帧关联保存，但当前 LiDAR–RTK 外参求解不使用 IMU，也不做运动畸变校正，因为每一站采集点云时车辆静止。IMU 外参应使用单独的激励数据和专用标定流程求解。
+## 3. 原始数据目录

-## 三批数据的角色
+大体积数据不提交Git。`DataRoot`下每个站点必须是一个独立dlog目录，至少包含：

-| 数据 | 原始格式 | 站点 | RTK 情况 | 在本仓库中的角色 |
-|---|---|---:|---|---|
-| 第一批 | 逐站 dlog，内嵌 GPS-POST-Z | 38 | 约 10 秒一条，部分站仅 1–11 个有效样本 | 辅助复核，不承担主要求解 |
-| 第二批 | 逐站 dlog，内嵌 GPS-POST-Z | 38 | 每站约 125–412 个有效样本，航向稳定 | 现部署外参的主要求解数据 |
-| data4 | 逐站 LiDAR dlog + 独立 RTK/IMU rscap | 34 | 11,678 个 LiDAR 帧均成功关联 fixed RTK、heading 和 IMU | 独立重算与跨批比较 |
+```text
+raw_dataset/
+├── stations/
+│   ├── 001/
+│   │   ├── dobject/
+│   │   └── dobject_recording/
+│   ├── 002/
+│   └── ...
+└── captures/
+    ├── rtk.rscap
+    └── imu.rscap
+```

-原始数据体积较大，不在 Git 仓库中。复现者应从云盘取得第一批、第二批或 data4 的原始目录，并在命令行传入路径。
+每个站点应在车辆完全静止后记录点云；建议不少于30站，并包含充足的直行、左转、右转和大角度转向姿态变化。

-## 环境
+## 4. 环境安装

- Windows PowerShell 5.1 或 PowerShell 7
- Python 3.10+
- `pip install -r requirements.txt`
- `small_gicp` 后端需要可导入 `small_gicp`；Open3D 后端需要 `open3d`
-
-所有脚本从自身位置推导仓库根目录。数据和输出路径均由参数传入，不依赖开发者电脑上的固定路径。
-
-## 从原始数据开始复现
-
-以下路径只表示格式，请替换为自己的目录。
-
-### A. 第一批、第二批旧式 dlog
+已验证环境为Windows、PowerShell、Python 3.11。安装依赖：

 ```powershell
-$Repo = "D:\你的代码目录\calibration"
-
-powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_legacy_stations.ps1" `
-  -DataRoot "D:\你的数据目录\batch2_raw" `
-  -OutputRoot "D:\你的输出目录\batch2_export"
-
-powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_legacy_dataset.ps1" `
-  -ExportRoot "D:\你的输出目录\batch2_export" `
-  -Output "D:\你的输出目录\batch2_prepared" `
-  -HeadingOffsetDeg 21.226 `
-  -AntennaLever -0.320,-0.365,0.620 `
-  -ExpectedStations 38 `
-  -HeadingStdLimitDeg 0.5
+python -m pip install -r requirements.txt
 ```

-第一批采用同一导出方式，但应在导出命令显式添加 `-RtkMaxDtMs 15000`；因其 RTK 稀疏，准备阶段也不建议沿用 `0.5°` 的严格站内航向离散度阈值。上述 `21.226°` 和杆臂 `[-0.320,-0.365,0.620] m` 是本项目已有两批数据采用的配置，不是通用常数；换车或改变天线安装后必须重新确认。
+依赖包括NumPy、SciPy、Open3D和small_gicp。若small_gicp没有对应Windows wheel，可在WSL2中安装后运行Python核心命令，或先只运行Open3D后端；完整共识流程需要两个后端都可用。

-### B. data4 式独立 RTK/IMU rscap
+## 5. 从原始数据一键复现
+
+在仓库根目录执行，路径由使用者通过参数传入，脚本内没有本机绝对路径：

 ```powershell
-powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_multisensor_stations.ps1" `
-  -DataRoot "D:\你的数据目录\data4_raw" `
-  -RtkCapture "D:\你的数据目录\captures\rtk.rscap" `
-  -ImuCapture "D:\你的数据目录\captures\imu.rscap" `
-  -OutputRoot "D:\你的输出目录\data4_export"
+$Repo = (Resolve-Path ".").Path
+$Raw = "E:\calibration_data\data4"
+$Out = "E:\calibration_output\rtk_lidar"

-powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_multisensor_dataset.ps1" `
-  -CombinedRoot "D:\你的输出目录\data4_export\combined" `
-  -Output "D:\你的输出目录\data4_prepared" `
-  -HeadingOffsetDeg 21.226 `
-  -AntennaLever -0.320,-0.365,0.620 `
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" `
+  -DataRoot "$Raw\stations" `
+  -RtkCapture "$Raw\captures\rtk.rscap" `
+  -ImuCapture "$Raw\captures\imu.rscap" `
+  -OutputRoot $Out `
+  -RtkReferenceHeightAboveGroundM 0.8535 `
  -ExpectedStations 34
 ```

-### C. 运行标定
-
-单批数据同时运行 small_gicp、Open3D GICP 和跨后端共识：
-
-```powershell
-powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_single_dataset.ps1" `
-  -Prepared "D:\你的输出目录\data4_prepared" `
-  -OutputRoot "D:\你的输出目录\data4_calibration" `
-  -BodyHeight 0.2335
-```
-
-复现本仓库“第二批求解、第一批辅助复核”的历史流程：
-
-```powershell
-powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" `
-  -Batch1Prepared "D:\你的输出目录\batch1_prepared" `
-  -Batch2Prepared "D:\你的输出目录\batch2_prepared" `
-  -OutputRoot "D:\你的输出目录\two_batch_calibration"
-```
-
-`BodyHeight=0.2335 m` 是后轮轴中心离地高度，用于把雷达地面平面约束转换到车体原点；它不是雷达离地高度。换轮胎、胎压或车体载荷后应重新测量并评估不确定度。
-
-## 如何判断结果
-
-不能仅凭 `AX=XB` 残差或一张叠图宣称外参正确。至少同时检查：
-
-1. B 的配准质量：收敛、有效对应点数/比例、RMSE、Hessian 信息矩阵特征值与条件数、small_gicp 与 Open3D 的相对运动一致性。
-2. X 的可观性：加权雅可比条件数越接近 1 越均衡；极大值说明存在弱方向，但没有脱离尺度和数据分布的单一硬阈值。本次 data4 为 `7.44`，不是病态。
-3. AX 残差分布：看 RMS、median、P90/P95、max 和逐对异常，而不是只看均值。
-4. 重采样稳定性：bootstrap 的六自由度标准差及置信区间。
-5. 跨批检查：同一 X 在独立批次的残差，以及两批独立求出的 X 之 SE(3) 差。
-6. 3D 可视化：模式 3 的 B 与模式 4 的 `X^-1 A X` 是否都使相同墙面、杆件和地面重合。
-
-当前没有把“增量小于某个值”当成绝对真值判据。工程筛查可先关注多数优质运动对是否约在厘米级到数厘米、亚度级；但阈值必须结合场景尺度、点云分辨率、RTK 质量和车辆运动幅度制定。模式 3 已错位时优先检查 B；模式 3 正常而模式 4 系统性错位时，再检查 A、坐标约定和 X。
-
-## 结果摘要
-
-| 结果 | 平移 xyz (m) | roll/pitch/yaw (deg) | AX 平移/旋转 RMS | 结论 |
-|---|---|---|---|---|
-| 部署值（第二批求解） | `[1.297760,-0.000067,0.720498]` | `[-0.785151,1.202661,-0.835510]` | `0.07985 m / 0.96118°`，39 对 | 当前建议部署 |
-| data4 独立候选 | `[1.300376,-0.001707,0.704877]` | `[-0.791892,1.393823,-0.970743]` | `0.11762 m / 1.24257°`，26 对 | 独立候选，不替换部署值 |
-
-data4 候选相对部署值变化 `1.592 cm / 0.234°`，其中 z 低 `1.562 cm`。旧部署值作用于 data4 的残差约 `0.11953 m / 1.24836°`；data4 候选作用于历史第二批的残差约 `0.07931 m / 0.98926°`。两个 X 的跨批表现接近，当前数据不足以证明 data4 的较低 z 更接近真值。
-
-## 仓库目录
+主要输出：

 ```text
-code/       标定、配准筛选、共识、比较和可视化核心程序
-tools/      原始 dlog/rscap 解析、时间关联、NPZ 导出和数据准备
-run/        不含本机固定路径的 PowerShell 入口
-results/    历史两批、data4 与跨批比较三个结果目录
+$Out/
+├── exported/
+│   ├── export/          # 各站LiDAR逐帧NPZ
+│   ├── parsed/          # RTK/IMU JSONL
+│   └── combined/        # 按LiDAR帧关联后的多传感器NPZ
+├── prepared_rtk_direct/
+│   ├── frames_all/      # 每站选中的静态帧
+│   └── reference_poses_rtk_gga_raw_heading.csv
+└── calibration/
+    ├── open3d_gicp/
+    ├── small_gicp/
+    ├── consensus/
+    ├── summary.json
+    └── final_T_RTK_lidar.json
 ```

-完整复现流程和所有主要文件职责均在本 README；`run/README.md`、`tools/README.md` 和 [results/README.md](results/README.md) 只是目录内快速索引。
+若已经有`combined/`，可跳过原始导出：

-## 代码、工具和运行入口职责
+```powershell
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
+  -CombinedRoot "E:\calibration_output\exported\combined" `
+  -WorkRoot "E:\calibration_output\prepared_rtk_direct" `
+  -OutputRoot "E:\calibration_output\calibration" `
+  -RtkReferenceHeightAboveGroundM 0.8535 `
+  -ExpectedStations 34
+```

-根 README 是本仓库唯一的完整复现说明。`run/README.md`、`tools/README.md` 和 `results/README.md` 只作为进入对应目录时的快速索引，不承载另一套流程。
+## 6. 3D可视化

-### code：标定核心
+```powershell
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
+  -Frames "$Out\prepared_rtk_direct\frames_all" `
+  -Pairs "$Out\calibration\consensus\B_consensus.npz" `
+  -Extrinsic "$Out\calibration\final_T_RTK_lidar.json" `
+  -PairIndex 0
+```

-| 文件 | 职责 |
-|---|---|
-| `rigorous_calibration.py` | 核心 CLI。`ground` 拟合每站地面；`pairs` 用 small_gicp 或 Open3D GICP 求 B 和质量指标；`calibrate` 联合 AX=XB 与地面约束求 X；`validate` 计算指定 X 的逐对残差。 |
-| `refine_pairs.py` | 根据收敛、RMSE、对应关系、Hessian/信息矩阵和运动覆盖筛选 B；不读取 X，避免循环挑选。 |
-| `cross_backend_filter.py` | 对齐 Open3D 与 small_gicp 的同一站点对，只保留两个后端相互一致的 B。 |
-| `finalize_consensus.py` | 汇总历史两批的 consensus B、外参和第一批辅助复核。 |
-| `summarize_results.py` | 汇总两个后端的外参、B 质量和跨批检查，生成推荐结果。 |
-| `compare_extrinsics.py` | 在 SE(3) 上计算两套外参的严格相对平移和旋转差。 |
-| `visualize_pair_3d.py` | 交互显示原始点云、RTK A、GICP B、`X^-1AX`，并打印 `B^-1(X^-1AX)` 数值增量。 |
-| `scan_extrinsic_sensitivity.py` | 对 X 左乘小角度 roll/pitch/yaw 扰动，检查指定运动对的局部敏感方向。 |
+窗口中：

-### tools：原始数据到 prepared
+- 蓝色：目标站点i；橙色：站点j；
+- `1`：原始点云；
+- `2`：RTK运动A直接作为初值；
+- `3`：GICP测得的B；
+- `4`：最终外参预测的 `X^-1 A X`；
+- `Q/Esc`：退出。

-| 文件 | 职责 |
-|---|---|
-| `frontlidar_dlog_export.py` | 从 Medulla dlog 导出传感器坐标系 `points_raw`；旧格式可同时匹配站内 GPS-POST-Z。 |
-| `prepare_station_dataset.py` | 从旧式逐站导出中每站选择一帧，计算 yaw-only RTK 后轮轴位姿并生成 prepared。 |
-| `build_multisensor_npz.py` | 将独立 LiDAR、RTK、IMU 统一到 LiDAR 帧索引并生成 combined NPZ。 |
-| `prepare_multisensor_station_dataset.py` | 从 combined NPZ 选择每站静止帧，生成与旧流程相同的 prepared 接口。 |
-| `rscap_v2/capture_format_v2.py` | 读取 rscap v2 文件头、原始记录块和文件尾。 |
-| `rscap_v2/audit_capture_v2.py` | 审计 capture 完整性、时间范围和记录统计。 |
-| `rscap_v2/parse_rtk_imu_v2.py` | 分别解析 RTK 与 IMU capture，输出 JSONL。 |
-| `rscap_v2/pipeline_common*.py` | rscap 解析、时间处理和采集格式兼容的共用逻辑。 |
+模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应查看多对，不能只挑视觉效果最好的一对。

-### run：推荐 PowerShell 入口
+## 7. data4参考结果

-| 文件 | 职责 |
-|---|---|
-| `export_legacy_stations.ps1` | 批量导出 LiDAR 与 RTK 同在逐站 dlog 中的旧格式。 |
-| `export_multisensor_stations.ps1` | 解析独立 RTK/IMU，导出逐站 LiDAR，并按 LiDAR 帧建立关联。 |
-| `prepare_legacy_dataset.ps1` | 旧式导出结果转换为 prepared。 |
-| `prepare_multisensor_dataset.ps1` | combined 多传感器结果转换为 prepared。 |
-| `run_single_dataset.ps1` | 单批数据同时完成两个 GICP 后端、B 筛选、consensus 和 X 求解。 |
-| `run_all.ps1` | 历史流程：第二批求解，第一批稀疏 RTK 数据作辅助复核。 |
-| `run_consensus_finish.ps1` | 在已有两个后端 B 的基础上重新生成 consensus 和汇总结果。 |
-| `run_sensitivity_scan.ps1` | 对历史运动对执行外参角度灵敏度扫描。 |
-| `view_result.ps1` | 传入匹配的 frames、B 和 X，运行交互式 3D 可视化。 |
-
-### 标定核心文件的数据关系
+仓库保留了精简参考产物，见[`results/reference_data4`](results/reference_data4/README.md)：

 ```text
-原始 dlog/rscap
-  -> tools 导出和时间关联
-  -> prepared/{frames_all, body_poses_*.csv}
-  -> rigorous_calibration.py pairs 生成 A、B
-  -> refine_pairs.py 做与 X 无关的 B 筛选
-  -> cross_backend_filter.py 生成 consensus B
-  -> rigorous_calibration.py calibrate 联合 AX=XB 与地面约束求 X
-  -> validate / visualize_pair_3d.py 做数值和三维检查
+translation_m = [1.638179350, -0.240844799, 0.084481236]
+RPY_deg_xyz   = [-0.817167459, 1.323288119, -22.104163318]
+
+站点：34
+共识运动对：25
+AX Translation RMS：0.100207 m
+AX Rotation RMS：1.252794°
+Weighted Jacobian condition：7.739413
+Open3D vs small_gicp：0.003889 m / 0.188431°
 ```

+唯一建议下游读取的参考结果是[`final_T_RTK_lidar.json`](results/reference_data4/final_T_RTK_lidar.json)。

-## 重要限制
+## 8. z与精度限制

- RTK 车体姿态当前是双天线 heading 构造的 yaw-only 轨迹；没有用 RTK pitch/roll 构造 A。
- 新式解析器保存 IMU 与 RTK pitch 等原始字段，但当前手眼方程未融合 IMU。
- 静止站点法不估计 LiDAR–RTK 时间偏移；时间戳关联必须在导出阶段通过审计。
- 地面约束负责 roll、pitch 和 z 的补充可观性，不会独立求出另一套六自由度外参。
- 仓库归档的是结果和轻量 B 文件，不包含云盘中的原始点云数据。
+平面阿克曼运动不能独立观测z。参考结果使用34站地面平面和RTK参考点离地`0.8535 m`约束z；该高度必须量到实际GGA参考点/天线相位中心。更改参考高度后必须重新求解。

-## 专题说明
+AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。

- [运动对诊断](PAIR_DIAGNOSTICS.md)
- [双后端共识筛选](CONSENSUS_SELECTION.md)
- [结果文件索引](results/README.md)
+此外，代码无法单独证明GGA对应哪根物理天线、`rawHeading`是ANT1→ANT2还是ANT2→ANT1；必须用接收机配置、接线和现场运动实验确认。方向错误会导致RTK坐标系yaw相差约180°。
+
+## 9. 仓库目录
+
+| 目录 | 职责 |
+|---|---|
+| [`code/`](code/) | GICP、运动对质量评价、AX=XB求解、结果封装和3D可视化 |
+| [`tools/`](tools/) | 原始dlog/rscap解析、按LiDAR帧关联及静态站点prepared生成 |
+| [`run/`](run/) | PowerShell入口；所有数据和输出路径都通过参数传入 |
+| [`results/reference_data4/`](results/reference_data4/) | 可提交Git的精简参考结果，不包含点云和本机过程目录 |
+| `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 |
+
+各代码文件职责见[`code/README.md`](code/README.md)，命令索引见[`run/README.md`](run/README.md)，工具说明见[`tools/README.md`](tools/README.md)。