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

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

当前交付标定（2026-08 室外车，27 站）约定如下：

项	值
RTK 坐标系	基线系（HeadingOffsetDeg = 0）
天线相位中心离地高	1.9165 m（1916.5 mm）
机械平移初值	(0.414179474, 0.210859360, 0.004000001) m
机械旋转初值	yaw ≈ 90°（雷达 X 朝车头、双天线基线左右装）
地面点 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 系：

p_RTK = T_RTK_lidar · p_lidar

本仓库默认 RTK 导航系（基线系 / baseline_raw_heading）：

• 原点：GGA 位置参考点（通常为 ANT1 相位中心，须结合接收机配置确认）；
• X 轴：rawHeading 双天线基线在水平面的投影；
• Y 轴：左；
• Z 轴：上；
• ENU 航向：yaw = 90° - rawHeading（heading_offset = 0）；
• roll、pitch：轨迹中固定为 0。

不要把基线系结果当成“车头向前系”。若下游需要车头向前，应另乘确认过的固定航向偏置，或显式使用 -HeadingOffsetDeg 90 整链重跑，不要事后只改 JSON 里的 yaw。

若下游需要 T_body_lidar，须另有已确认的 T_body_rtk：

T_body_lidar = T_body_rtk · T_RTK_lidar

机械初值文件：run/rtk_lidar_mechanical_initial.json
仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。

---

2. 算法流程

原始雷达 + RTK（+ 可选 IMU）
  → combined/（按站关联的多传感器 NPZ）
  → 每站选一帧静态点云 + yaw-only RTK pose（基线系）
  → Open3D GICP 与 small_gicp 分别求 B_ij = T_Li_Lj（无外参 seed）
  → 留出点、正反向、旋转共轭不变量等精筛
  → 双后端共识边 → consensus B
  → A_ij X = X B_ij + 地面法向/高度约束 → X = T_RTK_lidar
  → bootstrap、双后端差异、逐对残差与 3D 可视化

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 每站独立雷达目录（旧/标准站目录）

raw_dataset/
├── stations/001|002|.../   # H32 dlog 或 h32.rscap
└── captures/
    ├── rtk.rscap
    └── imu.rscap           # 仅关联，不参与外参求解

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 中时：

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：

python -m pip install -r requirements.txt

依赖：NumPy、SciPy、Open3D、small_gicp。完整共识需要两个配准后端。

---

5. 一键复现（匹配本次标定）

5.1 已有 combined/（推荐复现本次结果）

$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_baseline_h19165" `
  -OutputRoot "$Data\outputs_baseline_h19165" `
  -RtkReferenceHeightAboveGroundM 1.9165 `
  -HeadingOffsetDeg 0 `
  -ExpectedStations 27 `
  -MinStations 20 `
  -GroundZMin -2.5 `
  -GroundZMax -1.5 `
  -Bootstrap 200

关键参数：

参数	本次取值	说明
-RtkReferenceHeightAboveGroundM	1.9165	GGA/ANT1 相位中心离地高（m），必填
-HeadingOffsetDeg	0	基线系；非 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.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

主要输出：

$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

5.3 远程旧一键复现为何不能直接套用本批

相对当前本地默认，远程 origin/feature/lidar-rtk-direct-calibration 仍有几处与本次标定不符：

1. 地面 ROI 落到 py 默认 [-1.4, -0.4]（本车会拟合墙面）；
2. run_direct 里 MinStations 曾写死 30（本批 27 站失败）；
3. 缺少 export_g90_h32_windows_to_combined.py（本批导出链路）；
4. README 示例高度仍写历史车 0.758 m（本车应为 1.9165 m）。

航向上远程已是 HeadingOffsetDeg 0（基线系），与本次坐标系一致；请用本分支本地提交复现，不要照抄未更新的远程文档数字。

---

6. 3D 可视化

查看本次结果：

$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.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）

结果目录：D:\data\rtk_lidar_run\outputs_baseline_h19165\
交付文件：final_T_RTK_lidar.json / summary.json

translation_m = [0.412305582, 0.217309210, 0.104057606]
RPY_deg_xyz   = [0.465513, 0.743343, 89.460018]

T_RTK_lidar ≈
 0.009424  -0.999922   0.008247   0.412306
 0.999871   0.009529   0.012896   0.217309
-0.012973   0.008124   0.999883   0.104058
 0          0          0          1

指标	值
有效站点 / 共识对	27 / 20
平移残差 RMS / 中位 / P95 / max	0.070 / 0.042 / 0.121 / 0.186 m
旋转残差 RMS / 中位 / max	0.978 / 0.585 / 2.73 °
双后端差	3.2 mm / 0.17°
Jacobian 条件数	6.88
bootstrap σ(x,y,z)	4.7 / 6.3 / 0.8 mm
相对机械初值	旋转差 ≈ 1.03°（无近 180° 冲突）
frame_mode	baseline_raw_heading

与机械平移初值 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/	GICP、运动对质量、AX=XB、结果封装、3D 可视化
tools/	dlog/rscap 解析、G90 窗导出、combined / prepared
run/	PowerShell 入口；路径与高度均由参数传入
tests/	坐标契约、G90 host 关联等回归
work/、outputs/	本地生成物（.gitignore）

命令索引见 run/README.md，工具说明见 tools/README.md，操作手册见 雷达与RTK标定说明书.md。