300 lines
11 KiB
Markdown
300 lines
11 KiB
Markdown
# 双天线RTK—3D LiDAR直接手眼标定
|
||
|
||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **车头向前的 RTK 车体系**（主天线原点）。
|
||
求解不使用 RTK 到后轮轴的 XY 杆臂；与雷达–IMU 外参对照时旋转系一致，平移仍差天线原点。
|
||
|
||
当前交付标定（2026-08 室外车，27 站）约定如下：
|
||
|
||
| 项 | 值 |
|
||
|---|---|
|
||
| RTK 坐标系 | **车头向前**（`HeadingOffsetDeg = -90`；主从装反、基线朝右） |
|
||
| 天线相位中心离地高 | **1.9165 m**（1916.5 mm） |
|
||
| 机械初值（车头系） | \(t=(+0.21086,-0.41418,+0.07850)\) m，yaw=**0°**（CAD 纵向已按车头正向取 +X） |
|
||
| 物理基线 | `baseline_points=vehicle_right`（主天线车左，从天线车右，后轴中心左右对称） |
|
||
| 姿态 | 双天线 pitch/roll：`R = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(+90°)` |
|
||
| 地面点 ROI | LiDAR 系 **`z ∈ [-2.5, -1.5]`**（约 2 m 车顶安装） |
|
||
| pair 配准 | **禁止**使用外参 seed；B 与 X 独立 |
|
||
|
||
数据下载（历史 data4 等）：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 导航系（**车头向前 / vehicle_forward_heading_offset**）：
|
||
|
||
- 原点：GGA 位置参考点（主天线 / ANT1 相位中心）；
|
||
- X 轴：车头向前（`rawHeading + HeadingOffsetDeg`，本车 `HeadingOffsetDeg = -90`）；
|
||
- Y 轴：左；
|
||
- Z 轴：上；
|
||
- 姿态：先在基线系应用双天线 pitch/roll，再乘固定 `Rz(-heading_offset)`；不是 IMU 融合姿态。
|
||
|
||
> 改 `HeadingOffsetDeg` 或姿态模型后必须从 **prepare** 起重跑；禁止事后只改 JSON 里的 yaw。
|
||
> 旧基线系结果（`HeadingOffsetDeg = 0`）与车头系外参不可混用。
|
||
|
||
机械初值文件：[`run/rtk_lidar_mechanical_initial.json`](run/rtk_lidar_mechanical_initial.json)
|
||
**仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。**
|
||
|
||
---
|
||
|
||
## 2. 算法流程
|
||
|
||
```text
|
||
原始雷达 + RTK（+ 可选 IMU）
|
||
→ combined/（按站关联的多传感器 NPZ）
|
||
→ 每站选一帧静态点云 + RTK pose（车头向前，含双天线 pitch/roll）
|
||
→ Open3D GICP 与 small_gicp 分别求 B_ij = T_Li_Lj（无外参 seed）
|
||
→ 留出点、正反向、旋转共轭不变量等精筛
|
||
→ 双后端共识边 → 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
|
||
A_ij · X = X · B_ij
|
||
X = T_RTK_lidar
|
||
```
|
||
|
||
---
|
||
|
||
## 3. 原始数据与导出
|
||
|
||
大体积数据不提交 Git。常见两种采集形态：
|
||
|
||
### 3.1 每站独立雷达目录（旧/标准站目录）
|
||
|
||
```text
|
||
raw_dataset/
|
||
├── stations/001|002|.../ # H32 dlog 或 h32.rscap
|
||
└── captures/
|
||
├── rtk.rscap
|
||
└── imu.rscap # 仅关联，不参与外参求解
|
||
```
|
||
|
||
```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
|
||
```
|
||
|
||
默认时间基：`-TimeBasis device_gnss`（雷达设备时 ↔ GNSS week/TOW）。
|
||
|
||
### 3.2 G90 连续录制 + H32 DLog 按站时间窗（本次 27 站）
|
||
|
||
站不在独立目录，而在多个 Medulla DLog ZIP 与 G90 `.rscap` 中时：
|
||
|
||
```powershell
|
||
python tools\export_g90_h32_windows_to_combined.py `
|
||
--segments-csv <rtk_lidar_station_segments.csv> `
|
||
--lidar-dlog <dump_1.zip> --lidar-dlog <dump_2.zip> `
|
||
--rtk-rscap <g90_1.rscap> --rtk-rscap <g90_2.rscap> `
|
||
--out <output_root> --expected-stations 27 --frame-stride 5
|
||
```
|
||
|
||
该入口用 **主机接收 UTC** 做近邻关联（`time_basis_mode: host`），并保留设备时间供审计。
|
||
可加 `--reuse-export` 在已有 `export/` 上续跑。
|
||
|
||
采集建议：有效静站 ≥30（更好 40～60）；相邻站转角约 **15°～30°**；避免一长串同朝向停车；场内宜有墙/立柱及 2～3 块法向不同的固定平面板。
|
||
|
||
---
|
||
|
||
## 4. 环境安装
|
||
|
||
Windows + PowerShell + Python 3.11：
|
||
|
||
```powershell
|
||
python -m pip install -r requirements.txt
|
||
```
|
||
|
||
依赖：NumPy、SciPy、Open3D、small_gicp。完整共识需要两个配准后端。
|
||
|
||
---
|
||
|
||
## 5. 一键复现（匹配本次标定）
|
||
|
||
### 5.1 已有 `combined/`（推荐复现本次结果）
|
||
|
||
```powershell
|
||
$Repo = (Resolve-Path ".").Path
|
||
$Data = "D:\data\rtk_lidar_run" # 含 combined/
|
||
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
|
||
-CombinedRoot "$Data\combined" `
|
||
-WorkRoot "$Data\prepared_vehicle_h19165" `
|
||
-OutputRoot "$Data\outputs_vehicle_h19165" `
|
||
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||
-HeadingOffsetDeg -90 `
|
||
-ExpectedStations 27 `
|
||
-MinStations 20 `
|
||
-GroundZMin -2.5 `
|
||
-GroundZMax -1.5 `
|
||
-Bootstrap 200
|
||
```
|
||
|
||
关键参数：
|
||
|
||
| 参数 | 本次取值 | 说明 |
|
||
|---|---|---|
|
||
| `-RtkReferenceHeightAboveGroundM` | **1.9165** | GGA/ANT1 相位中心离地高（m），必填 |
|
||
| `-HeadingOffsetDeg` | **-90** | 车头向前（主从装反、基线朝右）；`0` 才是基线系 |
|
||
| `-GroundZMin/Max` | **-2.5 / -1.5** | 约 2 m 车顶雷达；旧默认 `[-1.4,-0.4]` 会拟合到墙 |
|
||
| `-ExpectedStations` | **27** | 本批站数 |
|
||
| `-MinStations` | **20** | 远程旧脚本曾写死 30，会跑不了本批 |
|
||
|
||
pair 阶段**不会**传入 `--initial-extrinsic`；机械初值只进最终 AX=XB。
|
||
|
||
### 5.2 站目录原始数据一键（导出 + 求解）
|
||
|
||
```powershell
|
||
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 1.9165 `
|
||
-ExpectedStations 27 `
|
||
-GroundZMin -2.5 `
|
||
-GroundZMax -1.5
|
||
```
|
||
|
||
主要输出：
|
||
|
||
```text
|
||
$Out/
|
||
├── exported/combined/ # 或外部已有 combined/
|
||
├── prepared_*/frames_all/
|
||
├── prepared_*/reference_poses_rtk_gga_raw_heading.csv
|
||
└── calibration/ 或 outputs_*/
|
||
├── open3d_gicp/ small_gicp/ consensus/
|
||
├── common/ground_planes.csv
|
||
├── summary.json
|
||
└── final_T_RTK_lidar.json
|
||
```
|
||
|
||
---
|
||
|
||
## 6. 3D 可视化
|
||
|
||
查看本次结果：
|
||
|
||
```powershell
|
||
$Repo = "D:\First-dev-dept\calibration-rtk-run"
|
||
$Out = "D:\data\rtk_lidar_run\outputs_baseline_h19165"
|
||
$Work = "D:\data\rtk_lidar_run\prepared_baseline_h19165"
|
||
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||
-Frames "$Work\frames_all" `
|
||
-Pairs "$Out\consensus\B_consensus.npz" `
|
||
-Extrinsic "$Out\final_T_RTK_lidar.json" `
|
||
-PairIndex 0
|
||
```
|
||
|
||
通用模板（把路径换成你的 `WorkRoot` / `OutputRoot`）：
|
||
|
||
```powershell
|
||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||
-Frames "$WorkRoot\frames_all" `
|
||
-Pairs "$OutputRoot\consensus\B_consensus.npz" `
|
||
-Extrinsic "$OutputRoot\final_T_RTK_lidar.json" `
|
||
-PairIndex 0
|
||
```
|
||
|
||
| 按键 | 含义 |
|
||
|---|---|
|
||
| `1` | 原始点云 |
|
||
| `2` | 仅用 RTK 运动作初值 |
|
||
| `3` | GICP 测得的 B |
|
||
| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） |
|
||
| `N` / `]` | 下一运动对 |
|
||
| `P` / `[` | 上一运动对 |
|
||
| `Q` / `Esc` | 退出 |
|
||
|
||
蓝 = 站 i，橙 = 站 j。请用 `N`/`P` **多看大转角对**，不要只看前几对同朝向站。
|
||
|
||
---
|
||
|
||
## 7. 当前标定结果（车头向前，h = 1.9165 m）
|
||
|
||
> **状态：可作车头系候选交付**（`recommended_for_deployment: true`）。
|
||
> 约定：`HeadingOffsetDeg=-90`，双天线 pitch/roll，机械初值 \(t=(+0.21086,-0.41418,+0.07850)\)，yaw=0。
|
||
> 旧 `outputs_baseline_h19165` 仍不可用。
|
||
|
||
结果目录：`D:\data\rtk_lidar_run\outputs_vehicle_h19165\`
|
||
文件：`final_T_RTK_lidar.json` / `summary.json`
|
||
|
||
```text
|
||
translation_m = [0.217822250, -0.411347802, 0.106542337]
|
||
RPY_deg_xyz = [0.066239, 0.809662, -0.551322]
|
||
|
||
T_RTK_lidar ≈
|
||
0.999854 0.009639 0.014119 0.217822
|
||
-0.009621 0.999953 -0.001292 -0.411348
|
||
-0.014131 0.001156 0.999899 0.106542
|
||
0 0 0 1
|
||
```
|
||
|
||
| 指标 | 值 |
|
||
|---|---:|
|
||
| 有效站点 / 共识对 | 27 / 20 |
|
||
| 平移残差 RMS | ≈ 0.071 m |
|
||
| 旋转残差 RMS | ≈ 0.982 ° |
|
||
| 双后端差 | ≈ 3.1 mm / 0.12° |
|
||
| `frame_mode` | `vehicle_forward_heading_offset` |
|
||
| 相对机械初值 | XY 近机械杆臂；yaw≈0；无近 180° 冲突 |
|
||
|
||
与机械平移初值 XY 相差约数毫米；z 由天线高度约束，CAD 的 4 mm 不能代替实测 1.9165 m。
|
||
|
||
### 为何 RMS 尚可、尾部（P95/max）较差？
|
||
|
||
1. **前段多站几乎同航向**（STATION-01～05 约 250°～255°）。最差对（如 2→4）站间转角仅约 5°，小转角对平均平移残差约 7.4 cm，大转角对约 3.7 cm。
|
||
2. **GICP heldout RMSE** 本身多在 0.11～0.14 m，场景重叠/结构限制了配准下限。
|
||
3. 本批导出为 **host 时间关联**，静站可用，但仍可能引入厘米级位姿—点云错位。
|
||
4. AX 残差衡量的是「RTK 运动 A」与「外参预测 XBX」的一致性，**不是**相对 CAD 的毫米误差，也不能单独证明 ±3 cm 绝对真值。
|
||
|
||
改进方向：相邻站转角 15°～30°、站数 ≥40、固定平面板、有条件改用 `device_gnss`。
|
||
|
||
---
|
||
|
||
## 8. z 与精度限制
|
||
|
||
平面阿克曼运动不能独立观测 z。z 由「LiDAR 地面平面 + 外供 RTK 参考点离地高」约束：
|
||
|
||
- 本次：**1.9165 m**（相位中心离地）；
|
||
- 历史 data4/data5 文档中的 **0.758 m** 是**另一台车**的测量，不能用于本车。
|
||
|
||
更改高度后必须重新求解，禁止只改 JSON 里的 z。
|
||
GGA 对应哪根天线、`rawHeading` 方向须现场确认；搞反会导致 yaw 差约 180°。
|
||
|
||
---
|
||
|
||
## 9. 历史 data4 / data4+data5（参考）
|
||
|
||
旧联合实验使用 ANT1 离地 `0.758 m`，外参量级与本车不同，**不要与第 7 节结果混比**。联合流程见 `run/run_joint_rtk_lidar.ps1`。
|
||
`results/reference_data4/` 若存在，仅为历史精简产物，不作为当前交付外参。
|
||
|
||
---
|
||
|
||
## 10. 仓库目录
|
||
|
||
| 目录 | 职责 |
|
||
|---|---|
|
||
| [`code/`](code/) | GICP、运动对质量、AX=XB、结果封装、3D 可视化 |
|
||
| [`tools/`](tools/) | dlog/rscap 解析、G90 窗导出、combined / prepared |
|
||
| [`run/`](run/) | PowerShell 入口；路径与高度均由参数传入 |
|
||
| `tests/` | 坐标契约、G90 host 关联等回归 |
|
||
| `work/`、`outputs/` | 本地生成物（`.gitignore`） |
|
||
|
||
命令索引见 [`run/README.md`](run/README.md)，工具说明见 [`tools/README.md`](tools/README.md)，操作手册见 [`雷达与RTK标定说明书.md`](雷达与RTK标定说明书.md)。
|