双天线 RTK—3D 激光雷达严谨手眼标定

本仓库是当前建议提交和继续维护的正式版本。第二批 38 站高频 RTK 用于主要求解；第一批 38 站稀疏 RTK 只作辅助复核。Open3D GICP 和 small_gicp 分别生成雷达运动 (B)，两套独立结果及跨后端共识结果均保留。

实际部署只读取 results/final_extrinsic_deployment.json。算法原始求解、残差、Bootstrap 和筛选记录保留在其他结果文件中，不应与部署文件混用。

标定总体流程

1. 准备静止站点数据：每站车辆静止，保存一帧原始雷达点云和该站 RTK；程序只使用雷达传感器坐标系下的 points_raw。

2. 由 RTK 生成车体运动 (A)：将双天线 RTK 的位置、水平航向、杆臂和 heading offset 转为车体位姿 (T_{WB_i})，再计算

   [ A_{ij}=T_{WB_i}^{-1}T_{WB_j}. ]

3. 由点云配准生成雷达运动 (B)：Open3D GICP 和 small_gicp 分别把第 j 站点云配准到第 i 站，得到

   [ B_{ij}=T_{L_iL_j}. ]

4. 先验证和筛选 B，再求 X：使用对称留出点、重叠率、留出点 RMSE、海森矩阵、正反向一致性、多初值稳定性、旋转共轭不变量和闭环误差检查 B。这个阶段不读取待求外参 X，也不使用 AX 残差筛选 B。

5. 建立跨后端共识 B：只保留 Open3D 与 small_gicp 对同一运动对的 B 相差不超过 0.05 m / 0.50° 的边；最终使用 Open3D B，small_gicp 作为独立一致性门控。

6. 求解手眼外参 X：固定 A 和通过筛选的 B 后，求解

   [ A_{ij}X=XB_{ij},\qquad X=T_{body\leftarrow lidar}. ]

   使用 Huber 鲁棒最小二乘、12 个初值、地面平面约束和 100 次 Bootstrap。

7. 结果复核：第二批共识对用于主要求解和整体残差统计；第一批重新独立生成 B，只作为稀疏 RTK 辅助复核；3D 可视化分别比较模式 3 的 GICP B 与模式 4 的 (X^{-1}AX)。

8. 形成部署结果：第二批跨后端共识结果给出完整的 [x,y,z,roll,pitch,yaw]；部署 JSON 直接采用该算法结果。。

验证逻辑

顺序固定为：

先生成 B
  -> 用不依赖 X 的点云与几何指标筛 B
  -> 用另一个 GICP 后端复核同一 B
  -> 固定共识 B
  -> 最后才求 X
  -> AX 残差只用于结果评价，不反过来选择 B

代码证据：

• code/refine_pairs.py 不读取外参文件，只使用留出点、正反向和旋转共轭不变量等指标；
• code/cross_backend_filter.py 只比较两个后端的 B，不读取 X；
• code/rigorous_calibration.py 在 B 集合固定后才执行 calibrate 求 X；
• 第一批辅助复核的 B 也重新独立生成，且不使用 AX 残差筛选。

但这些检查仍不能把 B 变成绝对真值：重复结构、动态物体或错误局部最优仍可能让两个后端同时出错，所以必须结合模式 3 点云质量、留出集指标和多运动对整体统计判断。

最终部署外参

坐标变换定义：

[ X=T_{body\leftarrow lidar} ]

即将原始雷达点从雷达坐标系变换到以后轮轴中心为原点、X 前/Y 左/Z 上的车体坐标系。

部署值：

• 平移 [x,y,z] = [1.297760, -0.000067, 0.720498] m
• RPY [roll,pitch,yaw] = [-0.785151, 1.202661, -0.835510] deg
• 唯一部署文件：results/final_extrinsic_deployment.json

该值来自第二批 39 对跨后端共识、完整地面平面约束和 Bootstrap 稳定性检查。

算法在完整地面平面模型下的原始结果为：

• 平移 [1.297760, -0.000067, 0.720498] m
• RPY [-0.785151, 1.202661, -0.835510] deg
• 文件：results/final_extrinsic_recommended.json

final_extrinsic_recommended.json 保留完整求解、残差和 Bootstrap 记录，用于复现和审计；final_extrinsic_deployment.json 是唯一部署入口。

共识结果统计：

• 第二批 39 对 AX RMS：0.07985 m / 0.96118°
• 第一批 22 对辅助复核：0.06067 m / 1.00298°
• 结果摘要：results/final_summary.json

两套后端分别求得：

后端	平移 [x,y,z] m	RPY deg	第二批 AX RMS
Open3D GICP	[1.297883,-0.003100,0.721789]	[-0.757207,1.146042,-0.879781]	0.1002 m / 1.0115°
small_gicp	[1.299630,-0.003565,0.721886]	[-0.787156,1.141456,-0.935757]	0.1187 m / 1.1351°

两后端单独求出的 X 相差 1.81 mm、0.063°。共识求解的 100 次 Bootstrap 标准差 [x,y,z,roll,pitch,yaw] 为：

[0.00322 m, 0.00333 m, 0.00169 m, 0.0980°, 0.0746°, 0.1189°]

这里的 Z Bootstrap 只描述当前地面模型条件下的内部稳定性，不代表机械高度的绝对精度。

重要限制：旋转 RMS 仍约 1°，所以这不是厘米级绝对精度认证。远距离点云仍可能看到角度错层；Bootstrap 也不包含 RTK 参考点、航向偏置或杆臂定义错误等系统误差。

code/ 文件职责

code/ 中的 Python 脚本按“生成 B → 独立筛选 B → 求解 X → 汇总/诊断”的顺序组织。通常应通过 run/ 下的 PowerShell 脚本调度，而不是手工跳过中间筛选步骤。

文件	职责	主要输入 → 主要输出
rigorous_calibration.py	主求解器，实现坐标变换、Open3D GICP/small_gicp 配准、质量计算和手眼优化。ground 从 points_raw 做 RANSAC 地面平面；pairs 将 RTK 车体轨迹构造成 A、将点云配准构造成 B，并输出海森矩阵、留出点、正反向和多初值质量报告；calibrate 用 Huber 鲁棒最小二乘、地面约束、多初值和 Bootstrap 求 T_body_lidar；validate 只计算给定 X 在独立 B 集上的 AX 残差。	prepared 的 frames_all、车体位姿 CSV、B/地面文件 → .npz B 集、质量 JSON/CSV、外参 JSON、辅助复核 JSON。
refine_pairs.py	第二级 B 精筛器。按留出点重叠率/RMSE、旋转共轭不变量、正反向一致性剔除差的 B；不读取 X，也不用 AX 残差选 B。	初始 B .npz + 配准质量 JSON → *_refined.npz + *.refinement.json 审计记录。
cross_backend_filter.py	跨后端一致性门控。只保留 Open3D 与 small_gicp 对同一运动对的 B 差异不超过默认 0.05 m / 0.50° 的边；最终 B 本体采用 Open3D，small_gicp 仅作为独立门控。	两套 refined B .npz → consensus/B_batch*_consensus.npz + *.consensus.json。
finalize_consensus.py	发布共识解。读取共识 B 求得的外参、第一批辅助复核和两单后端外参，写出带选择依据的 final_extrinsic_recommended.json 与 final_summary.json。	共识外参/复核 JSON、两后端外参 JSON → 推荐外参和最终摘要 JSON。
summarize_results.py	单后端阶段汇总器。比较 Open3D 与 small_gicp 的外参差异、第二批拟合残差和第一批辅助复核，生成 comparison_summary.json；完整流程随后由 finalize_consensus.py 以共识解覆盖推荐结果。	两单后端外参、质量、辅助复核 JSON → 对比摘要 JSON 与阶段性推荐 JSON。
scan_extrinsic_sensitivity.py	局部灵敏度诊断器。对 X 在车体系左乘小的 roll/pitch/yaw 修正，比较某一对与全部 B 对的残差、改善/变差数量、地面指标和 Z 可观性。只生成扫描 JSON/CSV，绝不改写 X。	B .npz、外参 JSON、可选地面 CSV → diagnostics/*_scan.json/.csv。
visualize_pair_3d.py	交互式 3D 点云核查器。蓝色为目标站 i、橙色为源站 j；按键 1/2/3/4 分别查看原始、RTK 初值 A、GICP B、外参预测 X^-1 A X，并打印 B^-1(X^-1AX) 的 cm/deg 增量。可选按键 5 显示临时 RPY 修正，修正不写入文件。	站点点云、B .npz、外参 JSON → Open3D 交互窗口与终端增量。

坐标与公式

[ A_{ij}=T_{WB_i}^{-1}T_{WB_j},\qquad B_{ij}=T_{L_iL_j},\qquad A_{ij}X=XB_{ij} ]

B_ij 把第 j 站雷达点变换到第 i 站雷达坐标系。程序只读取 points_raw，禁止使用已经变到车体坐标系的点。

RTK 轨迹使用 body_poses_rear_gga_raw_rear_to_front.csv，对应后天线 GGA 位置和双天线后到前方向。

地面约束采用完整平面变换：

[ n_B=R_Xn_L,\qquad r_h=d_L-n_B^Tt_X-h_{body} ]

不再使用旧近似式 d_L-(t_z+h_body)。

详细求解流程

1. 第二批高频 RTK 生成 A；第一批稀疏 RTK 不参与主要求解。
2. 不使用手量外参作初值：生成 B 时 (X_0=I)，所以 (B_0=A)；求 X 也从单位变换和随机初值开始。
3. 两后端均采用三级粗到细：体素 0.30/0.15/0.08 m，对应距离 1.20/0.50/0.25 m。
4. 逐对检查对称留出点、海森矩阵、正反向一致性、多初值稳定性和 B 三角闭环。
5. 使用重叠率、留出点 RMSE 和旋转共轭不变量筛选 B；不使用 AX 残差筛选。
6. 两后端分别求 X；随后使用跨后端 B 一致性得到最终 39 对共识集合。
7. 使用 Huber 鲁棒最小二乘、12 个随机初值、完整地面约束和 100 次 Bootstrap 求算法 X。
8. 第一批重新独立生成 B，仅作辅助复核，不参与第二批求解。
9. 使用共识算法完整解形成精简部署 JSON，不覆盖算法原始结果。

目录

code/
  rigorous_calibration.py
  refine_pairs.py
  cross_backend_filter.py
  summarize_results.py
  finalize_consensus.py
  scan_extrinsic_sensitivity.py
  visualize_pair_3d.py
run/
  run_all.ps1
  run_consensus_finish.ps1
  run_sensitivity_scan.ps1
  view_open3d_result.ps1
  view_small_gicp_result.ps1
tools/
  frontlidar_dlog_export.py           # 从单个原始 dlog 导出 LiDAR NPZ 与 RTK sidecar
  prepare_station_dataset.py          # 从逐站导出结果构建标定所需 prepared 数据集
results/
  common/
  open3d_gicp/
  small_gicp/
  consensus/
  diagnostics/
  final_extrinsic_deployment.json     # 实际部署只读取这个文件
  final_extrinsic_recommended.json    # 算法原始结果及详细诊断
  final_summary.json

从原始数据到最终外参

下面是从云盘中的原始 dlog到最终 LiDAR—车体外参的完整流程。第 2、3 步由本仓库的 tools/ 脚本完成；第 4 步及之后由 run/ 调度。不要把云盘中的原始数据直接传给 run_all.ps1。

原始静止站点 dlog（两批）
  → 逐站导出 LiDAR 原始点云 + RTK 旁路表（NPZ）
  → 逐站质量筛选、选取静止帧、重建 RTK 车体位姿（prepared）
  → Open3D / small_gicp 分别求 B
  → X 无关的 B 精筛与跨后端共识
  → AX=XB + 地面平面约束求 X
  → 第一批辅助复核、3D 点云核查、确认部署 JSON

1. 整理原始数据与标定元数据

每一个静止站点目录必须完整保留 LiDAR DObject 记录、对应 .dorec 文件和 RTK 日志。dlog 导出器要求站点目录至少具有：

<station_dlog>/
  dobject/                 # DObject 索引日志
  dobject_recording/       # 与索引对应的 .dorec 二进制记录
  GPS-POST-Z/              # RTK 文本记录；建议同时保留完整原始 N4 串口流

两批数据要分开存放：第二批高频 RTK 用于主要求解，第一批仅用于辅助复核。与数据一同归档的标定元数据必须包括：LiDAR DObject 名称、时区、RTK 天线参考点、双天线基线方向、heading offset、天线到后轮轴中心的杆臂、车体系定义，以及 LiDAR/RTK/IMU 的时间基准。

对于后续新录数据，RTK 原始记录应保留 gnss_week、gnss_tow_ms、GGA UTC、pitch_deg、基线长度、解算状态/类型、标准差、HDOP 和完整原文。PC 接收时间只能作为延迟诊断，不能替代 GNSS 测量时间。

2. 将每个原始站点 dlog 导出为 NPZ

本仓库的 tools/frontlidar_dlog_export.py 从每个静止站点的原始 dlog 导出 LiDAR 原始点云与 RTK sidecar。对每个站点分别执行；必须开启 RTK sidecar 与审计报告：

$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
$Exporter = "$Repo\tools\frontlidar_dlog_export.py"
$RawStation = "<某一个原始静止站点 dlog 目录>"
$StationOut = "<工作目录>\export\001"

python $Exporter `
  --dlog "$RawStation" `
  --out "$StationOut" `
  --object frontlidar `
  --format npz `
  --timezone +08:00 `
  --rtk-sidecars `
  --write-reports

导出后的站点目录应包含：

<StationOut>/
  frames/                  # 多帧 LiDAR NPZ；每帧保留 points_raw
  rtk/                      # gps_post_z.csv/jsonl/npz 等 RTK 旁路表
  reports/manifest.csv      # 每帧导出状态、时间及 RTK 匹配信息
  reports/validation_report.json
  reports/metadata.json

先检查每个站点的 validation_report.json、manifest.csv 和 rtk/gps_post_z.csv：LiDAR payload 必须可读；候选帧必须存在；RTK 位置/航向有效；并记录 LiDAR—RTK 时间差。若新插件已开始输出 GNSS 周/周内时间与 pitch，导出器也必须同步扩展并写出这些字段；旧导出器只保留旧插件字段时，不能伪称已完成严格时间同步。

3. 从导出 NPZ 构建 prepared 静止站点数据

对每个批次，将所有站点导出目录汇总后执行以下预处理逻辑，生成供本仓库使用的 prepared 目录：

1. 每个站点只选择一帧车辆已静止且 RTK 质量合格的 LiDAR 帧；通常取有效候选帧中的中间帧，避免启动/停车边缘。
2. 仅保留 position_valid=true、heading_valid=true、Fix 为 4 或 5，且航向标准差满足项目门限的 RTK 样本；记录每站的样本数、时间跨度、航向圆标准差与被剔除原因。
3. 使用 GNSS 测量时间把位置、heading 和 pitch 配成同一时刻；将经纬高转换到同一 ENU 世界系；根据天线杆臂和双天线方向求后轮轴中心车体位姿 T_WB。双天线可提供 heading/pitch，但不能提供 roll；roll 在后续 LiDAR—IMU 流程中由 IMU 补充。
4. 将选出的 LiDAR 帧原样复制到 frames_all/。标定程序从每帧的 points_raw 转为 LiDAR 传感器系 XYZ，不得使用已经变换到车体系的点。
5. 输出时间戳车体位姿 CSV。当前主求解器要求列为：

time,x,y,z,qx,qy,qz,qw

最终目录必须类似：

<BatchPrepared>/
  frames_all/
    station_01.npz
    station_02.npz
    ...
  body_poses_rear_gga_raw_rear_to_front.csv
  station_summary.csv       # 推荐保留：站点质量和选择原因
  manifest.json             # 推荐保留：参考点、杆臂、heading/pitch 定义与筛选配置

本仓库的 tools/prepare_station_dataset.py 完成上述固定站点筛选、点云复制和车体位姿重建。它要求每个站点均已由上一步使用 --rtk-sidecars --write-reports 导出。下面的参数仅用于复现本仓库历史结果：lat/lon/h 被假定为后天线相位中心，raw_heading_deg 被假定为后天线指向前天线；车体系为后轮轴中心、X 前 Y 左 Z 上。

$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
$Prepare = "$Repo\tools\prepare_station_dataset.py"

# 第二批：主要求解数据。ExportBatch2 下为 38 个逐站导出目录。
python $Prepare --export-root "<ExportBatch2>" --output "<PreparedBatch2>" --expected-stations 38 --heading-std-limit-deg 0.5 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front

# 第一批：辅助复核数据。若航向质量较低，不应设置过严的 heading 标准差阈值。
python $Prepare --export-root "<ExportBatch1>" --output "<PreparedBatch1>" --expected-stations 38 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front

命令完成后检查 <PreparedBatch*>/station_summary.csv：每行均应有有效 RTK 样本，且没有超出设定的航向离散度。manifest.json 记录实际采用的杆臂、航向偏移、ENU 原点和输入站点；它应与原始数据一同归档。

不要把上述 21.226° 和 [-0.320,-0.365,0.620] m 当成通用常数：它们是本车、后天线、后天线到前天线航向定义、以及后轮轴中心车体原点的历史配置。换车、换参考天线、改变车体原点或改变 rawHeading 定义后，必须先复核并替换这些参数，再重建 A。

当前导出器可复现本仓库历史日志，但历史 RTK 插件只记录工控机接收时间，并未保存 GNSS 周/周内时间、双天线 pitch、质量标准差及完整原始串口流。因此它不能把旧数据宣称为严格 GNSS 时间同步。插件升级后，应先扩展导出器以保存新字段，再以 GNSS 测量时间重建位置/航向（及可用的 pitch）轨迹。

4. 安装标定依赖并运行第一阶段

$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
python -m pip install -r "$Repo\requirements.txt"
python -c "import numpy, scipy, open3d, small_gicp; print('dependencies OK')"

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" `
  -Batch1Prepared "<第一批 prepared 目录>" `
  -Batch2Prepared "<第二批 prepared 目录>"

run_all.ps1 完成：第二批地面拟合；Open3D 与 small_gicp 两套 B 生成和精筛；两套单后端 X 求解；第一批独立 B 与辅助复核；以及单后端对比摘要。第一阶段至少应生成：

results/common/ground_planes_batch2.csv
results/open3d_gicp/B_batch2_refined.npz
results/small_gicp/B_batch2_refined.npz
results/open3d_gicp/extrinsic_batch2_refined.json
results/small_gicp/extrinsic_batch2_refined.json
results/comparison_summary.json

5. 构造跨后端共识并生成算法结果

第一阶段成功后执行：

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"

该步骤以 0.05 m / 0.50° 门限对同一运动对的 Open3D 与 small_gicp B 做一致性门控，用共识 B 重求第二批外参，并将第一批只用于辅助复核。主要输出为：

results/consensus/B_batch2_consensus.npz
results/consensus/extrinsic_batch2_consensus.json
results/consensus/batch1_auxiliary_check.json
results/final_extrinsic_recommended.json
results/final_summary.json

6. 验证、可视化与部署确认

先检查 results/final_summary.json：当前历史数据的参考值是第二批共识 39 对、AX RMS 约 0.07985 m / 0.96118°。若明显偏离，应从第 2、3 步检查站点选择、RTK 时间/参考点/杆臂/heading 定义，而不是直接手调 X。

再使用 3D 可视化检查：模式 3 为点云配准 B，模式 4 为外参预测 X^-1AX；终端打印 B^-1(X^-1AX) 的平移与旋转增量。只有 B 质量、全体运动对统计、地面约束和可视化均合理时，才可确认外参。

final_extrinsic_recommended.json 是完整算法审计结果。run_consensus_finish.ps1 不会自动覆盖 results/final_extrinsic_deployment.json；部署 JSON 必须在人工确认坐标定义和高度约束后明确写入。

prepared 阶段的快速重跑

$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
python -m pip install -r "$Repo\requirements.txt"
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1"
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"

如果 prepared 数据移动了：

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" -Batch1Prepared "你的第一批prepared目录" -Batch2Prepared "你的第二批prepared目录"
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"

重跑 run_consensus_finish.ps1 会重新生成算法原始结果，但不会自动生成或覆盖已经人工确认的 final_extrinsic_deployment.json。

3D 可视化

Open3D 单后端结果：

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_open3d_result.ps1" -PairIndex 0

small_gicp 单后端结果：

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_small_gicp_result.ps1" -PairIndex 0

蓝色是目标站 i，橙色是源站 j。按键：1 原始；2 RTK 初始 A；3 GICP B；4 外参预测的 (X^{-1}AX)；指定左乘 RPY 试验量后，5 为试验结果。

程序打印的差值为：

[ B^{-1}(X^{-1}AX) ]

其 xyz/RPY 分量位于站点 j 的雷达局部坐标系，不是屏幕坐标。模式 3 好而模式 4 差，说明 A/X 一致性不足；模式 3 本身差，说明该 B 不能用于判断外参。

共识运动对可直接指定部署外参：

python "$Repo\code\visualize_pair_3d.py" --frames "C:\Users\admin\Documents\Codex\2026-07-15\wo\outputs\calibration_data1_20260720\prepared\frames_all" --pairs "$Repo\results\consensus\B_batch2_consensus.npz" --extrinsic "$Repo\results\final_extrinsic_deployment.json" --pair-index 0

局部灵敏度扫描和判断规则见 PAIR_DIAGNOSTICS.md。

关键质量文件

• results/open3d_gicp/B_batch2_quality.json：海森矩阵、留出点、正反向和多初值信息；
• results/open3d_gicp/B_batch2_refined.refinement.json：二级筛选原因；
• results/consensus/B_batch2_consensus.consensus.json：跨后端一致性筛选；
• results/consensus/batch1_auxiliary_check.json：第一批辅助复核；
• results/final_summary.json：算法结果摘要；
• results/diagnostics/：局部修正对单对及全体运动对的影响。

必须外部确认的假设

• lat/lon/h 确为后天线相位中心；
• rawHeading 确为后天线指向前天线；
• heading offset 数值和正负号正确；

任何一项变化，都必须更新相应约束；RTK 定义变化时需要重新生成 A 并重跑。