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

本仓库从静态站点原始数据复现 T_RTK_lidar：把原始雷达坐标转换到RTK导航坐标系。它不是 base_link 车体外参，也不会在求解阶段使用车体航向偏置或RTK到后轮轴的XY杆臂。 数据下载地址：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导航系在本仓库中定义为：

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

如果下游需要 T_body_lidar，必须另有经过确认的 T_body_rtk：

T_body_lidar = T_body_rtk · T_RTK_lidar

2. 算法流程

逐站 H32.rscap + 全程 RTK.rscap + IMU.rscap
  → tools/export_raw_to_combined.py（一步导出标定中间包 combined/）
  → 每站选择一帧静态点云，位置转局部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可视化检查

代码实际使用：

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。新车默认布局（H32 / G90 / N300 新插件，不再出 dlog）：

raw_dataset/
├── stations/                 # 每站停稳后单独录一段雷达
│   ├── 001/h32.rscap
│   ├── 002/h32.rscap
│   └── ...
└── captures/
    ├── rtk.rscap             # 进场到收工连续录（G90：#PVTSLNA + #UNIHEADINGA）
    └── imu.rscap             # 连续录（N300；仅关联，不参与外参求解）

一键导出默认：雷达用 MSOP 设备时间，RTK 用 GNSS week/TOW 做最近邻关联（-TimeBasis device_gnss）。旧 dlog 数据集可继续放在同结构的 dobject/ + dobject_recording/ 下，并用 -TimeBasis host。

每个站点应在车辆完全静止后记录点云；建议不少于30站，并包含充足的直行、左转、右转和大角度转向姿态变化。

4. 环境安装

已验证环境为Windows、PowerShell、Python 3.11。安装依赖：

python -m pip install -r requirements.txt

依赖包括NumPy、SciPy、Open3D和small_gicp。若small_gicp没有对应Windows wheel，可在WSL2中安装后运行Python核心命令，或先只运行Open3D后端；完整共识流程需要两个后端都可用。

5. 从原始数据一键复现

导出与 Lidar-IMU 的 export_rscap_to_v1 同级：一条命令把原始 rscap 变成标定可直接使用的 combined/。

仅导出中间包：

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

产物：

$Out\exported\
├── export/              # 内部：各站雷达帧（调试用）
├── parsed/              # 内部：RTK/IMU JSONL
├── combined/            # ★ 标定入口：关联后的多传感器 NPZ + manifest.csv
└── export_summary.json

完整求解（导出 + prepare + AX=XB）在仓库根目录执行：

$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

主要输出：

$Out/
├── exported/
│   ├── export/          # 内部各站LiDAR帧
│   ├── 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.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.758 `
  -ExpectedStations 34

6. 3D可视化

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；
• N / ]：下一运动对；
• P / [：上一运动对；
• Q / Esc：退出。

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

7. data4与data4+data5结果对比

data5补充了30个有效静态站点及两个法向方向不同的固定平面板。当前联合流程只合并data4、data5各自的批内运动对，不构造跨批次运动，因此两次采集的时间、ENU原点和绝对位置不同不会直接影响共享外参；前提是传感器安装未改变，并且两批数据使用相同的RTK坐标定义和LiDAR原始坐标定义。

两块平面板在现有代码中作为点云场景结构参与GICP配准，但没有作为已知RTK/ENU平面方程单独加入优化；若后续能测得板面方程，才可新增绝对平面约束。

为公平比较，下面两组结果都使用ANT1参考点离地高度0.758 m重新求解：

指标	data4单独	data4+data5联合	变化
有效站点	34	64	+30
共识运动对	25	36	+11
平移残差RMS	0.100394 m	0.086902 m	-13.4%
平移残差中位数	0.062075 m	0.053647 m	-13.6%
平移残差P95	0.123039 m	0.125166 m	+1.7%
平移残差最大值	0.353438 m	0.355864 m	+0.7%
旋转残差RMS	1.252391°	1.115207°	-11.0%
旋转残差中位数	0.747183°	0.685164°	-8.3%
旋转残差P90	1.825297°	1.481369°	-18.8%
旋转残差P95	1.965290°	1.875860°	-4.6%
Weighted Jacobian condition	7.713973	8.379217	+8.6%
bootstrap z标准差	0.003147 m	0.001957 m	-37.8%
bootstrap roll标准差	0.099319°	0.062245°	-37.3%
bootstrap pitch标准差	0.096049°	0.064370°	-33.0%

联合结果为：

translation_m = [1.642932528, -0.242302311, 0.180599708]
RPY_deg_xyz   = [-0.886210651, 1.372780243, -22.112054052]

T_RTK_lidar =
 0.926183553  0.376030869  0.028014474  1.642932528
-0.376311142  0.926478119  0.005312208 -0.242302311
-0.023957243 -0.015462238  0.999593402  0.180599708
 0.000000000  0.000000000  0.000000000  1.000000000

与相同高度下的data4单独结果相比，联合外参相差5.25 mm / 0.064°。data5使RMS、中位数、旋转P90和bootstrap稳定性改善，但平移P95及最大值没有改善，说明少数高残差运动对仍然存在；不应仅为降低最大值而按最终外参残差删边。

已经分别得到各批次的共识运动对和地面平面时，可运行：

$Names = @("data4", "data5")
$Pairs = @("E:\data4\consensus\B_consensus.npz", "E:\data5\consensus\B_consensus.npz")
$Planes = @("E:\data4\common\ground_planes.csv", "E:\data5\common\ground_planes.csv")

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" `
  -BatchNames $Names -Pairs $Pairs -GroundPlanes $Planes `
  -OutputRoot "E:\calibration_output\data4_data5_joint" `
  -RtkReferenceHeightAboveGroundM 0.758 -Bootstrap 200

脚本会先分别拟合各批次外参；任一批与首批相差超过0.25 m或5°时中止，提示检查RTK航向/坐标定义和传感器安装。阈值可通过-MaxBatchTranslationDifferenceM和-MaxBatchRotationDifferenceDeg显式调整。

仓库内results/reference_data4是历史data4参考产物，使用旧高度配置，不应与上表直接比较，也不应继续作为当前联合外参下发。

8. z与精度限制

平面阿克曼运动不能独立观测z。当前联合结果使用64站地面平面和ANT1参考点离地0.758 m约束z；其中0.758 m来自本次现场粗测的天线底部安装参考高度0.710 m，加上天线标签给出的L1/L2 PCO高度46/50 mm的中值48 mm。该值仍是测量输入，不是手眼运动方程自行估计出来的量。

旧联合结果曾使用0.8535 m = 0.2335 m + 0.620 m，得到z = 0.085093 m；改用0.758 m并重新求解后得到z = 0.180600 m，z增加约0.095507 m，而x、y和旋转基本不变。所有标定入口现均要求显式提供参考高度，更改高度后必须重新求解，不能只手工修改输出JSON中的z。

AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。

此外，代码无法单独证明GGA对应哪根物理天线、rawHeading是ANT1→ANT2还是ANT2→ANT1；必须用接收机配置、接线和现场运动实验确认。方向错误会导致RTK坐标系yaw相差约180°。

9. 仓库目录

目录	职责
code/	GICP、运动对质量评价、AX=XB求解、结果封装和3D可视化
tools/	原始dlog/rscap解析、按LiDAR帧关联及静态站点prepared生成
run/	PowerShell入口；所有数据和输出路径都通过参数传入
results/reference_data4/	历史data4精简参考结果，不包含点云和本机过程目录
work/、outputs/	本地运行生成物，已由.gitignore排除

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