From 68cb5eaf90d44619868f7d30fd0ec0c80869d84a Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Mon, 10 Aug 2026 22:37:18 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=96=87=E6=A1=A3=E4=B8=8E?= =?UTF-8?q?=E4=B8=80=E9=94=AE=E8=84=9A=E6=9C=AC=E9=BB=98=E8=AE=A4=E5=80=BC?= =?UTF-8?q?=E4=BB=A5=E5=8C=B9=E9=85=8D=E5=9F=BA=E7=BA=BF=E7=B3=BB=E6=9C=AC?= =?UTF-8?q?=E6=AC=A1=E6=A0=87=E5=AE=9A=EF=BC=881.9165m=E3=80=81=E5=9C=B0?= =?UTF-8?q?=E9=9D=A2ROI=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- README.md | 393 ++++++++++++++++++++++---------------- run/README.md | 70 ++++--- run/run_full_pipeline.ps1 | 9 +- 雷达与RTK标定说明书.md | 201 +++++++------------ 4 files changed, 347 insertions(+), 326 deletions(-) diff --git a/README.md b/README.md index e377216..9bce912 100644 --- a/README.md +++ b/README.md @@ -1,91 +1,90 @@ # 双天线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 +本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **RTK 基线导航系**。 +它**不是** `base_link` 车体外参；求解阶段不使用车体航向偏置，也不使用 RTK 到后轮轴的 XY 杆臂。 -## 1. 输出坐标约定 +当前交付标定（2026-08 室外车，27 站）约定如下： -统一约定 `T_A_B` 把B系点变换到A系： +| 项 | 值 | +|---|---| +| 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 系： ```text p_RTK = T_RTK_lidar · p_lidar ``` -RTK导航系在本仓库中定义为： +本仓库默认 RTK 导航系（**基线系 / baseline_raw_heading**）： -- 原点：GGA位置参考点（通常为ANT1相位中心，必须结合接收机配置确认）； -- X轴：`rawHeading`所表示的双天线基线在水平面的投影； -- Y轴：左； -- Z轴：上； -- ENU航向：`yaw = 90° - rawHeading`； -- roll、pitch：当前轨迹中固定为0。 +- 原点：GGA 位置参考点（通常为 ANT1 相位中心，须结合接收机配置确认）； +- X 轴：`rawHeading` 双天线基线在水平面的投影； +- Y 轴：左； +- Z 轴：上； +- ENU 航向：`yaw = 90° - rawHeading`（`heading_offset = 0`）； +- roll、pitch：轨迹中固定为 0。 -如果下游需要 `T_body_lidar`，必须另有经过确认的 `T_body_rtk`： +> 不要把基线系结果当成“车头向前系”。若下游需要车头向前，应另乘确认过的固定航向偏置，或显式使用 `-HeadingOffsetDeg 90` **整链重跑**，不要事后只改 JSON 里的 yaw。 + +若下游需要 `T_body_lidar`，须另有已确认的 `T_body_rtk`： ```text T_body_lidar = T_body_rtk · T_RTK_lidar ``` +机械初值文件：[`run/rtk_lidar_mechanical_initial.json`](run/rtk_lidar_mechanical_initial.json) +**仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。** + +--- + ## 2. 算法流程 ```text -逐站 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可视化检查 +原始雷达 + 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 可视化 ``` -代码实际使用： - ```text A_ij = inv(T_W_Ri) · T_W_Rj = T_Ri_Rj -B_ij = T_Li_Lj # 将站点j点云变换到站点i +B_ij = T_Li_Lj A_ij · X = X · B_ij X = T_RTK_lidar ``` -## 3. 原始数据目录 +--- -大体积数据不提交Git。**新车默认布局**（H32 用 `RSLidarH32_3D_DLogCaptureNet48`；G90/N300 仍为 `.rscap`）： +## 3. 原始数据与导出 + +大体积数据不提交 Git。常见两种采集形态： + +### 3.1 每站独立雷达目录（旧/标准站目录） ```text raw_dataset/ -├── stations/ # 每站停稳后单独录一段雷达 dlog -│ ├── 001/ # dobject/ + dobject_recording/ -│ │ ├── dobject/ # 含 frontlidar-msop-raw / frontlidar-difop-raw -│ │ └── dobject_recording/ -│ ├── 002/ -│ └── ... +├── stations/001|002|.../ # H32 dlog 或 h32.rscap └── captures/ - ├── rtk.rscap # 进场到收工连续录（G90：#PVTSLNA + #UNIHEADINGA） - └── imu.rscap # 连续录（N300；仅关联，不参与外参求解） + ├── rtk.rscap + └── imu.rscap # 仅关联，不参与外参求解 ``` -一键导出默认：雷达用 **MSOP 设备时间** + DIFOP 通道角，RTK 用 **GNSS week/TOW** 做最近邻关联（`-TimeBasis device_gnss`）。 -仍兼容旧站 `h32.rscap`；旧「已解码点云」dlog 需用 `-TimeBasis host`。 - -每个站点应在车辆完全静止后记录点云；建议不少于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. 从原始数据一键复现 - -导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级：**一条命令**把原始 H32 dlog + G90/N300 rscap 变成标定可直接使用的 `combined/`。 - -仅导出中间包： - ```powershell python tools\export_raw_to_combined.py ` --stations-root "$Raw\stations" ` @@ -95,160 +94,220 @@ python tools\export_raw_to_combined.py ` --overwrite ``` -产物： +默认时间基：`-TimeBasis device_gnss`（雷达设备时 ↔ GNSS week/TOW）。 -```text -$Out\exported\ -├── export/ # 内部：各站雷达帧（调试用） -├── parsed/ # 内部：RTK/IMU JSONL -├── combined/ # ★ 标定入口：关联后的多传感器 NPZ + manifest.csv -└── export_summary.json +### 3.2 G90 连续录制 + H32 DLog 按站时间窗（本次 27 站） + +站不在独立目录，而在多个 Medulla DLog ZIP 与 G90 `.rscap` 中时： + +```powershell +python tools\export_g90_h32_windows_to_combined.py ` + --segments-csv ` + --lidar-dlog --lidar-dlog ` + --rtk-rscap --rtk-rscap ` + --out --expected-stations 27 --frame-stride 5 ``` -完整求解（导出 + prepare + AX=XB）在仓库根目录执行： +该入口用 **主机接收 UTC** 做近邻关联（`time_basis_mode: host`），并保留设备时间供审计。 +可加 `--reuse-export` 在已有 `export/` 上续跑。 + +采集建议：有效静站 ≥30（更好 40～60）；相邻站转角约 **15°～30°**；避免一长串同朝向停车；场内宜有墙/立柱及 2～3 块法向不同的固定平面板。 + +--- + +## 4. 环境安装 + +Windows + PowerShell + Python 3.11： + +```powershell +python -m pip install -r requirements.txt +``` + +依赖：NumPy、SciPy、Open3D、small_gicp。完整共识需要两个配准后端。 + +--- + +## 5. 一键复现（匹配本次标定） + +### 5.1 已有 `combined/`（推荐复现本次结果） ```powershell $Repo = (Resolve-Path ".").Path -$Raw = "E:\calibration_data\data4" -$Out = "E:\calibration_output\rtk_lidar" +$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 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 + -RtkReferenceHeightAboveGroundM 1.9165 ` + -ExpectedStations 27 ` + -GroundZMin -2.5 ` + -GroundZMax -1.5 ``` 主要输出： ```text $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/ +├── 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 ``` -若已经有`combined/`，可跳过原始导出： +### 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 可视化 + +查看本次结果： ```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.758 ` - -ExpectedStations 34 -``` +$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" -## 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" ` + -Frames "$Work\frames_all" ` + -Pairs "$Out\consensus\B_consensus.npz" ` + -Extrinsic "$Out\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% | - -联合结果为： - -```text -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及最大值没有改善，说明少数高残差运动对仍然存在；不应仅为降低最大值而按最终外参残差删边。 - -已经分别得到各批次的共识运动对和地面平面时，可运行： +通用模板（把路径换成你的 `WorkRoot` / `OutputRoot`）： ```powershell -$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 +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 ``` -脚本会先分别拟合各批次外参；任一批与首批相差超过`0.25 m`或`5°`时中止，提示检查RTK航向/坐标定义和传感器安装。阈值可通过`-MaxBatchTranslationDifferenceM`和`-MaxBatchRotationDifferenceDeg`显式调整。 +| 按键 | 含义 | +|---|---| +| `1` | 原始点云 | +| `2` | 仅用 RTK 运动作初值 | +| `3` | GICP 测得的 B | +| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） | +| `N` / `]` | 下一运动对 | +| `P` / `[` | 上一运动对 | +| `Q` / `Esc` | 退出 | -仓库内[`results/reference_data4`](results/reference_data4/README.md)是历史data4参考产物，使用旧高度配置，不应与上表直接比较，也不应继续作为当前联合外参下发。 +蓝 = 站 i，橙 = 站 j。请用 `N`/`P` **多看大转角对**，不要只看前几对同朝向站。 -## 8. z与精度限制 +--- -平面阿克曼运动不能独立观测z。当前联合结果使用64站地面平面和ANT1参考点离地`0.758 m`约束z；其中`0.758 m`来自本次现场粗测的天线底部安装参考高度`0.710 m`，加上天线标签给出的L1/L2 PCO高度`46/50 mm`的中值`48 mm`。该值仍是测量输入，不是手眼运动方程自行估计出来的量。 +## 7. 当前标定结果（基线系，h = 1.9165 m） -旧联合结果曾使用`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。 +结果目录：`D:\data\rtk_lidar_run\outputs_baseline_h19165\` +交付文件：`final_T_RTK_lidar.json` / `summary.json` -AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。 +```text +translation_m = [0.412305582, 0.217309210, 0.104057606] +RPY_deg_xyz = [0.465513, 0.743343, 89.460018] -此外，代码无法单独证明GGA对应哪根物理天线、`rawHeading`是ANT1→ANT2还是ANT2→ANT1；必须用接收机配置、接线和现场运动实验确认。方向错误会导致RTK坐标系yaw相差约180°。 +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 +``` -## 9. 仓库目录 +| 指标 | 值 | +|---|---:| +| 有效站点 / 共识对 | 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/`](code/) | GICP、运动对质量评价、AX=XB求解、结果封装和3D可视化 | -| [`tools/`](tools/) | 原始dlog/rscap解析、按LiDAR帧关联及静态站点prepared生成 | -| [`run/`](run/) | PowerShell入口；所有数据和输出路径都通过参数传入 | -| [`results/reference_data4/`](results/reference_data4/) | 历史data4精简参考结果，不包含点云和本机过程目录 | -| `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 | +| [`code/`](code/) | GICP、运动对质量、AX=XB、结果封装、3D 可视化 | +| [`tools/`](tools/) | dlog/rscap 解析、G90 窗导出、combined / prepared | +| [`run/`](run/) | PowerShell 入口；路径与高度均由参数传入 | +| `tests/` | 坐标契约、G90 host 关联等回归 | +| `work/`、`outputs/` | 本地生成物（`.gitignore`） | -各代码文件职责见[`code/README.md`](code/README.md)，命令索引见[`run/README.md`](run/README.md)，工具说明见[`tools/README.md`](tools/README.md)。 +命令索引见 [`run/README.md`](run/README.md)，工具说明见 [`tools/README.md`](tools/README.md)，操作手册见 [`雷达与RTK标定说明书.md`](雷达与RTK标定说明书.md)。 diff --git a/run/README.md b/run/README.md index 227911e..62dd2ca 100644 --- a/run/README.md +++ b/run/README.md @@ -1,35 +1,42 @@ # run目录 -根README包含完整复现命令；这里仅列入口职责。 +根 README 含完整复现与本次结果说明；这里只列入口职责。 | 脚本 | 用途 | |---|---| -| `run_full_pipeline.ps1` | 调用一步导出得到 `combined/`，再跑到最终 `T_RTK_lidar` | -| `export_multisensor_stations.ps1` | 薄封装：调用 `tools/export_raw_to_combined.py` | -| `prepare_multisensor_dataset.ps1` | 每站选一帧，生成yaw-only RTK参考轨迹和`frames_all` | -| `run_direct_rtk_lidar.ps1` | 从combined数据运行RTK直接标定和最终结果封装 | -| `run_single_dataset.ps1` | 执行地面、两个GICP后端、精筛、共识和AX=XB求解 | -| `run_joint_rtk_lidar.ps1` | 合并多个独立批次的批内共识运动对和地面平面，求解共享外参 | -| `view_result.ps1` | 打开3D运动对对比并打印数值增量 | +| `run_full_pipeline.ps1` | 站目录导出 `combined/` 后跑到 `T_RTK_lidar` | +| `export_multisensor_stations.ps1` | 薄封装：`tools/export_raw_to_combined.py` | +| `prepare_multisensor_dataset.ps1` | 每站一帧 + yaw-only RTK 位姿 | +| `run_direct_rtk_lidar.ps1` | 从 `combined/` 标定并封装最终结果（**默认基线系**） | +| `run_single_dataset.ps1` | 地面、双 GICP、精筛、共识、AX=XB | +| `run_joint_rtk_lidar.ps1` | 多批共识对联合求解 | +| `view_result.ps1` | 3D 运动对对比 | +| `rtk_lidar_mechanical_initial.json` | 仅 AX=XB 初值；**禁止**用于 pair | -原始→中间包请优先直接用 Python 一步导出（与 Lidar-IMU 用法对齐）： +## 默认参数（匹配当前约 2 m 车顶雷达 / 基线系） + +| 参数 | 默认 | +|---|---| +| `HeadingOffsetDeg` | `0`（基线系） | +| `GroundZMin/Max` | `-2.5` / `-1.5` | +| `ExpectedStations` | `27` | +| `MinStations` | `20` | +| `RtkReferenceHeightAboveGroundM` | **无默认，必填**（本车 1.9165） | + +pair 注册**不传** `--initial-extrinsic`。 + +## 原始 → combined + +站目录： ```powershell python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu-rscap ... --out ... --overwrite ``` -常用参数： +- `-TimeBasis device_gnss`（默认）：设备时 ↔ GNSS +- `-TimeBasis host`：主机接收时间 -- 新车站目录为 H32 raw dlog（自动识别 `frontlidar-msop-raw` / `frontlidar-difop-raw`） -- `-LidarCaptureName h32.rscap`：仅旧站 `.rscap` 文件名（也接受 `lidar.rscap`） -- `-TimeBasis device_gnss`（默认）：雷达设备时 ↔ GNSS week/TOW（新 dlog / rscap） -- `-TimeBasis host`：旧「已解码点云」dlog + 主机接收时间关联 - -所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度，避免静默使用与实车不符的默认值；默认生成目录`work/`和`outputs/`不会提交Git。 - -## G90 + H32 连续录制数据按站导出 - -如果各站不是独立目录，而是记录在多个 Medulla DLog ZIP 和 G90 `.rscap` 中，使用： +G90 连续录制 + 站时间窗： ```powershell python tools\export_g90_h32_windows_to_combined.py ` @@ -39,12 +46,27 @@ python tools\export_g90_h32_windows_to_combined.py ` --out --expected-stations 27 --frame-stride 5 ``` -该入口使用 MSOP 的主机接收 UTC 与 G90 串口主机接收 UTC 做近邻关联，同时保留雷达包内设备时间作为审计字段。它只读取原始文件，生成 `export/`、`combined/` 和摘要，不使用 IMU。`--frame-stride` 仅控制参与标定的点云帧密度，不改变站点时间窗。 +可加 `--reuse-export` 续跑。 -如果雷达 ZIP 扫描已经完成但后续步骤中断，可保留 `export/` 并续跑： +## 已有 combined 复现本次结果 ```powershell -python tools\export_g90_h32_windows_to_combined.py --reuse-export --segments-csv --rtk-rscap --out --expected-stations 27 +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" ` + -CombinedRoot "D:\data\rtk_lidar_run\combined" ` + -WorkRoot "D:\data\rtk_lidar_run\prepared_baseline_h19165" ` + -OutputRoot "D:\data\rtk_lidar_run\outputs_baseline_h19165" ` + -RtkReferenceHeightAboveGroundM 1.9165 ` + -HeadingOffsetDeg 0 ` + -ExpectedStations 27 ` + -GroundZMin -2.5 -GroundZMax -1.5 ``` -在尚未测得 GGA/ANT1 相位中心离地高度时，可先生成 `combined/`，再运行 `prepare_multisensor_dataset.ps1` 得到 `prepared/`。最终外参求解仍需显式传入真实离地高度，不应留空或猜测。 +## 可视化本次结果 + +```powershell +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" ` + -Frames "D:\data\rtk_lidar_run\prepared_baseline_h19165\frames_all" ` + -Pairs "D:\data\rtk_lidar_run\outputs_baseline_h19165\consensus\B_consensus.npz" ` + -Extrinsic "D:\data\rtk_lidar_run\outputs_baseline_h19165\final_T_RTK_lidar.json" ` + -PairIndex 0 +``` diff --git a/run/run_full_pipeline.ps1 b/run/run_full_pipeline.ps1 index f521675..6c434a5 100644 --- a/run/run_full_pipeline.ps1 +++ b/run/run_full_pipeline.ps1 @@ -8,12 +8,14 @@ [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM, [string]$Timezone = "+08:00", [ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss", - [int]$ExpectedStations = 34, + [int]$ExpectedStations = 27, [int]$MinStations = 20, [int]$MinPairs = 20, [int]$Bootstrap = 200, - [double]$GroundZMin = -1.4, - [double]$GroundZMax = -0.4 + # Roof-mounted H32 (~2 m). Do not reuse [-1.4, -0.4] on this vehicle. + [double]$GroundZMin = -2.5, + [double]$GroundZMax = -1.5, + [double]$HeadingOffsetDeg = 0.0 ) $ErrorActionPreference = "Stop" @@ -31,6 +33,7 @@ if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" } -CombinedRoot (Join-Path $ExportRoot "combined") ` -WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot ` -RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM ` + -HeadingOffsetDeg $HeadingOffsetDeg ` -MinStations $MinStations ` -ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap ` -GroundZMin $GroundZMin -GroundZMax $GroundZMax diff --git a/雷达与RTK标定说明书.md b/雷达与RTK标定说明书.md index 0a22c45..ad3a7bd 100644 --- a/雷达与RTK标定说明书.md +++ b/雷达与RTK标定说明书.md @@ -1,13 +1,12 @@ # 雷达与 RTK 标定说明书 -本文说明如何用本仓库完成 **双天线 RTK ↔ 3D 激光雷达** 外参标定，得到可直接使用的 `T_RTK_lidar`。 +本文说明如何用本仓库完成 **双天线 RTK ↔ 3D 激光雷达** 外参标定，得到可直接使用的 `T_RTK_lidar`。 +默认交付坐标系为 **基线系**（`HeadingOffsetDeg = 0`）。更完整的指标与本次结果见根目录 [`README.md`](README.md)。 --- ## 1. 标定目标 -求解外参 `T_RTK_lidar`，把雷达点变换到 RTK 导航系： - ```text p_RTK = T_RTK_lidar · p_lidar ``` @@ -15,11 +14,15 @@ p_RTK = T_RTK_lidar · p_lidar | 项目 | 说明 | |---|---| | 输出文件 | `final_T_RTK_lidar.json` | -| 坐标系 | RTK 导航系（GGA 原点 + 双天线航向），**不是**车体后轮轴系 | +| 坐标系 | **基线系**：GGA 原点 + `rawHeading` 基线方向为 X（不是车体后轮轴系） | | 不用到的量 | 车体航向偏置、天线 XY 杆臂、IMU 姿态 | -| 必须提供 | RTK 参考点（通常 ANT1）离地高度 | +| 必须提供 | RTK 参考点（通常 ANT1）**相位中心**离地高度 | +| pair 配准 | **禁止**使用外参 seed | +| 求解初值 | 可用 `run/rtk_lidar_mechanical_initial.json`（仅 AX=XB） | -若下游需要车体外参，需另有已确认的 `T_body_rtk`： +当前车（2026-08）示例参数：高度 **1.9165 m**，地面 ROI **`[-2.5, -1.5]`**，期望站数 **27**。 + +若下游需要车体外参： ```text T_body_lidar = T_body_rtk · T_RTK_lidar @@ -31,59 +34,41 @@ T_body_lidar = T_body_rtk · T_RTK_lidar - 系统：Windows + PowerShell - Python：3.11 -- 安装依赖： - -```powershell -python -m pip install -r requirements.txt -``` - -依赖：NumPy、SciPy、Open3D、small_gicp。完整流程需要 **Open3D 与 small_gicp 两个配准后端**；若 Windows 无 small_gicp wheel，可改用 WSL2。 +- `python -m pip install -r requirements.txt`（NumPy、SciPy、Open3D、small_gicp） --- ## 3. 数据采集 -### 3.1 目录结构 - -**新车（默认）**：每站一段 H32 雷达 dlog（`RSLidarH32_3D_DLogCaptureNet48`，含 raw MSOP/DIFOP），RTK/IMU 全程各一条 `.rscap`： +### 3.1 每站独立目录 ```text raw_dataset/ -├── stations/ -│ ├── 001/ # dobject/ + dobject_recording/ -│ ├── 002/ -│ └── ... -└── captures/ - ├── rtk.rscap # G90：#PVTSLNA 位置 + #UNIHEADINGA 航向 - └── imu.rscap # N300；仅关联保存，不参与外参求解 +├── stations/001|002|.../ +└── captures/rtk.rscap , imu.rscap ``` -仍兼容旧站 `h32.rscap`。仅含已解码点云、无 raw MSOP 的旧 dlog 仍可识别，关联时间请用 `-TimeBasis host`。 +### 3.2 G90 连续录制 + 站时间窗（本次 27 站） -### 3.2 采集要求 +用 `tools/export_g90_h32_windows_to_combined.py`（见 [`run/README.md`](run/README.md)），时间基多为 host UTC。 + +### 3.3 采集要求 | 要求 | 建议 | |---|---| -| 站点数 | ≥ 30 站 | +| 站点数 | ≥ 30（更好 40～60）；本批 27 为最低可跑规模 | +| 相邻站转角 | 约 **15°～30°**，避免一长串同朝向 | | 车辆状态 | **完全静止**后再记点云 | -| 姿态覆盖 | 直行、左转、右转、大角度转向都要有 | -| RTK 质量 | 固定解（质量 4/5），航向有效 | -| 站内航向稳定 | 圆标准差 ≤ 0.5° | -| 必测量 | **ANT1（GGA 参考点）离地高度**，含天线相位中心修正 | +| RTK | 固定解，航向有效；站内航向圆标准差 ≤ 0.5° | +| 必测量 | ANT1 相位中心离地高度 | -### 3.3 现场确认（标定前必做） - -1. **哪根天线是 GGA 原点**（通常 ANT1） -2. **`rawHeading` 方向**：ANT1→ANT2 还是相反（搞反会导致 yaw 差约 180°） -3. **离地高度测法**：例如安装底面高度 + 天线 PCO，写入求解参数，不要事后只改 JSON 里的 z +现场确认：GGA 对应哪根天线、`rawHeading` 方向、离地高度测法（改高度必须重跑求解）。 --- ## 4. 一键标定 -### 4.1 仅导出标定中间包（推荐先跑通） - -与 Lidar-IMU 的 `export_rscap_to_v1` 同级：原始数据 → `combined/`。 +### 4.1 站目录 → combined ```powershell python tools\export_raw_to_combined.py ` @@ -94,13 +79,11 @@ python tools\export_raw_to_combined.py ` --overwrite ``` -### 4.2 导出 + 求解到最终外参 - -在仓库根目录执行（路径按本机修改）： +### 4.2 导出 + 求解 ```powershell $Repo = (Resolve-Path ".").Path -$Raw = "E:\calibration_data\data4" +$Raw = "E:\calibration_data\stations_batch" $Out = "E:\calibration_output\rtk_lidar" powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" ` @@ -108,123 +91,77 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipe -RtkCapture "$Raw\captures\rtk.rscap" ` -ImuCapture "$Raw\captures\imu.rscap" ` -OutputRoot $Out ` - -RtkReferenceHeightAboveGroundM 0.758 ` - -ExpectedStations 34 + -RtkReferenceHeightAboveGroundM 1.9165 ` + -HeadingOffsetDeg 0 ` + -ExpectedStations 27 ` + -GroundZMin -2.5 ` + -GroundZMax -1.5 ``` -| 关键参数 | 含义 | +| 参数 | 含义 | |---|---| -| `-RtkReferenceHeightAboveGroundM` | RTK 参考点离地高度（米），**必填** | -| `-ExpectedStations` | 期望站点数 | -| `-MinPairs` | 最少共识运动对，默认 20 | -| `-Bootstrap` | bootstrap 次数，默认 200 | +| `-RtkReferenceHeightAboveGroundM` | 相位中心离地高（m），**必填**；本车 1.9165 | +| `-HeadingOffsetDeg` | 默认 0 = 基线系 | +| `-GroundZMin/Max` | 约 2 m 雷达用 `[-2.5,-1.5]` | +| `-ExpectedStations` / `-MinStations` | 本批 27 / 20 | -### 已有 combined 数据时 - -可跳过原始导出，直接标定： +### 4.3 已有 combined ```powershell powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" ` - -CombinedRoot "...\exported\combined" ` - -WorkRoot "...\prepared_rtk_direct" ` - -OutputRoot "...\calibration" ` - -RtkReferenceHeightAboveGroundM 0.758 ` - -ExpectedStations 34 + -CombinedRoot "D:\data\rtk_lidar_run\combined" ` + -WorkRoot "D:\data\rtk_lidar_run\prepared_baseline_h19165" ` + -OutputRoot "D:\data\rtk_lidar_run\outputs_baseline_h19165" ` + -RtkReferenceHeightAboveGroundM 1.9165 ` + -HeadingOffsetDeg 0 ` + -ExpectedStations 27 ` + -GroundZMin -2.5 ` + -GroundZMax -1.5 ``` +> 历史文档中的 **0.758 m / 34 站** 属于另一台车（data4），不要用于本车。 + --- ## 5. 输出说明 ```text -$Out/ -├── exported/ # 解析与关联中间结果 -├── prepared_rtk_direct/ # 每站一帧 + RTK 位姿表 -└── calibration/ - ├── open3d_gicp/ # 后端 1 - ├── small_gicp/ # 后端 2 - ├── consensus/ # 双后端共识运动对 - ├── summary.json # 质量汇总 - └── final_T_RTK_lidar.json ← 最终交付物 +$OutputRoot/ +├── open3d_gicp/ small_gicp/ consensus/ +├── common/ground_planes.csv +├── summary.json +└── final_T_RTK_lidar.json ``` -`final_T_RTK_lidar.json` 主要字段： - -- `translation_m`：平移 (x, y, z)，单位米 -- `rotation_rpy_deg_xyz`：滚转 / 俯仰 / 偏航，单位度 -- `matrix_4x4`：4×4 齐次变换矩阵 - -质量指标看 `summary.json`：共识对数、AX 残差 RMS/中位数/P95、bootstrap 标准差、双后端差异。 - -> 内部一致性好 ≠ 已达到 ±3 cm 绝对真值；正式部署前建议再做独立轨迹验证。 +看 `summary.json` 的共识对数、AX 残差、bootstrap、双后端差。内部一致性 ≠ ±3 cm 绝对真值。 --- -## 6. 结果检查（可视化） +## 6. 可视化 ```powershell +$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 "$Out\prepared_rtk_direct\frames_all" ` - -Pairs "$Out\calibration\consensus\B_consensus.npz" ` - -Extrinsic "$Out\calibration\final_T_RTK_lidar.json" ` + -Frames "$Work\frames_all" ` + -Pairs "$Out\consensus\B_consensus.npz" ` + -Extrinsic "$Out\final_T_RTK_lidar.json" ` -PairIndex 0 ``` -| 按键 | 含义 | -|---|---| -| `1` | 原始点云 | -| `2` | 仅用 RTK 运动作初值 | -| `3` | GICP 测得的 B | -| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） | -| `N` / `]` | 下一运动对 | -| `P` / `[` | 上一运动对 | -| `Q` / `Esc` | 退出 | - -蓝 = 目标站 i，橙 = 源站 j。重点看模式 **3 与 4**：墙面、立柱、路缘、地面应基本重合。用 `N`/`P` **多看几对**，不要只挑视觉最好的一对。 +重点看模式 **3 与 4**；用 `N`/`P` 多看**大转角**运动对。 --- -## 7. 多批次联合（可选） +## 7. 注意事项 -传感器安装未变、坐标定义一致时，可合并多批共识运动对再求共享外参： +1. z 靠实测天线高度约束；改高度必须重跑求解。 +2. 不要改站点顺序；pair 索引依赖顺序。 +3. 只用 `points_raw`，禁止已变到车体的点。 +4. pair 阶段禁止外参 seed。 +5. IMU 只关联，不求解 IMU 外参。 +6. 尾部残差（P95/max）常被同朝向小转角站拉高，见根 README 第 7 节。 -```powershell -powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" ` - -BatchNames @("data4","data5") ` - -Pairs @("...\data4\consensus\B_consensus.npz","...\data5\consensus\B_consensus.npz") ` - -GroundPlanes @("...\data4\common\ground_planes.csv","...\data5\common\ground_planes.csv") ` - -OutputRoot "...\data4_data5_joint" ` - -RtkReferenceHeightAboveGroundM 0.758 ` - -Bootstrap 200 -``` - -任一批与首批相差超过 **0.25 m** 或 **5°** 会中止，需先检查航向定义与安装是否一致。 - ---- - -## 8. 注意事项 - -1. **z 不能只靠水平运动估出来**，必须靠实测天线高度约束；改高度后要 **重新跑求解**，禁止只改 JSON 的 z。 -2. **不要改站点目录名 / `station_*.npz` 顺序**，运动对索引依赖该顺序。 -3. 标定用 **原始雷达点**（`points_raw`），不要用已变换到车体的点。 -4. 当前时间对齐以主机接收时间为主，尚未估计设备时钟偏差。 -5. IMU 只解析关联，**不求解 IMU 外参**，静止站也不做运动去畸变。 -6. 仓库内 `results/reference_data4` 为历史参考（旧高度），**不要**当作当前部署外参直接下发。 - ---- - -## 9. 流程一览 - -```text -静止多站采集（LiDAR dlog + RTK/IMU rscap） - ↓ -解析关联 → 每站选一帧 + yaw-only RTK 位姿 - ↓ -双后端 GICP 求站间运动 B → 精筛 → 共识 - ↓ -AX=XB + 地面高度约束 → T_RTK_lidar - ↓ -可视化 / summary 检查 → 交付 final_T_RTK_lidar.json -``` - -更细的算法说明与指标对比见根目录 [`README.md`](README.md)；脚本入口见 [`run/README.md`](run/README.md)。 +更细算法与本次数值结果见 [`README.md`](README.md)。