LiDAR–双天线 RTK 手眼标定

本仓库提供一套可从原始 Medulla 记录复现的静态站点标定流程，求解三维激光雷达到后轮轴中心车体系的外参

X = T_body_lidar

约定 T_A_B 将 B 系坐标变换到 A 系。对任意站点 i、j：

A_ij = T_W_Bi^-1 T_W_Bj       # RTK 给出的车体相对运动
B_ij = T_Li_Lj                # GICP 给出的雷达相对运动
A_ij X = X B_ij

当前部署建议仍采用 results/01_previous_two_batches/final_extrinsic_deployment.json。data4 是一次独立重算，结果与部署值相差约 1.592 cm / 0.234°，但自身 AX 残差更高，因此只作为候选和稳定性证据，不自动替换部署值。

标定总流程

flowchart LR
  raw["原始站点 dlog / RTK、IMU rscap"] --> export["分别解析并统一到时间轴"]
  export --> assoc["按每个 LiDAR 帧关联 RTK/IMU，导出 NPZ"]
  assoc --> prep["每站选一帧，构建 RTK 车体位姿 A"]
  prep --> b1["small_gicp 求 B"]
  prep --> b2["Open3D GICP 求 B"]
  b1 --> gate["与 X 无关的质量筛选及双后端一致性"]
  b2 --> gate
  gate --> solve["AX=XB + 地面约束求 X"]
  solve --> check["残差、bootstrap、条件数、跨批复核和 3D 可视化"]

流程有两个原始数据入口：

• 旧式数据：LiDAR 和 GPS-POST-Z 位于每个站点 dlog 中，使用 export_legacy_stations.ps1。
• 新式多传感器数据：LiDAR 位于逐站 dlog，RTK 与 IMU 是独立 .rscap，使用 export_multisensor_stations.ps1。处理顺序是统一时间轴、分别解析、按 LiDAR 帧关联、导出 NPZ。

IMU 会在新式数据中原样解析并随 LiDAR 帧关联保存，但当前 LiDAR–RTK 外参求解不使用 IMU，也不做运动畸变校正，因为每一站采集点云时车辆静止。IMU 外参应使用单独的激励数据和专用标定流程求解。

三批数据的角色

数据	原始格式	站点	RTK 情况	在本仓库中的角色
第一批	逐站 dlog，内嵌 GPS-POST-Z	38	约 10 秒一条，部分站仅 1–11 个有效样本	辅助复核，不承担主要求解
第二批	逐站 dlog，内嵌 GPS-POST-Z	38	每站约 125–412 个有效样本，航向稳定	现部署外参的主要求解数据
data4	逐站 LiDAR dlog + 独立 RTK/IMU rscap	34	11,678 个 LiDAR 帧均成功关联 fixed RTK、heading 和 IMU	独立重算与跨批比较

原始数据体积较大，不在 Git 仓库中。复现者应从云盘取得第一批、第二批或 data4 的原始目录，并在命令行传入路径。

环境

• Windows PowerShell 5.1 或 PowerShell 7
• Python 3.10+
• pip install -r requirements.txt
• small_gicp 后端需要可导入 small_gicp；Open3D 后端需要 open3d

所有脚本从自身位置推导仓库根目录。数据和输出路径均由参数传入，不依赖开发者电脑上的固定路径。

从原始数据开始复现

以下路径只表示格式，请替换为自己的目录。

A. 第一批、第二批旧式 dlog

$Repo = "D:\你的代码目录\calibration"

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_legacy_stations.ps1" `
  -DataRoot "D:\你的数据目录\batch2_raw" `
  -OutputRoot "D:\你的输出目录\batch2_export"

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_legacy_dataset.ps1" `
  -ExportRoot "D:\你的输出目录\batch2_export" `
  -Output "D:\你的输出目录\batch2_prepared" `
  -HeadingOffsetDeg 21.226 `
  -AntennaLever -0.320,-0.365,0.620 `
  -ExpectedStations 38 `
  -HeadingStdLimitDeg 0.5

第一批采用同一导出方式，但应在导出命令显式添加 -RtkMaxDtMs 15000；因其 RTK 稀疏，准备阶段也不建议沿用 0.5° 的严格站内航向离散度阈值。上述 21.226° 和杆臂 [-0.320,-0.365,0.620] m 是本项目已有两批数据采用的配置，不是通用常数；换车或改变天线安装后必须重新确认。

B. data4 式独立 RTK/IMU rscap

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_multisensor_stations.ps1" `
  -DataRoot "D:\你的数据目录\data4_raw" `
  -RtkCapture "D:\你的数据目录\captures\rtk.rscap" `
  -ImuCapture "D:\你的数据目录\captures\imu.rscap" `
  -OutputRoot "D:\你的输出目录\data4_export"

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_multisensor_dataset.ps1" `
  -CombinedRoot "D:\你的输出目录\data4_export\combined" `
  -Output "D:\你的输出目录\data4_prepared" `
  -HeadingOffsetDeg 21.226 `
  -AntennaLever -0.320,-0.365,0.620 `
  -ExpectedStations 34

C. 运行标定

单批数据同时运行 small_gicp、Open3D GICP 和跨后端共识：

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_single_dataset.ps1" `
  -Prepared "D:\你的输出目录\data4_prepared" `
  -OutputRoot "D:\你的输出目录\data4_calibration" `
  -BodyHeight 0.2335

复现本仓库“第二批求解、第一批辅助复核”的历史流程：

powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" `
  -Batch1Prepared "D:\你的输出目录\batch1_prepared" `
  -Batch2Prepared "D:\你的输出目录\batch2_prepared" `
  -OutputRoot "D:\你的输出目录\two_batch_calibration"

BodyHeight=0.2335 m 是后轮轴中心离地高度，用于把雷达地面平面约束转换到车体原点；它不是雷达离地高度。换轮胎、胎压或车体载荷后应重新测量并评估不确定度。

如何判断结果

不能仅凭 AX=XB 残差或一张叠图宣称外参正确。至少同时检查：

1. B 的配准质量：收敛、有效对应点数/比例、RMSE、Hessian 信息矩阵特征值与条件数、small_gicp 与 Open3D 的相对运动一致性。
2. X 的可观性：加权雅可比条件数越接近 1 越均衡；极大值说明存在弱方向，但没有脱离尺度和数据分布的单一硬阈值。本次 data4 为 7.44，不是病态。
3. AX 残差分布：看 RMS、median、P90/P95、max 和逐对异常，而不是只看均值。
4. 重采样稳定性：bootstrap 的六自由度标准差及置信区间。
5. 跨批检查：同一 X 在独立批次的残差，以及两批独立求出的 X 之 SE(3) 差。
6. 3D 可视化：模式 3 的 B 与模式 4 的 X^-1 A X 是否都使相同墙面、杆件和地面重合。

当前没有把“增量小于某个值”当成绝对真值判据。工程筛查可先关注多数优质运动对是否约在厘米级到数厘米、亚度级；但阈值必须结合场景尺度、点云分辨率、RTK 质量和车辆运动幅度制定。模式 3 已错位时优先检查 B；模式 3 正常而模式 4 系统性错位时，再检查 A、坐标约定和 X。

结果摘要

结果	平移 xyz (m)	roll/pitch/yaw (deg)	AX 平移/旋转 RMS	结论
部署值（第二批求解）	[1.297760,-0.000067,0.720498]	[-0.785151,1.202661,-0.835510]	0.07985 m / 0.96118°，39 对	当前建议部署
data4 独立候选	[1.300376,-0.001707,0.704877]	[-0.791892,1.393823,-0.970743]	0.11762 m / 1.24257°，26 对	独立候选，不替换部署值

data4 候选相对部署值变化 1.592 cm / 0.234°，其中 z 低 1.562 cm。旧部署值作用于 data4 的残差约 0.11953 m / 1.24836°；data4 候选作用于历史第二批的残差约 0.07931 m / 0.98926°。两个 X 的跨批表现接近，当前数据不足以证明 data4 的较低 z 更接近真值。

仓库目录

code/       标定、配准筛选、共识、比较和可视化核心程序
tools/      原始 dlog/rscap 解析、时间关联、NPZ 导出和数据准备
run/        不含本机固定路径的 PowerShell 入口
results/    历史两批、data4 与跨批比较三个结果目录

完整复现流程和所有主要文件职责均在本 README；run/README.md、tools/README.md 和 results/README.md 只是目录内快速索引。

代码、工具和运行入口职责

根 README 是本仓库唯一的完整复现说明。run/README.md、tools/README.md 和 results/README.md 只作为进入对应目录时的快速索引，不承载另一套流程。

code：标定核心

文件	职责
rigorous_calibration.py	核心 CLI。ground 拟合每站地面；pairs 用 small_gicp 或 Open3D GICP 求 B 和质量指标；calibrate 联合 AX=XB 与地面约束求 X；validate 计算指定 X 的逐对残差。
refine_pairs.py	根据收敛、RMSE、对应关系、Hessian/信息矩阵和运动覆盖筛选 B；不读取 X，避免循环挑选。
cross_backend_filter.py	对齐 Open3D 与 small_gicp 的同一站点对，只保留两个后端相互一致的 B。
finalize_consensus.py	汇总历史两批的 consensus B、外参和第一批辅助复核。
summarize_results.py	汇总两个后端的外参、B 质量和跨批检查，生成推荐结果。
compare_extrinsics.py	在 SE(3) 上计算两套外参的严格相对平移和旋转差。
visualize_pair_3d.py	交互显示原始点云、RTK A、GICP B、X^-1AX，并打印 B^-1(X^-1AX) 数值增量。
scan_extrinsic_sensitivity.py	对 X 左乘小角度 roll/pitch/yaw 扰动，检查指定运动对的局部敏感方向。

tools：原始数据到 prepared

文件	职责
frontlidar_dlog_export.py	从 Medulla dlog 导出传感器坐标系 points_raw；旧格式可同时匹配站内 GPS-POST-Z。
prepare_station_dataset.py	从旧式逐站导出中每站选择一帧，计算 yaw-only RTK 后轮轴位姿并生成 prepared。
build_multisensor_npz.py	将独立 LiDAR、RTK、IMU 统一到 LiDAR 帧索引并生成 combined NPZ。
prepare_multisensor_station_dataset.py	从 combined NPZ 选择每站静止帧，生成与旧流程相同的 prepared 接口。
rscap_v2/capture_format_v2.py	读取 rscap v2 文件头、原始记录块和文件尾。
rscap_v2/audit_capture_v2.py	审计 capture 完整性、时间范围和记录统计。
rscap_v2/parse_rtk_imu_v2.py	分别解析 RTK 与 IMU capture，输出 JSONL。
rscap_v2/pipeline_common*.py	rscap 解析、时间处理和采集格式兼容的共用逻辑。

run：推荐 PowerShell 入口

文件	职责
export_legacy_stations.ps1	批量导出 LiDAR 与 RTK 同在逐站 dlog 中的旧格式。
export_multisensor_stations.ps1	解析独立 RTK/IMU，导出逐站 LiDAR，并按 LiDAR 帧建立关联。
prepare_legacy_dataset.ps1	旧式导出结果转换为 prepared。
prepare_multisensor_dataset.ps1	combined 多传感器结果转换为 prepared。
run_single_dataset.ps1	单批数据同时完成两个 GICP 后端、B 筛选、consensus 和 X 求解。
run_all.ps1	历史流程：第二批求解，第一批稀疏 RTK 数据作辅助复核。
run_consensus_finish.ps1	在已有两个后端 B 的基础上重新生成 consensus 和汇总结果。
run_sensitivity_scan.ps1	对历史运动对执行外参角度灵敏度扫描。
view_result.ps1	传入匹配的 frames、B 和 X，运行交互式 3D 可视化。

标定核心文件的数据关系

原始 dlog/rscap
  -> tools 导出和时间关联
  -> prepared/{frames_all, body_poses_*.csv}
  -> rigorous_calibration.py pairs 生成 A、B
  -> refine_pairs.py 做与 X 无关的 B 筛选
  -> cross_backend_filter.py 生成 consensus B
  -> rigorous_calibration.py calibrate 联合 AX=XB 与地面约束求 X
  -> validate / visualize_pair_3d.py 做数值和三维检查

重要限制

• RTK 车体姿态当前是双天线 heading 构造的 yaw-only 轨迹；没有用 RTK pitch/roll 构造 A。
• 新式解析器保存 IMU 与 RTK pitch 等原始字段，但当前手眼方程未融合 IMU。
• 静止站点法不估计 LiDAR–RTK 时间偏移；时间戳关联必须在导出阶段通过审计。
• 地面约束负责 roll、pitch 和 z 的补充可观性，不会独立求出另一套六自由度外参。
• 仓库归档的是结果和轻量 B 文件，不包含云盘中的原始点云数据。

专题说明

• 运动对诊断
• 双后端共识筛选
• 结果文件索引