255 lines
11 KiB
Markdown
255 lines
11 KiB
Markdown
# 双天线RTK—3D LiDAR直接手眼标定
|
||
|
||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达坐标转换到RTK导航坐标系。它**不是** `base_link` 车体外参，也不会在求解阶段使用车体航向偏置或RTK到后轮轴的XY杆臂。
|
||
数据下载地址：https://fs.fairylandtech.com:5001/FRLD/#file_id=963272954246902180 账号：lichun.qu@fairylandtech.com 密码：lichun.qu
|
||
|
||
## 1. 输出坐标约定
|
||
|
||
统一约定 `T_A_B` 把B系点变换到A系：
|
||
|
||
```text
|
||
p_RTK = T_RTK_lidar · p_lidar
|
||
```
|
||
|
||
RTK导航系在本仓库中定义为：
|
||
|
||
- 原点：GGA位置参考点（通常为ANT1相位中心，必须结合接收机配置确认）；
|
||
- X轴：`rawHeading`所表示的双天线基线在水平面的投影；
|
||
- Y轴：左；
|
||
- Z轴：上；
|
||
- ENU航向：`yaw = 90° - rawHeading`；
|
||
- roll、pitch：当前轨迹中固定为0。
|
||
|
||
如果下游需要 `T_body_lidar`，必须另有经过确认的 `T_body_rtk`：
|
||
|
||
```text
|
||
T_body_lidar = T_body_rtk · T_RTK_lidar
|
||
```
|
||
|
||
## 2. 算法流程
|
||
|
||
```text
|
||
逐站 H32.rscap + 全程 RTK.rscap + IMU.rscap
|
||
→ tools/export_raw_to_combined.py（一步导出标定中间包 combined/）
|
||
→ 每站选择一帧静态点云，位置转局部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可视化检查
|
||
```
|
||
|
||
代码实际使用：
|
||
|
||
```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
|
||
```
|
||
|
||
## 3. 原始数据目录
|
||
|
||
大体积数据不提交Git。**新车默认布局**（H32 用 `RSLidarH32_3D_DLogCaptureNet48`；G90/N300 仍为 `.rscap`）：
|
||
|
||
```text
|
||
raw_dataset/
|
||
├── stations/ # 每站停稳后单独录一段雷达 dlog
|
||
│ ├── 001/ # dobject/ + dobject_recording/
|
||
│ │ ├── dobject/ # 含 frontlidar-msop-raw / frontlidar-difop-raw
|
||
│ │ └── dobject_recording/
|
||
│ ├── 002/
|
||
│ └── ...
|
||
└── captures/
|
||
├── rtk.rscap # 进场到收工连续录（G90：#PVTSLNA + #UNIHEADINGA）
|
||
└── imu.rscap # 连续录（N300；仅关联，不参与外参求解）
|
||
```
|
||
|
||
一键导出默认：雷达用 **MSOP 设备时间** + DIFOP 通道角，RTK 用 **GNSS week/TOW** 做最近邻关联（`-TimeBasis device_gnss`）。
|
||
仍兼容旧站 `h32.rscap`；旧「已解码点云」dlog 需用 `-TimeBasis host`。
|
||
|
||
每个站点应在车辆完全静止后记录点云；建议不少于30站，并包含充足的直行、左转、右转和大角度转向姿态变化。
|
||
|
||
## 4. 环境安装
|
||
|
||
已验证环境为Windows、PowerShell、Python 3.11。安装依赖：
|
||
|
||
```powershell
|
||
python -m pip install -r requirements.txt
|
||
```
|
||
|
||
依赖包括NumPy、SciPy、Open3D和small_gicp。若small_gicp没有对应Windows wheel，可在WSL2中安装后运行Python核心命令，或先只运行Open3D后端；完整共识流程需要两个后端都可用。
|
||
|
||
## 5. 从原始数据一键复现
|
||
|
||
导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级：**一条命令**把原始 H32 dlog + G90/N300 rscap 变成标定可直接使用的 `combined/`。
|
||
|
||
仅导出中间包：
|
||
|
||
```powershell
|
||
python tools\export_raw_to_combined.py `
|
||
--stations-root "$Raw\stations" `
|
||
--rtk-rscap "$Raw\captures\rtk.rscap" `
|
||
--imu-rscap "$Raw\captures\imu.rscap" `
|
||
--out "$Out\exported" `
|
||
--overwrite
|
||
```
|
||
|
||
产物：
|
||
|
||
```text
|
||
$Out\exported\
|
||
├── export/ # 内部：各站雷达帧（调试用）
|
||
├── parsed/ # 内部：RTK/IMU JSONL
|
||
├── combined/ # ★ 标定入口：关联后的多传感器 NPZ + manifest.csv
|
||
└── export_summary.json
|
||
```
|
||
|
||
完整求解（导出 + prepare + AX=XB）在仓库根目录执行：
|
||
|
||
```powershell
|
||
$Repo = (Resolve-Path ".").Path
|
||
$Raw = "E:\calibration_data\data4"
|
||
$Out = "E:\calibration_output\rtk_lidar"
|
||
|
||
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.758 `
|
||
-ExpectedStations 34
|
||
```
|
||
|
||
主要输出：
|
||
|
||
```text
|
||
$Out/
|
||
├── exported/
|
||
│ ├── export/ # 内部各站LiDAR帧
|
||
│ ├── 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
|
||
```
|
||
|
||
若已经有`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.758 `
|
||
-ExpectedStations 34
|
||
```
|
||
|
||
## 6. 3D可视化
|
||
|
||
```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
|
||
```
|
||
|
||
窗口中：
|
||
|
||
- 蓝色：目标站点i；橙色：站点j；
|
||
- `1`：原始点云；
|
||
- `2`：RTK运动A直接作为初值；
|
||
- `3`：GICP测得的B；
|
||
- `4`：最终外参预测的 `X^-1 A X`；
|
||
- `N` / `]`：下一运动对；
|
||
- `P` / `[`：上一运动对；
|
||
- `Q` / `Esc`：退出。
|
||
|
||
模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应用 `N`/`P` 多看几对，不能只挑视觉效果最好的一对。
|
||
|
||
## 7. data4与data4+data5结果对比
|
||
|
||
data5补充了30个有效静态站点及两个法向方向不同的固定平面板。当前联合流程只合并data4、data5各自的批内运动对，不构造跨批次运动，因此两次采集的时间、ENU原点和绝对位置不同不会直接影响共享外参；前提是传感器安装未改变，并且两批数据使用相同的RTK坐标定义和LiDAR原始坐标定义。
|
||
|
||
两块平面板在现有代码中作为点云场景结构参与GICP配准，但没有作为已知RTK/ENU平面方程单独加入优化；若后续能测得板面方程，才可新增绝对平面约束。
|
||
|
||
为公平比较，下面两组结果都使用ANT1参考点离地高度`0.758 m`重新求解：
|
||
|
||
| 指标 | data4单独 | data4+data5联合 | 变化 |
|
||
|---|---:|---:|---:|
|
||
| 有效站点 | 34 | 64 | +30 |
|
||
| 共识运动对 | 25 | 36 | +11 |
|
||
| 平移残差RMS | 0.100394 m | 0.086902 m | -13.4% |
|
||
| 平移残差中位数 | 0.062075 m | 0.053647 m | -13.6% |
|
||
| 平移残差P95 | 0.123039 m | 0.125166 m | +1.7% |
|
||
| 平移残差最大值 | 0.353438 m | 0.355864 m | +0.7% |
|
||
| 旋转残差RMS | 1.252391° | 1.115207° | -11.0% |
|
||
| 旋转残差中位数 | 0.747183° | 0.685164° | -8.3% |
|
||
| 旋转残差P90 | 1.825297° | 1.481369° | -18.8% |
|
||
| 旋转残差P95 | 1.965290° | 1.875860° | -4.6% |
|
||
| Weighted Jacobian condition | 7.713973 | 8.379217 | +8.6% |
|
||
| bootstrap z标准差 | 0.003147 m | 0.001957 m | -37.8% |
|
||
| bootstrap roll标准差 | 0.099319° | 0.062245° | -37.3% |
|
||
| bootstrap pitch标准差 | 0.096049° | 0.064370° | -33.0% |
|
||
|
||
联合结果为：
|
||
|
||
```text
|
||
translation_m = [1.642932528, -0.242302311, 0.180599708]
|
||
RPY_deg_xyz = [-0.886210651, 1.372780243, -22.112054052]
|
||
|
||
T_RTK_lidar =
|
||
0.926183553 0.376030869 0.028014474 1.642932528
|
||
-0.376311142 0.926478119 0.005312208 -0.242302311
|
||
-0.023957243 -0.015462238 0.999593402 0.180599708
|
||
0.000000000 0.000000000 0.000000000 1.000000000
|
||
```
|
||
|
||
与相同高度下的data4单独结果相比，联合外参相差`5.25 mm / 0.064°`。data5使RMS、中位数、旋转P90和bootstrap稳定性改善，但平移P95及最大值没有改善，说明少数高残差运动对仍然存在；不应仅为降低最大值而按最终外参残差删边。
|
||
|
||
已经分别得到各批次的共识运动对和地面平面时，可运行：
|
||
|
||
```powershell
|
||
$Names = @("data4", "data5")
|
||
$Pairs = @("E:\data4\consensus\B_consensus.npz", "E:\data5\consensus\B_consensus.npz")
|
||
$Planes = @("E:\data4\common\ground_planes.csv", "E:\data5\common\ground_planes.csv")
|
||
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" `
|
||
-BatchNames $Names -Pairs $Pairs -GroundPlanes $Planes `
|
||
-OutputRoot "E:\calibration_output\data4_data5_joint" `
|
||
-RtkReferenceHeightAboveGroundM 0.758 -Bootstrap 200
|
||
```
|
||
|
||
脚本会先分别拟合各批次外参；任一批与首批相差超过`0.25 m`或`5°`时中止，提示检查RTK航向/坐标定义和传感器安装。阈值可通过`-MaxBatchTranslationDifferenceM`和`-MaxBatchRotationDifferenceDeg`显式调整。
|
||
|
||
仓库内[`results/reference_data4`](results/reference_data4/README.md)是历史data4参考产物，使用旧高度配置，不应与上表直接比较，也不应继续作为当前联合外参下发。
|
||
|
||
## 8. z与精度限制
|
||
|
||
平面阿克曼运动不能独立观测z。当前联合结果使用64站地面平面和ANT1参考点离地`0.758 m`约束z；其中`0.758 m`来自本次现场粗测的天线底部安装参考高度`0.710 m`，加上天线标签给出的L1/L2 PCO高度`46/50 mm`的中值`48 mm`。该值仍是测量输入，不是手眼运动方程自行估计出来的量。
|
||
|
||
旧联合结果曾使用`0.8535 m = 0.2335 m + 0.620 m`，得到`z = 0.085093 m`；改用`0.758 m`并重新求解后得到`z = 0.180600 m`，z增加约`0.095507 m`，而x、y和旋转基本不变。所有标定入口现均要求显式提供参考高度，更改高度后必须重新求解，不能只手工修改输出JSON中的z。
|
||
|
||
AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。
|
||
|
||
此外，代码无法单独证明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/) | 历史data4精简参考结果，不包含点云和本机过程目录 |
|
||
| `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 |
|
||
|
||
各代码文件职责见[`code/README.md`](code/README.md)，命令索引见[`run/README.md`](run/README.md)，工具说明见[`tools/README.md`](tools/README.md)。
|