229 lines
10 KiB
Markdown
229 lines
10 KiB
Markdown
# 双天线 RTK—3D 激光雷达严谨手眼标定
|
||
|
||
本仓库是当前建议提交和继续维护的正式版本。第二批 38 站高频 RTK 用于主要求解；第一批 38 站稀疏 RTK 只作辅助复核。Open3D GICP 和 small_gicp 分别生成雷达运动 (B)，两套独立结果及跨后端共识结果均保留。
|
||
|
||
实际部署只读取 [`results/final_extrinsic_deployment.json`](results/final_extrinsic_deployment.json)。算法原始求解、残差、Bootstrap 和筛选记录保留在其他结果文件中，不应与部署文件混用。
|
||
|
||
## 标定总体流程
|
||
|
||
1. **准备静止站点数据**：每站车辆静止，保存一帧原始雷达点云和该站 RTK；程序只使用雷达传感器坐标系下的 `points_raw`。
|
||
2. **由 RTK 生成车体运动 (A)**：将双天线 RTK 的位置、水平航向、杆臂和 heading offset 转为车体位姿 (T_{WB_i})，再计算
|
||
|
||
\[
|
||
A_{ij}=T_{WB_i}^{-1}T_{WB_j}.
|
||
\]
|
||
3. **由点云配准生成雷达运动 (B)**：Open3D GICP 和 small_gicp 分别把第 j 站点云配准到第 i 站，得到
|
||
|
||
\[
|
||
B_{ij}=T_{L_iL_j}.
|
||
\]
|
||
4. **先验证和筛选 B，再求 X**：使用对称留出点、重叠率、留出点 RMSE、海森矩阵、正反向一致性、多初值稳定性、旋转共轭不变量和闭环误差检查 B。这个阶段不读取待求外参 X，也不使用 AX 残差筛选 B。
|
||
5. **建立跨后端共识 B**：只保留 Open3D 与 small_gicp 对同一运动对的 B 相差不超过 `0.05 m / 0.50°` 的边；最终使用 Open3D B，small_gicp 作为独立一致性门控。
|
||
6. **求解手眼外参 X**：固定 A 和通过筛选的 B 后，求解
|
||
|
||
\[
|
||
A_{ij}X=XB_{ij},\qquad X=T_{body\leftarrow lidar}.
|
||
\]
|
||
|
||
使用 Huber 鲁棒最小二乘、12 个初值、地面平面约束和 100 次 Bootstrap。
|
||
7. **结果复核**：第二批共识对用于主要求解和整体残差统计；第一批重新独立生成 B，只作为稀疏 RTK 辅助复核；3D 可视化分别比较模式 3 的 GICP B 与模式 4 的 (X^{-1}AX)。
|
||
8. **形成部署结果**：第二批跨后端共识结果给出完整的 `[x,y,z,roll,pitch,yaw]`；部署 JSON 直接采用该算法结果。。
|
||
|
||
### 验证逻辑
|
||
|
||
顺序固定为：
|
||
|
||
```text
|
||
先生成 B
|
||
-> 用不依赖 X 的点云与几何指标筛 B
|
||
-> 用另一个 GICP 后端复核同一 B
|
||
-> 固定共识 B
|
||
-> 最后才求 X
|
||
-> AX 残差只用于结果评价，不反过来选择 B
|
||
```
|
||
|
||
代码证据：
|
||
|
||
- [`code/refine_pairs.py`](code/refine_pairs.py) 不读取外参文件，只使用留出点、正反向和旋转共轭不变量等指标；
|
||
- [`code/cross_backend_filter.py`](code/cross_backend_filter.py) 只比较两个后端的 B，不读取 X；
|
||
- [`code/rigorous_calibration.py`](code/rigorous_calibration.py) 在 B 集合固定后才执行 `calibrate` 求 X；
|
||
- 第一批辅助复核的 B 也重新独立生成，且不使用 AX 残差筛选。
|
||
|
||
但这些检查仍不能把 B 变成绝对真值：重复结构、动态物体或错误局部最优仍可能让两个后端同时出错，所以必须结合模式 3 点云质量、留出集指标和多运动对整体统计判断。
|
||
|
||
## 最终部署外参
|
||
|
||
坐标变换定义：
|
||
|
||
\[
|
||
X=T_{body\leftarrow lidar}
|
||
\]
|
||
|
||
即将原始雷达点从雷达坐标系变换到以后轮轴中心为原点、X 前/Y 左/Z 上的车体坐标系。
|
||
|
||
部署值：
|
||
|
||
- 平移 `[x,y,z] = [1.297760, -0.000067, 0.720498] m`
|
||
- RPY `[roll,pitch,yaw] = [-0.785151, 1.202661, -0.835510] deg`
|
||
- 唯一部署文件：[`results/final_extrinsic_deployment.json`](results/final_extrinsic_deployment.json)
|
||
|
||
该值来自第二批 39 对跨后端共识、完整地面平面约束和 Bootstrap 稳定性检查。
|
||
|
||
算法在完整地面平面模型下的原始结果为：
|
||
|
||
- 平移 `[1.297760, -0.000067, 0.720498] m`
|
||
- RPY `[-0.785151, 1.202661, -0.835510] deg`
|
||
- 文件：[`results/final_extrinsic_recommended.json`](results/final_extrinsic_recommended.json)
|
||
|
||
`final_extrinsic_recommended.json` 保留完整求解、残差和 Bootstrap 记录，用于复现和审计；`final_extrinsic_deployment.json` 是唯一部署入口。
|
||
|
||
共识结果统计：
|
||
|
||
- 第二批 39 对 AX RMS：`0.07985 m / 0.96118°`
|
||
- 第一批 22 对辅助复核：`0.06067 m / 1.00298°`
|
||
- 结果摘要：[`results/final_summary.json`](results/final_summary.json)
|
||
|
||
两套后端分别求得：
|
||
|
||
| 后端 | 平移 `[x,y,z]` m | RPY deg | 第二批 AX RMS |
|
||
|---|---|---|---|
|
||
| Open3D GICP | `[1.297883,-0.003100,0.721789]` | `[-0.757207,1.146042,-0.879781]` | `0.1002 m / 1.0115°` |
|
||
| small_gicp | `[1.299630,-0.003565,0.721886]` | `[-0.787156,1.141456,-0.935757]` | `0.1187 m / 1.1351°` |
|
||
|
||
两后端单独求出的 X 相差 1.81 mm、0.063°。共识求解的 100 次 Bootstrap 标准差 `[x,y,z,roll,pitch,yaw]` 为：
|
||
|
||
```text
|
||
[0.00322 m, 0.00333 m, 0.00169 m, 0.0980°, 0.0746°, 0.1189°]
|
||
```
|
||
|
||
这里的 Z Bootstrap 只描述当前地面模型条件下的内部稳定性，不代表机械高度的绝对精度。
|
||
|
||
重要限制：旋转 RMS 仍约 1°，所以这不是厘米级绝对精度认证。远距离点云仍可能看到角度错层；Bootstrap 也不包含 RTK 参考点、航向偏置或杆臂定义错误等系统误差。
|
||
|
||
## 坐标与公式
|
||
|
||
\[
|
||
A_{ij}=T_{WB_i}^{-1}T_{WB_j},\qquad
|
||
B_{ij}=T_{L_iL_j},\qquad
|
||
A_{ij}X=XB_{ij}
|
||
\]
|
||
|
||
`B_ij` 把第 j 站雷达点变换到第 i 站雷达坐标系。程序只读取 `points_raw`，禁止使用已经变到车体坐标系的点。
|
||
|
||
RTK 轨迹使用 `body_poses_rear_gga_raw_rear_to_front.csv`，对应后天线 GGA 位置和双天线后到前方向。
|
||
|
||
地面约束采用完整平面变换：
|
||
|
||
\[
|
||
n_B=R_Xn_L,\qquad r_h=d_L-n_B^Tt_X-h_{body}
|
||
\]
|
||
|
||
不再使用旧近似式 `d_L-(t_z+h_body)`。
|
||
|
||
## 详细求解流程
|
||
|
||
1. 第二批高频 RTK 生成 A；第一批稀疏 RTK 不参与主要求解。
|
||
2. 不使用手量外参作初值：生成 B 时 (X_0=I)，所以 (B_0=A)；求 X 也从单位变换和随机初值开始。
|
||
3. 两后端均采用三级粗到细：体素 0.30/0.15/0.08 m，对应距离 1.20/0.50/0.25 m。
|
||
4. 逐对检查对称留出点、海森矩阵、正反向一致性、多初值稳定性和 B 三角闭环。
|
||
5. 使用重叠率、留出点 RMSE 和旋转共轭不变量筛选 B；不使用 AX 残差筛选。
|
||
6. 两后端分别求 X；随后使用跨后端 B 一致性得到最终 39 对共识集合。
|
||
7. 使用 Huber 鲁棒最小二乘、12 个随机初值、完整地面约束和 100 次 Bootstrap 求算法 X。
|
||
8. 第一批重新独立生成 B，仅作辅助复核，不参与第二批求解。
|
||
9. 使用共识算法完整解形成精简部署 JSON，不覆盖算法原始结果。
|
||
|
||
## 目录
|
||
|
||
```text
|
||
code/
|
||
rigorous_calibration.py
|
||
refine_pairs.py
|
||
cross_backend_filter.py
|
||
summarize_results.py
|
||
finalize_consensus.py
|
||
scan_extrinsic_sensitivity.py
|
||
visualize_pair_3d.py
|
||
run/
|
||
run_all.ps1
|
||
run_consensus_finish.ps1
|
||
run_sensitivity_scan.ps1
|
||
view_open3d_result.ps1
|
||
view_small_gicp_result.ps1
|
||
results/
|
||
common/
|
||
open3d_gicp/
|
||
small_gicp/
|
||
consensus/
|
||
diagnostics/
|
||
final_extrinsic_deployment.json # 实际部署只读取这个文件
|
||
final_extrinsic_recommended.json # 算法原始结果及详细诊断
|
||
final_summary.json
|
||
```
|
||
|
||
## 安装与完整重跑
|
||
|
||
```powershell
|
||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||
python -m pip install -r "$Repo\requirements.txt"
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1"
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"
|
||
```
|
||
|
||
如果 prepared 数据移动了：
|
||
|
||
```powershell
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" -Batch1Prepared "你的第一批prepared目录" -Batch2Prepared "你的第二批prepared目录"
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"
|
||
```
|
||
|
||
重跑 `run_consensus_finish.ps1` 会重新生成算法原始结果，但不会自动生成或覆盖已经人工确认的 `final_extrinsic_deployment.json`。
|
||
|
||
## 3D 可视化
|
||
|
||
Open3D 单后端结果：
|
||
|
||
```powershell
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_open3d_result.ps1" -PairIndex 0
|
||
```
|
||
|
||
small_gicp 单后端结果：
|
||
|
||
```powershell
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_small_gicp_result.ps1" -PairIndex 0
|
||
```
|
||
|
||
蓝色是目标站 i，橙色是源站 j。按键：`1` 原始；`2` RTK 初始 A；`3` GICP B；`4` 外参预测的 (X^{-1}AX)；指定左乘 RPY 试验量后，`5` 为试验结果。
|
||
|
||
程序打印的差值为：
|
||
|
||
\[
|
||
B^{-1}(X^{-1}AX)
|
||
\]
|
||
|
||
其 xyz/RPY 分量位于站点 j 的雷达局部坐标系，不是屏幕坐标。模式 3 好而模式 4 差，说明 A/X 一致性不足；模式 3 本身差，说明该 B 不能用于判断外参。
|
||
|
||
共识运动对可直接指定部署外参：
|
||
|
||
```powershell
|
||
python "$Repo\code\visualize_pair_3d.py" --frames "C:\Users\admin\Documents\Codex\2026-07-15\wo\outputs\calibration_data1_20260720\prepared\frames_all" --pairs "$Repo\results\consensus\B_batch2_consensus.npz" --extrinsic "$Repo\results\final_extrinsic_deployment.json" --pair-index 0
|
||
```
|
||
|
||
局部灵敏度扫描和判断规则见 [`PAIR_DIAGNOSTICS.md`](PAIR_DIAGNOSTICS.md)。
|
||
|
||
## 关键质量文件
|
||
|
||
- [`results/open3d_gicp/B_batch2_quality.json`](results/open3d_gicp/B_batch2_quality.json)：海森矩阵、留出点、正反向和多初值信息；
|
||
- [`results/open3d_gicp/B_batch2_refined.refinement.json`](results/open3d_gicp/B_batch2_refined.refinement.json)：二级筛选原因；
|
||
- [`results/consensus/B_batch2_consensus.consensus.json`](results/consensus/B_batch2_consensus.consensus.json)：跨后端一致性筛选；
|
||
- [`results/consensus/batch1_auxiliary_check.json`](results/consensus/batch1_auxiliary_check.json)：第一批辅助复核；
|
||
- [`results/final_summary.json`](results/final_summary.json)：算法结果摘要；
|
||
- [`results/diagnostics/`](results/diagnostics/)：局部修正对单对及全体运动对的影响。
|
||
|
||
## 必须外部确认的假设
|
||
|
||
- `lat/lon/h` 确为后天线相位中心；
|
||
- `rawHeading` 确为后天线指向前天线；
|
||
- heading offset 数值和正负号正确；
|
||
|
||
任何一项变化，都必须更新相应约束；RTK 定义变化时需要重新生成 A 并重跑。
|