# 双天线RTK—3D LiDAR直接手眼标定

本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达坐标转换到RTK导航坐标系。它**不是** `base_link` 车体外参，也不会在求解阶段使用车体航向偏置或RTK到后轮轴的XY杆臂。

## 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
逐站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可视化检查
```

代码实际使用：

```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。`DataRoot`下每个站点必须是一个独立dlog目录，至少包含：

```text
raw_dataset/
├── stations/
│   ├── 001/
│   │   ├── dobject/
│   │   └── dobject_recording/
│   ├── 002/
│   └── ...
└── captures/
    ├── rtk.rscap
    └── imu.rscap
```

每个站点应在车辆完全静止后记录点云；建议不少于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. 从原始数据一键复现

在仓库根目录执行，路径由使用者通过参数传入，脚本内没有本机绝对路径：

```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.8535 `
  -ExpectedStations 34
```

主要输出：

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

若已经有`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
```

## 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`；
- `Q/Esc`：退出。

模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应查看多对，不能只挑视觉效果最好的一对。

## 7. data4参考结果

仓库保留了精简参考产物，见[`results/reference_data4`](results/reference_data4/README.md)：

```text
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与精度限制

平面阿克曼运动不能独立观测z。参考结果使用34站地面平面和RTK参考点离地`0.8535 m`约束z；该高度必须量到实际GGA参考点/天线相位中心。更改参考高度后必须重新求解。

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/) | 可提交Git的精简参考结果，不包含点云和本机过程目录 |
| `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 |

各代码文件职责见[`code/README.md`](code/README.md)，命令索引见[`run/README.md`](run/README.md)，工具说明见[`tools/README.md`](tools/README.md)。