# 雷达与 RTK 标定说明书

本文说明如何用本仓库完成 **双天线 RTK ↔ 3D 激光雷达** 外参标定，得到可直接使用的 `T_RTK_lidar`。

---

## 1. 标定目标

求解外参 `T_RTK_lidar`，把雷达点变换到 RTK 导航系：

```text
p_RTK = T_RTK_lidar · p_lidar
```

| 项目 | 说明 |
|---|---|
| 输出文件 | `final_T_RTK_lidar.json` |
| 坐标系 | RTK 导航系（GGA 原点 + 双天线航向），**不是**车体后轮轴系 |
| 不用到的量 | 车体航向偏置、天线 XY 杆臂、IMU 姿态 |
| 必须提供 | RTK 参考点（通常 ANT1）离地高度 |

若下游需要车体外参，需另有已确认的 `T_body_rtk`：

```text
T_body_lidar = T_body_rtk · T_RTK_lidar
```

---

## 2. 环境准备

- 系统：Windows + PowerShell  
- Python：3.11  
- 安装依赖：

```powershell
python -m pip install -r requirements.txt
```

依赖：NumPy、SciPy、Open3D、small_gicp。完整流程需要 **Open3D 与 small_gicp 两个配准后端**；若 Windows 无 small_gicp wheel，可改用 WSL2。

---

## 3. 数据采集

### 3.1 目录结构

**新车（默认）**：每站一段 H32 雷达 `.rscap`，RTK/IMU 全程各一条：

```text
raw_dataset/
├── stations/
│   ├── 001/h32.rscap
│   ├── 002/h32.rscap
│   └── ...
└── captures/
    ├── rtk.rscap             # G90：#PVTSLNA 位置 + #UNIHEADINGA 航向
    └── imu.rscap             # N300；仅关联保存，不参与外参求解
```

旧车 dlog 布局（`dobject/` + `dobject_recording/`）仍可被导出脚本识别；关联时间请用 `-TimeBasis host`。

### 3.2 采集要求

| 要求 | 建议 |
|---|---|
| 站点数 | ≥ 30 站 |
| 车辆状态 | **完全静止**后再记点云 |
| 姿态覆盖 | 直行、左转、右转、大角度转向都要有 |
| RTK 质量 | 固定解（质量 4/5），航向有效 |
| 站内航向稳定 | 圆标准差 ≤ 0.5° |
| 必测量 | **ANT1（GGA 参考点）离地高度**，含天线相位中心修正 |

### 3.3 现场确认（标定前必做）

1. **哪根天线是 GGA 原点**（通常 ANT1）  
2. **`rawHeading` 方向**：ANT1→ANT2 还是相反（搞反会导致 yaw 差约 180°）  
3. **离地高度测法**：例如安装底面高度 + 天线 PCO，写入求解参数，不要事后只改 JSON 里的 z

---

## 4. 一键标定

### 4.1 仅导出标定中间包（推荐先跑通）

与 Lidar-IMU 的 `export_rscap_to_v1` 同级：原始数据 → `combined/`。

```powershell
python tools\export_raw_to_combined.py `
  --stations-root "E:\calibration_data\stations" `
  --rtk-rscap "E:\calibration_data\captures\rtk.rscap" `
  --imu-rscap "E:\calibration_data\captures\imu.rscap" `
  --out "E:\calibration_output\exported" `
  --overwrite
```

### 4.2 导出 + 求解到最终外参

在仓库根目录执行（路径按本机修改）：

```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
```

| 关键参数 | 含义 |
|---|---|
| `-RtkReferenceHeightAboveGroundM` | RTK 参考点离地高度（米），**必填** |
| `-ExpectedStations` | 期望站点数 |
| `-MinPairs` | 最少共识运动对，默认 20 |
| `-Bootstrap` | bootstrap 次数，默认 200 |

### 已有 combined 数据时

可跳过原始导出，直接标定：

```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
  -CombinedRoot "...\exported\combined" `
  -WorkRoot "...\prepared_rtk_direct" `
  -OutputRoot "...\calibration" `
  -RtkReferenceHeightAboveGroundM 0.758 `
  -ExpectedStations 34
```

---

## 5. 输出说明

```text
$Out/
├── exported/                 # 解析与关联中间结果
├── prepared_rtk_direct/      # 每站一帧 + RTK 位姿表
└── calibration/
    ├── open3d_gicp/          # 后端 1
    ├── small_gicp/           # 后端 2
    ├── consensus/            # 双后端共识运动对
    ├── summary.json          # 质量汇总
    └── final_T_RTK_lidar.json  ← 最终交付物
```

`final_T_RTK_lidar.json` 主要字段：

- `translation_m`：平移 (x, y, z)，单位米  
- `rotation_rpy_deg_xyz`：滚转 / 俯仰 / 偏航，单位度  
- `matrix_4x4`：4×4 齐次变换矩阵  

质量指标看 `summary.json`：共识对数、AX 残差 RMS/中位数/P95、bootstrap 标准差、双后端差异。

> 内部一致性好 ≠ 已达到 ±3 cm 绝对真值；正式部署前建议再做独立轨迹验证。

---

## 6. 结果检查（可视化）

```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
```

| 按键 | 含义 |
|---|---|
| `1` | 原始点云 |
| `2` | 仅用 RTK 运动作初值 |
| `3` | GICP 测得的 B |
| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） |
| `N` / `]` | 下一运动对 |
| `P` / `[` | 上一运动对 |
| `Q` / `Esc` | 退出 |

蓝 = 目标站 i，橙 = 源站 j。重点看模式 **3 与 4**：墙面、立柱、路缘、地面应基本重合。用 `N`/`P` **多看几对**，不要只挑视觉最好的一对。

---

## 7. 多批次联合（可选）

传感器安装未变、坐标定义一致时，可合并多批共识运动对再求共享外参：

```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" `
  -BatchNames @("data4","data5") `
  -Pairs @("...\data4\consensus\B_consensus.npz","...\data5\consensus\B_consensus.npz") `
  -GroundPlanes @("...\data4\common\ground_planes.csv","...\data5\common\ground_planes.csv") `
  -OutputRoot "...\data4_data5_joint" `
  -RtkReferenceHeightAboveGroundM 0.758 `
  -Bootstrap 200
```

任一批与首批相差超过 **0.25 m** 或 **5°** 会中止，需先检查航向定义与安装是否一致。

---

## 8. 注意事项

1. **z 不能只靠水平运动估出来**，必须靠实测天线高度约束；改高度后要 **重新跑求解**，禁止只改 JSON 的 z。  
2. **不要改站点目录名 / `station_*.npz` 顺序**，运动对索引依赖该顺序。  
3. 标定用 **原始雷达点**（`points_raw`），不要用已变换到车体的点。  
4. 当前时间对齐以主机接收时间为主，尚未估计设备时钟偏差。  
5. IMU 只解析关联，**不求解 IMU 外参**，静止站也不做运动去畸变。  
6. 仓库内 `results/reference_data4` 为历史参考（旧高度），**不要**当作当前部署外参直接下发。

---

## 9. 流程一览

```text
静止多站采集（LiDAR dlog + RTK/IMU rscap）
        ↓
解析关联 → 每站选一帧 + yaw-only RTK 位姿
        ↓
双后端 GICP 求站间运动 B → 精筛 → 共识
        ↓
AX=XB + 地面高度约束 → T_RTK_lidar
        ↓
可视化 / summary 检查 → 交付 final_T_RTK_lidar.json
```

更细的算法说明与指标对比见根目录 [`README.md`](README.md)；脚本入口见 [`run/README.md`](run/README.md)。