Compare commits
6
Commits
8477935ad2
...
3b8282353c
| Author | SHA1 | Date | |
|---|---|---|---|
  lichun.quandCursor
|
3b8282353c | ||
|
lichun.quandCursor
|
e33a7a7657 | ||
|
lichun.quandCursor
|
68cb5eaf90 | ||
|
lichun.quandCursor
|
46d2fa1d69 | ||
|
lichun.quandCursor
|
69bb44bccd | ||
|
lichun.qu
|
b2271d05ba |
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"ExpandedNodes": [
|
||||
""
|
||||
],
|
||||
"PreviewInSolutionExplorer": false
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,49 @@
|
||||
{
|
||||
"Version": 1,
|
||||
"WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\",
|
||||
"Documents": [
|
||||
{
|
||||
"AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}",
|
||||
"RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}"
|
||||
}
|
||||
],
|
||||
"DocumentGroupContainers": [
|
||||
{
|
||||
"Orientation": 0,
|
||||
"VerticalTabListWidth": 256,
|
||||
"DocumentGroups": [
|
||||
{
|
||||
"DockedWidth": 200,
|
||||
"SelectedChildIndex": 3,
|
||||
"Children": [
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
|
||||
},
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:0:0:{1c4feeaa-4718-4aa9-859d-94ce25d182ba}"
|
||||
},
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}"
|
||||
},
|
||||
{
|
||||
"$type": "Document",
|
||||
"DocumentIndex": 0,
|
||||
"Title": "README.md",
|
||||
"DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeDocumentMoniker": "README.md",
|
||||
"ToolTip": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeToolTip": "README.md",
|
||||
"ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==",
|
||||
"Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|",
|
||||
"WhenOpened": "2026-07-24T03:20:21.56Z",
|
||||
"EditorCaption": ""
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
{
|
||||
"Version": 1,
|
||||
"WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\",
|
||||
"Documents": [
|
||||
{
|
||||
"AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}",
|
||||
"RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}"
|
||||
}
|
||||
],
|
||||
"DocumentGroupContainers": [
|
||||
{
|
||||
"Orientation": 0,
|
||||
"VerticalTabListWidth": 256,
|
||||
"DocumentGroups": [
|
||||
{
|
||||
"DockedWidth": 200,
|
||||
"SelectedChildIndex": 2,
|
||||
"Children": [
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
|
||||
},
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}"
|
||||
},
|
||||
{
|
||||
"$type": "Document",
|
||||
"DocumentIndex": 0,
|
||||
"Title": "README.md",
|
||||
"DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeDocumentMoniker": "README.md",
|
||||
"ToolTip": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeToolTip": "README.md",
|
||||
"ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==",
|
||||
"Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|",
|
||||
"WhenOpened": "2026-07-24T03:20:21.56Z",
|
||||
"EditorCaption": ""
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Binary file not shown.
@@ -1,88 +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 / G90 / N300 新插件，不再出 dlog）：
|
||||
## 3. 原始数据与导出
|
||||
|
||||
大体积数据不提交 Git。常见两种采集形态：
|
||||
|
||||
### 3.1 每站独立雷达目录（旧/标准站目录）
|
||||
|
||||
```text
|
||||
raw_dataset/
|
||||
├── stations/ # 每站停稳后单独录一段雷达
|
||||
│ ├── 001/h32.rscap
|
||||
│ ├── 002/h32.rscap
|
||||
│ └── ...
|
||||
├── stations/001|002|.../ # H32 dlog 或 h32.rscap
|
||||
└── captures/
|
||||
├── rtk.rscap # 进场到收工连续录（G90：#PVTSLNA + #UNIHEADINGA）
|
||||
└── imu.rscap # 连续录（N300；仅关联，不参与外参求解）
|
||||
├── rtk.rscap
|
||||
└── imu.rscap # 仅关联，不参与外参求解
|
||||
```
|
||||
|
||||
一键导出默认：雷达用 **MSOP 设备时间**，RTK 用 **GNSS week/TOW** 做最近邻关联（`-TimeBasis device_gnss`）。旧 dlog 数据集可继续放在同结构的 `dobject/` + `dobject_recording/` 下，并用 `-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` 同级：**一条命令**把原始 rscap 变成标定可直接使用的 `combined/`。
|
||||
|
||||
仅导出中间包：
|
||||
|
||||
```powershell
|
||||
python tools\export_raw_to_combined.py `
|
||||
--stations-root "$Raw\stations" `
|
||||
@@ -92,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 <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
|
||||
```
|
||||
|
||||
完整求解（导出 + 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)。
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
{
|
||||
"class_name" : "PinholeCameraParameters",
|
||||
"extrinsic" :
|
||||
[
|
||||
0.99504759165761958,
|
||||
-0.069911734310665122,
|
||||
0.070658614068252801,
|
||||
0.0,
|
||||
-0.09933361974813136,
|
||||
-0.67348569005019454,
|
||||
0.73249563635925308,
|
||||
0.0,
|
||||
-0.0036224748591220518,
|
||||
-0.7358867947607759,
|
||||
-0.67709489953226409,
|
||||
0.0,
|
||||
2.6365670299167046,
|
||||
1.9293765342813529,
|
||||
11.882725892422524,
|
||||
1.0
|
||||
],
|
||||
"intrinsic" :
|
||||
{
|
||||
"height" : 900,
|
||||
"intrinsic_matrix" :
|
||||
[
|
||||
779.4228634059948,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
779.4228634059948,
|
||||
0.0,
|
||||
699.5,
|
||||
449.5,
|
||||
1.0
|
||||
],
|
||||
"width" : 1400
|
||||
},
|
||||
"version_major" : 1,
|
||||
"version_minor" : 0
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
{
|
||||
"class_name" : "PinholeCameraParameters",
|
||||
"extrinsic" :
|
||||
[
|
||||
0.99504759165761958,
|
||||
-0.069911734310665122,
|
||||
0.070658614068252801,
|
||||
0.0,
|
||||
-0.09933361974813136,
|
||||
-0.67348569005019454,
|
||||
0.73249563635925308,
|
||||
0.0,
|
||||
-0.0036224748591220518,
|
||||
-0.7358867947607759,
|
||||
-0.67709489953226409,
|
||||
0.0,
|
||||
2.6365670299167046,
|
||||
1.9293765342813529,
|
||||
11.882725892422524,
|
||||
1.0
|
||||
],
|
||||
"intrinsic" :
|
||||
{
|
||||
"height" : 900,
|
||||
"intrinsic_matrix" :
|
||||
[
|
||||
779.4228634059948,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
779.4228634059948,
|
||||
0.0,
|
||||
699.5,
|
||||
449.5,
|
||||
1.0
|
||||
],
|
||||
"width" : 1400
|
||||
},
|
||||
"version_major" : 1,
|
||||
"version_minor" : 0
|
||||
}
|
||||
@@ -34,7 +34,50 @@ def delta(a: np.ndarray, b: np.ndarray) -> dict:
|
||||
}
|
||||
|
||||
|
||||
def corrected(raw: dict, backend: str, reference_height: float) -> dict:
|
||||
def coordinate_contract_audit(raw: dict) -> dict:
|
||||
"""Compare the data-driven solution with the declared mechanical initial.
|
||||
|
||||
A near-180-degree disagreement is not auto-corrected: it normally means
|
||||
that one physical forward-axis statement is reversed. Silently rotating
|
||||
the point cloud would preserve residuals while changing the frame contract.
|
||||
"""
|
||||
path_text = raw.get("solver_initial_extrinsic")
|
||||
if not path_text:
|
||||
return {
|
||||
"status": "mechanical_initial_not_available",
|
||||
"requires_physical_axis_confirmation": False,
|
||||
}
|
||||
path = Path(path_text)
|
||||
if not path.exists():
|
||||
return {
|
||||
"status": "mechanical_initial_file_missing",
|
||||
"requires_physical_axis_confirmation": False,
|
||||
"mechanical_initial_path": str(path),
|
||||
}
|
||||
initial_document = load(path)
|
||||
initial = np.asarray(initial_document["matrix_4x4"], float)
|
||||
solution = np.asarray(raw["matrix_4x4"], float)
|
||||
comparison = delta(initial, solution)
|
||||
near_180 = abs(comparison["rotation_deg"] - 180.0) <= 15.0
|
||||
return {
|
||||
"status": "near_180_degree_axis_conflict" if near_180 else "no_near_180_degree_axis_conflict",
|
||||
"requires_physical_axis_confirmation": near_180,
|
||||
"mechanical_initial_path": str(path.resolve()),
|
||||
"solution_relative_to_mechanical_initial": comparison,
|
||||
"note": (
|
||||
"No automatic 180-degree point-cloud flip was applied. Confirm the Helios "
|
||||
"aviation-connector side and the G90 vehicle-forward definition before deployment."
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
def corrected(raw: dict, backend: str, reference_height: float, heading_offset_deg: float) -> dict:
|
||||
baseline_frame = abs(heading_offset_deg) <= 1e-12
|
||||
x_axis = (
|
||||
"horizontal projection of the rawHeading baseline direction reported by the receiver"
|
||||
if baseline_frame else
|
||||
"vehicle forward after applying the configured G90 heading offset"
|
||||
)
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"success": bool(raw["success"]),
|
||||
@@ -43,20 +86,24 @@ def corrected(raw: dict, backend: str, reference_height: float) -> dict:
|
||||
"frames": {
|
||||
"RTK": {
|
||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||
"x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver",
|
||||
"x_axis": x_axis,
|
||||
"y_axis": "left",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": "90 - rawHeadingDeg",
|
||||
"yaw_enu_deg": f"90 - (rawHeadingDeg + {heading_offset_deg:g})",
|
||||
"frame_mode": "baseline_raw_heading" if baseline_frame else "vehicle_forward_heading_offset",
|
||||
},
|
||||
"LiDAR": "raw LiDAR sensor frame",
|
||||
},
|
||||
"backend": backend,
|
||||
"measured_lidar_extrinsic_used_as_initial": False,
|
||||
"body_heading_offset_used": False,
|
||||
"measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")),
|
||||
"solver_initial_extrinsic": raw.get("solver_initial_extrinsic"),
|
||||
"body_heading_offset_deg": heading_offset_deg,
|
||||
"body_heading_offset_used": abs(heading_offset_deg) > 1e-12,
|
||||
"body_antenna_lever_xy_used": False,
|
||||
"translation_m": raw["translation_m"],
|
||||
"rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
|
||||
"quaternion_xyzw": raw["quaternion_xyzw"],
|
||||
"coordinate_contract_audit": coordinate_contract_audit(raw),
|
||||
"matrix_4x4": raw["matrix_4x4"],
|
||||
"quality": {
|
||||
"stations": raw["estimation"]["stations"],
|
||||
@@ -80,6 +127,7 @@ def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--result-root", type=Path, required=True)
|
||||
parser.add_argument("--reference-height", type=float, required=True)
|
||||
parser.add_argument("--heading-offset-deg", type=float, required=True)
|
||||
args = parser.parse_args()
|
||||
|
||||
def solver_output(directory: str) -> Path:
|
||||
@@ -94,16 +142,26 @@ def main() -> None:
|
||||
}
|
||||
docs = {}
|
||||
for backend, path in paths.items():
|
||||
document = corrected(load(path), backend, args.reference_height)
|
||||
document = corrected(
|
||||
load(path), backend, args.reference_height, args.heading_offset_deg
|
||||
)
|
||||
write(path.with_name("extrinsic_rtk_lidar.json"), document)
|
||||
docs[backend] = document
|
||||
|
||||
open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
|
||||
small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
|
||||
final = dict(docs["consensus"])
|
||||
needs_axis_confirmation = bool(
|
||||
final["coordinate_contract_audit"]["requires_physical_axis_confirmation"]
|
||||
)
|
||||
final["selection"] = {
|
||||
"recommended": True,
|
||||
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
||||
"recommended": not needs_axis_confirmation,
|
||||
"reason": (
|
||||
"Physical axis confirmation is required because the data-driven solution differs "
|
||||
"from the declared mechanical initial by approximately 180 degrees"
|
||||
if needs_axis_confirmation else
|
||||
"Uses only motion pairs accepted independently by both Open3D GICP and small_gicp"
|
||||
),
|
||||
"open3d_vs_small_gicp": delta(open_t, small_t),
|
||||
}
|
||||
|
||||
@@ -117,6 +175,8 @@ def main() -> None:
|
||||
"translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"],
|
||||
"rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"],
|
||||
"condition_number": final["quality"]["weighted_jacobian_condition_number"],
|
||||
"coordinate_contract_status": final["coordinate_contract_audit"]["status"],
|
||||
"recommended_for_deployment": final["selection"]["recommended"],
|
||||
},
|
||||
"backend_difference": delta(open_t, small_t),
|
||||
}
|
||||
|
||||
@@ -80,6 +80,20 @@ def params_transform(params):
|
||||
return make_transform(params[:3], so3_exp(params[3:]))
|
||||
|
||||
|
||||
def transform_params(transform):
|
||||
from scipy.spatial.transform import Rotation
|
||||
transform = np.asarray(transform, float)
|
||||
return np.r_[transform[:3, 3], Rotation.from_matrix(transform[:3, :3]).as_rotvec()]
|
||||
|
||||
|
||||
def load_extrinsic_matrix(path):
|
||||
document = json.loads(Path(path).read_text(encoding="utf-8-sig"))
|
||||
transform = np.asarray(document["matrix_4x4"], dtype=float)
|
||||
if transform.shape != (4, 4):
|
||||
raise ValueError("initial extrinsic matrix_4x4 must be 4x4")
|
||||
return transform
|
||||
|
||||
|
||||
def inverse_transform(transform):
|
||||
answer = np.eye(4)
|
||||
answer[:3, :3] = transform[:3, :3].T
|
||||
@@ -135,7 +149,8 @@ def load_npz_xyz(path, min_range=1.0, max_range=50.0):
|
||||
if "points_raw" not in data:
|
||||
raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden")
|
||||
raw = np.asarray(data["points_raw"], dtype=np.float64)
|
||||
timestamp = float(np.ravel(data["unix_time_ns"])[0]) / 1e9
|
||||
time_key = "lidar_association_time_ns" if "lidar_association_time_ns" in data else "unix_time_ns"
|
||||
timestamp = float(np.ravel(data[time_key])[0]) / 1e9
|
||||
counter = int(np.ravel(data["frame_counter"])[0])
|
||||
distance = raw[:, 0] * 0.001
|
||||
azimuth = np.deg2rad(raw[:, 1])
|
||||
@@ -179,6 +194,63 @@ def make_o3d_cloud(points, voxel):
|
||||
return cloud.voxel_down_sample(voxel)
|
||||
|
||||
|
||||
def make_global_features(points, voxel):
|
||||
import open3d as o3d
|
||||
cloud = make_o3d_cloud(points, voxel)
|
||||
cloud.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(
|
||||
radius=voxel * 2.5, max_nn=50
|
||||
))
|
||||
features = o3d.pipelines.registration.compute_fpfh_feature(
|
||||
cloud,
|
||||
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel * 5.0, max_nn=100),
|
||||
)
|
||||
return cloud, features
|
||||
|
||||
|
||||
def global_lidar_initialization(target_features, source_features, args, pair_seed):
|
||||
"""Estimate source-to-target motion from LiDAR geometry without RTK or an extrinsic."""
|
||||
import open3d as o3d
|
||||
registration = o3d.pipelines.registration
|
||||
target_cloud, target_fpfh = target_features
|
||||
source_cloud, source_fpfh = source_features
|
||||
attempts = []
|
||||
for attempt in range(args.global_ransac_attempts):
|
||||
o3d.utility.random.seed(int(pair_seed + attempt))
|
||||
answer = registration.registration_ransac_based_on_feature_matching(
|
||||
source_cloud,
|
||||
target_cloud,
|
||||
source_fpfh,
|
||||
target_fpfh,
|
||||
True,
|
||||
args.global_correspondence,
|
||||
registration.TransformationEstimationPointToPoint(False),
|
||||
4,
|
||||
[
|
||||
registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
|
||||
registration.CorrespondenceCheckerBasedOnDistance(args.global_correspondence),
|
||||
],
|
||||
registration.RANSACConvergenceCriteria(
|
||||
args.global_ransac_iterations, args.global_ransac_confidence
|
||||
),
|
||||
)
|
||||
attempts.append({
|
||||
"transform": np.asarray(answer.transformation, float),
|
||||
"fitness": float(answer.fitness),
|
||||
"inlier_rmse_m": float(answer.inlier_rmse),
|
||||
})
|
||||
best = max(attempts, key=lambda item: (item["fitness"], -item["inlier_rmse_m"]))
|
||||
return {
|
||||
"transform": best["transform"],
|
||||
"method": "LiDAR-only FPFH RANSAC",
|
||||
"fitness": best["fitness"],
|
||||
"inlier_rmse_m": best["inlier_rmse_m"],
|
||||
"attempts": [
|
||||
{key: value for key, value in item.items() if key != "transform"}
|
||||
for item in attempts
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def align_open3d(target, source, initial, voxels, correspondences, iterations):
|
||||
import open3d as o3d
|
||||
registration = o3d.pipelines.registration
|
||||
@@ -381,6 +453,8 @@ def cmd_pairs(args):
|
||||
reference_poses = np.asarray(reference_poses)
|
||||
split = [split_holdout(station[3], args.holdout_fraction, i)
|
||||
for i, station in enumerate(stations)]
|
||||
global_features = [make_global_features(points[0], args.global_voxel)
|
||||
for points in split]
|
||||
rng = np.random.default_rng(args.seed)
|
||||
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
|
||||
accepted_transforms = {}
|
||||
@@ -389,9 +463,15 @@ def cmd_pairs(args):
|
||||
a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j]
|
||||
translation = float(np.linalg.norm(a_ij[:2, 3]))
|
||||
rotation = rotation_angle_deg(a_ij[:3, :3])
|
||||
if args.max_reference_translation is not None and translation > args.max_reference_translation:
|
||||
continue
|
||||
if translation < args.min_translation and rotation < args.min_rotation:
|
||||
continue
|
||||
initial_b = a_ij.copy() # X0=I; no measured extrinsic.
|
||||
global_initial = global_lidar_initialization(
|
||||
global_features[i], global_features[j], args,
|
||||
args.seed + i * 1009 + j * 9176,
|
||||
)
|
||||
initial_b = global_initial["transform"]
|
||||
target_fit, target_holdout = split[i]
|
||||
source_fit, source_holdout = split[j]
|
||||
forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
|
||||
@@ -447,7 +527,10 @@ def cmd_pairs(args):
|
||||
"frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1],
|
||||
"rtk_translation_m": translation, "rtk_rotation_deg": rotation,
|
||||
"nearest_rtk_dt_i_s": reference_dt[i], "nearest_rtk_dt_j_s": reference_dt[j],
|
||||
"initial_B_source": "X0=identity; B0=A (no measured extrinsic)",
|
||||
"initial_B_source": global_initial["method"],
|
||||
"global_lidar_initialization": {
|
||||
key: value for key, value in global_initial.items() if key != "transform"
|
||||
},
|
||||
"B_ij_4x4": forward["transform"].tolist(),
|
||||
"backend": args.backend, "backend_converged": forward["converged"],
|
||||
"backend_iterations": forward["iterations"],
|
||||
@@ -482,7 +565,11 @@ def cmd_pairs(args):
|
||||
"backend": args.backend,
|
||||
"transform_convention": "B_ij=T_Li_Lj maps station j points into station i",
|
||||
"raw_point_field": "points_raw",
|
||||
"measured_extrinsic_used_as_initial": False,
|
||||
"registration_initial_extrinsic": None,
|
||||
"selection_is_X_independent": True,
|
||||
"B_estimation_is_RTK_independent": True,
|
||||
"candidate_pair_selection_uses_reference_motion": True,
|
||||
"initialization_warning": None,
|
||||
"stations": len(stations), "candidate_pairs": len(reports),
|
||||
"accepted_pairs": len(accepted_a),
|
||||
"parameters": vars(args),
|
||||
@@ -585,9 +672,11 @@ def pair_metrics(a_array, b_array, x):
|
||||
|
||||
def solve_extrinsic(a_array, b_array, planes, args):
|
||||
rng = np.random.default_rng(args.seed)
|
||||
starts = [np.zeros(6)]
|
||||
center = (transform_params(load_extrinsic_matrix(args.initial_extrinsic))
|
||||
if args.initial_extrinsic else np.zeros(6))
|
||||
starts = [center]
|
||||
for _ in range(args.solver_multistart - 1):
|
||||
starts.append(np.r_[
|
||||
starts.append(center + np.r_[
|
||||
rng.normal(0.0, args.start_translation_sigma, 3),
|
||||
np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
|
||||
])
|
||||
@@ -647,7 +736,10 @@ def cmd_calibrate(args):
|
||||
"message": best.message,
|
||||
"convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame",
|
||||
"equation": "A_ij X = X B_ij",
|
||||
"measured_extrinsic_used_as_initial": False,
|
||||
"measured_extrinsic_used_as_initial": bool(args.initial_extrinsic),
|
||||
"solver_initial_extrinsic": (
|
||||
str(Path(args.initial_extrinsic).resolve()) if args.initial_extrinsic else None
|
||||
),
|
||||
"translation_m": x[:3, 3].tolist(),
|
||||
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
|
||||
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
|
||||
@@ -707,7 +799,8 @@ def build_parser():
|
||||
ground = commands.add_parser("ground")
|
||||
ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True)
|
||||
ground.add_argument("--min-range", type=float, default=1.0); ground.add_argument("--max-range", type=float, default=30.0)
|
||||
ground.add_argument("--z-min", type=float, default=-1.4); ground.add_argument("--z-max", type=float, default=-0.4)
|
||||
# Default ROI for ~2 m roof LiDAR (Z-up). Override for other mounting heights.
|
||||
ground.add_argument("--z-min", type=float, default=-2.5); ground.add_argument("--z-max", type=float, default=-1.5)
|
||||
ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025)
|
||||
ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500)
|
||||
ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground)
|
||||
@@ -720,10 +813,16 @@ def build_parser():
|
||||
pairs.add_argument("--time-offset", type=float, default=0.0)
|
||||
pairs.add_argument("--min-stations", type=int, default=30); pairs.add_argument("--min-pairs", type=int, default=25)
|
||||
pairs.add_argument("--min-gap", type=int, default=1); pairs.add_argument("--max-gap", type=int, default=5)
|
||||
pairs.add_argument("--max-reference-translation", type=float)
|
||||
pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0)
|
||||
pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0)
|
||||
pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0)
|
||||
pairs.add_argument("--min-roi-points", type=int, default=1000)
|
||||
pairs.add_argument("--global-voxel", type=float, default=0.50)
|
||||
pairs.add_argument("--global-correspondence", type=float, default=1.25)
|
||||
pairs.add_argument("--global-ransac-attempts", type=int, default=3)
|
||||
pairs.add_argument("--global-ransac-iterations", type=int, default=100000)
|
||||
pairs.add_argument("--global-ransac-confidence", type=float, default=0.999)
|
||||
pairs.add_argument("--holdout-fraction", type=float, default=0.20)
|
||||
pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08])
|
||||
pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25])
|
||||
@@ -744,6 +843,7 @@ def build_parser():
|
||||
calibrate = commands.add_parser("calibrate")
|
||||
calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True)
|
||||
calibrate.add_argument("--output", required=True)
|
||||
calibrate.add_argument("--initial-extrinsic")
|
||||
calibrate.add_argument("--translation-sigma", type=float, default=0.05)
|
||||
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
|
||||
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
|
||||
|
||||
+2
-2
@@ -1,12 +1,12 @@
|
||||
# 数据说明
|
||||
|
||||
原始 H32/G90/N300 `.rscap`（以及旧版 LiDAR dlog）、逐帧 NPZ 和 prepared 点云体积较大，不进入 Git。请从项目云盘取得数据，并按根 README 中的目录示例放置；实际路径通过命令参数传入。
|
||||
原始 H32 dlog / G90·N300 `.rscap`（以及旧版 LiDAR dlog / `h32.rscap`）、逐帧 NPZ 和 prepared 点云体积较大，不进入 Git。请从项目云盘取得数据，并按根 README 中的目录示例放置；实际路径通过命令参数传入。
|
||||
|
||||
推荐原始布局：
|
||||
|
||||
```text
|
||||
raw_dataset/
|
||||
├── stations/<站号>/h32.rscap
|
||||
├── stations/<站号>/ # dobject/ + dobject_recording/（MSOP+DIFOP）
|
||||
└── captures/rtk.rscap, imu.rscap
|
||||
```
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
+59
-14
@@ -1,27 +1,72 @@
|
||||
# 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`：主机接收时间
|
||||
|
||||
- `-LidarCaptureName h32.rscap`：每站雷达文件名（也接受 `lidar.rscap`）
|
||||
- `-TimeBasis device_gnss`（默认）：雷达设备时 ↔ GNSS week/TOW
|
||||
- `-TimeBasis host`：旧 dlog + 主机接收时间关联
|
||||
G90 连续录制 + 站时间窗：
|
||||
|
||||
所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度，避免静默使用与实车不符的默认值；默认生成目录`work/`和`outputs/`不会提交Git。
|
||||
```powershell
|
||||
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
|
||||
```
|
||||
|
||||
可加 `--reuse-export` 续跑。
|
||||
|
||||
## 已有 combined 复现本次结果
|
||||
|
||||
```powershell
|
||||
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
|
||||
```
|
||||
|
||||
## 可视化本次结果
|
||||
|
||||
```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
|
||||
```
|
||||
|
||||
@@ -4,7 +4,7 @@ param(
|
||||
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
|
||||
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
|
||||
[string]$PoseName = "rtk_gga_raw_heading",
|
||||
[int]$MinStations = 30,
|
||||
[int]$MinStations = 20,
|
||||
[int]$ExpectedStations = 0,
|
||||
[double]$HeadingStdLimitDeg = 0.5,
|
||||
[switch]$Overwrite
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
{
|
||||
"schema_version": 1,
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK baseline frame (X = rawHeading baseline, Y left, Z up; heading_offset_deg = 0)",
|
||||
"translation_m": [
|
||||
0.414179474,
|
||||
0.210859360,
|
||||
0.004000001
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
0.0,
|
||||
0.0,
|
||||
90.0
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[0.0, -1.0, 0.0, 0.414179474],
|
||||
[1.0, 0.0, 0.0, 0.210859360],
|
||||
[0.0, 0.0, 1.0, 0.004000001],
|
||||
[0.0, 0.0, 0.0, 1.0]
|
||||
],
|
||||
"use": "Final AX=XB solver initialization only; never use for LiDAR pair registration",
|
||||
"yaw_note": "≈90 deg yaw is expected when LiDAR X is vehicle-forward and the dual-antenna baseline is left-right",
|
||||
"z_note": "CAD/mechanical z only; final z is constrained by measured GGA/ANT1 phase-center height above ground"
|
||||
}
|
||||
@@ -3,32 +3,66 @@ param(
|
||||
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
||||
[string]$OutputRoot = "",
|
||||
[string]$WorkRoot = "",
|
||||
[int]$ExpectedStations = 34,
|
||||
[int]$ExpectedStations = 27,
|
||||
[int]$MinStations = 20,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 200
|
||||
[int]$Bootstrap = 200,
|
||||
# Roof-mounted H32 (~2 m): ground points are near z≈-2 in the LiDAR frame (Z-up).
|
||||
# The old [-1.4, -0.4] window fits walls on this vehicle and must not be reused.
|
||||
[double]$GroundZMin = -2.5,
|
||||
[double]$GroundZMax = -1.5,
|
||||
[int]$SmallGicpMaxGap = 26,
|
||||
[int]$Open3DMaxGap = 26,
|
||||
[double]$MaxReferenceTranslationM = 8.0,
|
||||
# Baseline frame: rawHeading as RTK X. Use 90 only when deliberately targeting vehicle-forward.
|
||||
[double]$HeadingOffsetDeg = 0.0,
|
||||
[string]$SolverInitialExtrinsic = "",
|
||||
[double]$RefineMinInlierRatio = 0.63,
|
||||
[double]$RefineMaxInlierRmseM = 0.14
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
if ([string]::IsNullOrWhiteSpace($OutputRoot)) { $OutputRoot = Join-Path $Repo "outputs\rtk_lidar_calibration" }
|
||||
if ([string]::IsNullOrWhiteSpace($WorkRoot)) { $WorkRoot = Join-Path $Repo "work\prepared_rtk_direct" }
|
||||
if ([string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
|
||||
$SolverInitialExtrinsic = Join-Path $PSScriptRoot "rtk_lidar_mechanical_initial.json"
|
||||
}
|
||||
|
||||
$PoseName = if ([math]::Abs($HeadingOffsetDeg) -le 1e-12) {
|
||||
"rtk_gga_raw_heading"
|
||||
} else {
|
||||
"rtk_vehicle_heading"
|
||||
}
|
||||
$ReferencePoseFile = "reference_poses_${PoseName}.csv"
|
||||
$Prepared = $WorkRoot
|
||||
|
||||
& (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") `
|
||||
-CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg 0 `
|
||||
-AntennaLever @(0.0,0.0,0.0) -PoseName "rtk_gga_raw_heading" -MinStations 30 -ExpectedStations $ExpectedStations -Overwrite
|
||||
-CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg $HeadingOffsetDeg `
|
||||
-AntennaLever @(0.0,0.0,0.0) -PoseName $PoseName -MinStations $MinStations `
|
||||
-ExpectedStations $ExpectedStations -Overwrite
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }
|
||||
|
||||
# Pair registration intentionally has no --initial-extrinsic (B must stay X-independent).
|
||||
# SolverInitialExtrinsic is applied only in the final AX=XB calibrate stage.
|
||||
& (Join-Path $Repo "run\run_single_dataset.ps1") `
|
||||
-Prepared $Prepared -OutputRoot $OutputRoot `
|
||||
-ReferencePoseFile "reference_poses_rtk_gga_raw_heading.csv" `
|
||||
-ReferenceHeight $RtkReferenceHeightAboveGroundM -MinPairs $MinPairs -Bootstrap $Bootstrap
|
||||
-ReferencePoseFile $ReferencePoseFile `
|
||||
-ReferenceHeight $RtkReferenceHeightAboveGroundM `
|
||||
-MinStations $MinStations -MinPairs $MinPairs -Bootstrap $Bootstrap `
|
||||
-GroundZMin $GroundZMin -GroundZMax $GroundZMax `
|
||||
-SmallGicpMaxGap $SmallGicpMaxGap -Open3DMaxGap $Open3DMaxGap `
|
||||
-MaxReferenceTranslationM $MaxReferenceTranslationM `
|
||||
-SolverInitialExtrinsic $SolverInitialExtrinsic `
|
||||
-RefineMinInlierRatio $RefineMinInlierRatio `
|
||||
-RefineMaxInlierRmseM $RefineMaxInlierRmseM
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct calibration failed" }
|
||||
|
||||
$Finalize = @(
|
||||
(Join-Path $Repo "code\finalize_direct_rtk_lidar.py"),
|
||||
"--result-root", $OutputRoot,
|
||||
"--reference-height", "$RtkReferenceHeightAboveGroundM"
|
||||
"--reference-height", "$RtkReferenceHeightAboveGroundM",
|
||||
"--heading-offset-deg", "$HeadingOffsetDeg"
|
||||
)
|
||||
& python @Finalize
|
||||
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
|
||||
|
||||
@@ -8,9 +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
|
||||
[int]$Bootstrap = 200,
|
||||
# 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"
|
||||
@@ -28,7 +33,10 @@ if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" }
|
||||
-CombinedRoot (Join-Path $ExportRoot "combined") `
|
||||
-WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
|
||||
-RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM `
|
||||
-ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap
|
||||
-HeadingOffsetDeg $HeadingOffsetDeg `
|
||||
-MinStations $MinStations `
|
||||
-ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap `
|
||||
-GroundZMin $GroundZMin -GroundZMax $GroundZMax
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" }
|
||||
|
||||
Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')"
|
||||
|
||||
@@ -3,8 +3,18 @@ param(
|
||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
||||
[Parameter(Mandatory = $true)][double]$ReferenceHeight,
|
||||
[string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
|
||||
[int]$MinStations = 20,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 100
|
||||
[int]$Bootstrap = 100,
|
||||
# Roof-mounted H32 (~2 m): ground near z≈-2. Old [-1.4,-0.4] fits walls on this vehicle.
|
||||
[double]$GroundZMin = -2.5,
|
||||
[double]$GroundZMax = -1.5,
|
||||
[int]$SmallGicpMaxGap = 26,
|
||||
[int]$Open3DMaxGap = 26,
|
||||
[double]$MaxReferenceTranslationM = 8.0,
|
||||
[string]$SolverInitialExtrinsic = "",
|
||||
[double]$RefineMinInlierRatio = 0.63,
|
||||
[double]$RefineMaxInlierRmseM = 0.14
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
@@ -32,27 +42,38 @@ foreach ($Path in @($Frames, $ReferencePoses)) {
|
||||
New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null
|
||||
|
||||
$Ground = Join-Path $Common "ground_planes.csv"
|
||||
Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground)
|
||||
Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground,
|
||||
"--z-min", "$GroundZMin", "--z-max", "$GroundZMax")
|
||||
|
||||
foreach ($Backend in @("small_gicp", "open3d")) {
|
||||
$Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
|
||||
$Raw = Join-Path $Directory "B_estimation.npz"
|
||||
$QualityJson = Join-Path $Directory "B_quality.json"
|
||||
$QualityCsv = Join-Path $Directory "B_quality.csv"
|
||||
$MaxGap = if ($Backend -eq "open3d") { $Open3DMaxGap } else { $SmallGicpMaxGap }
|
||||
$PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--reference-poses", $ReferencePoses,
|
||||
"--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv,
|
||||
"--min-pairs", "$MinPairs")
|
||||
if ($Backend -eq "open3d") { $PairArgs += @("--max-gap", "3", "--multistart", "1", "--iterations", "40") }
|
||||
"--min-stations", "$MinStations", "--min-pairs", "$MinPairs", "--max-gap", "$MaxGap")
|
||||
if ($MaxReferenceTranslationM -gt 0) {
|
||||
$PairArgs += @("--max-reference-translation", "$MaxReferenceTranslationM")
|
||||
}
|
||||
if ($Backend -eq "open3d") { $PairArgs += @("--multistart", "1", "--iterations", "40") }
|
||||
Run-Python "$Backend pairs" $PairArgs
|
||||
Run-Python "$Backend X-independent refinement" @(
|
||||
$Refine, "--pairs", $Raw, "--quality-json", $QualityJson,
|
||||
"--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs"
|
||||
"--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs",
|
||||
"--min-inlier-ratio", "$RefineMinInlierRatio",
|
||||
"--max-inlier-rmse", "$RefineMaxInlierRmseM"
|
||||
)
|
||||
Run-Python "$Backend calibration" @(
|
||||
$CalibrationArgs = @(
|
||||
$Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
|
||||
"--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
|
||||
"--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
|
||||
)
|
||||
if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
|
||||
$CalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
|
||||
}
|
||||
Run-Python "$Backend calibration" $CalibrationArgs
|
||||
}
|
||||
|
||||
$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
|
||||
@@ -61,10 +82,14 @@ Run-Python "cross-backend consensus" @(
|
||||
"--small-pairs", (Join-Path $Small "B_refined.npz"),
|
||||
"--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
|
||||
)
|
||||
Run-Python "consensus calibration" @(
|
||||
$ConsensusCalibrationArgs = @(
|
||||
$Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
|
||||
"--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
|
||||
"--output", (Join-Path $ConsensusOut "extrinsic.json")
|
||||
)
|
||||
if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
|
||||
$ConsensusCalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
|
||||
}
|
||||
Run-Python "consensus calibration" $ConsensusCalibrationArgs
|
||||
|
||||
Write-Host "Calibration results: $OutputRoot"
|
||||
|
||||
@@ -0,0 +1,116 @@
|
||||
"""Regression tests for G90 GNHPR parsing and host-time LiDAR association."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
TOOLS = ROOT / "tools"
|
||||
CODE = ROOT / "code"
|
||||
sys.path.insert(0, str(TOOLS))
|
||||
sys.path.insert(0, str(TOOLS / "rscap_v2"))
|
||||
sys.path.insert(0, str(CODE))
|
||||
|
||||
from build_multisensor_npz import build_combined # noqa: E402
|
||||
from pipeline_common import parse_gnhpr # noqa: E402
|
||||
from rigorous_calibration import load_npz_xyz # noqa: E402
|
||||
|
||||
|
||||
def test_parse_gnhpr_fixed_heading():
|
||||
row = parse_gnhpr("$GNHPR,070411.40,354.7437,000.2518,000.0000,4,26,0.00,0999*58")
|
||||
assert row["type"] == "GNHPR"
|
||||
assert row["raw_heading_deg"] == 354.7437
|
||||
assert row["pitch_deg"] == 0.2518
|
||||
assert row["roll_deg"] == 0.0
|
||||
assert row["heading_quality"] == 4
|
||||
assert row["satellites"] == 26
|
||||
assert row["heading_valid"] is True
|
||||
|
||||
|
||||
def test_host_time_uses_lidar_receive_time_and_preserves_device_time(tmp_path: Path):
|
||||
host_ns = 1_786_240_000_000_000_000
|
||||
device_ns = 1_500_000_000_000_000_000
|
||||
frame_dir = tmp_path / "lidar"
|
||||
frame_dir.mkdir()
|
||||
np.savez_compressed(
|
||||
frame_dir / "frame.npz",
|
||||
points=np.zeros((4, 4), dtype=np.float32),
|
||||
unix_time_ns=np.asarray([device_ns], dtype=np.int64),
|
||||
host_receive_utc_ns=np.asarray([host_ns], dtype=np.int64),
|
||||
)
|
||||
|
||||
rtk = tmp_path / "rtk.jsonl"
|
||||
rows = [
|
||||
{
|
||||
"type": "GGA",
|
||||
"checksum_valid": True,
|
||||
"host_receive_utc_ns": host_ns + 20_000_000,
|
||||
"lat_deg": 31.0,
|
||||
"lon_deg": 121.0,
|
||||
"altitude_m": 10.0,
|
||||
"fix_quality": 4,
|
||||
"satellites": 20,
|
||||
"raw_line": "$GNGGA,...",
|
||||
},
|
||||
{
|
||||
"type": "GNHPR",
|
||||
"checksum_valid": True,
|
||||
"host_receive_utc_ns": host_ns - 10_000_000,
|
||||
"raw_heading_deg": 90.0,
|
||||
"pitch_deg": 1.0,
|
||||
"roll_deg": 0.0,
|
||||
"heading_quality": 4,
|
||||
"heading_solution": "GNHPR_QUALITY_4",
|
||||
"heading_valid": True,
|
||||
"satellites": 22,
|
||||
"raw_line": "$GNHPR,...",
|
||||
},
|
||||
]
|
||||
rtk.write_text("".join(json.dumps(row) + "\n" for row in rows), encoding="utf-8")
|
||||
imu = tmp_path / "imu.jsonl"
|
||||
imu.write_text("", encoding="utf-8")
|
||||
|
||||
out = tmp_path / "combined"
|
||||
summary = build_combined(
|
||||
[("STATION-01", frame_dir)],
|
||||
[rtk],
|
||||
[imu],
|
||||
out,
|
||||
time_basis="host",
|
||||
rtk_max_dt_ms=100.0,
|
||||
)
|
||||
|
||||
assert summary["frames"] == 1
|
||||
assert summary["rtk_valid"] == 1
|
||||
assert summary["heading_valid"] == 1
|
||||
assert summary["rtk_fixed"] == 1
|
||||
with np.load(next((out / "frames").glob("*.npz")), allow_pickle=False) as frame:
|
||||
assert int(frame["lidar_association_time_ns"][0]) == host_ns
|
||||
assert int(frame["unix_time_ns"][0]) == device_ns
|
||||
assert int(frame["rtk_gga_dt_ns"][0]) == 20_000_000
|
||||
assert int(frame["rtk_heading_dt_ns"][0]) == -10_000_000
|
||||
|
||||
|
||||
def test_registration_prefers_lidar_association_time(tmp_path: Path):
|
||||
host_ns = 1_786_240_000_000_000_000
|
||||
device_ns = 1_500_000_000_000_000_000
|
||||
source = tmp_path / "frame.npz"
|
||||
np.savez_compressed(
|
||||
source,
|
||||
points_raw=np.asarray(
|
||||
[[1000.0, 0.0, 0.0, 1.0], [2000.0, 90.0, 0.0, 1.0]],
|
||||
dtype=np.float32,
|
||||
),
|
||||
unix_time_ns=np.asarray([device_ns], dtype=np.int64),
|
||||
lidar_association_time_ns=np.asarray([host_ns], dtype=np.int64),
|
||||
frame_counter=np.asarray([7], dtype=np.int64),
|
||||
)
|
||||
|
||||
timestamp, counter, xyz = load_npz_xyz(source)
|
||||
assert timestamp == host_ns / 1e9
|
||||
assert counter == 7
|
||||
assert xyz.shape == (2, 3)
|
||||
@@ -0,0 +1,190 @@
|
||||
"""Unit tests for H32 Medulla raw dlog → station frame export helpers."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
TOOLS = ROOT / "tools"
|
||||
sys.path.insert(0, str(TOOLS))
|
||||
sys.path.insert(0, str(TOOLS / "rscap_v2"))
|
||||
|
||||
from export_h32_rscap_station import export_station_h32_dlog, is_h32_raw_dlog_station # noqa: E402
|
||||
from h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles # noqa: E402
|
||||
from h32_dlog.dobject import discover_records, iter_payloads, resolve_dlog_root # noqa: E402
|
||||
from h32_dlog.load_session import load_h32_dlog_lidar # noqa: E402
|
||||
from h32_dlog.payload_v1 import ( # noqa: E402
|
||||
MsopPacketItem,
|
||||
build_difop_payload,
|
||||
build_msop_batch_payload,
|
||||
parse_difop_payload,
|
||||
parse_msop_batch_payload,
|
||||
)
|
||||
from h32_msop import PACKET_LENGTH, iter_h32_frames_polar_from_packets # noqa: E402
|
||||
|
||||
|
||||
def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
|
||||
packet = bytearray(PACKET_LENGTH)
|
||||
packet[17] = 1
|
||||
packet[20:26] = int(seconds).to_bytes(6, "big")
|
||||
packet[26:30] = int(microseconds).to_bytes(4, "big")
|
||||
az_raw = int(round(az_deg * 100))
|
||||
for block in range(12):
|
||||
offset = 42 + block * 100
|
||||
packet[offset] = 255
|
||||
packet[offset + 1] = 238
|
||||
packet[offset + 2] = (az_raw >> 8) & 0xFF
|
||||
packet[offset + 3] = az_raw & 0xFF
|
||||
idx = offset + 4
|
||||
for _ch in range(CHANNELS):
|
||||
packet[idx] = (1600 >> 8) & 0xFF
|
||||
packet[idx + 1] = 1600 & 0xFF
|
||||
packet[idx + 2] = 10
|
||||
idx += 3
|
||||
return bytes(packet)
|
||||
|
||||
|
||||
def _write_signed_angle(buf: bytearray, index: int, degrees: float) -> None:
|
||||
sign = 1 if degrees < 0 else 0
|
||||
raw = int(round(abs(degrees) * 100))
|
||||
buf[index] = sign
|
||||
buf[index + 1] = (raw >> 8) & 0xFF
|
||||
buf[index + 2] = raw & 0xFF
|
||||
|
||||
|
||||
def _make_difop_packet(*, vertical: list[float], horizontal: list[float] | None = None) -> bytes:
|
||||
packet = bytearray(1248)
|
||||
horiz = horizontal if horizontal is not None else [0.0] * CHANNELS
|
||||
for channel, angle in enumerate(vertical):
|
||||
_write_signed_angle(packet, VERTICAL_START + channel * 3, angle)
|
||||
for channel, angle in enumerate(horiz):
|
||||
_write_signed_angle(packet, HORIZONTAL_START + channel * 3, angle)
|
||||
return bytes(packet)
|
||||
|
||||
|
||||
def _write_dorec_record(
|
||||
path: Path,
|
||||
*,
|
||||
object_name: str,
|
||||
ticks: int,
|
||||
record_id: str,
|
||||
payload: bytes,
|
||||
) -> int:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
name_b = object_name.encode("ascii")
|
||||
id_b = record_id.encode("ascii")
|
||||
blob = (
|
||||
bytes([len(name_b)])
|
||||
+ name_b
|
||||
+ struct.pack("<q", ticks)
|
||||
+ bytes([len(id_b)])
|
||||
+ id_b
|
||||
+ struct.pack("<i", len(payload))
|
||||
+ payload
|
||||
)
|
||||
with path.open("ab" if path.exists() else "wb") as handle:
|
||||
start = handle.tell()
|
||||
handle.write(blob)
|
||||
return start
|
||||
|
||||
|
||||
def test_parse_msop_and_difop_payload_roundtrip():
|
||||
packet = _make_msop_packet(seconds=1700000000, microseconds=123456)
|
||||
item = MsopPacketItem(
|
||||
sequence=7,
|
||||
device_timestamp_us=1700000000 * 1_000_000 + 123456,
|
||||
device_timestamp_valid=True,
|
||||
host_receive_utc_ticks=111,
|
||||
host_receive_monotonic_ticks=222,
|
||||
raw=packet,
|
||||
)
|
||||
msop_payload = build_msop_batch_payload(packets=[item], session_id="sess-a")
|
||||
batch = parse_msop_batch_payload(msop_payload)
|
||||
assert batch.session_id == "sess-a"
|
||||
assert len(batch.packets) == 1
|
||||
assert batch.packets[0].raw == packet
|
||||
|
||||
vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
|
||||
difop_raw = _make_difop_packet(vertical=vertical, horizontal=[0.05] * CHANNELS)
|
||||
difop = parse_difop_payload(build_difop_payload(raw=difop_raw, sequence=3))
|
||||
angles = parse_difop_angles(difop.raw)
|
||||
assert np.allclose(angles.vertical_deg, vertical, atol=1e-2)
|
||||
assert np.allclose(angles.horizontal_deg, 0.05, atol=1e-2)
|
||||
|
||||
|
||||
def test_export_station_h32_dlog_mini(tmp_path: Path):
|
||||
station = tmp_path / "001"
|
||||
dorec_name = "raw.dorec"
|
||||
dorec_path = station / "dobject_recording" / dorec_name
|
||||
log_path = station / "dobject" / "rec.log"
|
||||
|
||||
vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
|
||||
difop_payload = build_difop_payload(raw=_make_difop_packet(vertical=vertical), sequence=1)
|
||||
msop_packet = _make_msop_packet(az_deg=15.0)
|
||||
msop_payload = build_msop_batch_payload(
|
||||
packets=[
|
||||
MsopPacketItem(
|
||||
sequence=1,
|
||||
device_timestamp_us=100_000_000,
|
||||
device_timestamp_valid=True,
|
||||
host_receive_utc_ticks=621355968000000000 + 10_000_000,
|
||||
host_receive_monotonic_ticks=2,
|
||||
raw=msop_packet,
|
||||
)
|
||||
]
|
||||
)
|
||||
off_difop = _write_dorec_record(
|
||||
dorec_path,
|
||||
object_name="frontlidar-difop-raw",
|
||||
ticks=1000,
|
||||
record_id="AA",
|
||||
payload=difop_payload,
|
||||
)
|
||||
off_msop = _write_dorec_record(
|
||||
dorec_path,
|
||||
object_name="frontlidar-msop-raw",
|
||||
ticks=1001,
|
||||
record_id="BB",
|
||||
payload=msop_payload,
|
||||
)
|
||||
log_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
log_path.write_text(
|
||||
"\n".join(
|
||||
[
|
||||
f"[t] DObject `frontlidar-difop-raw` post len={len(difop_payload)}B, "
|
||||
f"id:AA, tic:1000, @{dorec_name}:{off_difop}",
|
||||
f"[t] DObject `frontlidar-msop-raw` post len={len(msop_payload)}B, "
|
||||
f"id:BB, tic:1001, @{dorec_name}:{off_msop}",
|
||||
]
|
||||
)
|
||||
+ "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
|
||||
assert resolve_dlog_root(station) == station.resolve()
|
||||
assert is_h32_raw_dlog_station(station)
|
||||
assert len(discover_records(station, "frontlidar-msop-raw")) == 1
|
||||
assert len(list(iter_payloads(station, "frontlidar-msop-raw"))) == 1
|
||||
|
||||
session = load_h32_dlog_lidar(station, require_difop=True)
|
||||
assert session.angle_source == "difop_channel_angles"
|
||||
frames = iter_h32_frames_polar_from_packets(
|
||||
session.msop_packets,
|
||||
host_utc_ticks=session.msop_host_utc_ticks,
|
||||
min_frame_points=1,
|
||||
vertical_deg=session.vertical_deg,
|
||||
horizontal_deg=session.horizontal_deg,
|
||||
)
|
||||
assert len(frames) == 1
|
||||
assert frames[0].points_raw.shape[1] == 5
|
||||
|
||||
out = tmp_path / "export"
|
||||
meta = export_station_h32_dlog(station, out, require_difop=True, min_frame_points=1)
|
||||
assert meta["kind"] == "h32_dlog_raw"
|
||||
assert meta["angle_source"] == "difop_channel_angles"
|
||||
assert meta["frames_written"] >= 1
|
||||
assert any((out / "frames").glob("*.npz"))
|
||||
@@ -0,0 +1,68 @@
|
||||
"""Regression tests for the RTK–LiDAR coordinate and initialization contract."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
sys.path.insert(0, str(ROOT / "tools"))
|
||||
sys.path.insert(0, str(ROOT / "code"))
|
||||
|
||||
from finalize_direct_rtk_lidar import coordinate_contract_audit # noqa: E402
|
||||
from prepare_multisensor_station_dataset import heading_to_enu_yaw # noqa: E402
|
||||
from rigorous_calibration import ( # noqa: E402
|
||||
build_parser,
|
||||
load_extrinsic_matrix,
|
||||
params_transform,
|
||||
transform_params,
|
||||
)
|
||||
|
||||
|
||||
def test_left_baseline_heading_plus_90_points_vehicle_forward() -> None:
|
||||
corrected, yaw = heading_to_enu_yaw(270.0, 90.0)
|
||||
assert corrected == 0.0
|
||||
assert math.degrees(yaw) == 90.0
|
||||
|
||||
|
||||
def test_east_vehicle_heading_maps_to_zero_enu_yaw() -> None:
|
||||
corrected, yaw = heading_to_enu_yaw(0.0, 90.0)
|
||||
assert corrected == 90.0
|
||||
assert math.degrees(yaw) == 0.0
|
||||
|
||||
|
||||
def test_pair_registration_has_no_extrinsic_argument() -> None:
|
||||
parser = build_parser()
|
||||
pair_options = {
|
||||
option
|
||||
for action in parser._subparsers._group_actions[0].choices["pairs"]._actions
|
||||
for option in action.option_strings
|
||||
}
|
||||
assert "--initial-extrinsic" not in pair_options
|
||||
assert "--global-voxel" in pair_options
|
||||
|
||||
|
||||
def test_mechanical_initial_round_trip() -> None:
|
||||
path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
|
||||
transform = load_extrinsic_matrix(path)
|
||||
np.testing.assert_allclose(transform[:3, 3], [0.414179474, 0.210859360, 0.004000001])
|
||||
np.testing.assert_allclose(transform[:3, :3], [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
|
||||
np.testing.assert_allclose(params_transform(transform_params(transform)), transform, atol=1e-12)
|
||||
|
||||
|
||||
def test_near_180_degree_solution_is_flagged_for_physical_axis_check() -> None:
|
||||
initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
|
||||
initial = load_extrinsic_matrix(initial_path)
|
||||
# Flip the declared mechanical forward axis by ~180 deg about Z.
|
||||
solution = np.eye(4)
|
||||
solution[:3, :3] = initial[:3, :3] @ np.diag([-1.0, -1.0, 1.0])
|
||||
solution[:3, 3] = initial[:3, 3]
|
||||
audit = coordinate_contract_audit({
|
||||
"solver_initial_extrinsic": str(initial_path),
|
||||
"matrix_4x4": solution.tolist(),
|
||||
})
|
||||
assert audit["status"] == "near_180_degree_axis_conflict"
|
||||
assert audit["requires_physical_axis_confirmation"] is True
|
||||
+4
-2
@@ -2,9 +2,11 @@
|
||||
|
||||
| 文件 | 输入→输出 |
|
||||
|---|---|
|
||||
| **`export_raw_to_combined.py`** | **一步导出**：逐站 H32 + 全程 G90/N300 `.rscap` → `combined/`（标定直接入口，对标 Lidar-IMU `export_rscap_to_v1`） |
|
||||
| **`export_raw_to_combined.py`** | **一步导出**：逐站 H32（dlog MSOP+DIFOP 或旧 `.rscap`）+ 全程 G90/N300 `.rscap` → `combined/` |
|
||||
| **`export_g90_h32_windows_to_combined.py`** | **本次 27 站**：G90 连续 rscap + H32 DLog ZIP，按站时间窗 → `combined/`（host UTC 关联） |
|
||||
| `export_h32_rscap_station.py` | 内部零件：单站 H32 → 雷达帧 NPZ（一般不必单独跑） |
|
||||
| `frontlidar_dlog_export.py` | **旧数据** LiDAR dlog → 逐帧 NPZ；由一步导出在遇到 dlog 站时自动调用 |
|
||||
| `h32_dlog/` | 新 H32 DLogCapture：dobject 索引、MSOP/DIFOP payload、DIFOP 通道角 |
|
||||
| `frontlidar_dlog_export.py` | **旧数据** 已解码点云 dlog → 逐帧 NPZ；无 raw MSOP 时由一步导出回退调用 |
|
||||
| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU（调试用）；一步导出已内嵌同等逻辑 |
|
||||
| `rscap_v2/h32_msop.py` | H32 MSOP 解码（XYZ / 极坐标 `points_raw`） |
|
||||
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
|
||||
|
||||
@@ -25,6 +25,7 @@ import numpy as np
|
||||
|
||||
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
|
||||
POSITION_TYPES = {"GGA", "PVTSLNA"}
|
||||
HEADING_TYPES = {"UNIHEADINGA", "GNHPR"}
|
||||
|
||||
|
||||
def parse_named_path(text: str) -> tuple[str, Path]:
|
||||
@@ -230,7 +231,7 @@ def build_combined(
|
||||
|
||||
heading = []
|
||||
for row in rtk_rows:
|
||||
if row.get("type") != "UNIHEADINGA" or not row.get("checksum_valid") or not row.get("heading_valid"):
|
||||
if row.get("type") not in HEADING_TYPES or not row.get("checksum_valid") or not row.get("heading_valid"):
|
||||
continue
|
||||
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
|
||||
if assoc is None:
|
||||
@@ -258,7 +259,14 @@ def build_combined(
|
||||
for segment_index, source in enumerate(frame_paths):
|
||||
with np.load(source, allow_pickle=False) as frame:
|
||||
values = {key: np.asarray(frame[key]) for key in frame.files}
|
||||
lidar_time_ns = int(scalar(values["unix_time_ns"]))
|
||||
lidar_device_time_ns = int(scalar(values["unix_time_ns"]))
|
||||
if time_basis == "host":
|
||||
lidar_time_ns = int(scalar(values["host_receive_utc_ns"]))
|
||||
if lidar_time_ns <= 0:
|
||||
raise ValueError(f"host time requested but missing in {source}")
|
||||
else:
|
||||
lidar_time_ns = lidar_device_time_ns
|
||||
values["lidar_association_time_ns"] = np.asarray([lidar_time_ns], dtype=np.int64)
|
||||
|
||||
position_index = nearest_index(position_times, lidar_time_ns)
|
||||
heading_index = nearest_index(heading_times, lidar_time_ns)
|
||||
@@ -337,6 +345,7 @@ def build_combined(
|
||||
"output": str(output.relative_to(out)),
|
||||
"source_lidar": str(source.resolve()),
|
||||
"lidar_time_ns": lidar_time_ns,
|
||||
"lidar_device_time_ns": lidar_device_time_ns,
|
||||
"rtk_gga_dt_ns": position_dt,
|
||||
"rtk_heading_dt_ns": heading_dt,
|
||||
"rtk_valid": position_ok,
|
||||
|
||||
@@ -0,0 +1,239 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Build LiDAR GT/quality tables for a continuous LiDAR + dual-RTK + IMU run."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import datetime as dt
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def args() -> argparse.Namespace:
|
||||
p = argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument("--lidar-manifest", type=Path, required=True)
|
||||
p.add_argument("--rtk-jsonl", type=Path, required=True)
|
||||
p.add_argument("--imu-jsonl", type=Path, required=True)
|
||||
p.add_argument("--extrinsic", type=Path, required=True)
|
||||
p.add_argument("--out", type=Path, required=True)
|
||||
p.add_argument("--max-bracket-ms", type=float, default=150.0)
|
||||
p.add_argument("--heading-std-limit-deg", type=float, default=0.5)
|
||||
return p.parse_args()
|
||||
|
||||
|
||||
def read_jsonl(path: Path) -> list[dict[str, Any]]:
|
||||
with path.open(encoding="utf-8") as f:
|
||||
return [json.loads(line) for line in f if line.strip()]
|
||||
|
||||
|
||||
def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray:
|
||||
a, e2 = 6378137.0, 6.69437999014e-3
|
||||
lat, lon = math.radians(lat_deg), math.radians(lon_deg)
|
||||
slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
n = a / math.sqrt(1.0 - e2 * slat * slat)
|
||||
return np.array([(n + height_m) * clat * clon,
|
||||
(n + height_m) * clat * slon,
|
||||
(n * (1.0 - e2) + height_m) * slat], dtype=float)
|
||||
|
||||
|
||||
def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: float) -> np.ndarray:
|
||||
lat, lon = math.radians(lat_deg), math.radians(lon_deg)
|
||||
slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
r = np.array([[-slon, clon, 0.0],
|
||||
[-slat * clon, -slat * slon, clat],
|
||||
[clat * clon, clat * slon, slat]], dtype=float)
|
||||
return r @ (ecef - origin)
|
||||
|
||||
|
||||
def yaw_matrix(yaw_rad: float) -> np.ndarray:
|
||||
c, s = math.cos(yaw_rad), math.sin(yaw_rad)
|
||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float)
|
||||
|
||||
|
||||
def matrix_to_quat_xyzw(r: np.ndarray) -> np.ndarray:
|
||||
# Stable branch-based conversion; output convention is x,y,z,w.
|
||||
tr = float(np.trace(r))
|
||||
if tr > 0.0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
q = np.array([(r[2, 1] - r[1, 2]) / s,
|
||||
(r[0, 2] - r[2, 0]) / s,
|
||||
(r[1, 0] - r[0, 1]) / s, 0.25 * s])
|
||||
else:
|
||||
i = int(np.argmax(np.diag(r)))
|
||||
if i == 0:
|
||||
s = math.sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0
|
||||
q = np.array([0.25 * s, (r[0, 1] + r[1, 0]) / s,
|
||||
(r[0, 2] + r[2, 0]) / s, (r[2, 1] - r[1, 2]) / s])
|
||||
elif i == 1:
|
||||
s = math.sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0
|
||||
q = np.array([(r[0, 1] + r[1, 0]) / s, 0.25 * s,
|
||||
(r[1, 2] + r[2, 1]) / s, (r[0, 2] - r[2, 0]) / s])
|
||||
else:
|
||||
s = math.sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0
|
||||
q = np.array([(r[0, 2] + r[2, 0]) / s,
|
||||
(r[1, 2] + r[2, 1]) / s, 0.25 * s,
|
||||
(r[1, 0] - r[0, 1]) / s])
|
||||
if q[3] < 0.0:
|
||||
q = -q
|
||||
return q / np.linalg.norm(q)
|
||||
|
||||
|
||||
def bracket(rows: list[dict[str, Any]], times: np.ndarray, t: int,
|
||||
max_ns: int) -> tuple[dict[str, Any], dict[str, Any], float] | None:
|
||||
right = int(np.searchsorted(times, t, side="left"))
|
||||
if right == 0 or right >= len(times):
|
||||
return None
|
||||
left = right - 1
|
||||
t0, t1 = int(times[left]), int(times[right])
|
||||
if t1 <= t0 or t - t0 > max_ns or t1 - t > max_ns:
|
||||
return None
|
||||
return rows[left], rows[right], (t - t0) / (t1 - t0)
|
||||
|
||||
|
||||
def circular_lerp_deg(a: float, b: float, u: float) -> float:
|
||||
delta = (b - a + 180.0) % 360.0 - 180.0
|
||||
return (a + u * delta) % 360.0
|
||||
|
||||
|
||||
def iso_utc(ns: int) -> str:
|
||||
return dt.datetime.fromtimestamp(ns / 1e9, dt.timezone.utc).isoformat(timespec="microseconds")
|
||||
|
||||
|
||||
def write_imu_csv(rows: list[dict[str, Any]], path: Path) -> None:
|
||||
fields = [
|
||||
"host_receive_utc_ns", "device_timestamp_ms", "pps_sync_stamp_ms", "crc_valid",
|
||||
"accel_x_mps2", "accel_y_mps2", "accel_z_mps2",
|
||||
"gyro_x_radps", "gyro_y_radps", "gyro_z_radps",
|
||||
"mag_x_ut", "mag_y_ut", "mag_z_ut", "temperature_c", "air_pressure_pa",
|
||||
"roll_deg", "pitch_deg", "yaw_deg",
|
||||
"quaternion_x", "quaternion_y", "quaternion_z", "quaternion_w",
|
||||
"source_chunk_sequence_first", "source_raw_file_offset",
|
||||
]
|
||||
with path.open("w", encoding="utf-8", newline="") as f:
|
||||
w = csv.DictWriter(f, fieldnames=fields)
|
||||
w.writeheader()
|
||||
for row in rows:
|
||||
w.writerow({key: row.get(key) for key in fields})
|
||||
|
||||
|
||||
def main() -> int:
|
||||
a = args()
|
||||
a.out.mkdir(parents=True, exist_ok=True)
|
||||
with a.lidar_manifest.open(encoding="utf-8-sig", newline="") as f:
|
||||
lidar = [row for row in csv.DictReader(f) if not row.get("error")]
|
||||
rtk = read_jsonl(a.rtk_jsonl)
|
||||
imu = [row for row in read_jsonl(a.imu_jsonl) if row.get("crc_valid")]
|
||||
gga = sorted([r for r in rtk if r.get("type") == "GGA" and r.get("checksum_valid")
|
||||
and r.get("lat_deg") is not None], key=lambda r: int(r["host_receive_utc_ns"]))
|
||||
heading = sorted([r for r in rtk if r.get("type") == "UNIHEADINGA" and r.get("checksum_valid")
|
||||
and r.get("heading_valid") and r.get("raw_heading_deg") is not None],
|
||||
key=lambda r: int(r["host_receive_utc_ns"]))
|
||||
if not lidar or len(gga) < 2 or len(heading) < 2:
|
||||
raise RuntimeError("insufficient LiDAR/GGA/heading data")
|
||||
|
||||
ext = json.loads(a.extrinsic.read_text(encoding="utf-8"))
|
||||
t_r_l = np.asarray(ext["matrix_4x4"], dtype=float)
|
||||
if t_r_l.shape != (4, 4):
|
||||
raise ValueError("extrinsic matrix_4x4 must be 4x4")
|
||||
|
||||
gga_times = np.asarray([int(r["host_receive_utc_ns"]) for r in gga], dtype=np.int64)
|
||||
heading_times = np.asarray([int(r["host_receive_utc_ns"]) for r in heading], dtype=np.int64)
|
||||
origin_row = next(r for r in gga if int(r.get("fix_quality", -1)) == 4)
|
||||
origin_lat, origin_lon, origin_alt = (float(origin_row[k]) for k in ("lat_deg", "lon_deg", "altitude_m"))
|
||||
origin_ecef = geodetic_to_ecef(origin_lat, origin_lon, origin_alt)
|
||||
max_ns = int(a.max_bracket_ms * 1_000_000)
|
||||
pose_rows: list[dict[str, Any]] = []
|
||||
|
||||
for index, frame in enumerate(lidar):
|
||||
t = int(frame["unix_time_ns"])
|
||||
gb, hb = bracket(gga, gga_times, t, max_ns), bracket(heading, heading_times, t, max_ns)
|
||||
reasons: list[str] = []
|
||||
available = gb is not None and hb is not None
|
||||
row: dict[str, Any] = {
|
||||
"frame_index": index, "lidar_time_ns": t, "lidar_time_utc": iso_utc(t),
|
||||
"lidar_file": frame["output_file"], "point_count": frame["point_count"],
|
||||
"pose_available": int(available), "gt_valid": 0, "invalid_reason": "",
|
||||
}
|
||||
if not available:
|
||||
if gb is None: reasons.append("GGA_NOT_BRACKETED")
|
||||
if hb is None: reasons.append("HEADING_NOT_BRACKETED")
|
||||
row.update({k: "" for k in ("x_m", "y_m", "z_m", "qx", "qy", "qz", "qw",
|
||||
"rtk_x_m", "rtk_y_m", "rtk_z_m", "raw_heading_deg")})
|
||||
row["invalid_reason"] = ";".join(reasons)
|
||||
pose_rows.append(row)
|
||||
continue
|
||||
|
||||
g0, g1, gu = gb
|
||||
h0, h1, hu = hb
|
||||
p0 = geodetic_to_ecef(float(g0["lat_deg"]), float(g0["lon_deg"]), float(g0["altitude_m"]))
|
||||
p1 = geodetic_to_ecef(float(g1["lat_deg"]), float(g1["lon_deg"]), float(g1["altitude_m"]))
|
||||
p_rtk = ecef_to_enu((1.0 - gu) * p0 + gu * p1, origin_ecef, origin_lat, origin_lon)
|
||||
raw_heading = circular_lerp_deg(float(h0["raw_heading_deg"]), float(h1["raw_heading_deg"]), hu)
|
||||
yaw = math.radians(90.0 - raw_heading)
|
||||
t_w_r = np.eye(4)
|
||||
t_w_r[:3, :3] = yaw_matrix(yaw)
|
||||
t_w_r[:3, 3] = p_rtk
|
||||
t_w_l = t_w_r @ t_r_l
|
||||
q = matrix_to_quat_xyzw(t_w_l[:3, :3])
|
||||
|
||||
fix0, fix1 = int(g0.get("fix_quality", -1)), int(g1.get("fix_quality", -1))
|
||||
sol0, sol1 = str(h0.get("heading_solution", "")), str(h1.get("heading_solution", ""))
|
||||
std0 = float(h0.get("heading_stddev_deg") or math.inf)
|
||||
std1 = float(h1.get("heading_stddev_deg") or math.inf)
|
||||
if fix0 != 4 or fix1 != 4: reasons.append("RTK_POSITION_NOT_FIXED")
|
||||
if sol0 != "NARROW_INT" or sol1 != "NARROW_INT": reasons.append("HEADING_NOT_NARROW_INT")
|
||||
if max(std0, std1) > a.heading_std_limit_deg: reasons.append("HEADING_STD_EXCEEDED")
|
||||
row.update({
|
||||
"gt_valid": int(not reasons), "invalid_reason": ";".join(reasons),
|
||||
"x_m": t_w_l[0, 3], "y_m": t_w_l[1, 3], "z_m": t_w_l[2, 3],
|
||||
"qx": q[0], "qy": q[1], "qz": q[2], "qw": q[3],
|
||||
"rtk_x_m": p_rtk[0], "rtk_y_m": p_rtk[1], "rtk_z_m": p_rtk[2],
|
||||
"raw_heading_deg": raw_heading, "yaw_enu_deg": math.degrees(yaw),
|
||||
"gga_fix_before": fix0, "gga_fix_after": fix1,
|
||||
"heading_solution_before": sol0, "heading_solution_after": sol1,
|
||||
"heading_std_max_deg": max(std0, std1),
|
||||
"gga_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6,
|
||||
"gga_after_dt_ms": (int(g1["host_receive_utc_ns"]) - t) / 1e6,
|
||||
"heading_before_dt_ms": (t - int(h0["host_receive_utc_ns"])) / 1e6,
|
||||
"heading_after_dt_ms": (int(h1["host_receive_utc_ns"]) - t) / 1e6,
|
||||
})
|
||||
pose_rows.append(row)
|
||||
|
||||
fields = list(dict.fromkeys(k for row in pose_rows for k in row))
|
||||
pose_path = a.out / "lidar_gt_pose_enu.csv"
|
||||
with pose_path.open("w", encoding="utf-8", newline="") as f:
|
||||
w = csv.DictWriter(f, fieldnames=fields)
|
||||
w.writeheader(); w.writerows(pose_rows)
|
||||
write_imu_csv(imu, a.out / "imu_parsed.csv")
|
||||
|
||||
summary = {
|
||||
"coordinate_convention": "T_W_L maps raw LiDAR points to local ENU; T_W_L = T_W_RTK @ T_RTK_lidar",
|
||||
"world_frame": "local ENU, origin is the first RTK FIX GGA sample",
|
||||
"rtk_frame": "x is rawHeading baseline direction projected horizontally, y left, z up",
|
||||
"orientation_model": "RTK pose is yaw-only; IMU orientation is not fused",
|
||||
"time_basis": "LiDAR and serial host UTC; no jointly estimated clock offset/drift",
|
||||
"lidar_frames": len(pose_rows),
|
||||
"pose_available_frames": sum(int(r["pose_available"]) for r in pose_rows),
|
||||
"gt_valid_frames": sum(int(r["gt_valid"]) for r in pose_rows),
|
||||
"gt_invalid_frames": sum(not int(r["gt_valid"]) for r in pose_rows),
|
||||
"imu_frames": len(imu),
|
||||
"enu_origin": {"lat_deg": origin_lat, "lon_deg": origin_lon, "altitude_m": origin_alt},
|
||||
"quality_rule": "GGA endpoints fix_quality=4, heading endpoints NARROW_INT, heading std <= limit, both streams bracket LiDAR time",
|
||||
"heading_std_limit_deg": a.heading_std_limit_deg,
|
||||
"max_bracket_ms": a.max_bracket_ms,
|
||||
"warning": "gt_valid is a quality gate, not independent proof of +/-3 cm absolute accuracy",
|
||||
}
|
||||
(a.out / "delivery_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Convert exported LiDAR polar NPZ frames to portable XYZ-in-metres NPZ frames."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def main() -> int:
|
||||
p = argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument("--input", type=Path, required=True)
|
||||
p.add_argument("--output", type=Path, required=True)
|
||||
p.add_argument("--overwrite", action="store_true")
|
||||
a = p.parse_args()
|
||||
sources = sorted(a.input.glob("*.npz"))
|
||||
if not sources:
|
||||
raise FileNotFoundError(f"no NPZ frames in {a.input}")
|
||||
a.output.mkdir(parents=True, exist_ok=True)
|
||||
written = skipped = 0
|
||||
for index, source in enumerate(sources, 1):
|
||||
target = a.output / source.name
|
||||
if target.exists() and not a.overwrite:
|
||||
skipped += 1
|
||||
continue
|
||||
with np.load(source, allow_pickle=False) as f:
|
||||
raw = np.asarray(f["points_raw"], dtype=np.float32)
|
||||
time_ns = np.asarray(f["unix_time_ns"], dtype=np.int64)
|
||||
counter = np.asarray(f["frame_counter"], dtype=np.int32)
|
||||
distance_m = raw[:, 0] * np.float32(0.001)
|
||||
azimuth = np.deg2rad(raw[:, 1])
|
||||
altitude = np.deg2rad(raw[:, 2])
|
||||
cos_alt = np.cos(altitude)
|
||||
xyz = np.column_stack((distance_m * cos_alt * np.cos(azimuth),
|
||||
distance_m * cos_alt * np.sin(azimuth),
|
||||
distance_m * np.sin(altitude))).astype(np.float32, copy=False)
|
||||
np.savez_compressed(target, xyz_m=xyz, intensity=raw[:, 3].astype(np.float32, copy=False),
|
||||
progression=raw[:, 4].astype(np.float32, copy=False),
|
||||
unix_time_ns=time_ns, frame_counter=counter)
|
||||
written += 1
|
||||
if index % 100 == 0 or index == len(sources):
|
||||
print(f"[{index}/{len(sources)}] written={written} skipped={skipped}", flush=True)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,381 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export audited H32/G90 static windows directly to RTK--LiDAR combined data.
|
||||
|
||||
This adapter is for captures where multiple static stations live inside large
|
||||
DLog archives instead of one directory per station. It uses the H32 packet
|
||||
host-receive UTC ticks as the common software clock, parses G90 ``$GNGGA`` and
|
||||
``$GNHPR`` from one or more V2 captures, and deliberately does not require IMU.
|
||||
Raw inputs are opened read-only.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import bisect
|
||||
import csv
|
||||
import json
|
||||
import shutil
|
||||
import struct
|
||||
import sys
|
||||
import zipfile
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime, timedelta, timezone
|
||||
from pathlib import Path
|
||||
from typing import BinaryIO, Iterator
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parent
|
||||
sys.path.insert(0, str(ROOT))
|
||||
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
||||
|
||||
from build_multisensor_npz import build_combined # noqa: E402
|
||||
from h32_dlog.difop import DifopAngles, parse_difop_angles # noqa: E402
|
||||
from h32_dlog.dotnet_bin import read_dotnet_string # noqa: E402
|
||||
from h32_dlog.payload_v1 import parse_difop_payload, parse_msop_batch_payload # noqa: E402
|
||||
from capture_format_v2 import read_capture # noqa: E402
|
||||
from h32_msop import iter_h32_frames_polar_from_packets # noqa: E402
|
||||
from pipeline_common_corrected import parse_rtk_capture, write_jsonl # noqa: E402
|
||||
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
TICKS_PER_SECOND = 10_000_000
|
||||
LOCAL_TZ = timezone(timedelta(hours=8))
|
||||
MSOP_OBJECT = "frontlidar-msop-raw"
|
||||
DIFOP_OBJECT = "frontlidar-difop-raw"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Window:
|
||||
station_id: str
|
||||
start_ticks: int
|
||||
end_ticks: int
|
||||
|
||||
|
||||
def local_text_to_utc_ticks(text: str) -> int:
|
||||
value = datetime.strptime(text.strip(), "%Y-%m-%d %H:%M:%S.%f").replace(tzinfo=LOCAL_TZ)
|
||||
return int(round(value.timestamp() * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS
|
||||
|
||||
|
||||
def read_exact(stream: BinaryIO, length: int) -> bytes:
|
||||
value = stream.read(length)
|
||||
if len(value) != length:
|
||||
raise EOFError(f"expected {length} bytes, got {len(value)}")
|
||||
return value
|
||||
|
||||
|
||||
def iter_zip_dobject_payloads(path: Path) -> Iterator[tuple[str, bytes]]:
|
||||
"""Sequentially read DObject records from a standard Medulla DLog ZIP."""
|
||||
|
||||
with zipfile.ZipFile(path) as archive:
|
||||
candidates = [
|
||||
name for name in archive.namelist()
|
||||
if name.replace("\\", "/").endswith("dobject_recording/data.bin")
|
||||
]
|
||||
if len(candidates) != 1:
|
||||
raise ValueError(f"{path}: expected one dobject_recording/data.bin, got {candidates}")
|
||||
with archive.open(candidates[0], "r") as stream:
|
||||
while True:
|
||||
try:
|
||||
name = read_dotnet_string(stream)
|
||||
except EOFError:
|
||||
break
|
||||
read_exact(stream, 8) # outer DObject tick
|
||||
read_dotnet_string(stream) # record id
|
||||
length = struct.unpack("<i", read_exact(stream, 4))[0]
|
||||
if length < 0 or length > 128 * 1024 * 1024:
|
||||
raise ValueError(f"{path}: invalid DObject payload length {length}")
|
||||
yield name, read_exact(stream, length)
|
||||
|
||||
|
||||
def load_windows(path: Path) -> list[Window]:
|
||||
grouped: dict[str, list[tuple[int, int]]] = {}
|
||||
with path.open("r", encoding="utf-8-sig", newline="") as stream:
|
||||
for row in csv.DictReader(stream):
|
||||
station = row["station_id"].strip()
|
||||
grouped.setdefault(station, []).append(
|
||||
(local_text_to_utc_ticks(row["local_start"]), local_text_to_utc_ticks(row["local_end"]))
|
||||
)
|
||||
merged: list[Window] = []
|
||||
for station, ranges in grouped.items():
|
||||
current: list[list[int]] = []
|
||||
for start, end in sorted(ranges):
|
||||
if current and start <= current[-1][1]:
|
||||
current[-1][1] = max(current[-1][1], end)
|
||||
else:
|
||||
current.append([start, end])
|
||||
merged.extend(Window(station, start, end) for start, end in current)
|
||||
merged.sort(key=lambda item: item.start_ticks)
|
||||
for previous, current in zip(merged, merged[1:]):
|
||||
if current.start_ticks <= previous.end_ticks and current.station_id != previous.station_id:
|
||||
raise ValueError(f"overlapping stations: {previous} and {current}")
|
||||
return merged
|
||||
|
||||
|
||||
def station_lookup(windows: list[Window]):
|
||||
starts = [item.start_ticks for item in windows]
|
||||
|
||||
def lookup(ticks: int) -> str | None:
|
||||
index = bisect.bisect_right(starts, ticks) - 1
|
||||
if index >= 0 and ticks <= windows[index].end_ticks:
|
||||
return windows[index].station_id
|
||||
return None
|
||||
|
||||
return lookup
|
||||
|
||||
|
||||
def save_frames(
|
||||
station: str,
|
||||
packet_items: dict[tuple[str, int], tuple[int, int, bytes]],
|
||||
export_root: Path,
|
||||
angles: DifopAngles,
|
||||
source: Path,
|
||||
*,
|
||||
frame_stride: int,
|
||||
seen_frame_keys: set[tuple[str, int, int]],
|
||||
) -> int:
|
||||
if not packet_items:
|
||||
return 0
|
||||
ordered = sorted(packet_items.values(), key=lambda item: (item[0], item[1]))
|
||||
frames = iter_h32_frames_polar_from_packets(
|
||||
(item[2] for item in ordered),
|
||||
host_utc_ticks=[item[0] for item in ordered],
|
||||
frame_stride=frame_stride,
|
||||
min_frame_points=100,
|
||||
min_range_m=0.3,
|
||||
max_range_m=120.0,
|
||||
vertical_deg=angles.vertical_deg,
|
||||
horizontal_deg=angles.horizontal_deg,
|
||||
)
|
||||
frames_dir = export_root / station / "frames"
|
||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||
written = 0
|
||||
for frame in frames:
|
||||
# Overlapping archives contain identical revolutions. The host stamp
|
||||
# and 0.1 s device bucket make the key stable without comparing points.
|
||||
key = (station, int(round(frame.host_receive_utc_ns / 10_000_000)), int(round(frame.t_start_s * 10)))
|
||||
if key in seen_frame_keys:
|
||||
continue
|
||||
seen_frame_keys.add(key)
|
||||
device_ns = int(round(frame.t_start_s * 1_000_000_000))
|
||||
destination = frames_dir / f"h32_{frame.host_receive_utc_ns}_{device_ns}.npz"
|
||||
np.savez_compressed(
|
||||
destination,
|
||||
points_raw=np.asarray(frame.points_raw, dtype=np.float32),
|
||||
frame_counter=np.asarray([len(seen_frame_keys)], dtype=np.int32),
|
||||
point_count=np.asarray([len(frame.points_raw)], dtype=np.int32),
|
||||
unix_time_ns=np.asarray([device_ns], dtype=np.int64),
|
||||
device_time_s=np.asarray([frame.t_start_s], dtype=np.float64),
|
||||
device_time_end_s=np.asarray([frame.t_end_s], dtype=np.float64),
|
||||
host_receive_utc_ns=np.asarray([frame.host_receive_utc_ns], dtype=np.int64),
|
||||
source_file_utf8=np.frombuffer(str(source.resolve()).encode("utf-8"), dtype=np.uint8),
|
||||
)
|
||||
written += 1
|
||||
return written
|
||||
|
||||
|
||||
def scan_dlog_sources(
|
||||
sources: list[Path],
|
||||
windows: list[Window],
|
||||
export_root: Path,
|
||||
*,
|
||||
frame_stride: int,
|
||||
) -> dict[str, object]:
|
||||
lookup = station_lookup(windows)
|
||||
angles: DifopAngles | None = None
|
||||
seen_packets: dict[str, set[tuple[str, int]]] = {}
|
||||
seen_frames: set[tuple[str, int, int]] = set()
|
||||
frame_counts: dict[str, int] = {}
|
||||
source_stats: list[dict[str, object]] = []
|
||||
|
||||
for source_index, source in enumerate(sources, 1):
|
||||
print(f"[dlog {source_index}/{len(sources)}] {source}", flush=True)
|
||||
packets: dict[str, dict[tuple[str, int], tuple[int, int, bytes]]] = {}
|
||||
msop_batches = difop_records = selected_packets = duplicates = 0
|
||||
for object_name, payload in iter_zip_dobject_payloads(source):
|
||||
if object_name == DIFOP_OBJECT:
|
||||
difop_records += 1
|
||||
if angles is None:
|
||||
try:
|
||||
angles = parse_difop_angles(parse_difop_payload(payload).raw)
|
||||
except (EOFError, ValueError):
|
||||
pass
|
||||
continue
|
||||
if object_name != MSOP_OBJECT:
|
||||
continue
|
||||
batch = parse_msop_batch_payload(payload)
|
||||
msop_batches += 1
|
||||
for item in batch.packets:
|
||||
station = lookup(item.host_receive_utc_ticks)
|
||||
if station is None:
|
||||
continue
|
||||
packet_key = (batch.session_id, item.sequence)
|
||||
station_seen = seen_packets.setdefault(station, set())
|
||||
if packet_key in station_seen:
|
||||
duplicates += 1
|
||||
continue
|
||||
station_seen.add(packet_key)
|
||||
packets.setdefault(station, {})[packet_key] = (
|
||||
item.host_receive_utc_ticks,
|
||||
item.sequence,
|
||||
item.raw,
|
||||
)
|
||||
selected_packets += 1
|
||||
if angles is None:
|
||||
raise RuntimeError(f"no valid H32 DIFOP angles found before decoding {source}")
|
||||
written = 0
|
||||
for station, items in packets.items():
|
||||
count = save_frames(
|
||||
station,
|
||||
items,
|
||||
export_root,
|
||||
angles,
|
||||
source,
|
||||
frame_stride=frame_stride,
|
||||
seen_frame_keys=seen_frames,
|
||||
)
|
||||
frame_counts[station] = frame_counts.get(station, 0) + count
|
||||
written += count
|
||||
source_stats.append(
|
||||
{
|
||||
"source": str(source.resolve()),
|
||||
"msop_batches": msop_batches,
|
||||
"difop_records": difop_records,
|
||||
"selected_packets": selected_packets,
|
||||
"duplicate_packets": duplicates,
|
||||
"frames_written": written,
|
||||
}
|
||||
)
|
||||
print(f" selected_packets={selected_packets} frames={written} duplicates={duplicates}", flush=True)
|
||||
return {"frame_counts": frame_counts, "sources": source_stats}
|
||||
|
||||
|
||||
def parse_rtk_sources(paths: list[Path], parsed_root: Path) -> dict[str, object]:
|
||||
rows = []
|
||||
source_stats = []
|
||||
for path in paths:
|
||||
capture_rows = parse_rtk_capture(
|
||||
read_capture(path),
|
||||
accepted_prefixes=("$GNGGA", "$GPGGA", "$GNHPR"),
|
||||
)
|
||||
for row in capture_rows:
|
||||
row["capture_source"] = str(path.resolve())
|
||||
rows.extend(capture_rows)
|
||||
source_stats.append(
|
||||
{
|
||||
"source": str(path.resolve()),
|
||||
"rows": len(capture_rows),
|
||||
"gga_valid": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in capture_rows),
|
||||
"gnhpr_valid": sum(
|
||||
row.get("type") == "GNHPR" and row.get("checksum_valid") and row.get("heading_valid")
|
||||
for row in capture_rows
|
||||
),
|
||||
}
|
||||
)
|
||||
parsed_root.mkdir(parents=True, exist_ok=True)
|
||||
write_jsonl(parsed_root / "rtk.jsonl", rows)
|
||||
write_jsonl(parsed_root / "imu.jsonl", [])
|
||||
return {"rows": len(rows), "sources": source_stats}
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--segments-csv", type=Path, required=True)
|
||||
parser.add_argument("--lidar-dlog", type=Path, action="append", default=[])
|
||||
parser.add_argument("--rtk-rscap", type=Path, action="append", required=True)
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
parser.add_argument("--expected-stations", type=int, default=0)
|
||||
parser.add_argument("--frame-stride", type=int, default=5)
|
||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=200.0)
|
||||
parser.add_argument("--reuse-export", action="store_true", help="Keep existing export/ and resume parsed/combined stages.")
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.frame_stride < 1:
|
||||
raise SystemExit("--frame-stride must be >= 1")
|
||||
if not args.reuse_export and not args.lidar_dlog:
|
||||
raise SystemExit("at least one --lidar-dlog is required unless --reuse-export is used")
|
||||
for source in [args.segments_csv, *args.lidar_dlog, *args.rtk_rscap]:
|
||||
if not source.is_file():
|
||||
raise FileNotFoundError(source)
|
||||
if args.reuse_export:
|
||||
export_root = args.out / "export"
|
||||
if not export_root.is_dir():
|
||||
raise FileNotFoundError(f"--reuse-export requested but missing {export_root}")
|
||||
for name in ("parsed", "combined", "export_summary.json"):
|
||||
target = args.out / name
|
||||
if target.is_dir():
|
||||
shutil.rmtree(target)
|
||||
elif target.exists():
|
||||
target.unlink()
|
||||
elif args.out.exists() and any(args.out.iterdir()):
|
||||
if not args.overwrite:
|
||||
raise FileExistsError(f"{args.out} is non-empty; pass --overwrite")
|
||||
for name in ("export", "parsed", "combined", "export_summary.json"):
|
||||
target = args.out / name
|
||||
if target.is_dir():
|
||||
shutil.rmtree(target)
|
||||
elif target.exists():
|
||||
target.unlink()
|
||||
args.out.mkdir(parents=True, exist_ok=True)
|
||||
windows = load_windows(args.segments_csv)
|
||||
expected_ids = sorted({item.station_id for item in windows})
|
||||
if args.reuse_export:
|
||||
frame_counts = {
|
||||
station.name: len(list((station / "frames").glob("*.npz")))
|
||||
for station in (args.out / "export").iterdir()
|
||||
if station.is_dir()
|
||||
}
|
||||
lidar_summary = {"frame_counts": frame_counts, "sources": [], "reused_export": True}
|
||||
else:
|
||||
lidar_summary = scan_dlog_sources(
|
||||
args.lidar_dlog,
|
||||
windows,
|
||||
args.out / "export",
|
||||
frame_stride=args.frame_stride,
|
||||
)
|
||||
frame_counts = lidar_summary["frame_counts"]
|
||||
exported_ids = sorted(station for station, count in frame_counts.items() if count)
|
||||
missing = sorted(set(expected_ids) - set(exported_ids))
|
||||
if missing:
|
||||
raise RuntimeError(f"stations without decoded H32 frames: {missing}")
|
||||
if args.expected_stations and len(exported_ids) != args.expected_stations:
|
||||
raise RuntimeError(f"expected {args.expected_stations} stations, exported {len(exported_ids)}")
|
||||
rtk_summary = parse_rtk_sources(args.rtk_rscap, args.out / "parsed")
|
||||
lidar_segments = [(station, args.out / "export" / station / "frames") for station in exported_ids]
|
||||
combined_summary = build_combined(
|
||||
lidar_segments,
|
||||
[args.out / "parsed" / "rtk.jsonl"],
|
||||
[],
|
||||
args.out / "combined",
|
||||
rtk_max_dt_ms=args.rtk_max_dt_ms,
|
||||
time_basis="host",
|
||||
overwrite=True,
|
||||
)
|
||||
summary = {
|
||||
"role": "G90 GNGGA/GNHPR + H32 DLog static-window export",
|
||||
"segments_csv": str(args.segments_csv.resolve()),
|
||||
"time_basis": "H32 MSOP host_receive_utc_ticks <-> G90 rscap host_receive_utc_ns",
|
||||
"imu_used": False,
|
||||
"expected_station_ids": expected_ids,
|
||||
"station_count": len(exported_ids),
|
||||
"lidar": lidar_summary,
|
||||
"rtk": rtk_summary,
|
||||
"combined": combined_summary,
|
||||
"outputs": {
|
||||
"combined": str((args.out / "combined").resolve()),
|
||||
"manifest": str((args.out / "combined" / "manifest.csv").resolve()),
|
||||
},
|
||||
}
|
||||
(args.out / "export_summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
print(json.dumps({"stations": len(exported_ids), "combined": combined_summary}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -1,5 +1,10 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export one static-station H32 V2 .rscap into LiDAR frame NPZs.
|
||||
"""Export one static-station H32 capture into LiDAR frame NPZs.
|
||||
|
||||
Supports:
|
||||
|
||||
- V2 ``.rscap`` (legacy MSOP-only RawCapture)
|
||||
- Medulla dlog from ``RSLidarH32_3D_DLogCaptureNet48`` (raw MSOP + DIFOP)
|
||||
|
||||
This is an **internal** helper used by ``export_raw_to_combined.py``.
|
||||
For RTK–LiDAR calibration, prefer the one-shot exporter that writes ``combined/``.
|
||||
@@ -10,7 +15,7 @@ Output frame contract (consumed by ``build_multisensor_npz.py``):
|
||||
- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns)
|
||||
- ``frame_counter``, ``point_count``, optional host receive stamp
|
||||
|
||||
Raw ``.rscap`` files are never modified.
|
||||
Raw ``.rscap`` / dlog files are never modified.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -25,10 +30,16 @@ from typing import Any
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parent
|
||||
sys.path.insert(0, str(ROOT))
|
||||
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture # noqa: E402
|
||||
from h32_msop import iter_h32_frames_polar # noqa: E402
|
||||
from h32_dlog.dobject import discover_records, resolve_dlog_root # noqa: E402
|
||||
from h32_dlog.load_session import load_h32_dlog_lidar # noqa: E402
|
||||
from h32_msop import ( # noqa: E402
|
||||
iter_h32_frames_polar,
|
||||
iter_h32_frames_polar_from_packets,
|
||||
)
|
||||
|
||||
|
||||
def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> Path:
|
||||
@@ -45,36 +56,47 @@ def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> P
|
||||
)
|
||||
|
||||
|
||||
def export_station_h32(
|
||||
station: Path,
|
||||
out: Path,
|
||||
*,
|
||||
capture_name: str = "h32.rscap",
|
||||
stride: int = 1,
|
||||
min_frame_points: int = 100,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
compress: bool = True,
|
||||
write_reports: bool = False,
|
||||
resume: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Decode one station H32 capture into ``out/frames/*.npz``. Returns metadata."""
|
||||
def try_resolve_dlog_root(station_dir: Path) -> Path | None:
|
||||
try:
|
||||
return resolve_dlog_root(station_dir)
|
||||
except FileNotFoundError:
|
||||
return None
|
||||
|
||||
rscap = station if station.is_file() and station.suffix.lower() == ".rscap" else resolve_lidar_rscap(station, capture_name)
|
||||
|
||||
def is_h32_raw_dlog_station(
|
||||
station_dir: Path,
|
||||
*,
|
||||
msop_object: str = "frontlidar-msop-raw",
|
||||
) -> bool:
|
||||
root = try_resolve_dlog_root(station_dir)
|
||||
if root is None:
|
||||
return False
|
||||
return len(discover_records(root, msop_object)) > 0
|
||||
|
||||
|
||||
def is_legacy_pointcloud_dlog_station(
|
||||
station_dir: Path,
|
||||
*,
|
||||
msop_object: str = "frontlidar-msop-raw",
|
||||
) -> bool:
|
||||
root = try_resolve_dlog_root(station_dir)
|
||||
if root is None:
|
||||
return False
|
||||
return not is_h32_raw_dlog_station(station_dir, msop_object=msop_object)
|
||||
|
||||
|
||||
def _write_polar_frames(
|
||||
*,
|
||||
out: Path,
|
||||
frames,
|
||||
source_label: str,
|
||||
compress: bool,
|
||||
write_reports: bool,
|
||||
resume: bool,
|
||||
metadata_extra: dict[str, Any],
|
||||
) -> dict[str, Any]:
|
||||
frames_dir = out / "frames"
|
||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
capture = read_capture(rscap)
|
||||
frames = iter_h32_frames_polar(
|
||||
capture,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=max(1, stride),
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if not frames:
|
||||
raise RuntimeError(f"no H32 frames decoded from {rscap}")
|
||||
|
||||
saver = np.savez_compressed if compress else np.savez
|
||||
manifest_rows: list[dict[str, Any]] = []
|
||||
written = 0
|
||||
@@ -93,7 +115,7 @@ def export_station_h32(
|
||||
"device_time_s": np.asarray([frame.t_start_s], dtype=np.float64),
|
||||
"device_time_end_s": np.asarray([frame.t_end_s], dtype=np.float64),
|
||||
"host_receive_utc_ns": np.asarray([frame.host_receive_utc_ns], dtype=np.int64),
|
||||
"source_file_utf8": np.frombuffer(str(rscap.resolve()).encode("utf-8"), dtype=np.uint8),
|
||||
"source_file_utf8": np.frombuffer(source_label.encode("utf-8"), dtype=np.uint8),
|
||||
}
|
||||
saver(destination, **payload)
|
||||
written += 1
|
||||
@@ -108,18 +130,17 @@ def export_station_h32(
|
||||
)
|
||||
|
||||
metadata: dict[str, Any] = {
|
||||
"source_rscap": str(rscap.resolve()),
|
||||
"capture": file_summary(capture),
|
||||
"frames_decoded": len(frames),
|
||||
"frames_written": written,
|
||||
"frames_dir": str(frames_dir.resolve()),
|
||||
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
|
||||
"points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
|
||||
**metadata_extra,
|
||||
}
|
||||
(out / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
(out / "README.md").write_text(
|
||||
"# H32 station export (internal)\n\n"
|
||||
f"- source: `{rscap}`\n"
|
||||
f"- source: `{source_label}`\n"
|
||||
f"- frames: `{frames_dir}`\n"
|
||||
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
|
||||
encoding="utf-8",
|
||||
@@ -128,18 +149,133 @@ def export_station_h32(
|
||||
reports = out / "reports"
|
||||
reports.mkdir(parents=True, exist_ok=True)
|
||||
with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"])
|
||||
writer = csv.DictWriter(
|
||||
stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"]
|
||||
)
|
||||
writer.writeheader()
|
||||
writer.writerows(manifest_rows)
|
||||
(reports / "export_summary.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
(reports / "export_summary.json").write_text(
|
||||
json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8"
|
||||
)
|
||||
return metadata
|
||||
|
||||
|
||||
def export_station_h32(
|
||||
station: Path,
|
||||
out: Path,
|
||||
*,
|
||||
capture_name: str = "h32.rscap",
|
||||
stride: int = 1,
|
||||
min_frame_points: int = 100,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
compress: bool = True,
|
||||
write_reports: bool = False,
|
||||
resume: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Decode one station H32 ``.rscap`` into ``out/frames/*.npz``."""
|
||||
|
||||
rscap = (
|
||||
station
|
||||
if station.is_file() and station.suffix.lower() == ".rscap"
|
||||
else resolve_lidar_rscap(station, capture_name)
|
||||
)
|
||||
capture = read_capture(rscap)
|
||||
frames = iter_h32_frames_polar(
|
||||
capture,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=max(1, stride),
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if not frames:
|
||||
raise RuntimeError(f"no H32 frames decoded from {rscap}")
|
||||
return _write_polar_frames(
|
||||
out=out,
|
||||
frames=frames,
|
||||
source_label=str(rscap.resolve()),
|
||||
compress=compress,
|
||||
write_reports=write_reports,
|
||||
resume=resume,
|
||||
metadata_extra={
|
||||
"kind": "h32_rscap",
|
||||
"source_rscap": str(rscap.resolve()),
|
||||
"capture": file_summary(capture),
|
||||
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def export_station_h32_dlog(
|
||||
station: Path,
|
||||
out: Path,
|
||||
*,
|
||||
msop_object: str = "frontlidar-msop-raw",
|
||||
difop_object: str = "frontlidar-difop-raw",
|
||||
require_difop: bool = True,
|
||||
stride: int = 1,
|
||||
min_frame_points: int = 100,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
compress: bool = True,
|
||||
write_reports: bool = False,
|
||||
resume: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Decode one station H32 raw-MSOP/DIFOP dlog into ``out/frames/*.npz``."""
|
||||
|
||||
session = load_h32_dlog_lidar(
|
||||
station,
|
||||
msop_object=msop_object,
|
||||
difop_object=difop_object,
|
||||
require_difop=require_difop,
|
||||
)
|
||||
frames = iter_h32_frames_polar_from_packets(
|
||||
session.msop_packets,
|
||||
host_utc_ticks=session.msop_host_utc_ticks,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=max(1, stride),
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
vertical_deg=session.vertical_deg,
|
||||
horizontal_deg=session.horizontal_deg,
|
||||
)
|
||||
if not frames:
|
||||
raise RuntimeError(f"no H32 frames decoded from dlog {session.dlog_root}")
|
||||
return _write_polar_frames(
|
||||
out=out,
|
||||
frames=frames,
|
||||
source_label=str(session.dlog_root.resolve()),
|
||||
compress=compress,
|
||||
write_reports=write_reports,
|
||||
resume=resume,
|
||||
metadata_extra={
|
||||
"kind": "h32_dlog_raw",
|
||||
"source_dlog": str(session.dlog_root.resolve()),
|
||||
"msop_object": session.msop_object,
|
||||
"difop_object": session.difop_object,
|
||||
"msop_packets": len(session.msop_packets),
|
||||
"msop_batches": session.msop_batch_count,
|
||||
"difop_records": session.difop_record_count,
|
||||
"session_id": session.session_id,
|
||||
"lidar_ip": session.lidar_ip,
|
||||
"angle_source": session.angle_source,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--station", type=Path, required=True, help="Station directory or .rscap file")
|
||||
parser.add_argument("--station", type=Path, required=True, help="Station directory, .rscap, or dlog root")
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
parser.add_argument("--capture-name", default="h32.rscap")
|
||||
parser.add_argument("--msop-object", default="frontlidar-msop-raw")
|
||||
parser.add_argument("--difop-object", default="frontlidar-difop-raw")
|
||||
parser.add_argument(
|
||||
"--require-difop",
|
||||
action=argparse.BooleanOptionalAction,
|
||||
default=True,
|
||||
help="For dlog stations, require valid DIFOP angles (default: true)",
|
||||
)
|
||||
parser.add_argument("--stride", type=int, default=1)
|
||||
parser.add_argument("--min-frame-points", type=int, default=100)
|
||||
parser.add_argument("--min-range-m", type=float, default=0.3)
|
||||
@@ -152,8 +288,38 @@ def parse_args() -> argparse.Namespace:
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
station = args.station
|
||||
if station.is_file() and station.suffix.lower() == ".rscap":
|
||||
metadata = export_station_h32(
|
||||
args.station,
|
||||
station,
|
||||
args.out,
|
||||
capture_name=args.capture_name,
|
||||
stride=args.stride,
|
||||
min_frame_points=args.min_frame_points,
|
||||
min_range_m=args.min_range_m,
|
||||
max_range_m=args.max_range_m,
|
||||
compress=args.compress,
|
||||
write_reports=args.write_reports,
|
||||
resume=args.resume,
|
||||
)
|
||||
elif is_h32_raw_dlog_station(station, msop_object=args.msop_object):
|
||||
metadata = export_station_h32_dlog(
|
||||
station,
|
||||
args.out,
|
||||
msop_object=args.msop_object,
|
||||
difop_object=args.difop_object,
|
||||
require_difop=args.require_difop,
|
||||
stride=args.stride,
|
||||
min_frame_points=args.min_frame_points,
|
||||
min_range_m=args.min_range_m,
|
||||
max_range_m=args.max_range_m,
|
||||
compress=args.compress,
|
||||
write_reports=args.write_reports,
|
||||
resume=args.resume,
|
||||
)
|
||||
else:
|
||||
metadata = export_station_h32(
|
||||
station,
|
||||
args.out,
|
||||
capture_name=args.capture_name,
|
||||
stride=args.stride,
|
||||
|
||||
@@ -1,20 +1,23 @@
|
||||
#!/usr/bin/env python3
|
||||
"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package.
|
||||
|
||||
Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw ``.rscap`` in,
|
||||
Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw captures in,
|
||||
calibration-ready intermediate out. Downstream prepare/solve consume ``combined/``
|
||||
only (``manifest.csv`` + associated frame NPZs).
|
||||
|
||||
Expected raw layout:
|
||||
Expected raw layout (new H32 DLogCapture):
|
||||
|
||||
stations/
|
||||
001/h32.rscap
|
||||
002/h32.rscap
|
||||
001/ # dobject/ + dobject_recording/ (or 001/dlog/...)
|
||||
002/
|
||||
...
|
||||
captures/ (paths passed explicitly)
|
||||
captures/
|
||||
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
|
||||
imu.rscap # N300 (associated only; not used in AX=XB)
|
||||
|
||||
Also accepts legacy per-station ``h32.rscap``, and older decoded-point-cloud dlog
|
||||
stations (prefer ``--time-basis host`` for those).
|
||||
|
||||
Output under ``--out``:
|
||||
|
||||
export/<station>/frames/*.npz # internal LiDAR frames
|
||||
@@ -22,9 +25,6 @@ Output under ``--out``:
|
||||
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
||||
export_summary.json
|
||||
|
||||
Legacy dlog stations (``dobject`` + ``dobject_recording``) are still accepted;
|
||||
use ``--time-basis host`` for those datasets.
|
||||
|
||||
Raw ``.rscap`` / dlog files are never modified.
|
||||
"""
|
||||
|
||||
@@ -45,7 +45,14 @@ sys.path.insert(0, str(ROOT / "rscap_v2"))
|
||||
|
||||
from build_multisensor_npz import build_combined # noqa: E402
|
||||
from capture_format_v2 import file_summary, read_capture # noqa: E402
|
||||
from export_h32_rscap_station import export_station_h32, resolve_lidar_rscap # noqa: E402
|
||||
from export_h32_rscap_station import ( # noqa: E402
|
||||
export_station_h32,
|
||||
export_station_h32_dlog,
|
||||
is_h32_raw_dlog_station,
|
||||
is_legacy_pointcloud_dlog_station,
|
||||
resolve_lidar_rscap,
|
||||
try_resolve_dlog_root,
|
||||
)
|
||||
from pipeline_common_corrected import ( # noqa: E402
|
||||
parse_imu_capture,
|
||||
parse_rtk_capture,
|
||||
@@ -57,16 +64,21 @@ from pipeline_common_corrected import ( # noqa: E402
|
||||
def is_h32_station(station: Path, capture_name: str) -> bool:
|
||||
try:
|
||||
resolve_lidar_rscap(station, capture_name)
|
||||
return True
|
||||
except FileNotFoundError:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def is_dlog_station(station: Path) -> bool:
|
||||
return (station / "dobject").is_dir() and (station / "dobject_recording").is_dir()
|
||||
return try_resolve_dlog_root(station) is not None
|
||||
|
||||
|
||||
def discover_stations(stations_root: Path, names: list[str], capture_name: str) -> list[Path]:
|
||||
def discover_stations(
|
||||
stations_root: Path,
|
||||
names: list[str],
|
||||
capture_name: str,
|
||||
msop_object: str,
|
||||
) -> list[Path]:
|
||||
if names:
|
||||
stations = [stations_root / name for name in names]
|
||||
missing = [str(path) for path in stations if not path.is_dir()]
|
||||
@@ -77,13 +89,19 @@ def discover_stations(stations_root: Path, names: list[str], capture_name: str)
|
||||
[
|
||||
path
|
||||
for path in stations_root.iterdir()
|
||||
if path.is_dir() and (is_h32_station(path, capture_name) or is_dlog_station(path))
|
||||
if path.is_dir()
|
||||
and (
|
||||
is_h32_station(path, capture_name)
|
||||
or is_h32_raw_dlog_station(path, msop_object=msop_object)
|
||||
or is_dlog_station(path)
|
||||
)
|
||||
],
|
||||
key=lambda path: path.name,
|
||||
)
|
||||
if not stations:
|
||||
raise FileNotFoundError(
|
||||
f"no station with {capture_name}/lidar.rscap or dobject+dobject_recording under {stations_root}"
|
||||
f"no station with H32 dlog/MSOP, {capture_name}/lidar.rscap, or "
|
||||
f"dobject+dobject_recording under {stations_root}"
|
||||
)
|
||||
return stations
|
||||
|
||||
@@ -101,7 +119,7 @@ def export_legacy_dlog_station(
|
||||
sys.executable,
|
||||
str(exporter),
|
||||
"--dlog",
|
||||
str(station),
|
||||
str(try_resolve_dlog_root(station) or station),
|
||||
"--out",
|
||||
str(out),
|
||||
"--object",
|
||||
@@ -154,6 +172,9 @@ def export_raw_to_combined(
|
||||
out: Path,
|
||||
station_names: list[str] | None = None,
|
||||
lidar_capture_name: str = "h32.rscap",
|
||||
msop_object: str = "frontlidar-msop-raw",
|
||||
difop_object: str = "frontlidar-difop-raw",
|
||||
require_difop: bool = True,
|
||||
lidar_object: str = "frontlidar",
|
||||
timezone: str = "+08:00",
|
||||
stride: int = 1,
|
||||
@@ -174,7 +195,6 @@ def export_raw_to_combined(
|
||||
if out.exists() and any(out.iterdir()) and not overwrite:
|
||||
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
||||
if overwrite and out.exists():
|
||||
# Keep out root but clear known children so rebuild is deterministic.
|
||||
for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
|
||||
target = out / child
|
||||
if target.is_dir():
|
||||
@@ -187,15 +207,29 @@ def export_raw_to_combined(
|
||||
parsed_root = out / "parsed"
|
||||
combined_root = out / "combined"
|
||||
|
||||
stations = discover_stations(stations_root, station_names or [], lidar_capture_name)
|
||||
stations = discover_stations(
|
||||
stations_root, station_names or [], lidar_capture_name, msop_object
|
||||
)
|
||||
parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root)
|
||||
|
||||
station_meta: list[dict[str, Any]] = []
|
||||
lidar_segments: list[tuple[str, Path]] = []
|
||||
saw_dlog = False
|
||||
saw_legacy_dlog = False
|
||||
for station in stations:
|
||||
station_out = export_root / station.name
|
||||
if is_h32_station(station, lidar_capture_name):
|
||||
if is_h32_raw_dlog_station(station, msop_object=msop_object):
|
||||
meta = export_station_h32_dlog(
|
||||
station,
|
||||
station_out,
|
||||
msop_object=msop_object,
|
||||
difop_object=difop_object,
|
||||
require_difop=require_difop,
|
||||
stride=stride,
|
||||
write_reports=True,
|
||||
resume=False,
|
||||
)
|
||||
kind = "h32_dlog_raw"
|
||||
elif is_h32_station(station, lidar_capture_name):
|
||||
meta = export_station_h32(
|
||||
station,
|
||||
station_out,
|
||||
@@ -205,8 +239,10 @@ def export_raw_to_combined(
|
||||
resume=False,
|
||||
)
|
||||
kind = "h32_rscap"
|
||||
elif is_dlog_station(station):
|
||||
saw_dlog = True
|
||||
elif is_legacy_pointcloud_dlog_station(station, msop_object=msop_object) or is_dlog_station(
|
||||
station
|
||||
):
|
||||
saw_legacy_dlog = True
|
||||
export_legacy_dlog_station(
|
||||
station,
|
||||
station_out,
|
||||
@@ -217,16 +253,18 @@ def export_raw_to_combined(
|
||||
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
||||
kind = "legacy_dlog"
|
||||
else:
|
||||
raise RuntimeError(f"station {station.name} has neither H32 .rscap nor dlog layout")
|
||||
raise RuntimeError(
|
||||
f"station {station.name} has neither H32 raw dlog, .rscap, nor legacy dlog layout"
|
||||
)
|
||||
frames_dir = station_out / "frames"
|
||||
if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")):
|
||||
raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}")
|
||||
lidar_segments.append((station.name, frames_dir))
|
||||
station_meta.append({"station": station.name, "kind": kind, "frames_dir": str(frames_dir), **meta})
|
||||
|
||||
if saw_dlog and time_basis == "device_gnss":
|
||||
if saw_legacy_dlog and time_basis == "device_gnss":
|
||||
print(
|
||||
"[warn] legacy dlog stations use host/DObject time; prefer --time-basis host",
|
||||
"[warn] legacy point-cloud dlog stations use host/DObject time; prefer --time-basis host",
|
||||
file=sys.stderr,
|
||||
)
|
||||
|
||||
@@ -260,7 +298,9 @@ def export_raw_to_combined(
|
||||
},
|
||||
"timestamp_policy": {
|
||||
"default_time_basis": time_basis,
|
||||
"lidar_h32": "MSOP device timestamp → unix_time_ns",
|
||||
"lidar_h32_dlog": "MSOP device timestamp → unix_time_ns; DIFOP channel angles for XYZ",
|
||||
"lidar_h32_rscap": "MSOP device timestamp → unix_time_ns; default vertical angles",
|
||||
"lidar_legacy_dlog": "DObject/host time; use time_basis=host",
|
||||
"rtk": "GNSS week/TOW when time_basis=device_gnss; else host_receive_utc_ns",
|
||||
"imu": "associated only; host-anchored device deltas in combined window",
|
||||
"host_utc": "kept for audit; not the default calibration timeline for new captures",
|
||||
@@ -281,8 +321,16 @@ def parse_args() -> argparse.Namespace:
|
||||
parser.add_argument("--imu-rscap", type=Path, required=True, help="Continuous N300/IMU V2 .rscap")
|
||||
parser.add_argument("--out", type=Path, required=True, help="Output package root (contains combined/)")
|
||||
parser.add_argument("--station", action="append", default=[], help="Optional station name filter; repeatable")
|
||||
parser.add_argument("--lidar-capture-name", default="h32.rscap")
|
||||
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy dlog DObject name")
|
||||
parser.add_argument("--lidar-capture-name", default="h32.rscap", help="Legacy H32 .rscap filename")
|
||||
parser.add_argument("--msop-object", default="frontlidar-msop-raw", help="Raw MSOP DObject name")
|
||||
parser.add_argument("--difop-object", default="frontlidar-difop-raw", help="Raw DIFOP DObject name")
|
||||
parser.add_argument(
|
||||
"--require-difop",
|
||||
action=argparse.BooleanOptionalAction,
|
||||
default=True,
|
||||
help="Require DIFOP channel angles for H32 raw dlog stations (default: true)",
|
||||
)
|
||||
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy decoded point-cloud DObject name")
|
||||
parser.add_argument("--timezone", default="+08:00", help="Legacy dlog tick timezone")
|
||||
parser.add_argument("--stride", type=int, default=1)
|
||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
||||
@@ -304,6 +352,9 @@ def main() -> int:
|
||||
out=args.out,
|
||||
station_names=args.station,
|
||||
lidar_capture_name=args.lidar_capture_name,
|
||||
msop_object=args.msop_object,
|
||||
difop_object=args.difop_object,
|
||||
require_difop=args.require_difop,
|
||||
lidar_object=args.lidar_object,
|
||||
timezone=args.timezone,
|
||||
stride=args.stride,
|
||||
|
||||
@@ -0,0 +1,17 @@
|
||||
"""Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP."""
|
||||
|
||||
from .difop import parse_difop_angles
|
||||
from .dobject import discover_records, iter_payloads, resolve_dlog_root
|
||||
from .load_session import H32DlogLidarSession, load_h32_dlog_lidar
|
||||
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||
|
||||
__all__ = [
|
||||
"H32DlogLidarSession",
|
||||
"discover_records",
|
||||
"iter_payloads",
|
||||
"load_h32_dlog_lidar",
|
||||
"parse_difop_angles",
|
||||
"parse_difop_payload",
|
||||
"parse_msop_batch_payload",
|
||||
"resolve_dlog_root",
|
||||
]
|
||||
@@ -0,0 +1,40 @@
|
||||
"""Parse RoboSense H32 DIFOP channel calibration angles."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
CHANNELS = 32
|
||||
VERTICAL_START = 468
|
||||
HORIZONTAL_START = 564
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DifopAngles:
|
||||
vertical_deg: np.ndarray # (32,)
|
||||
horizontal_deg: np.ndarray # (32,)
|
||||
|
||||
|
||||
def _read_u16_be(packet: bytes, index: int) -> int:
|
||||
return (packet[index] << 8) | packet[index + 1]
|
||||
|
||||
|
||||
def signed_angle_deg(packet: bytes, index: int) -> float:
|
||||
"""Match RSLidarH32 plugin SignedAngle: sign byte + BE u16 * 0.01 deg."""
|
||||
|
||||
sign = -1.0 if packet[index] > 0 else 1.0
|
||||
return sign * _read_u16_be(packet, index + 1) * 0.01
|
||||
|
||||
|
||||
def parse_difop_angles(packet: bytes) -> DifopAngles:
|
||||
needed = HORIZONTAL_START + CHANNELS * 3
|
||||
if len(packet) < needed:
|
||||
raise ValueError(f"DIFOP packet too short: {len(packet)} < {needed}")
|
||||
vertical = np.empty(CHANNELS, dtype=np.float64)
|
||||
horizontal = np.empty(CHANNELS, dtype=np.float64)
|
||||
for channel in range(CHANNELS):
|
||||
vertical[channel] = signed_angle_deg(packet, VERTICAL_START + channel * 3)
|
||||
horizontal[channel] = signed_angle_deg(packet, HORIZONTAL_START + channel * 3)
|
||||
return DifopAngles(vertical_deg=vertical, horizontal_deg=horizontal)
|
||||
@@ -0,0 +1,179 @@
|
||||
"""Index and read Medulla DObject recordings (dobject/ + dobject_recording/)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import BinaryIO, Iterator
|
||||
|
||||
|
||||
RECORD_RE = re.compile(
|
||||
r"^\[(?P<log_time>[^]]+)\].*?DObject `(?P<name>[^`]+)` post "
|
||||
r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), "
|
||||
r"@(?P<file>[^:]+):(?P<offset>\d+)"
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RecordRef:
|
||||
sequence: int
|
||||
object_name: str
|
||||
log_time: str
|
||||
source_log: str
|
||||
source_dorec: str
|
||||
source_offset: int
|
||||
payload_length: int
|
||||
log_record_id: str
|
||||
dotnet_ticks: int
|
||||
|
||||
|
||||
def resolve_dlog_root(value: Path | str) -> Path:
|
||||
root = Path(value).expanduser().resolve()
|
||||
if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir():
|
||||
return root
|
||||
child = root / "dlog"
|
||||
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
|
||||
return child
|
||||
raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording")
|
||||
|
||||
|
||||
def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
|
||||
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
|
||||
for log_path in sorted((dlog_root / "dobject").rglob("*.log")):
|
||||
relative_log = log_path.relative_to(dlog_root).as_posix()
|
||||
with log_path.open("r", encoding="utf-8", errors="replace") as stream:
|
||||
for line in stream:
|
||||
match = RECORD_RE.search(line)
|
||||
if not match or match.group("name").casefold() != object_name.casefold():
|
||||
continue
|
||||
pending.append(
|
||||
(
|
||||
match.group("name"),
|
||||
match.group("log_time"),
|
||||
relative_log,
|
||||
int(match.group("offset")),
|
||||
int(match.group("len")),
|
||||
match.group("id").upper(),
|
||||
int(match.group("tic")),
|
||||
match.group("file"),
|
||||
)
|
||||
)
|
||||
pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3]))
|
||||
seen: set[tuple[str, int, int]] = set()
|
||||
records: list[RecordRef] = []
|
||||
for item in pending:
|
||||
key = (item[7].casefold(), item[3], item[6])
|
||||
if key in seen:
|
||||
continue
|
||||
seen.add(key)
|
||||
records.append(
|
||||
RecordRef(
|
||||
sequence=len(records),
|
||||
object_name=item[0],
|
||||
log_time=item[1],
|
||||
source_log=item[2],
|
||||
source_dorec=item[7],
|
||||
source_offset=item[3],
|
||||
payload_length=item[4],
|
||||
log_record_id=item[5],
|
||||
dotnet_ticks=item[6],
|
||||
)
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
|
||||
result: dict[str, list[Path]] = {}
|
||||
for path in (dlog_root / "dobject_recording").rglob("*.dorec"):
|
||||
result.setdefault(path.name.casefold(), []).append(path)
|
||||
return result
|
||||
|
||||
|
||||
def choose_dorec(index: dict[str, list[Path]], name: str) -> Path:
|
||||
matches = index.get(Path(name).name.casefold(), [])
|
||||
if not matches:
|
||||
raise FileNotFoundError(f"missing recording file: {name}")
|
||||
if len(matches) > 1:
|
||||
raise RuntimeError(f"ambiguous recording file {name}: {matches}")
|
||||
return matches[0]
|
||||
|
||||
|
||||
def read_exact(stream: BinaryIO, size: int) -> bytes:
|
||||
data = stream.read(size)
|
||||
if len(data) != size:
|
||||
raise EOFError(f"expected {size} bytes, got {len(data)}")
|
||||
return data
|
||||
|
||||
|
||||
def read_record_payload(path: Path, record: RecordRef) -> bytes:
|
||||
with path.open("rb") as stream:
|
||||
stream.seek(record.source_offset)
|
||||
name_length = read_exact(stream, 1)[0]
|
||||
name = read_exact(stream, name_length).decode("ascii")
|
||||
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
|
||||
id_length = read_exact(stream, 1)[0]
|
||||
id_bytes = read_exact(stream, id_length)
|
||||
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
|
||||
payload = read_exact(stream, payload_length)
|
||||
|
||||
try:
|
||||
record_id = id_bytes.decode("ascii")
|
||||
except UnicodeDecodeError:
|
||||
record_id = id_bytes.hex().upper()
|
||||
if name != record.object_name:
|
||||
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
|
||||
if ticks != record.dotnet_ticks:
|
||||
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
|
||||
if payload_length != record.payload_length:
|
||||
raise ValueError(f"payload mismatch: log={record.payload_length}, dorec={payload_length}")
|
||||
if record_id.upper() != record.log_record_id.upper():
|
||||
raise ValueError(f"record id mismatch: log={record.log_record_id}, dorec={record_id}")
|
||||
return payload
|
||||
|
||||
|
||||
def iter_payloads(dlog_root: Path, object_name: str) -> Iterator[tuple[RecordRef, bytes]]:
|
||||
root = resolve_dlog_root(dlog_root)
|
||||
records = discover_records(root, object_name)
|
||||
if not records:
|
||||
return
|
||||
dorec_index = index_dorec_files(root)
|
||||
open_files: dict[str, tuple[Path, BinaryIO]] = {}
|
||||
try:
|
||||
for record in records:
|
||||
key = record.source_dorec.casefold()
|
||||
handle = open_files.get(key)
|
||||
if handle is None:
|
||||
path = choose_dorec(dorec_index, record.source_dorec)
|
||||
handle = (path, path.open("rb"))
|
||||
open_files[key] = handle
|
||||
path, stream = handle
|
||||
stream.seek(record.source_offset)
|
||||
name_length = read_exact(stream, 1)[0]
|
||||
name = read_exact(stream, name_length).decode("ascii")
|
||||
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
|
||||
id_length = read_exact(stream, 1)[0]
|
||||
id_bytes = read_exact(stream, id_length)
|
||||
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
|
||||
payload = read_exact(stream, payload_length)
|
||||
try:
|
||||
record_id = id_bytes.decode("ascii")
|
||||
except UnicodeDecodeError:
|
||||
record_id = id_bytes.hex().upper()
|
||||
if name != record.object_name:
|
||||
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
|
||||
if ticks != record.dotnet_ticks:
|
||||
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
|
||||
if payload_length != record.payload_length:
|
||||
raise ValueError(
|
||||
f"payload mismatch: log={record.payload_length}, dorec={payload_length}"
|
||||
)
|
||||
if record_id.upper() != record.log_record_id.upper():
|
||||
raise ValueError(
|
||||
f"record id mismatch: log={record.log_record_id}, dorec={record_id}"
|
||||
)
|
||||
yield record, payload
|
||||
finally:
|
||||
for _path, stream in open_files.values():
|
||||
stream.close()
|
||||
@@ -0,0 +1,69 @@
|
||||
"""Little-endian .NET BinaryReader/BinaryWriter helpers."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from typing import BinaryIO
|
||||
|
||||
|
||||
def read_7bit_int(stream: BinaryIO) -> int:
|
||||
value = 0
|
||||
shift = 0
|
||||
while True:
|
||||
raw = stream.read(1)
|
||||
if not raw:
|
||||
raise EOFError("truncated .NET 7-bit int")
|
||||
value |= (raw[0] & 0x7F) << shift
|
||||
if not raw[0] & 0x80:
|
||||
return value
|
||||
shift += 7
|
||||
if shift > 35:
|
||||
raise ValueError("invalid .NET 7-bit int")
|
||||
|
||||
|
||||
def write_7bit_int(stream: BinaryIO, value: int) -> None:
|
||||
if value < 0:
|
||||
raise ValueError("7-bit int must be non-negative")
|
||||
while value >= 0x80:
|
||||
stream.write(bytes([(value & 0x7F) | 0x80]))
|
||||
value >>= 7
|
||||
stream.write(bytes([value & 0x7F]))
|
||||
|
||||
|
||||
def read_dotnet_string(stream: BinaryIO) -> str:
|
||||
length = read_7bit_int(stream)
|
||||
raw = stream.read(length)
|
||||
if len(raw) != length:
|
||||
raise EOFError("truncated .NET string")
|
||||
return raw.decode("utf-8")
|
||||
|
||||
|
||||
def write_dotnet_string(stream: BinaryIO, text: str) -> None:
|
||||
raw = text.encode("utf-8")
|
||||
write_7bit_int(stream, len(raw))
|
||||
stream.write(raw)
|
||||
|
||||
|
||||
def read_i32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated int32")
|
||||
return struct.unpack("<i", raw)[0]
|
||||
|
||||
|
||||
def read_i64(stream: BinaryIO) -> int:
|
||||
raw = stream.read(8)
|
||||
if len(raw) != 8:
|
||||
raise EOFError("truncated int64")
|
||||
return struct.unpack("<q", raw)[0]
|
||||
|
||||
|
||||
def read_bool(stream: BinaryIO) -> bool:
|
||||
raw = stream.read(1)
|
||||
if not raw:
|
||||
raise EOFError("truncated bool")
|
||||
return raw[0] != 0
|
||||
|
||||
|
||||
def write_bool(stream: BinaryIO, value: bool) -> None:
|
||||
stream.write(b"\x01" if value else b"\x00")
|
||||
@@ -0,0 +1,109 @@
|
||||
"""Load H32 MSOP packets and DIFOP angles from a Medulla dlog session."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
_TOOLS = Path(__file__).resolve().parents[1]
|
||||
_RSCAP_V2 = _TOOLS / "rscap_v2"
|
||||
if str(_RSCAP_V2) not in sys.path:
|
||||
sys.path.insert(0, str(_RSCAP_V2))
|
||||
|
||||
from h32_msop import default_horizontal_deg, default_vertical_deg # noqa: E402
|
||||
|
||||
from .difop import DifopAngles, parse_difop_angles
|
||||
from .dobject import discover_records, iter_payloads, resolve_dlog_root
|
||||
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||
|
||||
|
||||
@dataclass
|
||||
class H32DlogLidarSession:
|
||||
dlog_root: Path
|
||||
msop_object: str
|
||||
difop_object: str
|
||||
msop_packets: list[bytes]
|
||||
msop_host_utc_ticks: list[int]
|
||||
msop_batch_count: int
|
||||
difop_record_count: int
|
||||
angle_source: str
|
||||
vertical_deg: np.ndarray
|
||||
horizontal_deg: np.ndarray
|
||||
session_id: str | None = None
|
||||
lidar_ip: str | None = None
|
||||
|
||||
|
||||
def load_h32_dlog_lidar(
|
||||
dlog_root: Path | str,
|
||||
*,
|
||||
msop_object: str = "frontlidar-msop-raw",
|
||||
difop_object: str = "frontlidar-difop-raw",
|
||||
require_difop: bool = False,
|
||||
) -> H32DlogLidarSession:
|
||||
root = resolve_dlog_root(dlog_root)
|
||||
msop_packets: list[bytes] = []
|
||||
msop_host_utc_ticks: list[int] = []
|
||||
batch_count = 0
|
||||
session_id: str | None = None
|
||||
lidar_ip: str | None = None
|
||||
|
||||
for _record, payload in iter_payloads(root, msop_object):
|
||||
batch = parse_msop_batch_payload(payload)
|
||||
batch_count += 1
|
||||
if session_id is None:
|
||||
session_id = batch.session_id
|
||||
lidar_ip = batch.lidar_ip
|
||||
for item in batch.packets:
|
||||
msop_packets.append(item.raw)
|
||||
msop_host_utc_ticks.append(int(item.host_receive_utc_ticks))
|
||||
|
||||
angles: DifopAngles | None = None
|
||||
difop_count = 0
|
||||
for _record, payload in iter_payloads(root, difop_object):
|
||||
difop = parse_difop_payload(payload)
|
||||
difop_count += 1
|
||||
try:
|
||||
angles = parse_difop_angles(difop.raw)
|
||||
except ValueError:
|
||||
continue
|
||||
if session_id is None:
|
||||
session_id = difop.session_id
|
||||
lidar_ip = difop.lidar_ip
|
||||
|
||||
if not msop_packets:
|
||||
msop_records = discover_records(root, msop_object)
|
||||
raise RuntimeError(
|
||||
f"no MSOP packets from DObject {msop_object!r} under {root} "
|
||||
f"(log records={len(msop_records)})"
|
||||
)
|
||||
|
||||
if angles is None:
|
||||
if require_difop:
|
||||
raise RuntimeError(
|
||||
f"no valid DIFOP calibration from DObject {difop_object!r} under {root}"
|
||||
)
|
||||
vertical = default_vertical_deg()
|
||||
horizontal = default_horizontal_deg()
|
||||
angle_source = "default_msop_only_vertical_-16_to_16_deg"
|
||||
else:
|
||||
vertical = angles.vertical_deg
|
||||
horizontal = angles.horizontal_deg
|
||||
angle_source = "difop_channel_angles"
|
||||
|
||||
return H32DlogLidarSession(
|
||||
dlog_root=root,
|
||||
msop_object=msop_object,
|
||||
difop_object=difop_object,
|
||||
msop_packets=msop_packets,
|
||||
msop_host_utc_ticks=msop_host_utc_ticks,
|
||||
msop_batch_count=batch_count,
|
||||
difop_record_count=difop_count,
|
||||
angle_source=angle_source,
|
||||
vertical_deg=vertical,
|
||||
horizontal_deg=horizontal,
|
||||
session_id=session_id,
|
||||
lidar_ip=lidar_ip,
|
||||
)
|
||||
@@ -0,0 +1,204 @@
|
||||
"""Parse RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP DObject payloads."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import io
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .dotnet_bin import read_bool, read_dotnet_string, read_i32, read_i64
|
||||
|
||||
MSOP_MAGIC = "RSLIDAR_H32_MSOP_DLOG_V1"
|
||||
DIFOP_MAGIC = "RSLIDAR_H32_DIFOP_DLOG_V1"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MsopPacketItem:
|
||||
sequence: int
|
||||
device_timestamp_us: int
|
||||
device_timestamp_valid: bool
|
||||
host_receive_utc_ticks: int
|
||||
host_receive_monotonic_ticks: int
|
||||
raw: bytes
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MsopBatch:
|
||||
version: int
|
||||
session_id: str
|
||||
session_start_utc_ticks: int
|
||||
session_start_monotonic_ticks: int
|
||||
monotonic_frequency: int
|
||||
lidar_ip: str
|
||||
msop_port: int
|
||||
packets: list[MsopPacketItem]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DifopRecord:
|
||||
version: int
|
||||
session_id: str
|
||||
session_start_utc_ticks: int
|
||||
session_start_monotonic_ticks: int
|
||||
monotonic_frequency: int
|
||||
lidar_ip: str
|
||||
difop_port: int
|
||||
sequence: int
|
||||
host_receive_utc_ticks: int
|
||||
host_receive_monotonic_ticks: int
|
||||
raw: bytes
|
||||
|
||||
|
||||
def _read_bytes(stream: io.BytesIO, length: int) -> bytes:
|
||||
if length < 0 or length > 64 * 1024 * 1024:
|
||||
raise ValueError(f"invalid byte length: {length}")
|
||||
raw = stream.read(length)
|
||||
if len(raw) != length:
|
||||
raise EOFError(f"expected {length} bytes, got {len(raw)}")
|
||||
return raw
|
||||
|
||||
|
||||
def parse_msop_batch_payload(payload: bytes) -> MsopBatch:
|
||||
stream = io.BytesIO(payload)
|
||||
magic = read_dotnet_string(stream)
|
||||
if magic != MSOP_MAGIC:
|
||||
raise ValueError(f"unexpected MSOP payload magic: {magic!r}")
|
||||
version = read_i32(stream)
|
||||
session_id = read_dotnet_string(stream)
|
||||
session_start_utc_ticks = read_i64(stream)
|
||||
session_start_monotonic_ticks = read_i64(stream)
|
||||
monotonic_frequency = read_i64(stream)
|
||||
lidar_ip = read_dotnet_string(stream)
|
||||
msop_port = read_i32(stream)
|
||||
packet_count = read_i32(stream)
|
||||
if packet_count < 0 or packet_count > 100_000:
|
||||
raise ValueError(f"invalid MSOP packet count: {packet_count}")
|
||||
packets: list[MsopPacketItem] = []
|
||||
for _ in range(packet_count):
|
||||
packets.append(
|
||||
MsopPacketItem(
|
||||
sequence=read_i64(stream),
|
||||
device_timestamp_us=read_i64(stream),
|
||||
device_timestamp_valid=read_bool(stream),
|
||||
host_receive_utc_ticks=read_i64(stream),
|
||||
host_receive_monotonic_ticks=read_i64(stream),
|
||||
raw=_read_bytes(stream, read_i32(stream)),
|
||||
)
|
||||
)
|
||||
return MsopBatch(
|
||||
version=version,
|
||||
session_id=session_id,
|
||||
session_start_utc_ticks=session_start_utc_ticks,
|
||||
session_start_monotonic_ticks=session_start_monotonic_ticks,
|
||||
monotonic_frequency=monotonic_frequency,
|
||||
lidar_ip=lidar_ip,
|
||||
msop_port=msop_port,
|
||||
packets=packets,
|
||||
)
|
||||
|
||||
|
||||
def parse_difop_payload(payload: bytes) -> DifopRecord:
|
||||
stream = io.BytesIO(payload)
|
||||
magic = read_dotnet_string(stream)
|
||||
if magic != DIFOP_MAGIC:
|
||||
raise ValueError(f"unexpected DIFOP payload magic: {magic!r}")
|
||||
version = read_i32(stream)
|
||||
session_id = read_dotnet_string(stream)
|
||||
session_start_utc_ticks = read_i64(stream)
|
||||
session_start_monotonic_ticks = read_i64(stream)
|
||||
monotonic_frequency = read_i64(stream)
|
||||
lidar_ip = read_dotnet_string(stream)
|
||||
difop_port = read_i32(stream)
|
||||
sequence = read_i64(stream)
|
||||
host_receive_utc_ticks = read_i64(stream)
|
||||
host_receive_monotonic_ticks = read_i64(stream)
|
||||
raw = _read_bytes(stream, read_i32(stream))
|
||||
return DifopRecord(
|
||||
version=version,
|
||||
session_id=session_id,
|
||||
session_start_utc_ticks=session_start_utc_ticks,
|
||||
session_start_monotonic_ticks=session_start_monotonic_ticks,
|
||||
monotonic_frequency=monotonic_frequency,
|
||||
lidar_ip=lidar_ip,
|
||||
difop_port=difop_port,
|
||||
sequence=sequence,
|
||||
host_receive_utc_ticks=host_receive_utc_ticks,
|
||||
host_receive_monotonic_ticks=host_receive_monotonic_ticks,
|
||||
raw=raw,
|
||||
)
|
||||
|
||||
|
||||
def build_msop_batch_payload(
|
||||
*,
|
||||
version: int = 1,
|
||||
session_id: str = "test",
|
||||
session_start_utc_ticks: int = 0,
|
||||
session_start_monotonic_ticks: int = 0,
|
||||
monotonic_frequency: int = 10_000_000,
|
||||
lidar_ip: str = "192.168.1.200",
|
||||
msop_port: int = 6699,
|
||||
packets: list[MsopPacketItem],
|
||||
) -> bytes:
|
||||
"""Test helper: write an MSOP batch matching the C# BinaryWriter layout."""
|
||||
|
||||
from .dotnet_bin import write_bool, write_dotnet_string
|
||||
|
||||
stream = io.BytesIO()
|
||||
write_dotnet_string(stream, MSOP_MAGIC)
|
||||
stream.write(struct.pack("<i", version))
|
||||
write_dotnet_string(stream, session_id)
|
||||
stream.write(struct.pack("<qqq", session_start_utc_ticks, session_start_monotonic_ticks, monotonic_frequency))
|
||||
write_dotnet_string(stream, lidar_ip)
|
||||
stream.write(struct.pack("<i", msop_port))
|
||||
stream.write(struct.pack("<i", len(packets)))
|
||||
for item in packets:
|
||||
stream.write(struct.pack("<qq", item.sequence, item.device_timestamp_us))
|
||||
write_bool(stream, item.device_timestamp_valid)
|
||||
stream.write(
|
||||
struct.pack(
|
||||
"<qqi",
|
||||
item.host_receive_utc_ticks,
|
||||
item.host_receive_monotonic_ticks,
|
||||
len(item.raw),
|
||||
)
|
||||
)
|
||||
stream.write(item.raw)
|
||||
return stream.getvalue()
|
||||
|
||||
|
||||
def build_difop_payload(
|
||||
*,
|
||||
version: int = 1,
|
||||
session_id: str = "test",
|
||||
session_start_utc_ticks: int = 0,
|
||||
session_start_monotonic_ticks: int = 0,
|
||||
monotonic_frequency: int = 10_000_000,
|
||||
lidar_ip: str = "192.168.1.200",
|
||||
difop_port: int = 7788,
|
||||
sequence: int = 1,
|
||||
host_receive_utc_ticks: int = 0,
|
||||
host_receive_monotonic_ticks: int = 0,
|
||||
raw: bytes,
|
||||
) -> bytes:
|
||||
"""Test helper: write a DIFOP record matching the C# BinaryWriter layout."""
|
||||
|
||||
from .dotnet_bin import write_dotnet_string
|
||||
|
||||
stream = io.BytesIO()
|
||||
write_dotnet_string(stream, DIFOP_MAGIC)
|
||||
stream.write(struct.pack("<i", version))
|
||||
write_dotnet_string(stream, session_id)
|
||||
stream.write(struct.pack("<qqq", session_start_utc_ticks, session_start_monotonic_ticks, monotonic_frequency))
|
||||
write_dotnet_string(stream, lidar_ip)
|
||||
stream.write(struct.pack("<i", difop_port))
|
||||
stream.write(
|
||||
struct.pack(
|
||||
"<qqqi",
|
||||
sequence,
|
||||
host_receive_utc_ticks,
|
||||
host_receive_monotonic_ticks,
|
||||
len(raw),
|
||||
)
|
||||
)
|
||||
stream.write(raw)
|
||||
return stream.getvalue()
|
||||
@@ -58,6 +58,17 @@ def yaw_rotation(yaw: float) -> np.ndarray:
|
||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
|
||||
|
||||
|
||||
def heading_to_enu_yaw(raw_heading_deg: float, heading_offset_deg: float) -> tuple[float, float]:
|
||||
"""Convert GNHPR navigation heading to mathematical ENU yaw.
|
||||
|
||||
``heading_offset_deg`` is added in the receiver's clockwise-from-north
|
||||
heading convention. It is therefore not interchangeable with a ROS yaw
|
||||
offset, whose sign and zero axis depend on the ROS frame definition.
|
||||
"""
|
||||
corrected_heading = (raw_heading_deg + heading_offset_deg) % 360.0
|
||||
return corrected_heading, math.radians(90.0 - corrected_heading)
|
||||
|
||||
|
||||
def scalar(data: np.lib.npyio.NpzFile, name: str) -> float:
|
||||
return float(np.asarray(data[name]).reshape(-1)[0])
|
||||
|
||||
@@ -112,13 +123,15 @@ def main() -> int:
|
||||
continue
|
||||
frame = good[len(good) // 2]
|
||||
source = args.combined_root / Path(frame["output"])
|
||||
reported_std = values[:, 5]
|
||||
reported_std_mean = float(np.nanmean(reported_std)) if np.isfinite(reported_std).any() else None
|
||||
selected.append({"station": segment, "source": source, "time": int(frame["lidar_time_ns"]) / 1e9,
|
||||
"lat": float(np.mean(values[:, 0])), "lon": float(np.mean(values[:, 1])),
|
||||
"alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3])})
|
||||
summaries.append({"station": segment, "frames": len(group), "valid_fixed_frames": len(good),
|
||||
"heading_mean_deg": circular_mean_deg(values[:, 3]),
|
||||
"heading_circular_std_deg": heading_std, "rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
|
||||
"reported_heading_std_mean_deg": float(np.nanmean(values[:, 5])),
|
||||
"reported_heading_std_mean_deg": reported_std_mean,
|
||||
"altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source)})
|
||||
|
||||
if args.expected_stations and len(selected) != args.expected_stations:
|
||||
@@ -138,8 +151,7 @@ def main() -> int:
|
||||
shutil.copy2(item["source"], destination)
|
||||
antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
|
||||
origin["lat"], origin["lon"])
|
||||
corrected_heading = (item["heading"] + args.heading_offset_deg) % 360.0
|
||||
yaw = math.radians(90.0 - corrected_heading)
|
||||
corrected_heading, yaw = heading_to_enu_yaw(item["heading"], args.heading_offset_deg)
|
||||
reference_position = antenna - yaw_rotation(yaw) @ lever
|
||||
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, 0.0, 0.0,
|
||||
math.sin(yaw / 2.0), math.cos(yaw / 2.0)])))
|
||||
@@ -156,6 +168,9 @@ def main() -> int:
|
||||
"selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
|
||||
"reference_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg,
|
||||
"antenna_lever_body_m": args.antenna_lever,
|
||||
"heading_offset_semantics": (
|
||||
"added to clockwise-from-north GNHPR heading before ENU yaw conversion"
|
||||
),
|
||||
"orientation_model": "yaw-only, identical to the previous calibration workflow"},
|
||||
"stations": [{"sequence": i + 1, "source_station": item["station"],
|
||||
"source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
|
||||
|
||||
+43
-10
@@ -1,6 +1,6 @@
|
||||
"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres).
|
||||
"""Decode RoboSense H32 MSOP packets into Cartesian / polar frames (metres).
|
||||
|
||||
Angle / distance conventions follow ``RSLidarH32_3D_RawCaptureNet48``:
|
||||
Angle / distance conventions follow the H32 Medulla plugins:
|
||||
azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
|
||||
distance_mm = raw * distance_unit_mm, then:
|
||||
|
||||
@@ -8,13 +8,14 @@ distance_mm = raw * distance_unit_mm, then:
|
||||
y = d_m * cos(alt) * sin(az)
|
||||
z = d_m * sin(alt)
|
||||
|
||||
MSOP-only captures do not include DIFOP; vertical angles default to a uniform
|
||||
-16°…+16° fan, horizontal channel offsets default to 0.
|
||||
When DIFOP is unavailable, vertical angles default to a uniform -16°…+16° fan
|
||||
and horizontal channel offsets default to 0.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Iterable, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -192,9 +193,10 @@ def _block_points_raw(
|
||||
return np.asarray(rows, dtype=np.float32)
|
||||
|
||||
|
||||
def iter_h32_frames_polar(
|
||||
capture: CaptureFile,
|
||||
def iter_h32_frames_polar_from_packets(
|
||||
packets: Iterable[bytes],
|
||||
*,
|
||||
host_utc_ticks: Sequence[int] | None = None,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
min_range_m: float = 0.3,
|
||||
@@ -203,7 +205,7 @@ def iter_h32_frames_polar(
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFramePolarExport]:
|
||||
"""Assemble MSOP packets into polar frames for the RTK–LiDAR combined contract."""
|
||||
"""Assemble raw MSOP packets into polar frames for the combined contract."""
|
||||
|
||||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||||
@@ -218,6 +220,7 @@ def iter_h32_frames_polar(
|
||||
prev_az: float | None = None
|
||||
kept = 0
|
||||
stride = max(1, int(frame_stride))
|
||||
host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
|
||||
|
||||
def emit() -> None:
|
||||
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
||||
@@ -250,13 +253,15 @@ def iter_h32_frames_polar(
|
||||
)
|
||||
)
|
||||
|
||||
for chunk in capture.chunks:
|
||||
packet = chunk.raw
|
||||
for index, packet in enumerate(packets):
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
continue
|
||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||
unit = distance_unit_mm(packet)
|
||||
chunk_host = ticks_to_unix_ns(chunk.receive_utc_ticks)
|
||||
if host_list is not None and index < len(host_list):
|
||||
chunk_host = ticks_to_unix_ns(int(host_list[index]))
|
||||
else:
|
||||
chunk_host = 0
|
||||
idx = DATA_START
|
||||
for _block in range(BLOCKS):
|
||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||||
@@ -287,6 +292,34 @@ def iter_h32_frames_polar(
|
||||
return frames
|
||||
|
||||
|
||||
def iter_h32_frames_polar(
|
||||
capture: CaptureFile,
|
||||
*,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
max_points_per_frame: int | None = None,
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFramePolarExport]:
|
||||
"""Assemble MSOP packets from a V2 .rscap into polar frames."""
|
||||
|
||||
packets = [chunk.raw for chunk in capture.chunks]
|
||||
host_ticks = [chunk.receive_utc_ticks for chunk in capture.chunks]
|
||||
return iter_h32_frames_polar_from_packets(
|
||||
packets,
|
||||
host_utc_ticks=host_ticks,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=frame_stride,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
max_points_per_frame=max_points_per_frame,
|
||||
vertical_deg=vertical_deg,
|
||||
horizontal_deg=horizontal_deg,
|
||||
)
|
||||
|
||||
|
||||
def iter_h32_frames(
|
||||
capture: CaptureFile,
|
||||
*,
|
||||
|
||||
@@ -131,6 +131,30 @@ def parse_heading(line: str) -> dict:
|
||||
}
|
||||
|
||||
|
||||
def parse_gnhpr(line: str) -> dict:
|
||||
"""Parse Wheeltec G90 ``$GNHPR`` heading/pitch output."""
|
||||
|
||||
fields = line[:line.rfind("*")].split(",")
|
||||
if len(fields) < 7:
|
||||
raise ValueError("GNHPR has too few fields")
|
||||
quality = safe_int(fields[5], -1)
|
||||
return {
|
||||
"type": "GNHPR",
|
||||
"position_time_utc": fields[1],
|
||||
"raw_heading_deg": safe_float(fields[2]),
|
||||
"pitch_deg": safe_float(fields[3]),
|
||||
"roll_deg": safe_float(fields[4]),
|
||||
"heading_quality": quality,
|
||||
"satellites": safe_int(fields[6], -1),
|
||||
"heading_solution": f"GNHPR_QUALITY_{quality}",
|
||||
"baseline_length_m": None,
|
||||
"heading_stddev_deg": None,
|
||||
"pitch_stddev_deg": None,
|
||||
"solution_satellites": safe_int(fields[6], -1),
|
||||
"heading_valid": quality in {4, 5},
|
||||
}
|
||||
|
||||
|
||||
def parse_pvtslna(line: str) -> dict:
|
||||
"""Parse Unicore/G90 ``#PVTSLNA`` into GGA-compatible position fields.
|
||||
|
||||
@@ -221,6 +245,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
row.update(parse_pvtslna(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
elif line.startswith("$GNHPR"):
|
||||
row.update(parse_gnhpr(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
|
||||
@@ -41,8 +41,14 @@ def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: in
|
||||
}
|
||||
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
def parse_rtk_capture(
|
||||
capture: CaptureFile,
|
||||
accepted_prefixes: tuple[str, ...] | None = None,
|
||||
) -> list[dict]:
|
||||
rows = []
|
||||
accepted_prefix_bytes = (
|
||||
tuple(prefix.encode("ascii") for prefix in accepted_prefixes) if accepted_prefixes is not None else None
|
||||
)
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
@@ -56,6 +62,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
if accepted_prefix_bytes is not None and not raw_line.startswith(accepted_prefix_bytes):
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
@@ -66,6 +74,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
row.update(parse_pvtslna(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
elif line.startswith("$GNHPR"):
|
||||
row.update(parse_gnhpr(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
|
||||
+68
-131
@@ -1,13 +1,12 @@
|
||||
# 雷达与 RTK 标定说明书
|
||||
|
||||
本文说明如何用本仓库完成 **双天线 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 雷达 `.rscap`，RTK/IMU 全程各一条：
|
||||
### 3.1 每站独立目录
|
||||
|
||||
```text
|
||||
raw_dataset/
|
||||
├── stations/
|
||||
│ ├── 001/h32.rscap
|
||||
│ ├── 002/h32.rscap
|
||||
│ └── ...
|
||||
└── captures/
|
||||
├── rtk.rscap # G90：#PVTSLNA 位置 + #UNIHEADINGA 航向
|
||||
└── imu.rscap # N300；仅关联保存，不参与外参求解
|
||||
├── stations/001|002|.../
|
||||
└── captures/rtk.rscap , imu.rscap
|
||||
```
|
||||
|
||||
旧车 dlog 布局（`dobject/` + `dobject_recording/`）仍可被导出脚本识别；关联时间请用 `-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)。
|
||||
|
||||
Reference in New Issue
Block a user