Compare commits
9
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 lichun.qu
|
3c8e4f92f6 | ||
 lichun.quandCursor
|
5f59bcd795 | ||
|
lichun.quandCursor
|
6242fd1081 | ||
|
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,9 +1,26 @@
|
|||||||
# 双天线RTK—3D LiDAR直接手眼标定
|
# 双天线RTK—3D LiDAR直接手眼标定
|
||||||
|
|
||||||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达坐标转换到RTK导航坐标系。它**不是** `base_link` 车体外参，也不会在求解阶段使用车体航向偏置或RTK到后轮轴的XY杆臂。
|
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **车头向前的 RTK 车体系**（主天线原点）。
|
||||||
数据下载地址：https://fs.fairylandtech.com:5001/FRLD/#file_id=963272954246902180 账号：lichun.qu@fairylandtech.com 密码：lichun.qu
|
求解不使用 RTK 到后轮轴的 XY 杆臂；与雷达–IMU 外参对照时旋转系一致，平移仍差天线原点。
|
||||||
|
|
||||||
## 1. 输出坐标约定
|
当前交付标定（2026-08 室外车，27 站）约定如下：
|
||||||
|
|
||||||
|
| 项 | 值 |
|
||||||
|
|---|---|
|
||||||
|
| RTK 坐标系 | **车头向前**（`HeadingOffsetDeg = -90`；主从装反、基线朝右） |
|
||||||
|
| 天线相位中心离地高 | **1.9165 m**（1916.5 mm） |
|
||||||
|
| 机械初值（车头系） | \(t=(+0.21086,-0.41418,+0.07850)\) m，yaw=**0°**（CAD 纵向已按车头正向取 +X） |
|
||||||
|
| 物理基线 | `baseline_points=vehicle_right`（主天线车左，从天线车右，后轴中心左右对称） |
|
||||||
|
| 姿态 | 双天线 pitch/roll：`R = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(+90°)` |
|
||||||
|
| 地面点 ROI | LiDAR 系 **`z ∈ [-2.5, -1.5]`**（约 2 m 车顶安装） |
|
||||||
|
| pair 配准 | **禁止**使用外参 seed；B 与 X 独立 |
|
||||||
|
|
||||||
|
数据下载：https://fs.fairylandtech.com:5001/FRLD/#file_id=966776353886090246
|
||||||
|
账号：lichun.qu@fairylandtech.com 密码：lichun.qu
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 输出坐标约定（车头向前）
|
||||||
|
|
||||||
统一约定 `T_A_B` 把 B 系点变换到 A 系：
|
统一约定 `T_A_B` 把 B 系点变换到 A 系：
|
||||||
|
|
||||||
@@ -11,78 +28,58 @@
|
|||||||
p_RTK = T_RTK_lidar · p_lidar
|
p_RTK = T_RTK_lidar · p_lidar
|
||||||
```
|
```
|
||||||
|
|
||||||
RTK导航系在本仓库中定义为：
|
本仓库默认 RTK 导航系（**车头向前 / vehicle_forward_heading_offset**）：
|
||||||
|
|
||||||
- 原点：GGA位置参考点（通常为ANT1相位中心，必须结合接收机配置确认）；
|
- 原点：GGA 位置参考点（主天线 / ANT1 相位中心）；
|
||||||
- X轴：`rawHeading`所表示的双天线基线在水平面的投影；
|
- X 轴：车头向前（`rawHeading + HeadingOffsetDeg`，本车 `HeadingOffsetDeg = -90`）；
|
||||||
- Y 轴：左；
|
- Y 轴：左；
|
||||||
- Z 轴：上；
|
- Z 轴：上；
|
||||||
- ENU航向：`yaw = 90° - rawHeading`；
|
- 姿态：先在基线系应用双天线 pitch/roll，再乘固定 `Rz(-heading_offset)`；不是 IMU 融合姿态。
|
||||||
- roll、pitch：当前轨迹中固定为0。
|
|
||||||
|
|
||||||
如果下游需要 `T_body_lidar`，必须另有经过确认的 `T_body_rtk`：
|
> 改 `HeadingOffsetDeg` 或姿态模型后必须从 **prepare** 起重跑；禁止事后只改 JSON 里的 yaw。
|
||||||
|
> 旧基线系结果（`HeadingOffsetDeg = 0`）与车头系外参不可混用。
|
||||||
|
|
||||||
```text
|
机械初值文件：[`run/rtk_lidar_mechanical_initial.json`](run/rtk_lidar_mechanical_initial.json)
|
||||||
T_body_lidar = T_body_rtk · T_RTK_lidar
|
**仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。**
|
||||||
```
|
|
||||||
|
---
|
||||||
|
|
||||||
## 2. 算法流程
|
## 2. 算法流程
|
||||||
|
|
||||||
```text
|
```text
|
||||||
逐站 H32.rscap + 全程 RTK.rscap + IMU.rscap
|
原始雷达 + RTK（+ 可选 IMU）
|
||||||
→ tools/export_raw_to_combined.py（一步导出标定中间包 combined/）
|
→ combined/（按站关联的多传感器 NPZ）
|
||||||
→ 每站选择一帧静态点云，位置转局部ENU，rawHeading构造yaw-only RTK pose
|
→ 每站选一帧静态点云 + RTK pose（车头向前，含双天线 pitch/roll）
|
||||||
→ Open3D GICP和small_gicp分别求 B_ij = T_Li_Lj
|
→ Open3D GICP 与 small_gicp 分别求 B_ij = T_Li_Lj（无外参 seed）
|
||||||
→ 留出点、Hessian、正反向、多初值和旋转共轭不变量筛选
|
→ 留出点、正反向、旋转共轭不变量等精筛
|
||||||
→ 两后端共同认可的边形成consensus B
|
→ 双后端共识边 → consensus B
|
||||||
→ A_ij X = X B_ij + 地面法向/高度约束求 X = T_RTK_lidar
|
→ A_ij X = X B_ij + 地面法向/高度约束 → X = T_RTK_lidar
|
||||||
→ bootstrap、双后端差异、逐对残差和3D可视化检查
|
→ bootstrap、双后端差异、逐对残差与 3D 可视化
|
||||||
```
|
```
|
||||||
|
|
||||||
代码实际使用：
|
|
||||||
|
|
||||||
```text
|
```text
|
||||||
A_ij = inv(T_W_Ri) · T_W_Rj = T_Ri_Rj
|
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
|
A_ij · X = X · B_ij
|
||||||
X = T_RTK_lidar
|
X = T_RTK_lidar
|
||||||
```
|
```
|
||||||
|
|
||||||
## 3. 原始数据目录
|
---
|
||||||
|
|
||||||
大体积数据不提交Git。**新车默认布局**（H32 / G90 / N300 新插件，不再出 dlog）：
|
## 3. 原始数据与导出
|
||||||
|
|
||||||
|
大体积数据不提交 Git。常见两种采集形态：
|
||||||
|
|
||||||
|
### 3.1 每站独立雷达目录（旧/标准站目录）
|
||||||
|
|
||||||
```text
|
```text
|
||||||
raw_dataset/
|
raw_dataset/
|
||||||
├── stations/ # 每站停稳后单独录一段雷达
|
├── stations/001|002|.../ # H32 dlog 或 h32.rscap
|
||||||
│ ├── 001/h32.rscap
|
|
||||||
│ ├── 002/h32.rscap
|
|
||||||
│ └── ...
|
|
||||||
└── captures/
|
└── captures/
|
||||||
├── rtk.rscap # 进场到收工连续录（G90：#PVTSLNA + #UNIHEADINGA）
|
├── rtk.rscap
|
||||||
└── imu.rscap # 连续录（N300；仅关联，不参与外参求解）
|
└── 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
|
```powershell
|
||||||
python tools\export_raw_to_combined.py `
|
python tools\export_raw_to_combined.py `
|
||||||
--stations-root "$Raw\stations" `
|
--stations-root "$Raw\stations" `
|
||||||
@@ -92,160 +89,204 @@ python tools\export_raw_to_combined.py `
|
|||||||
--overwrite
|
--overwrite
|
||||||
```
|
```
|
||||||
|
|
||||||
产物：
|
默认时间基：`-TimeBasis device_gnss`（雷达设备时 ↔ GNSS week/TOW）。
|
||||||
|
|
||||||
```text
|
### 3.2 G90 连续录制 + H32 DLog 按站时间窗（本次 27 站）
|
||||||
$Out\exported\
|
|
||||||
├── export/ # 内部：各站雷达帧（调试用）
|
站不在独立目录，而在多个 Medulla DLog ZIP 与 G90 `.rscap` 中时：
|
||||||
├── parsed/ # 内部：RTK/IMU JSONL
|
|
||||||
├── combined/ # ★ 标定入口：关联后的多传感器 NPZ + manifest.csv
|
```powershell
|
||||||
└── export_summary.json
|
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
|
```powershell
|
||||||
$Repo = (Resolve-Path ".").Path
|
$Repo = (Resolve-Path ".").Path
|
||||||
$Raw = "E:\calibration_data\data4"
|
$Data = "D:\data\rtk_lidar_run" # 含 combined/
|
||||||
$Out = "E:\calibration_output\rtk_lidar"
|
|
||||||
|
|
||||||
|
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
|
||||||
|
-CombinedRoot "$Data\combined" `
|
||||||
|
-WorkRoot "$Data\prepared_vehicle_h19165" `
|
||||||
|
-OutputRoot "$Data\outputs_vehicle_h19165" `
|
||||||
|
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||||
|
-HeadingOffsetDeg -90 `
|
||||||
|
-ExpectedStations 27 `
|
||||||
|
-MinStations 20 `
|
||||||
|
-GroundZMin -2.5 `
|
||||||
|
-GroundZMax -1.5 `
|
||||||
|
-Bootstrap 200
|
||||||
|
```
|
||||||
|
|
||||||
|
关键参数：
|
||||||
|
|
||||||
|
| 参数 | 本次取值 | 说明 |
|
||||||
|
|---|---|---|
|
||||||
|
| `-RtkReferenceHeightAboveGroundM` | **1.9165** | GGA/ANT1 相位中心离地高（m），必填 |
|
||||||
|
| `-HeadingOffsetDeg` | **-90** | 车头向前（主从装反、基线朝右）；`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" `
|
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" `
|
||||||
-DataRoot "$Raw\stations" `
|
-DataRoot "$Raw\stations" `
|
||||||
-RtkCapture "$Raw\captures\rtk.rscap" `
|
-RtkCapture "$Raw\captures\rtk.rscap" `
|
||||||
-ImuCapture "$Raw\captures\imu.rscap" `
|
-ImuCapture "$Raw\captures\imu.rscap" `
|
||||||
-OutputRoot $Out `
|
-OutputRoot $Out `
|
||||||
-RtkReferenceHeightAboveGroundM 0.758 `
|
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||||
-ExpectedStations 34
|
-ExpectedStations 27 `
|
||||||
|
-GroundZMin -2.5 `
|
||||||
|
-GroundZMax -1.5
|
||||||
```
|
```
|
||||||
|
|
||||||
主要输出：
|
主要输出：
|
||||||
|
|
||||||
```text
|
```text
|
||||||
$Out/
|
$Out/
|
||||||
├── exported/
|
├── exported/combined/ # 或外部已有 combined/
|
||||||
│ ├── export/ # 内部各站LiDAR帧
|
├── prepared_*/frames_all/
|
||||||
│ ├── parsed/ # 内部 RTK/IMU JSONL
|
├── prepared_*/reference_poses_rtk_gga_raw_heading.csv
|
||||||
│ └── combined/ # ★ 按LiDAR帧关联后的多传感器NPZ
|
└── calibration/ 或 outputs_*/
|
||||||
├── prepared_rtk_direct/
|
├── open3d_gicp/ small_gicp/ consensus/
|
||||||
│ ├── frames_all/ # 每站选中的静态帧
|
├── common/ground_planes.csv
|
||||||
│ └── reference_poses_rtk_gga_raw_heading.csv
|
|
||||||
└── calibration/
|
|
||||||
├── open3d_gicp/
|
|
||||||
├── small_gicp/
|
|
||||||
├── consensus/
|
|
||||||
├── summary.json
|
├── summary.json
|
||||||
└── final_T_RTK_lidar.json
|
└── final_T_RTK_lidar.json
|
||||||
```
|
```
|
||||||
|
|
||||||
若已经有`combined/`，可跳过原始导出：
|
---
|
||||||
|
|
||||||
```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
|
|
||||||
```
|
|
||||||
|
|
||||||
## 6. 3D 可视化
|
## 6. 3D 可视化
|
||||||
|
|
||||||
|
查看本次结果：
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
|
$Repo = "D:\First-dev-dept\calibration-rtk-run"
|
||||||
|
$Out = "D:\data\rtk_lidar_run\outputs_vehicle_h19165"
|
||||||
|
$Work = "D:\data\rtk_lidar_run\prepared_vehicle_h19165"
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||||||
-Frames "$Out\prepared_rtk_direct\frames_all" `
|
-Frames "$Work\frames_all" `
|
||||||
-Pairs "$Out\calibration\consensus\B_consensus.npz" `
|
-Pairs "$Out\consensus\B_consensus.npz" `
|
||||||
-Extrinsic "$Out\calibration\final_T_RTK_lidar.json" `
|
-Extrinsic "$Out\final_T_RTK_lidar.json" `
|
||||||
-PairIndex 0
|
-PairIndex 0
|
||||||
```
|
```
|
||||||
|
|
||||||
窗口中：
|
通用模板（把路径换成你的 `WorkRoot` / `OutputRoot`）：
|
||||||
|
|
||||||
- 蓝色：目标站点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及最大值没有改善，说明少数高残差运动对仍然存在；不应仅为降低最大值而按最终外参残差删边。
|
|
||||||
|
|
||||||
已经分别得到各批次的共识运动对和地面平面时，可运行：
|
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
$Names = @("data4", "data5")
|
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||||||
$Pairs = @("E:\data4\consensus\B_consensus.npz", "E:\data5\consensus\B_consensus.npz")
|
-Frames "$WorkRoot\frames_all" `
|
||||||
$Planes = @("E:\data4\common\ground_planes.csv", "E:\data5\common\ground_planes.csv")
|
-Pairs "$OutputRoot\consensus\B_consensus.npz" `
|
||||||
|
-Extrinsic "$OutputRoot\final_T_RTK_lidar.json" `
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" `
|
-PairIndex 0
|
||||||
-BatchNames $Names -Pairs $Pairs -GroundPlanes $Planes `
|
|
||||||
-OutputRoot "E:\calibration_output\data4_data5_joint" `
|
|
||||||
-RtkReferenceHeightAboveGroundM 0.758 -Bootstrap 200
|
|
||||||
```
|
```
|
||||||
|
|
||||||
脚本会先分别拟合各批次外参；任一批与首批相差超过`0.25 m`或`5°`时中止，提示检查RTK航向/坐标定义和传感器安装。阈值可通过`-MaxBatchTranslationDifferenceM`和`-MaxBatchRotationDifferenceDeg`显式调整。
|
| 按键 | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| `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` **多看大转角对**，不要只看前几对同朝向站。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 当前标定结果（车头向前，h = 1.9165 m）
|
||||||
|
|
||||||
|
> **状态：可作车头系候选交付**（`recommended_for_deployment: true`）。
|
||||||
|
> 约定：`HeadingOffsetDeg=-90`，双天线 pitch/roll，机械初值 \(t=(+0.21086,-0.41418,+0.07850)\)，yaw=0。
|
||||||
|
|
||||||
|
仓库内结果：[`results/vehicle_20260808/`](results/vehicle_20260808/)（来自本机 `outputs_vehicle_h19165`）。
|
||||||
|
|
||||||
|
```text
|
||||||
|
translation_m = [0.217822250, -0.411347802, 0.106542337]
|
||||||
|
RPY_deg_xyz = [0.066239, 0.809662, -0.551322]
|
||||||
|
|
||||||
|
T_RTK_lidar ≈
|
||||||
|
0.999854 0.009639 0.014119 0.217822
|
||||||
|
-0.009621 0.999953 -0.001292 -0.411348
|
||||||
|
-0.014131 0.001156 0.999899 0.106542
|
||||||
|
0 0 0 1
|
||||||
|
```
|
||||||
|
|
||||||
|
| 指标 | 值 |
|
||||||
|
|---|---:|
|
||||||
|
| 有效站点 / 共识对 | 27 / 20 |
|
||||||
|
| 平移残差 RMS | ≈ 0.071 m |
|
||||||
|
| 旋转残差 RMS | ≈ 0.982 ° |
|
||||||
|
| 双后端差 | ≈ 3.1 mm / 0.12° |
|
||||||
|
| `frame_mode` | `vehicle_forward_heading_offset` |
|
||||||
|
| 相对机械初值 | XY 近机械杆臂；yaw≈0；无近 180° 冲突 |
|
||||||
|
|
||||||
|
与机械平移初值 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 与精度限制
|
## 8. z 与精度限制
|
||||||
|
|
||||||
平面阿克曼运动不能独立观测z。当前联合结果使用64站地面平面和ANT1参考点离地`0.758 m`约束z；其中`0.758 m`来自本次现场粗测的天线底部安装参考高度`0.710 m`，加上天线标签给出的L1/L2 PCO高度`46/50 mm`的中值`48 mm`。该值仍是测量输入，不是手眼运动方程自行估计出来的量。
|
平面阿克曼运动不能独立观测 z。z 由「LiDAR 地面平面 + 外供 RTK 参考点离地高」约束：
|
||||||
|
|
||||||
旧联合结果曾使用`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。
|
- 本次：**1.9165 m**（相位中心离地）；
|
||||||
|
- 不得复用其他车辆或历史采集的天线离地高度。
|
||||||
|
|
||||||
AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。
|
更改高度后必须重新求解，禁止只改 JSON 里的 z。
|
||||||
|
GGA 对应哪根天线、`rawHeading` 方向须现场确认；搞反会导致 yaw 差约 180°。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
此外，代码无法单独证明GGA对应哪根物理天线、`rawHeading`是ANT1→ANT2还是ANT2→ANT1；必须用接收机配置、接线和现场运动实验确认。方向错误会导致RTK坐标系yaw相差约180°。
|
|
||||||
|
|
||||||
## 9. 仓库目录
|
## 9. 仓库目录
|
||||||
|
|
||||||
| 目录 | 职责 |
|
| 目录 | 职责 |
|
||||||
|---|---|
|
|---|---|
|
||||||
| [`code/`](code/) | GICP、运动对质量评价、AX=XB求解、结果封装和3D可视化 |
|
| [`code/`](code/) | GICP、运动对质量、AX=XB、结果封装、3D 可视化 |
|
||||||
| [`tools/`](tools/) | 原始dlog/rscap解析、按LiDAR帧关联及静态站点prepared生成 |
|
| [`tools/`](tools/) | dlog/rscap 解析、G90 窗导出、combined / prepared |
|
||||||
| [`run/`](run/) | PowerShell入口；所有数据和输出路径都通过参数传入 |
|
| [`run/`](run/) | PowerShell 入口；路径与高度均由参数传入 |
|
||||||
| [`results/reference_data4/`](results/reference_data4/) | 历史data4精简参考结果，不包含点云和本机过程目录 |
|
| [`results/`](results/) | 当前车辆的最终外参与质量摘要；不含原始数据和中间点云 |
|
||||||
| `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 |
|
| `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)。
|
||||||
|
|||||||
Binary file not shown.
|
Before Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 80 KiB |
@@ -1,240 +0,0 @@
|
|||||||
LiDAR、双天线 RTK、IMU 标定数据说明
|
|
||||||
====================================
|
|
||||||
|
|
||||||
文档日期：2026-07-23
|
|
||||||
配套代码仓库：calibration
|
|
||||||
标定目标：求解 3D LiDAR 到后轮轴中心车体系的外参 T_body_lidar。
|
|
||||||
|
|
||||||
|
|
||||||
一、重要说明
|
|
||||||
------------
|
|
||||||
|
|
||||||
1. 三批数据的记录格式和用途并不完全相同。export 是每站多帧的完整解码数据，prepared 是从中每站选一帧并配好 RTK 车体位姿后的标定输入。
|
|
||||||
2. 第一批（data1）、第二批(data2)目前上传的是已经从原始 dlog 数据以及导出的 NPZ 数据。
|
|
||||||
3. data4 保留了完整逐站 LiDAR dlog 和独立 RTK/IMU rscap，可以从原始记录开始复现。
|
|
||||||
4. 每一个站点采集 LiDAR 点云时车辆均静止，因此当前 LiDAR-RTK 标定没有使用 IMU进行点云运动畸变校正。
|
|
||||||
5. data4 中的 IMU 数据保持在 IMU 原始传感器坐标系；当前流程只解析、关联和保存 IMU，没有求解 IMU 外参。
|
|
||||||
6. 不要修改站点目录名称、prepared 中 station_*.npz 的顺序或 manifest。B 文件中的运动对索引依赖这些顺序。
|
|
||||||
|
|
||||||
|
|
||||||
二、目录结构
|
|
||||||
----------------------
|
|
||||||
|
|
||||||
LiDAR_RTK_calibration_data/
|
|
||||||
数据说明.txt
|
|
||||||
data1/
|
|
||||||
raw/
|
|
||||||
export/
|
|
||||||
prepared/
|
|
||||||
data2/
|
|
||||||
raw/
|
|
||||||
export/
|
|
||||||
prepared/
|
|
||||||
data4/
|
|
||||||
raw/
|
|
||||||
RTKIMUraw/
|
|
||||||
prepared/
|
|
||||||
combined/(三传感器混合后的数据，data1、2没有IMU的数据。)
|
|
||||||
说明：
|
|
||||||
|
|
||||||
- data1/export 和 data2/export 是从逐站 dlog 导出的数据。raw 是完整原始数据。
|
|
||||||
- data1/prepared 和 data2/prepared 是每站选择一帧并重建 RTK 车体位姿后的标定输入。
|
|
||||||
- data4/raw 是完整原始数据，能够重新生成 export、parsed、combined 和 prepared。
|
|
||||||
它们可以由 raw 重新生成。
|
|
||||||
|
|
||||||
|
|
||||||
三、第一批数据（data1）
|
|
||||||
-----------------------
|
|
||||||
|
|
||||||
1. 数据范围
|
|
||||||
|
|
||||||
- 静止站点数量：38 站。
|
|
||||||
- 站点编号：01～38。
|
|
||||||
- 传感器：3D LiDAR + 双天线 RTK。
|
|
||||||
- 不包含 IMU。
|
|
||||||
- 记录格式：旧式逐站 dlog 中同时记录 LiDAR 和 GPS-POST-Z；当前云盘包从已导出的 NPZ 开始。
|
|
||||||
|
|
||||||
2. RTK 特点
|
|
||||||
|
|
||||||
- 实际 RTK 记录约 10 秒一条，并非预期的 10 Hz。
|
|
||||||
- 每站有效 RTK 样本较少，部分站点约 1～11 个有效样本。
|
|
||||||
- 因车辆在每站静止，仍可对站内 RTK 样本做均值并构造站点位姿；但时间同步精度和航向统计能力弱于第二批。
|
|
||||||
|
|
||||||
3. 当前用途
|
|
||||||
|
|
||||||
- 第一批只作为辅助复核数据。
|
|
||||||
- 不作为当前部署外参的主要求解数据。
|
|
||||||
- 不应把第一批写成严格独立的“验证集”，因为 RTK 过于稀疏，且它和第二批的场景、采集流程相近。
|
|
||||||
|
|
||||||
4. 上传内容
|
|
||||||
|
|
||||||
data1/export/
|
|
||||||
|
|
||||||
- 当前文件数约 803。
|
|
||||||
- 当前大小约 0.088 GB（约 90 MB）。
|
|
||||||
- 含逐站导出的 LiDAR NPZ、RTK sidecar、manifest 和质量报告。
|
|
||||||
|
|
||||||
data1/prepared/
|
|
||||||
|
|
||||||
- 当前文件数约 118。
|
|
||||||
- 当前大小约 0.049 GB（约 50 MB）。
|
|
||||||
- 核心内容包括：
|
|
||||||
frames_all/station_*.npz
|
|
||||||
body_poses_rear_gga_raw_rear_to_front.csv
|
|
||||||
station_summary.csv
|
|
||||||
manifest.json
|
|
||||||
|
|
||||||
|
|
||||||
四、第二批数据（data2）
|
|
||||||
-----------------------
|
|
||||||
|
|
||||||
1. 数据范围
|
|
||||||
|
|
||||||
- 静止站点数量：38 站。
|
|
||||||
- 原始站点编号：39～76。
|
|
||||||
- prepared 中重新顺序编号为 station_01～station_38。
|
|
||||||
- 传感器：3D LiDAR + 双天线 RTK。
|
|
||||||
- 不包含 IMU。
|
|
||||||
- 记录格式与第一批相同，但 RTK 采样正常且明显更密集。
|
|
||||||
|
|
||||||
2. RTK 特点
|
|
||||||
|
|
||||||
- 每站约有 125～412 个有效 RTK 样本。
|
|
||||||
- 已使用 fix 4/fix 5 和有效 heading 进行筛选。
|
|
||||||
- 站内 heading 圆标准差上限使用 0.5°，本批 38 站均通过。
|
|
||||||
|
|
||||||
3. 当前用途
|
|
||||||
|
|
||||||
- 第二批是当前部署外参的主要求解数据。
|
|
||||||
- 使用 small_gicp 和 Open3D GICP 分别求 B，再做与 X 无关的质量筛选和跨后端一致性筛选。
|
|
||||||
- 当前部署外参主要由第二批求得，第一批仅辅助复核。
|
|
||||||
|
|
||||||
4. 上传内容
|
|
||||||
data2/export/
|
|
||||||
|
|
||||||
- 当前文件数约 759。
|
|
||||||
- 当前大小约 0.127 GB（约 130 MB）。
|
|
||||||
- 含逐站导出的 LiDAR NPZ、RTK 信息、manifest 和质量报告。
|
|
||||||
|
|
||||||
data2/prepared/
|
|
||||||
|
|
||||||
- 当前文件数约 82。
|
|
||||||
- 当前大小约 0.033 GB（约 34 MB）。
|
|
||||||
- 核心内容包括：
|
|
||||||
frames_all/station_*.npz
|
|
||||||
body_poses_rear_gga_raw_rear_to_front.csv
|
|
||||||
station_summary.csv
|
|
||||||
manifest.json
|
|
||||||
|
|
||||||
|
|
||||||
五、data4 数据
|
|
||||||
--------------
|
|
||||||
|
|
||||||
1. 数据范围
|
|
||||||
|
|
||||||
- 静止站点数量：34 站。
|
|
||||||
- 站点编号：001～034。
|
|
||||||
- 传感器：3D LiDAR + 双天线 RTK + IMU。
|
|
||||||
- LiDAR 位于每个站点自己的原始 dlog 中。
|
|
||||||
- RTK 和 IMU 位于独立 rscap 文件中，存放在 RTKIMUraw 目录。
|
|
||||||
- 原始目录共约 152 个文件，大小约 12.488 GB。
|
|
||||||
|
|
||||||
2. RTK/IMU 原始记录
|
|
||||||
|
|
||||||
- RTKIMUraw 中当前包含 3 个 RTK rscap 和 3 个 IMU rscap。
|
|
||||||
- 本次 34 站标定使用 20260723-051627 开始的长时间 RTK/IMU session。
|
|
||||||
- 该 session 的 capture 审计结果：
|
|
||||||
RTK：54256 个记录块，missing_chunks=0，bad_record_crc=0，干净关闭，footer CRC 有效。
|
|
||||||
IMU：26719 个记录块，missing_chunks=0，bad_record_crc=0，干净关闭，footer CRC 有效。
|
|
||||||
|
|
||||||
3. 时间关联与导出结果
|
|
||||||
|
|
||||||
处理顺序为：
|
|
||||||
|
|
||||||
统一时间轴
|
|
||||||
-> 分别解析 LiDAR、RTK、IMU
|
|
||||||
-> 按每个 LiDAR 帧关联最近有效 RTK
|
|
||||||
-> 保存 LiDAR 帧前后各 100 ms 的 IMU 窗口
|
|
||||||
-> 导出 combined NPZ
|
|
||||||
-> 每站选择一个静止帧生成 prepared
|
|
||||||
|
|
||||||
当前关联统计：
|
|
||||||
|
|
||||||
- LiDAR 帧总数：11678。
|
|
||||||
- 34 个站点全部有数据。
|
|
||||||
- rtk_valid：11678。
|
|
||||||
- heading_valid：11678。
|
|
||||||
- fixed RTK：11678。
|
|
||||||
- IMU 窗口非空：11678。
|
|
||||||
- RTK 最大允许关联时间差：150 ms。
|
|
||||||
- IMU 窗口：LiDAR 时刻前后各 100 ms。
|
|
||||||
|
|
||||||
时间基础：LiDAR 和串口 host UTC 用于当前关联；RTK GNSS 时间和 IMU 设备时间同时保留，供后续进一步建立精确时钟模型。
|
|
||||||
|
|
||||||
4. 当前用途
|
|
||||||
|
|
||||||
- data4 用于独立重新求解一套外参，并与历史第二批结果做跨批比较。
|
|
||||||
- data4 中 IMU 没有参与当前 LiDAR-RTK 外参求解。
|
|
||||||
- data4 结果与历史部署外参相差约 1.592 cm / 0.234°，但 data4 自身 AX 残差更高，因此当前仍保留历史第二批结果作为部署值。
|
|
||||||
|
|
||||||
如果已经上传 data4/raw，则 export、combined 和 parsed 均可以用代码重新生成。为了节省云盘空间，可只额外上传 prepared 和 calibration。
|
|
||||||
|
|
||||||
|
|
||||||
六、三批数据差异汇总
|
|
||||||
--------------------
|
|
||||||
|
|
||||||
第一批：
|
|
||||||
- 38 站，旧式 LiDAR+RTK dlog，无 IMU。
|
|
||||||
- RTK 极稀疏，约 10 秒一条。
|
|
||||||
- 当前上传从 export 开始。
|
|
||||||
- 只用于辅助复核。
|
|
||||||
|
|
||||||
第二批：
|
|
||||||
- 38 站，旧式 LiDAR+RTK dlog，无 IMU。
|
|
||||||
- RTK 密集、航向稳定。
|
|
||||||
- 当前上传从 export 开始。
|
|
||||||
- 用于当前部署外参的主要求解。
|
|
||||||
|
|
||||||
data4：
|
|
||||||
- 34 站，逐站 LiDAR dlog + 独立 RTK/IMU rscap。
|
|
||||||
- 保存完整原始数据，可从 raw 开始复现。
|
|
||||||
- 用于独立重算和跨批比较。
|
|
||||||
- IMU 只保存和关联，尚未完成 IMU 外参标定。
|
|
||||||
|
|
||||||
|
|
||||||
七、标定坐标与主要参数
|
|
||||||
----------------------
|
|
||||||
|
|
||||||
- 外参定义：T_body_lidar，将 LiDAR 原始点变换到后轮轴中心车体系。
|
|
||||||
- 车体系：x 向前，y 向左，z 向上。
|
|
||||||
- 手眼方程：A_ij X = X B_ij。
|
|
||||||
- A_ij：由 RTK 后轮轴中心位置和双天线 heading 构造；当前为 yaw-only 姿态。
|
|
||||||
- B_ij：由两个静止站点的原始 LiDAR 点云通过 GICP 求得。
|
|
||||||
- heading_offset_deg：21.226°（本车安装参数，不是通用常数）。
|
|
||||||
- 后天线在车体系杆臂：[ -0.320, -0.365, 0.620 ] m。（手量）
|
|
||||||
- 后轮轴中心离地高度：0.2335 m，用于地面约束；不是 LiDAR 离地高度。
|
|
||||||
|
|
||||||
|
|
||||||
八、数据使用注意事项
|
|
||||||
--------------------
|
|
||||||
|
|
||||||
1. data1、2 的manifest 中可能仍保留旧脚本生成的 train/validation 字段。这些字段是历史元数据；当前严谨流程将第二批用于求解、第一批用于辅助复核，不把同一批内部的小样本划分描述为高可信度验证集。
|
|
||||||
2. 不要使用已经变换到车体系的点云求 B，必须使用 NPZ 中的 points_raw。
|
|
||||||
3. 可视化时，frames_all 必须与生成 B 文件时的站点数量和顺序完全一致。
|
|
||||||
4. 模式 3（GICP B）本身已经错位时，应优先检查点云配准和场景退化；只有模式 3 正常而模式 4（X^-1 A X）系统性错位时，才优先检查 RTK A、坐标约定或外参 X。
|
|
||||||
5. 当前结果是工程标定结果，不是由全站仪或高精度标靶认证的绝对真值。
|
|
||||||
|
|
||||||
|
|
||||||
九、配套代码位置
|
|
||||||
----------------
|
|
||||||
|
|
||||||
本机代码仓库：
|
|
||||||
calibration
|
|
||||||
|
|
||||||
根 README.md 包含：
|
|
||||||
- 从原始数据/导出数据开始的完整复现流程；
|
|
||||||
- code、tools、run 中每个主要文件的职责；
|
|
||||||
- small_gicp、Open3D GICP、consensus B 的处理逻辑；
|
|
||||||
- AX=XB 与地面约束求 X 的方式；
|
|
||||||
- 残差、Hessian/条件数、bootstrap、跨批检查和 3D 可视化方法。
|
|
||||||
|
|
||||||
@@ -15,7 +15,17 @@ def load(path: Path) -> dict:
|
|||||||
|
|
||||||
def write(path: Path, document: dict) -> None:
|
def write(path: Path, document: dict) -> None:
|
||||||
path.parent.mkdir(parents=True, exist_ok=True)
|
path.parent.mkdir(parents=True, exist_ok=True)
|
||||||
path.write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
|
def default(obj):
|
||||||
|
if isinstance(obj, (np.bool_, np.integer)):
|
||||||
|
return obj.item()
|
||||||
|
if isinstance(obj, np.floating):
|
||||||
|
return float(obj)
|
||||||
|
if isinstance(obj, np.ndarray):
|
||||||
|
return obj.tolist()
|
||||||
|
raise TypeError(f"Object of type {type(obj).__name__} is not JSON serializable")
|
||||||
|
|
||||||
|
path.write_text(json.dumps(document, ensure_ascii=False, indent=2, default=default), encoding="utf-8")
|
||||||
|
|
||||||
|
|
||||||
def inverse(t: np.ndarray) -> np.ndarray:
|
def inverse(t: np.ndarray) -> np.ndarray:
|
||||||
@@ -34,7 +44,180 @@ def delta(a: np.ndarray, b: np.ndarray) -> dict:
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
def corrected(raw: dict, backend: str, reference_height: float) -> dict:
|
def wrap180(deg: float) -> float:
|
||||||
|
return (deg + 180.0) % 360.0 - 180.0
|
||||||
|
|
||||||
|
|
||||||
|
def yaw_deg_of(transform: np.ndarray) -> float:
|
||||||
|
return float(Rotation.from_matrix(transform[:3, :3]).as_euler("xyz", degrees=True)[2])
|
||||||
|
|
||||||
|
|
||||||
|
def mechanical_self_consistency(document: dict) -> dict:
|
||||||
|
"""Reject mechanical JSON that mixes incompatible baseline / body definitions."""
|
||||||
|
translation = np.asarray(document["translation_m"], float)
|
||||||
|
yaw = float(document["rotation_rpy_deg_xyz"][2])
|
||||||
|
side = str(document.get("baseline_points", "")).strip().lower()
|
||||||
|
frame_mode = str(document.get("frame_mode", "")).strip().lower()
|
||||||
|
heading_offset = float(document.get("heading_offset_deg", 0.0) or 0.0)
|
||||||
|
vehicle_forward = (
|
||||||
|
frame_mode == "vehicle_forward_heading_offset"
|
||||||
|
or abs(heading_offset) > 1e-6
|
||||||
|
)
|
||||||
|
issues: list[str] = []
|
||||||
|
|
||||||
|
if vehicle_forward:
|
||||||
|
if abs(wrap180(yaw)) > 15.0:
|
||||||
|
issues.append(
|
||||||
|
f"vehicle-forward mechanical initial requires yaw≈0°, got {yaw:g}°"
|
||||||
|
)
|
||||||
|
lever = document.get("vehicle_flu_lever_master_to_lidar_m")
|
||||||
|
if lever is not None:
|
||||||
|
if float(np.linalg.norm(translation - np.asarray(lever, float))) > 0.05:
|
||||||
|
issues.append(
|
||||||
|
"vehicle-forward translation_m must match vehicle_flu_lever_master_to_lidar_m"
|
||||||
|
)
|
||||||
|
if abs(heading_offset + 90.0) > 1e-6 and abs(heading_offset - 90.0) > 1e-6:
|
||||||
|
issues.append(
|
||||||
|
f"vehicle-forward heading_offset_deg should be ±90 for left/right baseline, got {heading_offset:g}"
|
||||||
|
)
|
||||||
|
elif side in {"vehicle_left", "left"}:
|
||||||
|
if abs(wrap180(yaw - (-90.0))) > 15.0:
|
||||||
|
issues.append(
|
||||||
|
f"baseline_points=vehicle_left requires yaw≈-90°, got {yaw:g}°"
|
||||||
|
)
|
||||||
|
if translation[0] <= 0.0 or translation[1] <= 0.0:
|
||||||
|
issues.append(
|
||||||
|
"baseline_points=vehicle_left expects +X/+Y lever in RTK baseline frame "
|
||||||
|
f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
|
||||||
|
)
|
||||||
|
elif side in {"vehicle_right", "right"}:
|
||||||
|
if abs(wrap180(yaw - 90.0)) > 15.0:
|
||||||
|
issues.append(
|
||||||
|
f"baseline_points=vehicle_right requires yaw≈+90°, got {yaw:g}°"
|
||||||
|
)
|
||||||
|
# Swapped but centerline-symmetric master (vehicle left): +X / -Y in baseline frame.
|
||||||
|
if translation[0] <= 0.0 or translation[1] >= 0.0:
|
||||||
|
issues.append(
|
||||||
|
"baseline_points=vehicle_right (master on vehicle left, baseline to the right) "
|
||||||
|
"expects +X/-Y lever in RTK baseline frame "
|
||||||
|
f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
left_xy = translation[0] > 0.05 and translation[1] > 0.05
|
||||||
|
right_xy = translation[0] < -0.05 and translation[1] < -0.05
|
||||||
|
swapped_right_xy = translation[0] > 0.05 and translation[1] < -0.05
|
||||||
|
if left_xy and abs(wrap180(yaw - 90.0)) <= 15.0:
|
||||||
|
issues.append(
|
||||||
|
"mixed baseline definition: +X/+Y translation (left-baseline) combined with yaw≈+90° (right-baseline)"
|
||||||
|
)
|
||||||
|
if right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
|
||||||
|
issues.append(
|
||||||
|
"mixed baseline definition: -X/-Y translation combined with yaw≈-90°"
|
||||||
|
)
|
||||||
|
if swapped_right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
|
||||||
|
issues.append(
|
||||||
|
"mixed baseline definition: +X/-Y translation (swapped-master right-baseline) "
|
||||||
|
"combined with yaw≈-90° (left-baseline)"
|
||||||
|
)
|
||||||
|
|
||||||
|
return {
|
||||||
|
"baseline_points": side or None,
|
||||||
|
"frame_mode": frame_mode or None,
|
||||||
|
"heading_offset_deg": heading_offset,
|
||||||
|
"consistent": not issues,
|
||||||
|
"issues": issues,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def solution_matches_declared_side(solution: np.ndarray, document: dict) -> dict:
|
||||||
|
"""Check whether the solved extrinsic agrees with the mechanical baseline side."""
|
||||||
|
side = str(document.get("baseline_points", "")).strip().lower()
|
||||||
|
yaw = yaw_deg_of(solution)
|
||||||
|
t = solution[:3, 3]
|
||||||
|
expected_yaw = float(document["rotation_rpy_deg_xyz"][2])
|
||||||
|
yaw_err = abs(wrap180(yaw - expected_yaw))
|
||||||
|
xy_err = float(np.linalg.norm(t[:2] - np.asarray(document["translation_m"][:2], float)))
|
||||||
|
z_err = float(abs(t[2] - float(document["translation_m"][2])))
|
||||||
|
|
||||||
|
opposite_yaw = abs(wrap180(yaw - expected_yaw) - 180.0) <= 15.0 or abs(
|
||||||
|
wrap180(yaw - expected_yaw) + 180.0
|
||||||
|
) <= 15.0
|
||||||
|
# Same XY sign as mechanical but yaw flipped ~180° (classic mixed inheritance).
|
||||||
|
same_xy_sign = (t[0] * float(document["translation_m"][0]) > 0.0) and (
|
||||||
|
t[1] * float(document["translation_m"][1]) > 0.0
|
||||||
|
)
|
||||||
|
mixed_inheritance = same_xy_sign and opposite_yaw
|
||||||
|
|
||||||
|
return {
|
||||||
|
"baseline_points": side or None,
|
||||||
|
"solution_yaw_deg": yaw,
|
||||||
|
"expected_yaw_deg": expected_yaw,
|
||||||
|
"yaw_error_deg": yaw_err,
|
||||||
|
"xy_error_m": xy_err,
|
||||||
|
"z_error_m": z_err,
|
||||||
|
"mixed_translation_rotation_inheritance": bool(mixed_inheritance),
|
||||||
|
"near_expected_pose": bool(yaw_err <= 15.0 and xy_err <= 0.25),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def coordinate_contract_audit(raw: dict) -> dict:
|
||||||
|
"""Audit mechanical self-consistency and solution agreement.
|
||||||
|
|
||||||
|
A near-180-degree disagreement is not auto-corrected: it normally means
|
||||||
|
that one physical forward-axis / baseline-direction statement is reversed.
|
||||||
|
"""
|
||||||
|
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
|
||||||
|
mech_check = mechanical_self_consistency(initial_document)
|
||||||
|
match = solution_matches_declared_side(solution, initial_document)
|
||||||
|
|
||||||
|
if not mech_check["consistent"]:
|
||||||
|
status = "mechanical_initial_inconsistent"
|
||||||
|
elif match["mixed_translation_rotation_inheritance"] or near_180:
|
||||||
|
status = "near_180_degree_axis_conflict"
|
||||||
|
elif not match["near_expected_pose"]:
|
||||||
|
status = "solution_disagrees_with_mechanical_baseline_side"
|
||||||
|
else:
|
||||||
|
status = "no_near_180_degree_axis_conflict"
|
||||||
|
|
||||||
|
requires = status != "no_near_180_degree_axis_conflict"
|
||||||
|
return {
|
||||||
|
"status": status,
|
||||||
|
"requires_physical_axis_confirmation": requires,
|
||||||
|
"mechanical_initial_path": str(path.resolve()),
|
||||||
|
"mechanical_self_consistency": mech_check,
|
||||||
|
"solution_vs_declared_baseline_side": match,
|
||||||
|
"solution_relative_to_mechanical_initial": comparison,
|
||||||
|
"note": (
|
||||||
|
"No automatic 180-degree correction was applied. Confirm static GNHPR "
|
||||||
|
"left/right vs vehicle heading and Helios +X vs vehicle forward 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 {
|
return {
|
||||||
"schema_version": 1,
|
"schema_version": 1,
|
||||||
"success": bool(raw["success"]),
|
"success": bool(raw["success"]),
|
||||||
@@ -43,20 +226,30 @@ def corrected(raw: dict, backend: str, reference_height: float) -> dict:
|
|||||||
"frames": {
|
"frames": {
|
||||||
"RTK": {
|
"RTK": {
|
||||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
"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",
|
"y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
|
||||||
"z_axis": "up",
|
"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": {
|
||||||
|
"description": "raw Helios sensor frame from points_raw polar decode",
|
||||||
|
"x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
|
||||||
|
"y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
|
||||||
|
"z_axis": "up",
|
||||||
|
"origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ",
|
||||||
},
|
},
|
||||||
"LiDAR": "raw LiDAR sensor frame",
|
|
||||||
},
|
},
|
||||||
"backend": backend,
|
"backend": backend,
|
||||||
"measured_lidar_extrinsic_used_as_initial": False,
|
"measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")),
|
||||||
"body_heading_offset_used": False,
|
"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,
|
"body_antenna_lever_xy_used": False,
|
||||||
"translation_m": raw["translation_m"],
|
"translation_m": raw["translation_m"],
|
||||||
"rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
|
"rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
|
||||||
"quaternion_xyzw": raw["quaternion_xyzw"],
|
"quaternion_xyzw": raw["quaternion_xyzw"],
|
||||||
|
"coordinate_contract_audit": coordinate_contract_audit(raw),
|
||||||
"matrix_4x4": raw["matrix_4x4"],
|
"matrix_4x4": raw["matrix_4x4"],
|
||||||
"quality": {
|
"quality": {
|
||||||
"stations": raw["estimation"]["stations"],
|
"stations": raw["estimation"]["stations"],
|
||||||
@@ -80,6 +273,7 @@ def main() -> None:
|
|||||||
parser = argparse.ArgumentParser()
|
parser = argparse.ArgumentParser()
|
||||||
parser.add_argument("--result-root", type=Path, required=True)
|
parser.add_argument("--result-root", type=Path, required=True)
|
||||||
parser.add_argument("--reference-height", type=float, 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()
|
args = parser.parse_args()
|
||||||
|
|
||||||
def solver_output(directory: str) -> Path:
|
def solver_output(directory: str) -> Path:
|
||||||
@@ -94,20 +288,45 @@ def main() -> None:
|
|||||||
}
|
}
|
||||||
docs = {}
|
docs = {}
|
||||||
for backend, path in paths.items():
|
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)
|
write(path.with_name("extrinsic_rtk_lidar.json"), document)
|
||||||
docs[backend] = document
|
docs[backend] = document
|
||||||
|
|
||||||
open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
|
open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
|
||||||
small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
|
small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
|
||||||
final = dict(docs["consensus"])
|
final = dict(docs["consensus"])
|
||||||
|
needs_axis_confirmation = bool(
|
||||||
|
final["coordinate_contract_audit"]["requires_physical_axis_confirmation"]
|
||||||
|
)
|
||||||
|
status = final["coordinate_contract_audit"]["status"]
|
||||||
|
reason_map = {
|
||||||
|
"mechanical_initial_inconsistent": (
|
||||||
|
"Mechanical initial mixes incompatible baseline-left/right translation and yaw; "
|
||||||
|
"fix run/rtk_lidar_mechanical_initial.json before trusting deployment"
|
||||||
|
),
|
||||||
|
"near_180_degree_axis_conflict": (
|
||||||
|
"Physical axis confirmation is required because the data-driven solution differs "
|
||||||
|
"from the declared mechanical initial by approximately 180 degrees "
|
||||||
|
"(or inherits mixed translation/rotation signs)"
|
||||||
|
),
|
||||||
|
"solution_disagrees_with_mechanical_baseline_side": (
|
||||||
|
"Solution yaw/XY disagree with the declared mechanical baseline side; "
|
||||||
|
"confirm static GNHPR direction before deployment"
|
||||||
|
),
|
||||||
|
}
|
||||||
final["selection"] = {
|
final["selection"] = {
|
||||||
"recommended": True,
|
"recommended": not needs_axis_confirmation,
|
||||||
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
"reason": (
|
||||||
|
reason_map.get(
|
||||||
|
status,
|
||||||
|
"Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
||||||
|
)
|
||||||
|
),
|
||||||
"open3d_vs_small_gicp": delta(open_t, small_t),
|
"open3d_vs_small_gicp": delta(open_t, small_t),
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
write(args.result_root / "final_T_RTK_lidar.json", final)
|
write(args.result_root / "final_T_RTK_lidar.json", final)
|
||||||
summary = {
|
summary = {
|
||||||
"final": {
|
"final": {
|
||||||
@@ -117,6 +336,8 @@ def main() -> None:
|
|||||||
"translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"],
|
"translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"],
|
||||||
"rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"],
|
"rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"],
|
||||||
"condition_number": final["quality"]["weighted_jacobian_condition_number"],
|
"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),
|
"backend_difference": delta(open_t, small_t),
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -80,6 +80,20 @@ def params_transform(params):
|
|||||||
return make_transform(params[:3], so3_exp(params[3:]))
|
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):
|
def inverse_transform(transform):
|
||||||
answer = np.eye(4)
|
answer = np.eye(4)
|
||||||
answer[:3, :3] = transform[:3, :3].T
|
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:
|
if "points_raw" not in data:
|
||||||
raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden")
|
raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden")
|
||||||
raw = np.asarray(data["points_raw"], dtype=np.float64)
|
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])
|
counter = int(np.ravel(data["frame_counter"])[0])
|
||||||
distance = raw[:, 0] * 0.001
|
distance = raw[:, 0] * 0.001
|
||||||
azimuth = np.deg2rad(raw[:, 1])
|
azimuth = np.deg2rad(raw[:, 1])
|
||||||
@@ -179,6 +194,63 @@ def make_o3d_cloud(points, voxel):
|
|||||||
return cloud.voxel_down_sample(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):
|
def align_open3d(target, source, initial, voxels, correspondences, iterations):
|
||||||
import open3d as o3d
|
import open3d as o3d
|
||||||
registration = o3d.pipelines.registration
|
registration = o3d.pipelines.registration
|
||||||
@@ -381,6 +453,8 @@ def cmd_pairs(args):
|
|||||||
reference_poses = np.asarray(reference_poses)
|
reference_poses = np.asarray(reference_poses)
|
||||||
split = [split_holdout(station[3], args.holdout_fraction, i)
|
split = [split_holdout(station[3], args.holdout_fraction, i)
|
||||||
for i, station in enumerate(stations)]
|
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)
|
rng = np.random.default_rng(args.seed)
|
||||||
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
|
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
|
||||||
accepted_transforms = {}
|
accepted_transforms = {}
|
||||||
@@ -389,9 +463,15 @@ def cmd_pairs(args):
|
|||||||
a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j]
|
a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j]
|
||||||
translation = float(np.linalg.norm(a_ij[:2, 3]))
|
translation = float(np.linalg.norm(a_ij[:2, 3]))
|
||||||
rotation = rotation_angle_deg(a_ij[:3, :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:
|
if translation < args.min_translation and rotation < args.min_rotation:
|
||||||
continue
|
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]
|
target_fit, target_holdout = split[i]
|
||||||
source_fit, source_holdout = split[j]
|
source_fit, source_holdout = split[j]
|
||||||
forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
|
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],
|
"frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1],
|
||||||
"rtk_translation_m": translation, "rtk_rotation_deg": rotation,
|
"rtk_translation_m": translation, "rtk_rotation_deg": rotation,
|
||||||
"nearest_rtk_dt_i_s": reference_dt[i], "nearest_rtk_dt_j_s": reference_dt[j],
|
"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(),
|
"B_ij_4x4": forward["transform"].tolist(),
|
||||||
"backend": args.backend, "backend_converged": forward["converged"],
|
"backend": args.backend, "backend_converged": forward["converged"],
|
||||||
"backend_iterations": forward["iterations"],
|
"backend_iterations": forward["iterations"],
|
||||||
@@ -482,7 +565,11 @@ def cmd_pairs(args):
|
|||||||
"backend": args.backend,
|
"backend": args.backend,
|
||||||
"transform_convention": "B_ij=T_Li_Lj maps station j points into station i",
|
"transform_convention": "B_ij=T_Li_Lj maps station j points into station i",
|
||||||
"raw_point_field": "points_raw",
|
"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),
|
"stations": len(stations), "candidate_pairs": len(reports),
|
||||||
"accepted_pairs": len(accepted_a),
|
"accepted_pairs": len(accepted_a),
|
||||||
"parameters": vars(args),
|
"parameters": vars(args),
|
||||||
@@ -585,9 +672,11 @@ def pair_metrics(a_array, b_array, x):
|
|||||||
|
|
||||||
def solve_extrinsic(a_array, b_array, planes, args):
|
def solve_extrinsic(a_array, b_array, planes, args):
|
||||||
rng = np.random.default_rng(args.seed)
|
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):
|
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),
|
rng.normal(0.0, args.start_translation_sigma, 3),
|
||||||
np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
|
np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
|
||||||
])
|
])
|
||||||
@@ -647,7 +736,10 @@ def cmd_calibrate(args):
|
|||||||
"message": best.message,
|
"message": best.message,
|
||||||
"convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame",
|
"convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame",
|
||||||
"equation": "A_ij X = X B_ij",
|
"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(),
|
"translation_m": x[:3, 3].tolist(),
|
||||||
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
|
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
|
||||||
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
|
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
|
||||||
@@ -707,7 +799,8 @@ def build_parser():
|
|||||||
ground = commands.add_parser("ground")
|
ground = commands.add_parser("ground")
|
||||||
ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True)
|
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("--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("--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("--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)
|
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("--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-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("--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-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("--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("--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("--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("--holdout-fraction", type=float, default=0.20)
|
||||||
pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08])
|
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])
|
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 = commands.add_parser("calibrate")
|
||||||
calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True)
|
calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True)
|
||||||
calibrate.add_argument("--output", 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("--translation-sigma", type=float, default=0.05)
|
||||||
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
|
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
|
||||||
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
|
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
|
||||||
|
|||||||
@@ -1,13 +0,0 @@
|
|||||||
# 数据说明
|
|
||||||
|
|
||||||
原始 H32/G90/N300 `.rscap`（以及旧版 LiDAR dlog）、逐帧 NPZ 和 prepared 点云体积较大，不进入 Git。请从项目云盘取得数据，并按根 README 中的目录示例放置；实际路径通过命令参数传入。
|
|
||||||
|
|
||||||
推荐原始布局：
|
|
||||||
|
|
||||||
```text
|
|
||||||
raw_dataset/
|
|
||||||
├── stations/<站号>/h32.rscap
|
|
||||||
└── captures/rtk.rscap, imu.rscap
|
|
||||||
```
|
|
||||||
|
|
||||||
公开数据包应同时提供：采集日期、车辆/传感器安装版本、站点数量、ANT1/ANT2 接线、rawHeading 方向、RTK 参考点离地高度及其测量方法。
|
|
||||||
+13
-3
@@ -1,5 +1,15 @@
|
|||||||
# results目录
|
# 当前车辆标定结果
|
||||||
|
|
||||||
`reference_data4/`是本仓库附带的精简参考结果。新的运行结果应写到仓库外目录或`outputs/`，不要覆盖参考结果。
|
本目录只保存当前车辆、当前传感器安装条件下的最终可交付结果；不保存历史车辆数据、原始采集包、点云帧或中间配准产物。
|
||||||
|
|
||||||
参考结果保留最终矩阵、共识B、两后端精筛B、逐对CSV/筛选审计和地面平面；未保留原始点云、逐帧combined数据、冗长的初筛JSON和带本机绝对路径的过程文件。
|
## 2026-08 车辆 / 27 个静止站点
|
||||||
|
|
||||||
|
目录 [`vehicle_20260808/`](vehicle_20260808/) 对应本机运行目录 `D:\data\rtk_lidar_run\outputs_vehicle_h19165`。
|
||||||
|
|
||||||
|
- 外参：`final_T_RTK_lidar.json`
|
||||||
|
- 质量摘要：`summary.json`
|
||||||
|
- 坐标约定：`p_RTK = T_RTK_lidar · p_lidar`，RTK 为车头向前坐标系（`HeadingOffsetDeg=-90`）。
|
||||||
|
- RTK 参考点高度：1.9165 m（ANT1 相位中心）。
|
||||||
|
- 质量：27 个站点、20 个共识运动对、平移 RMS 0.07116 m、旋转 RMS 0.98209°。
|
||||||
|
|
||||||
|
这些文件记录的是 host 时间关联版本的现有最终结果。后续采用 RTK 测量时间重新导出后，应写入新的结果目录，不能覆盖本目录。
|
||||||
|
|||||||
@@ -1,45 +0,0 @@
|
|||||||
{
|
|
||||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame: p_RTK = T_RTK_lidar * p_lidar",
|
|
||||||
"translation_m": [
|
|
||||||
1.6381793500373911,
|
|
||||||
-0.24084479868828831,
|
|
||||||
0.08448123595331278
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.8171674587248069,
|
|
||||||
1.323288118779805,
|
|
||||||
-22.104163317857477
|
|
||||||
],
|
|
||||||
"quaternion_xyzw": [
|
|
||||||
-0.004784711987091957,
|
|
||||||
0.012700096588977744,
|
|
||||||
-0.19160273666049008,
|
|
||||||
0.9813787267829084
|
|
||||||
],
|
|
||||||
"matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.926254197701683,
|
|
||||||
0.37594816689521215,
|
|
||||||
0.026760737062740007,
|
|
||||||
1.6381793500373911
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.3761912321127582,
|
|
||||||
0.926530995670823,
|
|
||||||
0.004524482591229066,
|
|
||||||
-0.24084479868828831
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.02309368141930373,
|
|
||||||
-0.014257975640431828,
|
|
||||||
0.9996316281556624,
|
|
||||||
0.08448123595331278
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
]
|
|
||||||
}
|
|
||||||
@@ -1,23 +0,0 @@
|
|||||||
# data4参考结果
|
|
||||||
|
|
||||||
推荐下游只读取`final_T_RTK_lidar.json`，其方向为：
|
|
||||||
|
|
||||||
```text
|
|
||||||
p_RTK = T_RTK_lidar · p_lidar
|
|
||||||
```
|
|
||||||
|
|
||||||
```text
|
|
||||||
translation_m = [1.638179350, -0.240844799, 0.084481236]
|
|
||||||
RPY_deg_xyz = [-0.817167459, 1.323288119, -22.104163318]
|
|
||||||
```
|
|
||||||
|
|
||||||
| 路径 | 内容 |
|
|
||||||
|---|---|
|
|
||||||
| `final_T_RTK_lidar.json` | 唯一推荐使用的最终外参 |
|
|
||||||
| `summary.json` | 最终残差、条件数和两后端差异摘要 |
|
|
||||||
| `common/ground_planes.csv` | 34站地面RANSAC平面 |
|
|
||||||
| `open3d_gicp/` | Open3D精筛B、精筛审计、逐对质量CSV和独立X |
|
|
||||||
| `small_gicp/` | small_gicp对应产物 |
|
|
||||||
| `consensus/` | 两后端共同认可的25对B、共识审计和最终X原始求解记录 |
|
|
||||||
|
|
||||||
`z=0.084481 m`依赖RTK参考点离地`0.8535 m`，不是平面AX=XB独立观测值。当前AX RMS约`0.100207 m / 1.252794°`，结果适合算法联调和继续验证，不应据此单独宣称逐帧GT达到±3 cm。
|
|
||||||
@@ -1,35 +0,0 @@
|
|||||||
time,nx,ny,nz,d,inliers,rms_m,frame_counter
|
|
||||||
1784783825.357129,-0.0071009788712051635,-0.01614775434066578,0.9998444009588814,0.9449714797885561,2216,0.01353681235386204,382
|
|
||||||
1784783905.353819,0.0037577953662433442,-0.00645689216229629,0.999972093369405,0.9404093678203617,1992,0.01182484680356077,1182
|
|
||||||
1784783971.0503054,-0.021571557237587136,-0.004187980739348835,0.9997585352152151,0.9464426916222599,1922,0.01241302113439391,1839
|
|
||||||
1784784059.7468228,-0.02328381206588687,0.004386003115594592,0.999719274132669,0.9506398743758987,1825,0.013207042662497559,2726
|
|
||||||
1784784149.2434597,-0.03034651356298797,8.407602826756324e-05,0.9995394349628197,0.9287260086761917,1631,0.012431422856007433,3621
|
|
||||||
1784784224.2408776,-0.02735108660242708,0.010336463557273514,0.9995724463903534,0.8697042041945898,1745,0.012010699567274814,4371
|
|
||||||
1784784301.6372502,-0.008085199791897242,-0.01261499998640764,0.9998877393586082,0.9555128377061561,2057,0.011385954851802133,5145
|
|
||||||
1784784387.733771,-0.007167201772462397,-0.008280272080485561,0.9999400323584541,0.9272403036781977,2190,0.012588169113362788,6006
|
|
||||||
1784784474.9314597,-0.015033673776128801,-0.05115510005163043,0.9985775605287256,0.9072885818985176,1852,0.011643717831315507,6878
|
|
||||||
1784784549.4274275,-0.03189811408163763,0.004490776571011993,0.999481036960594,0.9463330979345341,1638,0.013973864979699106,7623
|
|
||||||
1784784614.7244046,-0.027669126419476022,-0.013618714393779237,0.999524361914928,0.9348654978703125,1794,0.011102769699524444,8276
|
|
||||||
1784784682.921899,-0.022151829941254066,-0.026156017173182243,0.9994124069651578,0.9227652769218206,1638,0.01266204532799032,8958
|
|
||||||
1784784758.8187964,-0.022601789108727538,-0.030665670278286643,0.999274124450077,0.9305425654631599,1795,0.013340493954266352,9717
|
|
||||||
1784784836.0155501,-0.017598498968453644,-0.02647415434199472,0.999494578267403,0.961769336191532,2015,0.01382548272951725,10489
|
|
||||||
1784784921.1126208,-0.021874728045639568,-0.019005922983034846,0.9995800474021539,0.9186945373736978,1808,0.01319739563436461,11340
|
|
||||||
1784784992.709947,-0.019622211270580107,-0.02528822634559132,0.9994876059427386,0.9414876680920201,2158,0.013280256398986076,12056
|
|
||||||
1784785067.6067727,-0.031060743296391496,-0.010214597374617485,0.9994653031628212,0.9402571806911639,1758,0.013392641298608525,12805
|
|
||||||
1784785215.9006598,-0.023673459396853343,-0.027330465291762113,0.9993460926961797,0.9143984233881569,2134,0.012198902426752438,14288
|
|
||||||
1784785296.4990919,-0.013579953767407775,-0.025158411654770292,0.9995912360453568,0.929929365058787,2445,0.012790980094197171,15094
|
|
||||||
1784785363.1952267,-0.0316919344947604,-0.008177115456525313,0.9994642345130668,0.9579125765731408,1861,0.012743464679231025,15761
|
|
||||||
1784785434.592462,-0.022678088603046032,0.004435617789851151,0.9997329791459992,0.9537383917106543,1853,0.013464094518301148,16475
|
|
||||||
1784785506.389296,-0.0273694753756875,-0.01771941378886425,0.9994683257575693,0.9327361226688458,1749,0.011502338837958854,17193
|
|
||||||
1784785587.5863533,-0.034617559115875766,0.0015843237885758451,0.999399376885433,0.9191959537091571,1744,0.013401267879037225,18005
|
|
||||||
1784785681.9825997,-0.033298152875437845,-0.018946770164947627,0.9992658569747096,0.9446967789786688,2037,0.012804059875312601,18949
|
|
||||||
1784785815.4779446,-0.006681441650905292,-0.023720354219030532,0.9996963054514052,0.964084392350492,2080,0.013159652224503205,20284
|
|
||||||
1784785891.9768085,-0.026044255070688936,0.004021590005713901,0.9996527014876911,0.934003424141447,1610,0.012619787010493816,21049
|
|
||||||
1784785967.8726046,-0.026983516555153,-0.01791249491380091,0.9994753785663161,0.9339463871372334,1482,0.013831424226303278,21808
|
|
||||||
1784786031.5701303,-0.028810092762610772,-0.015551490666087386,0.9994639211562729,0.938325903591574,1592,0.013415706854842701,22445
|
|
||||||
1784786087.9670725,-0.026424364888569394,-0.014575043111831797,0.9995445568150146,0.9345909622822591,1466,0.013090809334738121,23009
|
|
||||||
1784786160.8647907,-0.032665212336793446,0.0597663130328534,0.9976777895340014,1.0611447790248285,1511,0.012251618222434443,23738
|
|
||||||
1784786252.6621523,-0.03732336904542465,-0.020702346452411244,0.9990887743211128,0.9478808317179221,1719,0.012520822005912273,24656
|
|
||||||
1784786319.6581354,-0.025461816426307887,-0.02602901616210242,0.9993368732424047,0.9466783627035876,1589,0.013468160971926036,25326
|
|
||||||
1784786396.7558627,-0.025295174442520576,-0.02343833470627969,0.9994052224278793,0.9710592140122102,1321,0.012895976784738191,26097
|
|
||||||
1784786557.4492514,-0.014426534652625146,-0.00926546906583815,0.9998530022862894,0.935013905231254,1251,0.012477706451163199,27704
|
|
||||||
|
@@ -1,332 +0,0 @@
|
|||||||
{
|
|
||||||
"selection_is_X_independent": true,
|
|
||||||
"B_source": "Open3D; small_gicp is used only as an agreement gate",
|
|
||||||
"max_translation_m": 0.05,
|
|
||||||
"max_rotation_deg": 0.5,
|
|
||||||
"input_open3d_pairs": 41,
|
|
||||||
"accepted_pairs": 25,
|
|
||||||
"pairs": [
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 1,
|
|
||||||
"open3d_small_translation_m": 0.014276780914058016,
|
|
||||||
"open3d_small_rotation_deg": 0.6136066194510507,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 2,
|
|
||||||
"open3d_small_translation_m": 0.019952450418738,
|
|
||||||
"open3d_small_rotation_deg": 0.14609270025586996,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 2,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 3,
|
|
||||||
"open3d_small_translation_m": 0.022407292900448784,
|
|
||||||
"open3d_small_rotation_deg": 0.1674801169908669,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.006161707315193906,
|
|
||||||
"open3d_small_rotation_deg": 0.13760639079130288,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.039798937040509075,
|
|
||||||
"open3d_small_rotation_deg": 0.21317157512260662,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 6,
|
|
||||||
"open3d_small_translation_m": 0.012411893826145848,
|
|
||||||
"open3d_small_rotation_deg": 0.6409039547122766,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 7,
|
|
||||||
"open3d_small_translation_m": 0.008481323658875535,
|
|
||||||
"open3d_small_rotation_deg": 0.21102463812659789,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 8,
|
|
||||||
"open3d_small_translation_m": 0.0392892670228761,
|
|
||||||
"open3d_small_rotation_deg": 0.3638880951335145,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 7,
|
|
||||||
"j": 8,
|
|
||||||
"open3d_small_translation_m": 0.008121615288997118,
|
|
||||||
"open3d_small_rotation_deg": 0.6269514061788293,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 10,
|
|
||||||
"j": 11,
|
|
||||||
"open3d_small_translation_m": 0.021311366483595485,
|
|
||||||
"open3d_small_rotation_deg": 0.5895827931090589,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 14,
|
|
||||||
"open3d_small_translation_m": 0.02159358315642783,
|
|
||||||
"open3d_small_rotation_deg": 0.2828054398005336,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 15,
|
|
||||||
"open3d_small_translation_m": 0.036801934207227605,
|
|
||||||
"open3d_small_rotation_deg": 0.5608491395108458,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 15,
|
|
||||||
"open3d_small_translation_m": 0.02340982602704092,
|
|
||||||
"open3d_small_rotation_deg": 0.5560478634176542,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 16,
|
|
||||||
"open3d_small_translation_m": 0.027602744462629097,
|
|
||||||
"open3d_small_rotation_deg": 0.2529072067309812,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 15,
|
|
||||||
"j": 16,
|
|
||||||
"open3d_small_translation_m": 0.02090824470779122,
|
|
||||||
"open3d_small_rotation_deg": 0.034004621265071464,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 15,
|
|
||||||
"j": 17,
|
|
||||||
"open3d_small_translation_m": 0.04186146133502706,
|
|
||||||
"open3d_small_rotation_deg": 0.13358624916951445,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 15,
|
|
||||||
"j": 18,
|
|
||||||
"open3d_small_translation_m": 0.01461017711105615,
|
|
||||||
"open3d_small_rotation_deg": 0.2537851025085552,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 16,
|
|
||||||
"j": 19,
|
|
||||||
"open3d_small_translation_m": 0.012703728170379172,
|
|
||||||
"open3d_small_rotation_deg": 0.36607447371235324,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 17,
|
|
||||||
"j": 18,
|
|
||||||
"open3d_small_translation_m": 0.030547382773647488,
|
|
||||||
"open3d_small_rotation_deg": 0.6293030402470645,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 22,
|
|
||||||
"open3d_small_translation_m": 0.008438605953756149,
|
|
||||||
"open3d_small_rotation_deg": 0.15351222882817242,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 23,
|
|
||||||
"open3d_small_translation_m": 0.0184790436482599,
|
|
||||||
"open3d_small_rotation_deg": 0.5781132894399331,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 24,
|
|
||||||
"open3d_small_translation_m": 0.03128464242771278,
|
|
||||||
"open3d_small_rotation_deg": 0.3921882672789548,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 23,
|
|
||||||
"open3d_small_translation_m": 0.01156826970365892,
|
|
||||||
"open3d_small_rotation_deg": 0.061357836393243825,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 25,
|
|
||||||
"open3d_small_translation_m": 0.04101882834929893,
|
|
||||||
"open3d_small_rotation_deg": 0.420339642399271,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 23,
|
|
||||||
"j": 24,
|
|
||||||
"open3d_small_translation_m": 0.009191025382465435,
|
|
||||||
"open3d_small_rotation_deg": 0.5691342384946491,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 26,
|
|
||||||
"open3d_small_translation_m": 0.02091454005252894,
|
|
||||||
"open3d_small_rotation_deg": 0.4875072590330582,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 27,
|
|
||||||
"open3d_small_translation_m": 0.09632206662156642,
|
|
||||||
"open3d_small_rotation_deg": 0.3699938120965201,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 28,
|
|
||||||
"open3d_small_translation_m": 0.0301381128127144,
|
|
||||||
"open3d_small_rotation_deg": 0.46587172800425497,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 27,
|
|
||||||
"open3d_small_translation_m": 0.01010757207206556,
|
|
||||||
"open3d_small_rotation_deg": 0.4201365669869023,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 28,
|
|
||||||
"open3d_small_translation_m": 0.006243998078352694,
|
|
||||||
"open3d_small_rotation_deg": 1.3246136424341457,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 29,
|
|
||||||
"open3d_small_translation_m": 0.03617825286340023,
|
|
||||||
"open3d_small_rotation_deg": 0.27823666091247784,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 28,
|
|
||||||
"open3d_small_translation_m": 0.008540949499580083,
|
|
||||||
"open3d_small_rotation_deg": 0.3676810414714511,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 29,
|
|
||||||
"open3d_small_translation_m": 0.01399120240175791,
|
|
||||||
"open3d_small_rotation_deg": 0.437290577300882,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 28,
|
|
||||||
"j": 29,
|
|
||||||
"open3d_small_translation_m": 0.12711262123542025,
|
|
||||||
"open3d_small_rotation_deg": 0.9231252582756669,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 28,
|
|
||||||
"j": 30,
|
|
||||||
"open3d_small_translation_m": 0.030113115024945805,
|
|
||||||
"open3d_small_rotation_deg": 0.33608547109429165,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 28,
|
|
||||||
"j": 31,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 31,
|
|
||||||
"open3d_small_translation_m": 0.056474627862098614,
|
|
||||||
"open3d_small_rotation_deg": 0.6666837905088073,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 32,
|
|
||||||
"open3d_small_translation_m": 0.0152913302124184,
|
|
||||||
"open3d_small_rotation_deg": 0.3092824993876474,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 31,
|
|
||||||
"j": 32,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 31,
|
|
||||||
"j": 33,
|
|
||||||
"open3d_small_translation_m": 0.013286737983556427,
|
|
||||||
"open3d_small_rotation_deg": 0.37286489690414826,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
Binary file not shown.
@@ -1,266 +0,0 @@
|
|||||||
{
|
|
||||||
"schema_version": 1,
|
|
||||||
"success": true,
|
|
||||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
|
||||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
|
||||||
"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",
|
|
||||||
"y_axis": "left",
|
|
||||||
"z_axis": "up",
|
|
||||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
|
||||||
},
|
|
||||||
"LiDAR": "raw LiDAR sensor frame"
|
|
||||||
},
|
|
||||||
"backend": "consensus",
|
|
||||||
"measured_lidar_extrinsic_used_as_initial": false,
|
|
||||||
"body_heading_offset_used": false,
|
|
||||||
"body_antenna_lever_xy_used": false,
|
|
||||||
"translation_m": [
|
|
||||||
1.6381793500373911,
|
|
||||||
-0.24084479868828831,
|
|
||||||
0.08448123595331278
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.8171674587248069,
|
|
||||||
1.323288118779805,
|
|
||||||
-22.104163317857477
|
|
||||||
],
|
|
||||||
"quaternion_xyzw": [
|
|
||||||
-0.004784711987091957,
|
|
||||||
0.012700096588977744,
|
|
||||||
-0.19160273666049008,
|
|
||||||
0.9813787267829084
|
|
||||||
],
|
|
||||||
"matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.926254197701683,
|
|
||||||
0.37594816689521215,
|
|
||||||
0.026760737062740007,
|
|
||||||
1.6381793500373911
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.3761912321127582,
|
|
||||||
0.926530995670823,
|
|
||||||
0.004524482591229066,
|
|
||||||
-0.24084479868828831
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.02309368141930373,
|
|
||||||
-0.014257975640431828,
|
|
||||||
0.9996316281556624,
|
|
||||||
0.08448123595331278
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
],
|
|
||||||
"quality": {
|
|
||||||
"stations": 34,
|
|
||||||
"pairs": 25,
|
|
||||||
"residuals": {
|
|
||||||
"pairs": 25,
|
|
||||||
"translation_m": {
|
|
||||||
"rms": 0.10020667268070801,
|
|
||||||
"median": 0.06249704966098745,
|
|
||||||
"p90": 0.1210855900297508,
|
|
||||||
"p95": 0.12306740757559503,
|
|
||||||
"max": 0.35253378022021187
|
|
||||||
},
|
|
||||||
"rotation_deg": {
|
|
||||||
"rms": 1.2527941187072538,
|
|
||||||
"median": 0.7469173100535645,
|
|
||||||
"p90": 1.8215127226094046,
|
|
||||||
"p95": 1.973738680622839,
|
|
||||||
"max": 4.325671952515919
|
|
||||||
},
|
|
||||||
"per_pair": [
|
|
||||||
{
|
|
||||||
"pair_index": 0,
|
|
||||||
"translation_m": 0.04167906865173614,
|
|
||||||
"rotation_deg": 0.7044680397451787
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 1,
|
|
||||||
"translation_m": 0.05434432733915097,
|
|
||||||
"rotation_deg": 0.3949090486960162
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 2,
|
|
||||||
"translation_m": 0.10468985225969502,
|
|
||||||
"rotation_deg": 0.2966790038612149
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 3,
|
|
||||||
"translation_m": 0.064702341381345,
|
|
||||||
"rotation_deg": 0.5538785486937366
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 4,
|
|
||||||
"translation_m": 0.06403624975051968,
|
|
||||||
"rotation_deg": 0.7469173100535645
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 5,
|
|
||||||
"translation_m": 0.019589736031301614,
|
|
||||||
"rotation_deg": 0.7060778312055135
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 6,
|
|
||||||
"translation_m": 0.04563086081489213,
|
|
||||||
"rotation_deg": 0.8701331718844921
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 7,
|
|
||||||
"translation_m": 0.05643960299219297,
|
|
||||||
"rotation_deg": 1.8737737246943913
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 8,
|
|
||||||
"translation_m": 0.1234266519889254,
|
|
||||||
"rotation_deg": 1.9987299196049513
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 9,
|
|
||||||
"translation_m": 0.06753558655646176,
|
|
||||||
"rotation_deg": 0.2674085857580904
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 10,
|
|
||||||
"translation_m": 0.057937366822662255,
|
|
||||||
"rotation_deg": 0.339735283375759
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 11,
|
|
||||||
"translation_m": 0.03457457850935045,
|
|
||||||
"rotation_deg": 0.5158041285368985
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 12,
|
|
||||||
"translation_m": 0.06294442058649205,
|
|
||||||
"rotation_deg": 0.6210132605220049
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 13,
|
|
||||||
"translation_m": 0.04800822576662163,
|
|
||||||
"rotation_deg": 1.7431212194819241
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 14,
|
|
||||||
"translation_m": 0.06249704966098745,
|
|
||||||
"rotation_deg": 0.8651715760380532
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 15,
|
|
||||||
"translation_m": 0.12026833019096655,
|
|
||||||
"rotation_deg": 4.325671952515919
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 16,
|
|
||||||
"translation_m": 0.04916024916487916,
|
|
||||||
"rotation_deg": 0.9331159567430728
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 17,
|
|
||||||
"translation_m": 0.02802786916783814,
|
|
||||||
"rotation_deg": 0.8755591753060336
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 18,
|
|
||||||
"translation_m": 0.054001671602238746,
|
|
||||||
"rotation_deg": 0.48777111889991587
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 19,
|
|
||||||
"translation_m": 0.10122861599246656,
|
|
||||||
"rotation_deg": 0.9367994244405716
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 20,
|
|
||||||
"translation_m": 0.023164324685695653,
|
|
||||||
"rotation_deg": 0.2827650802657415
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 21,
|
|
||||||
"translation_m": 0.06461070522327692,
|
|
||||||
"rotation_deg": 1.1522606437088527
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 22,
|
|
||||||
"translation_m": 0.35253378022021187,
|
|
||||||
"rotation_deg": 0.7805430662091233
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 23,
|
|
||||||
"translation_m": 0.11095170748867093,
|
|
||||||
"rotation_deg": 0.8561868088481429
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 24,
|
|
||||||
"translation_m": 0.12163042992227363,
|
|
||||||
"rotation_deg": 0.17586872890763125
|
|
||||||
}
|
|
||||||
]
|
|
||||||
},
|
|
||||||
"weighted_jacobian_condition_number": 7.739413195936781,
|
|
||||||
"linearized_one_sigma": {
|
|
||||||
"translation_m": [
|
|
||||||
0.008936804232414386,
|
|
||||||
0.009199590403303341,
|
|
||||||
0.004827611183429192
|
|
||||||
],
|
|
||||||
"rotation_deg": [
|
|
||||||
0.08042474849565617,
|
|
||||||
0.07825800357808266,
|
|
||||||
0.1694976028248396
|
|
||||||
],
|
|
||||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
|
||||||
},
|
|
||||||
"bootstrap": {
|
|
||||||
"runs": 200,
|
|
||||||
"order": [
|
|
||||||
"x_m",
|
|
||||||
"y_m",
|
|
||||||
"z_m",
|
|
||||||
"roll_deg",
|
|
||||||
"pitch_deg",
|
|
||||||
"yaw_deg"
|
|
||||||
],
|
|
||||||
"std": [
|
|
||||||
0.004007472116212317,
|
|
||||||
0.00447369016783699,
|
|
||||||
0.003161844455893403,
|
|
||||||
0.10272261218646783,
|
|
||||||
0.09618742156951016,
|
|
||||||
0.14509879375374413
|
|
||||||
],
|
|
||||||
"p025": [
|
|
||||||
1.6309949623104336,
|
|
||||||
-0.2470615833911814,
|
|
||||||
0.07875765037498306,
|
|
||||||
-0.9881676010783537,
|
|
||||||
1.1135179371917805,
|
|
||||||
-22.3851229846878
|
|
||||||
],
|
|
||||||
"p975": [
|
|
||||||
1.6456999445482652,
|
|
||||||
-0.22784916633142768,
|
|
||||||
0.09114084095723367,
|
|
||||||
-0.5808877804701225,
|
|
||||||
1.4821798187931374,
|
|
||||||
-21.82296828752347
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"z_constraint": {
|
|
||||||
"observable_from_planar_AX_XB": false,
|
|
||||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
|
||||||
"rtk_reference_height_above_ground_m": 0.8535,
|
|
||||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
|
||||||
},
|
|
||||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification"
|
|
||||||
}
|
|
||||||
@@ -1,300 +0,0 @@
|
|||||||
{
|
|
||||||
"schema_version": 1,
|
|
||||||
"success": true,
|
|
||||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
|
||||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
|
||||||
"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",
|
|
||||||
"y_axis": "left",
|
|
||||||
"z_axis": "up",
|
|
||||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
|
||||||
},
|
|
||||||
"LiDAR": "raw LiDAR sensor frame"
|
|
||||||
},
|
|
||||||
"backend": "consensus",
|
|
||||||
"measured_lidar_extrinsic_used_as_initial": false,
|
|
||||||
"body_heading_offset_used": false,
|
|
||||||
"body_antenna_lever_xy_used": false,
|
|
||||||
"translation_m": [
|
|
||||||
1.6381793500373911,
|
|
||||||
-0.24084479868828831,
|
|
||||||
0.08448123595331278
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.8171674587248069,
|
|
||||||
1.323288118779805,
|
|
||||||
-22.104163317857477
|
|
||||||
],
|
|
||||||
"quaternion_xyzw": [
|
|
||||||
-0.004784711987091957,
|
|
||||||
0.012700096588977744,
|
|
||||||
-0.19160273666049008,
|
|
||||||
0.9813787267829084
|
|
||||||
],
|
|
||||||
"matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.926254197701683,
|
|
||||||
0.37594816689521215,
|
|
||||||
0.026760737062740007,
|
|
||||||
1.6381793500373911
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.3761912321127582,
|
|
||||||
0.926530995670823,
|
|
||||||
0.004524482591229066,
|
|
||||||
-0.24084479868828831
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.02309368141930373,
|
|
||||||
-0.014257975640431828,
|
|
||||||
0.9996316281556624,
|
|
||||||
0.08448123595331278
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
],
|
|
||||||
"quality": {
|
|
||||||
"stations": 34,
|
|
||||||
"pairs": 25,
|
|
||||||
"residuals": {
|
|
||||||
"pairs": 25,
|
|
||||||
"translation_m": {
|
|
||||||
"rms": 0.10020667268070801,
|
|
||||||
"median": 0.06249704966098745,
|
|
||||||
"p90": 0.1210855900297508,
|
|
||||||
"p95": 0.12306740757559503,
|
|
||||||
"max": 0.35253378022021187
|
|
||||||
},
|
|
||||||
"rotation_deg": {
|
|
||||||
"rms": 1.2527941187072538,
|
|
||||||
"median": 0.7469173100535645,
|
|
||||||
"p90": 1.8215127226094046,
|
|
||||||
"p95": 1.973738680622839,
|
|
||||||
"max": 4.325671952515919
|
|
||||||
},
|
|
||||||
"per_pair": [
|
|
||||||
{
|
|
||||||
"pair_index": 0,
|
|
||||||
"translation_m": 0.04167906865173614,
|
|
||||||
"rotation_deg": 0.7044680397451787
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 1,
|
|
||||||
"translation_m": 0.05434432733915097,
|
|
||||||
"rotation_deg": 0.3949090486960162
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 2,
|
|
||||||
"translation_m": 0.10468985225969502,
|
|
||||||
"rotation_deg": 0.2966790038612149
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 3,
|
|
||||||
"translation_m": 0.064702341381345,
|
|
||||||
"rotation_deg": 0.5538785486937366
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 4,
|
|
||||||
"translation_m": 0.06403624975051968,
|
|
||||||
"rotation_deg": 0.7469173100535645
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 5,
|
|
||||||
"translation_m": 0.019589736031301614,
|
|
||||||
"rotation_deg": 0.7060778312055135
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 6,
|
|
||||||
"translation_m": 0.04563086081489213,
|
|
||||||
"rotation_deg": 0.8701331718844921
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 7,
|
|
||||||
"translation_m": 0.05643960299219297,
|
|
||||||
"rotation_deg": 1.8737737246943913
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 8,
|
|
||||||
"translation_m": 0.1234266519889254,
|
|
||||||
"rotation_deg": 1.9987299196049513
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 9,
|
|
||||||
"translation_m": 0.06753558655646176,
|
|
||||||
"rotation_deg": 0.2674085857580904
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 10,
|
|
||||||
"translation_m": 0.057937366822662255,
|
|
||||||
"rotation_deg": 0.339735283375759
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 11,
|
|
||||||
"translation_m": 0.03457457850935045,
|
|
||||||
"rotation_deg": 0.5158041285368985
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 12,
|
|
||||||
"translation_m": 0.06294442058649205,
|
|
||||||
"rotation_deg": 0.6210132605220049
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 13,
|
|
||||||
"translation_m": 0.04800822576662163,
|
|
||||||
"rotation_deg": 1.7431212194819241
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 14,
|
|
||||||
"translation_m": 0.06249704966098745,
|
|
||||||
"rotation_deg": 0.8651715760380532
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 15,
|
|
||||||
"translation_m": 0.12026833019096655,
|
|
||||||
"rotation_deg": 4.325671952515919
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 16,
|
|
||||||
"translation_m": 0.04916024916487916,
|
|
||||||
"rotation_deg": 0.9331159567430728
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 17,
|
|
||||||
"translation_m": 0.02802786916783814,
|
|
||||||
"rotation_deg": 0.8755591753060336
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 18,
|
|
||||||
"translation_m": 0.054001671602238746,
|
|
||||||
"rotation_deg": 0.48777111889991587
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 19,
|
|
||||||
"translation_m": 0.10122861599246656,
|
|
||||||
"rotation_deg": 0.9367994244405716
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 20,
|
|
||||||
"translation_m": 0.023164324685695653,
|
|
||||||
"rotation_deg": 0.2827650802657415
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 21,
|
|
||||||
"translation_m": 0.06461070522327692,
|
|
||||||
"rotation_deg": 1.1522606437088527
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 22,
|
|
||||||
"translation_m": 0.35253378022021187,
|
|
||||||
"rotation_deg": 0.7805430662091233
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 23,
|
|
||||||
"translation_m": 0.11095170748867093,
|
|
||||||
"rotation_deg": 0.8561868088481429
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 24,
|
|
||||||
"translation_m": 0.12163042992227363,
|
|
||||||
"rotation_deg": 0.17586872890763125
|
|
||||||
}
|
|
||||||
]
|
|
||||||
},
|
|
||||||
"weighted_jacobian_condition_number": 7.739413195936781,
|
|
||||||
"linearized_one_sigma": {
|
|
||||||
"translation_m": [
|
|
||||||
0.008936804232414386,
|
|
||||||
0.009199590403303341,
|
|
||||||
0.004827611183429192
|
|
||||||
],
|
|
||||||
"rotation_deg": [
|
|
||||||
0.08042474849565617,
|
|
||||||
0.07825800357808266,
|
|
||||||
0.1694976028248396
|
|
||||||
],
|
|
||||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
|
||||||
},
|
|
||||||
"bootstrap": {
|
|
||||||
"runs": 200,
|
|
||||||
"order": [
|
|
||||||
"x_m",
|
|
||||||
"y_m",
|
|
||||||
"z_m",
|
|
||||||
"roll_deg",
|
|
||||||
"pitch_deg",
|
|
||||||
"yaw_deg"
|
|
||||||
],
|
|
||||||
"std": [
|
|
||||||
0.004007472116212317,
|
|
||||||
0.00447369016783699,
|
|
||||||
0.003161844455893403,
|
|
||||||
0.10272261218646783,
|
|
||||||
0.09618742156951016,
|
|
||||||
0.14509879375374413
|
|
||||||
],
|
|
||||||
"p025": [
|
|
||||||
1.6309949623104336,
|
|
||||||
-0.2470615833911814,
|
|
||||||
0.07875765037498306,
|
|
||||||
-0.9881676010783537,
|
|
||||||
1.1135179371917805,
|
|
||||||
-22.3851229846878
|
|
||||||
],
|
|
||||||
"p975": [
|
|
||||||
1.6456999445482652,
|
|
||||||
-0.22784916633142768,
|
|
||||||
0.09114084095723367,
|
|
||||||
-0.5808877804701225,
|
|
||||||
1.4821798187931374,
|
|
||||||
-21.82296828752347
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"z_constraint": {
|
|
||||||
"observable_from_planar_AX_XB": false,
|
|
||||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
|
||||||
"rtk_reference_height_above_ground_m": 0.8535,
|
|
||||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
|
||||||
},
|
|
||||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
|
|
||||||
"selection": {
|
|
||||||
"recommended": true,
|
|
||||||
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
|
||||||
"open3d_vs_small_gicp": {
|
|
||||||
"translation_m": 0.003889255293759414,
|
|
||||||
"rotation_deg": 0.1884307130161592,
|
|
||||||
"delta_matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.9999968771376496,
|
|
||||||
0.0024968749848742764,
|
|
||||||
-0.00010644368722136346,
|
|
||||||
0.0008526003522697501
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.0024970968314049877,
|
|
||||||
0.9999945974960792,
|
|
||||||
-0.0021376356258303525,
|
|
||||||
-0.003658904827736509
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.00010110570323801577,
|
|
||||||
0.0021378947504826257,
|
|
||||||
0.9999977095892133,
|
|
||||||
0.0010058801324768218
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,97 +0,0 @@
|
|||||||
i,j,rtk_translation_m,rtk_rotation_deg,heldout_inlier_ratio,heldout_inlier_rmse_m,hessian_rank,hessian_condition,reverse_translation_m,reverse_rotation_deg,multistart_success_rate,accepted,rejection_reasons
|
|
||||||
0,1,1.6721594124489447,24.171297449440814,0.8061657032755298,0.10961296014103396,6,2.7038608113687213,0.004225163540003575,0.1530449067720668,1.0,True,
|
|
||||||
0,2,2.0412175279332088,80.09074797031303,0.7489394523717702,0.116305716193008,6,3.0720058333957327,0.02073003109723684,0.12418344306819311,1.0,True,
|
|
||||||
0,3,6.30529961936688,79.9158388329924,0.6310283235519265,0.12470979645173097,6,5.235632990817998,0.02154775870989652,0.25399044979598373,1.0,True,
|
|
||||||
1,2,1.2843386040405174,55.9194505208722,0.7867383512544803,0.11079021393934946,6,3.282529873989144,0.00768232673944143,0.0508043300468843,1.0,True,
|
|
||||||
1,3,5.433888607887495,55.744541383551606,0.6794562317367552,0.11907169138096609,6,4.183386002322132,0.024832409186708038,0.29364088503444125,1.0,True,
|
|
||||||
1,4,1.5299424710613851,106.08652205569952,0.6786112833230006,0.1125501582315264,6,3.2941312581877567,0.0077657602443488094,0.07274574571529673,1.0,True,
|
|
||||||
2,3,4.340252832276203,0.1749091373206093,0.7586776859504132,0.11541610277862546,6,3.730803369806122,0.007879904085790266,0.11659483159927261,1.0,True,
|
|
||||||
2,4,0.2520257253555564,50.1670715348273,0.7854572527608884,0.1098649389241602,6,2.641287751481567,0.017042953274276868,0.09383247792992644,1.0,True,
|
|
||||||
2,5,5.8286926576588955,8.735318060700322,0.7074574574574575,0.11912871456224486,6,3.8527093190832513,0.016640250279170064,0.24773491621516538,1.0,True,
|
|
||||||
3,4,4.1070657333447205,50.34198067214791,0.6974624291697462,0.11727630888949149,6,3.5010457716923216,0.0121487212194689,0.20793086843827258,1.0,True,
|
|
||||||
3,5,2.2886212019715484,8.910227198020936,0.7962985964476462,0.10646082199215787,6,3.037745853837991,0.011008298723512349,0.0821707761041294,1.0,True,
|
|
||||||
3,6,2.618668779147775,47.63555775102663,0.8376509054325956,0.10956583416752531,6,3.1930663579156175,0.001222821038315667,0.092075215117626,1.0,True,
|
|
||||||
4,5,5.5767898078953735,41.431753474126985,0.6652516676773802,0.12322094568315027,6,4.985227704290953,0.005399786589871835,0.13893507775898076,0.0,False,multistart_instability
|
|
||||||
4,6,6.68811709459501,97.97753842317455,0.04910385465259023,0.15664415071522125,6,4.835889397197473,2.574198069897065,12.647326063758534,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
4,7,6.153247042777442,123.28910472998353,0.020756115641215715,0.16997351387143192,6,17.054474046975617,5.52282870653845,6.186988956437115,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
5,6,2.0562758992403367,56.545784949047565,0.7526921648718901,0.10924852668609375,6,3.289802074469562,0.012374747586500019,0.14461856068328793,1.0,True,
|
|
||||||
5,7,0.5812996709001959,81.85735125585653,0.7833561729164071,0.11683079277948678,6,2.8807765869032624,0.013689874812447942,0.20006562776472953,1.0,True,
|
|
||||||
5,8,2.6887856969568644,172.47951556359513,0.3979730564825114,0.13180114830799514,6,4.4867592164383865,2.902311115345869,2.1073169352638135,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
6,7,1.9723544820714844,25.311566306808967,0.8497729566094854,0.10415909908074775,6,3.3759361297847534,0.008986805974950147,0.13099479506412062,1.0,True,
|
|
||||||
6,8,0.7288530799256238,115.93373061454484,0.7678928928928929,0.1116672507978127,6,3.260920573387359,0.010085391730567652,0.1175283699615622,1.0,True,
|
|
||||||
6,9,7.898758206937296,103.90638727582184,0.6071384156199477,0.12594282886521943,6,6.882498483502542,2.7065494720876333,9.167174886516003,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
7,8,2.6747845281541447,90.62216430773587,0.8299748110831234,0.10748525688830211,6,3.0071179226782405,0.00710646197885058,0.1069872847368705,1.0,True,
|
|
||||||
7,9,8.06955581661561,78.59482096901287,0.6188509200150206,0.12713205078393502,6,6.716979637303372,6.104542218165768,6.596313811797356,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
7,10,8.088675434881791,134.6003195530505,0.04729478766868887,0.16509079956796627,6,8.903210909129099,5.698690438550839,24.43813017288333,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
8,9,7.73033999229505,12.027343338722995,0.6326834719980131,0.12084265385763364,6,5.53968988049318,2.4349995045990127,17.158034002952295,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
8,10,8.378411682322204,43.97815524531458,0.6163861933423412,0.1293602846396598,6,4.789005902863083,0.011327540721531722,0.17059480981566605,0.0,False,multistart_instability
|
|
||||||
8,11,11.214115106391473,22.60670813095403,0.035782503501846426,0.17440177580299876,6,9.744579276034784,1.861364410474017,5.099464012971126,1.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
9,10,1.9264207261313253,56.00549858403757,0.6543345543345543,0.10729272360686744,6,3.3788563349731624,1.6360232143180424,5.55466982065639,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
9,11,4.747620352849464,34.63405146967702,0.5818780055682106,0.1171768781510077,6,3.555986532172078,0.00981302583568424,0.06035460198954135,1.0,True,
|
|
||||||
9,12,7.208566899019793,16.356227217122623,0.5379123584441162,0.12645695585785705,6,4.882558096197047,0.008862930161051686,0.11566409877698092,1.0,True,
|
|
||||||
10,11,3.0731501091601263,21.371447114360553,0.7118898623279099,0.1192865608074921,6,3.220203471557625,0.0029529340190141227,0.004176908093440082,1.0,True,
|
|
||||||
10,12,6.011368280631378,39.649271366914945,0.6120311738918656,0.12525652128126136,6,4.691686416856208,0.005686910809247203,0.10204044727299268,1.0,True,
|
|
||||||
10,13,9.76425648692991,72.41150002956132,0.516551290119572,0.1354903305714048,6,7.346543684414896,0.013565042721589003,0.32552304340992394,1.0,True,
|
|
||||||
11,12,3.3258193587172027,18.277824252554396,0.650555275113579,0.12104882943540898,6,4.612247786063477,0.003583199374507776,0.03300207707174117,1.0,True,
|
|
||||||
11,13,7.195203402213438,51.04005291520078,0.5698054068172914,0.1285866274322162,6,7.539783468898048,0.016001435752898144,0.11059625579950419,1.0,True,
|
|
||||||
11,14,3.634158560526847,20.548768889074672,0.6420881321982974,0.12414062948335593,6,4.952265047266808,0.012369101516230316,0.01870506040605898,1.0,True,
|
|
||||||
12,13,3.8697507070400543,32.762228662646386,0.6972966112450819,0.1168089621311632,6,4.28224799374136,0.01141023876783206,0.04779713993428384,1.0,True,
|
|
||||||
12,14,0.9871080784265185,2.2709446365202766,0.8749086479902558,0.09521299965540625,6,3.309695139564422,0.006159472215773367,0.014929948455569534,1.0,True,
|
|
||||||
12,15,4.171948395051693,25.86371030092923,0.7022030893897189,0.11797995277580106,6,4.277232786772136,0.008495008365046321,0.10278299144703042,1.0,True,
|
|
||||||
13,14,3.7992314627329202,30.491284026126113,0.6955810147299509,0.11455956122705321,6,3.350289886810734,0.01022866463344607,0.03515966054944392,1.0,True,
|
|
||||||
13,15,0.9105848166450461,6.898518361717151,0.868300353819945,0.1045421356808481,6,3.2437357133954023,0.002375025382773949,0.01072504764419793,1.0,True,
|
|
||||||
13,16,3.4957081323467,18.94489979461447,0.7265456392027422,0.1096252966406241,6,3.5227514244456217,0.008958927594996346,0.03041438512426757,1.0,True,
|
|
||||||
14,15,3.8816793634199405,23.592765664408958,0.7120070334086913,0.11868441290330703,6,4.62059246950246,0.002571958018980028,0.055069197511519646,1.0,True,
|
|
||||||
14,16,7.29101265002769,49.43618382074057,0.5918615984405458,0.12229328437386515,6,7.149509813179227,0.014273957859025563,0.25325650727957555,1.0,True,
|
|
||||||
14,17,5.915950814087913,0.8461207481731609,0.6694009445687298,0.12443900216431925,6,5.1577414296960065,0.0178952010004604,0.10920228290609924,1.0,True,
|
|
||||||
15,16,3.579287497246505,25.843418156331627,0.702887537993921,0.11495230769293859,6,3.5402899763525375,0.013545291843396808,0.03346625178333291,1.0,True,
|
|
||||||
15,17,2.2400625117908257,24.438886412582114,0.7429531936901991,0.11679524427533863,6,3.5266643942801554,0.009894110027911674,0.07786907370565768,1.0,True,
|
|
||||||
15,18,4.6956742726068,3.452521908779405,0.7209645010046886,0.11716134583909138,6,4.125231895423439,0.01165472931184747,0.13683586564190106,1.0,True,
|
|
||||||
16,17,2.956793995513645,50.28230456891372,0.618922305764411,0.11254196340940027,6,4.068632188828398,0.031047860213503222,0.10375098145236057,1.0,True,
|
|
||||||
16,18,3.369822545690391,22.390896247552213,0.6890156918687589,0.11084024896736888,6,4.421167108842175,0.01556225520373068,0.02495881796885156,1.0,True,
|
|
||||||
16,19,2.313038703742191,30.035090485266096,0.8685060899826,0.10135543575024479,6,2.9200722330018793,0.0029522513838272538,0.02753369989558306,1.0,True,
|
|
||||||
17,18,2.517968959880004,27.89140832136152,0.7697708305735859,0.10648049893472189,6,3.792822356453168,0.010582276181446961,0.03959905194910384,1.0,True,
|
|
||||||
17,19,3.518310045065406,80.31739505417983,0.6293759512937596,0.10954497717502036,6,3.648306929393189,0.012240937390578075,0.06030618467886912,1.0,True,
|
|
||||||
17,20,3.4199679241812992,152.98392843416642,0.6014520938674964,0.12300562605352787,6,4.719447385686111,0.03231013197809958,0.12856862709639913,0.0,False,multistart_instability
|
|
||||||
18,19,2.0416624616211574,52.42598673281832,0.6827314510833881,0.10834806615100022,6,3.5073857685652805,0.012418496053909043,0.11905018881087433,1.0,True,
|
|
||||||
18,20,5.836864764777489,125.0925201128049,0.5756313809779688,0.1265759478434111,6,5.389095141151799,0.012917057356044254,0.2404237301134517,0.0,False,multistart_instability
|
|
||||||
18,21,10.84206419743439,175.70238585457497,0.2352252017703723,0.15182541744787395,6,9.65894418804011,0.2039286327273425,0.13690658217533308,0.0,False,heldout_inlier_ratio;forward_reverse_translation;multistart_instability
|
|
||||||
19,20,6.120105723142265,72.6665333799865,0.6234734541714874,0.1277530580405749,6,5.958603794025071,0.009237066767190974,0.22540488888103255,1.0,True,
|
|
||||||
19,21,11.201089261967727,123.27639912174082,0.3788200074840963,0.1468780339612994,6,10.170069582593085,4.607025297377655,2.696881779819613,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
19,22,13.962230939038237,128.85294276465584,0.17798277982779828,0.15985830060250797,6,11.806596815871064,2.332292389085354,2.4044448240559984,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
20,21,5.091648376409225,50.60986574175432,0.6596992097884272,0.12147955429086157,6,4.082032735955382,0.014725537493639118,0.19884224722867958,1.0,True,
|
|
||||||
20,22,7.842914217245693,56.186409384669375,0.5739414499308958,0.13255415786946775,6,5.3284863413522086,0.006040617548523686,0.19706632354686843,1.0,True,
|
|
||||||
20,23,4.953146569484805,70.79178325235414,0.6456945156330087,0.12075756038041646,6,3.9642702950866346,0.02294901101348451,0.19112109479244785,1.0,True,
|
|
||||||
21,22,2.836151129858731,5.576543642915048,0.7716237647919971,0.1163227135854156,6,2.7962015303291894,0.009963578798274064,0.1533289219532281,1.0,True,
|
|
||||||
21,23,1.4635995041292513,20.181917510599828,0.8576224819696593,0.09865676260631218,6,3.036795069384516,0.00392257332509571,0.00898487649366286,1.0,True,
|
|
||||||
21,24,2.7001854883863183,54.59467208208592,0.7715940569126165,0.11361504553552869,6,2.7681582493333527,0.007475673605592584,0.04227769001294981,1.0,True,
|
|
||||||
22,23,3.806551883906871,14.605373867684776,0.7396689147762109,0.11702962848624102,6,3.414804725088913,0.018140159434305195,0.1184732181610567,1.0,True,
|
|
||||||
22,24,4.999470711928798,49.01812843917085,0.6983240223463687,0.11898873157361631,6,3.633465593686572,1.9214516862996405,12.118772315210817,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
22,25,2.281002791409386,12.391903814042275,0.736861094407697,0.1158639471011007,6,2.383007054117406,0.01855955252477569,0.06452602797902846,1.0,True,
|
|
||||||
23,24,1.2663665558774873,34.41275457148608,0.7853164556962026,0.11287254423109305,6,2.4099218288471635,0.002195759849349955,0.03211295915781923,1.0,True,
|
|
||||||
23,25,5.050865141821003,2.213470053642503,0.6881127450980392,0.12303206700403985,6,2.936372721803798,0.004939188019097873,0.12964064637099942,1.0,True,
|
|
||||||
23,26,5.595299803053147,40.730927532824325,0.6852618757612667,0.12040544972155913,6,3.059006509397719,0.006032445250251169,0.14039335223022864,1.0,True,
|
|
||||||
24,25,6.316196707646632,36.626224625128586,0.677667493796526,0.1234631580581415,6,3.6286524357748364,0.023479226891170584,0.16273859629355136,1.0,True,
|
|
||||||
24,26,6.840027061556236,6.318172961338242,0.6530209617755857,0.12710147612984235,6,3.5331859775372023,0.013311199903813952,0.18193579850150715,1.0,True,
|
|
||||||
24,27,7.477875812552711,31.60254826458195,0.6649014778325123,0.12481159614427853,6,4.10006355979974,0.01787282120544869,0.1680556511623414,1.0,True,
|
|
||||||
25,26,1.433673687807028,42.944397586466835,0.9127837514934289,0.09581429165893823,6,2.9017658682436958,0.0035341488401767693,0.018440169300164816,1.0,True,
|
|
||||||
25,27,1.973107678535245,68.22877288971054,0.8510739856801909,0.10418150333394123,6,2.3682534170899983,0.006103232696003387,0.13366555345654282,1.0,True,
|
|
||||||
25,28,2.578633669986193,88.09106909546726,0.8853518429870751,0.10761251229651997,6,2.6372860976794636,0.0034832316659127254,0.03697744201993421,1.0,True,
|
|
||||||
26,27,0.6711664435459649,25.284375303243706,0.9183867141162515,0.09074909570650827,6,2.8142315359282533,0.00040199505815399084,0.011554314408123986,1.0,True,
|
|
||||||
26,28,1.202247282991071,45.146671509000434,0.8853200095170116,0.1060418493965508,6,2.65640202778952,0.007320240476184076,0.12930335868606002,1.0,True,
|
|
||||||
26,29,0.8313560685788559,87.41573662125148,0.7880466815984911,0.10871274240898261,6,3.111957466886598,0.006395243061058376,0.039517035902872893,1.0,True,
|
|
||||||
27,28,0.6147316450225401,19.862296205756735,0.9289448669201521,0.08505095515326752,6,2.9632528684698194,0.005157655135593023,0.02266680787731125,1.0,True,
|
|
||||||
27,29,0.7540837235696874,62.13136131800778,0.7872365477452019,0.1045551346483567,6,2.9621627623005304,0.011650639607012715,0.16247191340775555,1.0,True,
|
|
||||||
27,30,5.07252652550922,152.0939255621477,0.03871268656716418,0.16956979514477974,6,145.85449900829832,4.5477385995336626,37.97511303619642,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
28,29,1.3242039147826599,42.26906511225104,0.8000944621560987,0.10865475473581254,6,3.146622410005858,0.001139416496730335,0.02119627748722763,1.0,True,
|
|
||||||
28,30,5.091487440029177,132.23162935639098,0.7721000935453695,0.10815845248211022,6,2.292141242686861,0.004462522533166182,0.03583195432667579,1.0,True,
|
|
||||||
28,31,6.538757407908195,158.84212980112872,0.7465330381074466,0.10669666534392452,6,2.40259836682247,0.0063576490595817345,0.04001661521442668,1.0,True,
|
|
||||||
29,30,4.767831371266539,89.96256424413991,0.7248812145092132,0.10870831469083345,6,2.947122380473709,0.004656385377159087,0.12465763189228557,0.0,False,multistart_instability
|
|
||||||
29,31,5.715598450796842,116.57306468887764,0.7243012243012243,0.11138751941762652,6,2.7645021322697088,0.0042639063218924715,0.04663739608639213,0.0,False,multistart_instability
|
|
||||||
29,32,5.281749147864957,159.39493219688646,0.32491640724086246,0.14745892800230104,6,2.8081088899077167,4.8118414680497805,1.8382424276135385,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
30,31,2.604154101624297,26.610500444737717,0.8185562292643862,0.09615795732951272,6,2.899558208444641,0.0049991634555749285,0.021384795873435915,1.0,True,
|
|
||||||
30,32,1.69105635082049,69.43236795274656,0.8041343079031521,0.09953343153684206,6,2.614616874224757,0.0031713764951448154,0.023767377748422268,1.0,True,
|
|
||||||
30,33,3.2411277385703925,160.7145717128097,0.05469213429825602,0.15997514255683,6,6.323554164510002,5.32763716297551,13.705863888332022,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
31,32,0.9137201114724802,42.82186750800884,0.8157085941946499,0.09925944770258255,6,3.048541140076824,0.004719596234885736,0.06146741991683861,1.0,True,
|
|
||||||
31,33,1.4965271484681508,134.10407126807198,0.760345235280208,0.09698073659477859,6,2.550150787416455,0.0019366659831763946,0.026538404789073603,1.0,True,
|
|
||||||
32,33,1.9169426499676907,91.28220376006315,0.7569928006609229,0.10008740880870787,6,3.6611044282667184,2.9881933505092046,2.6964434945984053,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
|
Binary file not shown.
@@ -1,889 +0,0 @@
|
|||||||
{
|
|
||||||
"selection_is_X_independent": true,
|
|
||||||
"criteria": {
|
|
||||||
"min_inlier_ratio": 0.7,
|
|
||||||
"max_inlier_rmse_m": 0.13,
|
|
||||||
"max_rotation_invariant_error_deg": 0.75,
|
|
||||||
"reverse_translation_tolerance_m": 0.05,
|
|
||||||
"reverse_rotation_tolerance_deg": 0.5
|
|
||||||
},
|
|
||||||
"input_pairs": 73,
|
|
||||||
"accepted_pairs": 41,
|
|
||||||
"pairs": [
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 1,
|
|
||||||
"heldout_inlier_ratio": 0.8061657032755298,
|
|
||||||
"heldout_inlier_rmse_m": 0.10961296014103396,
|
|
||||||
"rotation_invariant_error_deg": 0.021953694767642418,
|
|
||||||
"reverse_translation_m": 0.004225163540003575,
|
|
||||||
"reverse_rotation_deg": 0.1530449067720668,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 2,
|
|
||||||
"heldout_inlier_ratio": 0.7489394523717702,
|
|
||||||
"heldout_inlier_rmse_m": 0.116305716193008,
|
|
||||||
"rotation_invariant_error_deg": 0.672251363597141,
|
|
||||||
"reverse_translation_m": 0.02073003109723684,
|
|
||||||
"reverse_rotation_deg": 0.12418344306819311,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 3,
|
|
||||||
"heldout_inlier_ratio": 0.6310283235519265,
|
|
||||||
"heldout_inlier_rmse_m": 0.12470979645173097,
|
|
||||||
"rotation_invariant_error_deg": 0.3195502906704917,
|
|
||||||
"reverse_translation_m": 0.02154775870989652,
|
|
||||||
"reverse_rotation_deg": 0.25399044979598373,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 2,
|
|
||||||
"heldout_inlier_ratio": 0.7867383512544803,
|
|
||||||
"heldout_inlier_rmse_m": 0.11079021393934946,
|
|
||||||
"rotation_invariant_error_deg": 0.6532823847336218,
|
|
||||||
"reverse_translation_m": 0.00768232673944143,
|
|
||||||
"reverse_rotation_deg": 0.0508043300468843,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 3,
|
|
||||||
"heldout_inlier_ratio": 0.6794562317367552,
|
|
||||||
"heldout_inlier_rmse_m": 0.11907169138096609,
|
|
||||||
"rotation_invariant_error_deg": 0.28968149037613955,
|
|
||||||
"reverse_translation_m": 0.024832409186708038,
|
|
||||||
"reverse_rotation_deg": 0.29364088503444125,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 4,
|
|
||||||
"heldout_inlier_ratio": 0.6786112833230006,
|
|
||||||
"heldout_inlier_rmse_m": 0.1125501582315264,
|
|
||||||
"rotation_invariant_error_deg": 0.23509206217087808,
|
|
||||||
"reverse_translation_m": 0.0077657602443488094,
|
|
||||||
"reverse_rotation_deg": 0.07274574571529673,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 3,
|
|
||||||
"heldout_inlier_ratio": 0.7586776859504132,
|
|
||||||
"heldout_inlier_rmse_m": 0.11541610277862546,
|
|
||||||
"rotation_invariant_error_deg": 0.06385020919199427,
|
|
||||||
"reverse_translation_m": 0.007879904085790266,
|
|
||||||
"reverse_rotation_deg": 0.11659483159927261,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 4,
|
|
||||||
"heldout_inlier_ratio": 0.7854572527608884,
|
|
||||||
"heldout_inlier_rmse_m": 0.1098649389241602,
|
|
||||||
"rotation_invariant_error_deg": 0.8714733340459802,
|
|
||||||
"reverse_translation_m": 0.017042953274276868,
|
|
||||||
"reverse_rotation_deg": 0.09383247792992644,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 5,
|
|
||||||
"heldout_inlier_ratio": 0.7074574574574575,
|
|
||||||
"heldout_inlier_rmse_m": 0.11912871456224486,
|
|
||||||
"rotation_invariant_error_deg": 0.18237928888532906,
|
|
||||||
"reverse_translation_m": 0.016640250279170064,
|
|
||||||
"reverse_rotation_deg": 0.24773491621516538,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 4,
|
|
||||||
"heldout_inlier_ratio": 0.6974624291697462,
|
|
||||||
"heldout_inlier_rmse_m": 0.11727630888949149,
|
|
||||||
"rotation_invariant_error_deg": 0.4902436735823201,
|
|
||||||
"reverse_translation_m": 0.0121487212194689,
|
|
||||||
"reverse_rotation_deg": 0.20793086843827258,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 5,
|
|
||||||
"heldout_inlier_ratio": 0.7962985964476462,
|
|
||||||
"heldout_inlier_rmse_m": 0.10646082199215787,
|
|
||||||
"rotation_invariant_error_deg": 0.5522737825084345,
|
|
||||||
"reverse_translation_m": 0.011008298723512349,
|
|
||||||
"reverse_rotation_deg": 0.0821707761041294,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 6,
|
|
||||||
"heldout_inlier_ratio": 0.8376509054325956,
|
|
||||||
"heldout_inlier_rmse_m": 0.10956583416752531,
|
|
||||||
"rotation_invariant_error_deg": 0.24026261153986894,
|
|
||||||
"reverse_translation_m": 0.001222821038315667,
|
|
||||||
"reverse_rotation_deg": 0.092075215117626,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 5,
|
|
||||||
"j": 6,
|
|
||||||
"heldout_inlier_ratio": 0.7526921648718901,
|
|
||||||
"heldout_inlier_rmse_m": 0.10924852668609375,
|
|
||||||
"rotation_invariant_error_deg": 0.8049600839827491,
|
|
||||||
"reverse_translation_m": 0.012374747586500019,
|
|
||||||
"reverse_rotation_deg": 0.14461856068328793,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 5,
|
|
||||||
"j": 7,
|
|
||||||
"heldout_inlier_ratio": 0.7833561729164071,
|
|
||||||
"heldout_inlier_rmse_m": 0.11683079277948678,
|
|
||||||
"rotation_invariant_error_deg": 0.7937418989476015,
|
|
||||||
"reverse_translation_m": 0.013689874812447942,
|
|
||||||
"reverse_rotation_deg": 0.20006562776472953,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 7,
|
|
||||||
"heldout_inlier_ratio": 0.8497729566094854,
|
|
||||||
"heldout_inlier_rmse_m": 0.10415909908074775,
|
|
||||||
"rotation_invariant_error_deg": 0.026617733421641532,
|
|
||||||
"reverse_translation_m": 0.008986805974950147,
|
|
||||||
"reverse_rotation_deg": 0.13099479506412062,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 8,
|
|
||||||
"heldout_inlier_ratio": 0.7678928928928929,
|
|
||||||
"heldout_inlier_rmse_m": 0.1116672507978127,
|
|
||||||
"rotation_invariant_error_deg": 0.32815442351946444,
|
|
||||||
"reverse_translation_m": 0.010085391730567652,
|
|
||||||
"reverse_rotation_deg": 0.1175283699615622,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 7,
|
|
||||||
"j": 8,
|
|
||||||
"heldout_inlier_ratio": 0.8299748110831234,
|
|
||||||
"heldout_inlier_rmse_m": 0.10748525688830211,
|
|
||||||
"rotation_invariant_error_deg": 0.29848794137558343,
|
|
||||||
"reverse_translation_m": 0.00710646197885058,
|
|
||||||
"reverse_rotation_deg": 0.1069872847368705,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 9,
|
|
||||||
"j": 11,
|
|
||||||
"heldout_inlier_ratio": 0.5818780055682106,
|
|
||||||
"heldout_inlier_rmse_m": 0.1171768781510077,
|
|
||||||
"rotation_invariant_error_deg": 0.2635280280577561,
|
|
||||||
"reverse_translation_m": 0.00981302583568424,
|
|
||||||
"reverse_rotation_deg": 0.06035460198954135,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 9,
|
|
||||||
"j": 12,
|
|
||||||
"heldout_inlier_ratio": 0.5379123584441162,
|
|
||||||
"heldout_inlier_rmse_m": 0.12645695585785705,
|
|
||||||
"rotation_invariant_error_deg": 0.5448949372769505,
|
|
||||||
"reverse_translation_m": 0.008862930161051686,
|
|
||||||
"reverse_rotation_deg": 0.11566409877698092,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 10,
|
|
||||||
"j": 11,
|
|
||||||
"heldout_inlier_ratio": 0.7118898623279099,
|
|
||||||
"heldout_inlier_rmse_m": 0.1192865608074921,
|
|
||||||
"rotation_invariant_error_deg": 0.037876295004018345,
|
|
||||||
"reverse_translation_m": 0.0029529340190141227,
|
|
||||||
"reverse_rotation_deg": 0.004176908093440082,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 10,
|
|
||||||
"j": 12,
|
|
||||||
"heldout_inlier_ratio": 0.6120311738918656,
|
|
||||||
"heldout_inlier_rmse_m": 0.12525652128126136,
|
|
||||||
"rotation_invariant_error_deg": 0.22694248875488654,
|
|
||||||
"reverse_translation_m": 0.005686910809247203,
|
|
||||||
"reverse_rotation_deg": 0.10204044727299268,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 10,
|
|
||||||
"j": 13,
|
|
||||||
"heldout_inlier_ratio": 0.516551290119572,
|
|
||||||
"heldout_inlier_rmse_m": 0.1354903305714048,
|
|
||||||
"rotation_invariant_error_deg": 0.7336960316225287,
|
|
||||||
"reverse_translation_m": 0.013565042721589003,
|
|
||||||
"reverse_rotation_deg": 0.32552304340992394,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"heldout_rmse"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 11,
|
|
||||||
"j": 12,
|
|
||||||
"heldout_inlier_ratio": 0.650555275113579,
|
|
||||||
"heldout_inlier_rmse_m": 0.12104882943540898,
|
|
||||||
"rotation_invariant_error_deg": 0.16828520325654495,
|
|
||||||
"reverse_translation_m": 0.003583199374507776,
|
|
||||||
"reverse_rotation_deg": 0.03300207707174117,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 11,
|
|
||||||
"j": 13,
|
|
||||||
"heldout_inlier_ratio": 0.5698054068172914,
|
|
||||||
"heldout_inlier_rmse_m": 0.1285866274322162,
|
|
||||||
"rotation_invariant_error_deg": 0.7985328447719269,
|
|
||||||
"reverse_translation_m": 0.016001435752898144,
|
|
||||||
"reverse_rotation_deg": 0.11059625579950419,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 11,
|
|
||||||
"j": 14,
|
|
||||||
"heldout_inlier_ratio": 0.6420881321982974,
|
|
||||||
"heldout_inlier_rmse_m": 0.12414062948335593,
|
|
||||||
"rotation_invariant_error_deg": 0.9364144322988963,
|
|
||||||
"reverse_translation_m": 0.012369101516230316,
|
|
||||||
"reverse_rotation_deg": 0.01870506040605898,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 13,
|
|
||||||
"heldout_inlier_ratio": 0.6972966112450819,
|
|
||||||
"heldout_inlier_rmse_m": 0.1168089621311632,
|
|
||||||
"rotation_invariant_error_deg": 0.9873386907551662,
|
|
||||||
"reverse_translation_m": 0.01141023876783206,
|
|
||||||
"reverse_rotation_deg": 0.04779713993428384,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 14,
|
|
||||||
"heldout_inlier_ratio": 0.8749086479902558,
|
|
||||||
"heldout_inlier_rmse_m": 0.09521299965540625,
|
|
||||||
"rotation_invariant_error_deg": 0.7228372421049902,
|
|
||||||
"reverse_translation_m": 0.006159472215773367,
|
|
||||||
"reverse_rotation_deg": 0.014929948455569534,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 15,
|
|
||||||
"heldout_inlier_ratio": 0.7022030893897189,
|
|
||||||
"heldout_inlier_rmse_m": 0.11797995277580106,
|
|
||||||
"rotation_invariant_error_deg": 0.34088274652411243,
|
|
||||||
"reverse_translation_m": 0.008495008365046321,
|
|
||||||
"reverse_rotation_deg": 0.10278299144703042,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 14,
|
|
||||||
"heldout_inlier_ratio": 0.6955810147299509,
|
|
||||||
"heldout_inlier_rmse_m": 0.11455956122705321,
|
|
||||||
"rotation_invariant_error_deg": 1.7562842773469676,
|
|
||||||
"reverse_translation_m": 0.01022866463344607,
|
|
||||||
"reverse_rotation_deg": 0.03515966054944392,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 15,
|
|
||||||
"heldout_inlier_ratio": 0.868300353819945,
|
|
||||||
"heldout_inlier_rmse_m": 0.1045421356808481,
|
|
||||||
"rotation_invariant_error_deg": 0.6514660625063202,
|
|
||||||
"reverse_translation_m": 0.002375025382773949,
|
|
||||||
"reverse_rotation_deg": 0.01072504764419793,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 16,
|
|
||||||
"heldout_inlier_ratio": 0.7265456392027422,
|
|
||||||
"heldout_inlier_rmse_m": 0.1096252966406241,
|
|
||||||
"rotation_invariant_error_deg": 0.48470120712195097,
|
|
||||||
"reverse_translation_m": 0.008958927594996346,
|
|
||||||
"reverse_rotation_deg": 0.03041438512426757,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 14,
|
|
||||||
"j": 15,
|
|
||||||
"heldout_inlier_ratio": 0.7120070334086913,
|
|
||||||
"heldout_inlier_rmse_m": 0.11868441290330703,
|
|
||||||
"rotation_invariant_error_deg": 1.1046128571505562,
|
|
||||||
"reverse_translation_m": 0.002571958018980028,
|
|
||||||
"reverse_rotation_deg": 0.055069197511519646,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 14,
|
|
||||||
"j": 16,
|
|
||||||
"heldout_inlier_ratio": 0.5918615984405458,
|
|
||||||
"heldout_inlier_rmse_m": 0.12229328437386515,
|
|
||||||
"rotation_invariant_error_deg": 1.240484202449963,
|
|
||||||
"reverse_translation_m": 0.014273957859025563,
|
|
||||||
"reverse_rotation_deg": 0.25325650727957555,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 14,
|
|
||||||
"j": 17,
|
|
||||||
"heldout_inlier_ratio": 0.6694009445687298,
|
|
||||||
"heldout_inlier_rmse_m": 0.12443900216431925,
|
|
||||||
"rotation_invariant_error_deg": 0.4822313558358869,
|
|
||||||
"reverse_translation_m": 0.0178952010004604,
|
|
||||||
"reverse_rotation_deg": 0.10920228290609924,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 15,
|
|
||||||
"j": 16,
|
|
||||||
"heldout_inlier_ratio": 0.702887537993921,
|
|
||||||
"heldout_inlier_rmse_m": 0.11495230769293859,
|
|
||||||
"rotation_invariant_error_deg": 0.16340661916523302,
|
|
||||||
"reverse_translation_m": 0.013545291843396808,
|
|
||||||
"reverse_rotation_deg": 0.03346625178333291,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 15,
|
|
||||||
"j": 17,
|
|
||||||
"heldout_inlier_ratio": 0.7429531936901991,
|
|
||||||
"heldout_inlier_rmse_m": 0.11679524427533863,
|
|
||||||
"rotation_invariant_error_deg": 0.050851974711708436,
|
|
||||||
"reverse_translation_m": 0.009894110027911674,
|
|
||||||
"reverse_rotation_deg": 0.07786907370565768,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 15,
|
|
||||||
"j": 18,
|
|
||||||
"heldout_inlier_ratio": 0.7209645010046886,
|
|
||||||
"heldout_inlier_rmse_m": 0.11716134583909138,
|
|
||||||
"rotation_invariant_error_deg": 0.26477152026833295,
|
|
||||||
"reverse_translation_m": 0.01165472931184747,
|
|
||||||
"reverse_rotation_deg": 0.13683586564190106,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 16,
|
|
||||||
"j": 17,
|
|
||||||
"heldout_inlier_ratio": 0.618922305764411,
|
|
||||||
"heldout_inlier_rmse_m": 0.11254196340940027,
|
|
||||||
"rotation_invariant_error_deg": 0.05549924909794868,
|
|
||||||
"reverse_translation_m": 0.031047860213503222,
|
|
||||||
"reverse_rotation_deg": 0.10375098145236057,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 16,
|
|
||||||
"j": 18,
|
|
||||||
"heldout_inlier_ratio": 0.6890156918687589,
|
|
||||||
"heldout_inlier_rmse_m": 0.11084024896736888,
|
|
||||||
"rotation_invariant_error_deg": 0.06628387310017914,
|
|
||||||
"reverse_translation_m": 0.01556225520373068,
|
|
||||||
"reverse_rotation_deg": 0.02495881796885156,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 16,
|
|
||||||
"j": 19,
|
|
||||||
"heldout_inlier_ratio": 0.8685060899826,
|
|
||||||
"heldout_inlier_rmse_m": 0.10135543575024479,
|
|
||||||
"rotation_invariant_error_deg": 0.3152201090321931,
|
|
||||||
"reverse_translation_m": 0.0029522513838272538,
|
|
||||||
"reverse_rotation_deg": 0.02753369989558306,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 17,
|
|
||||||
"j": 18,
|
|
||||||
"heldout_inlier_ratio": 0.7697708305735859,
|
|
||||||
"heldout_inlier_rmse_m": 0.10648049893472189,
|
|
||||||
"rotation_invariant_error_deg": 0.2007356143443495,
|
|
||||||
"reverse_translation_m": 0.010582276181446961,
|
|
||||||
"reverse_rotation_deg": 0.03959905194910384,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 17,
|
|
||||||
"j": 19,
|
|
||||||
"heldout_inlier_ratio": 0.6293759512937596,
|
|
||||||
"heldout_inlier_rmse_m": 0.10954497717502036,
|
|
||||||
"rotation_invariant_error_deg": 0.25455213438726787,
|
|
||||||
"reverse_translation_m": 0.012240937390578075,
|
|
||||||
"reverse_rotation_deg": 0.06030618467886912,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 18,
|
|
||||||
"j": 19,
|
|
||||||
"heldout_inlier_ratio": 0.6827314510833881,
|
|
||||||
"heldout_inlier_rmse_m": 0.10834806615100022,
|
|
||||||
"rotation_invariant_error_deg": 0.4543752098998439,
|
|
||||||
"reverse_translation_m": 0.012418496053909043,
|
|
||||||
"reverse_rotation_deg": 0.11905018881087433,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 19,
|
|
||||||
"j": 20,
|
|
||||||
"heldout_inlier_ratio": 0.6234734541714874,
|
|
||||||
"heldout_inlier_rmse_m": 0.1277530580405749,
|
|
||||||
"rotation_invariant_error_deg": 0.038058886117710244,
|
|
||||||
"reverse_translation_m": 0.009237066767190974,
|
|
||||||
"reverse_rotation_deg": 0.22540488888103255,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 20,
|
|
||||||
"j": 21,
|
|
||||||
"heldout_inlier_ratio": 0.6596992097884272,
|
|
||||||
"heldout_inlier_rmse_m": 0.12147955429086157,
|
|
||||||
"rotation_invariant_error_deg": 0.33308699073501913,
|
|
||||||
"reverse_translation_m": 0.014725537493639118,
|
|
||||||
"reverse_rotation_deg": 0.19884224722867958,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 20,
|
|
||||||
"j": 22,
|
|
||||||
"heldout_inlier_ratio": 0.5739414499308958,
|
|
||||||
"heldout_inlier_rmse_m": 0.13255415786946775,
|
|
||||||
"rotation_invariant_error_deg": 0.49226728584466883,
|
|
||||||
"reverse_translation_m": 0.006040617548523686,
|
|
||||||
"reverse_rotation_deg": 0.19706632354686843,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"heldout_rmse"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 20,
|
|
||||||
"j": 23,
|
|
||||||
"heldout_inlier_ratio": 0.6456945156330087,
|
|
||||||
"heldout_inlier_rmse_m": 0.12075756038041646,
|
|
||||||
"rotation_invariant_error_deg": 0.4097857444953803,
|
|
||||||
"reverse_translation_m": 0.02294901101348451,
|
|
||||||
"reverse_rotation_deg": 0.19112109479244785,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 22,
|
|
||||||
"heldout_inlier_ratio": 0.7716237647919971,
|
|
||||||
"heldout_inlier_rmse_m": 0.1163227135854156,
|
|
||||||
"rotation_invariant_error_deg": 0.20299476740399935,
|
|
||||||
"reverse_translation_m": 0.009963578798274064,
|
|
||||||
"reverse_rotation_deg": 0.1533289219532281,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 23,
|
|
||||||
"heldout_inlier_ratio": 0.8576224819696593,
|
|
||||||
"heldout_inlier_rmse_m": 0.09865676260631218,
|
|
||||||
"rotation_invariant_error_deg": 0.08196486868098773,
|
|
||||||
"reverse_translation_m": 0.00392257332509571,
|
|
||||||
"reverse_rotation_deg": 0.00898487649366286,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 24,
|
|
||||||
"heldout_inlier_ratio": 0.7715940569126165,
|
|
||||||
"heldout_inlier_rmse_m": 0.11361504553552869,
|
|
||||||
"rotation_invariant_error_deg": 0.4941752812908575,
|
|
||||||
"reverse_translation_m": 0.007475673605592584,
|
|
||||||
"reverse_rotation_deg": 0.04227769001294981,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 23,
|
|
||||||
"heldout_inlier_ratio": 0.7396689147762109,
|
|
||||||
"heldout_inlier_rmse_m": 0.11702962848624102,
|
|
||||||
"rotation_invariant_error_deg": 0.07221906147914936,
|
|
||||||
"reverse_translation_m": 0.018140159434305195,
|
|
||||||
"reverse_rotation_deg": 0.1184732181610567,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 25,
|
|
||||||
"heldout_inlier_ratio": 0.736861094407697,
|
|
||||||
"heldout_inlier_rmse_m": 0.1158639471011007,
|
|
||||||
"rotation_invariant_error_deg": 0.19913456977445776,
|
|
||||||
"reverse_translation_m": 0.01855955252477569,
|
|
||||||
"reverse_rotation_deg": 0.06452602797902846,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 23,
|
|
||||||
"j": 24,
|
|
||||||
"heldout_inlier_ratio": 0.7853164556962026,
|
|
||||||
"heldout_inlier_rmse_m": 0.11287254423109305,
|
|
||||||
"rotation_invariant_error_deg": 0.5709657470221714,
|
|
||||||
"reverse_translation_m": 0.002195759849349955,
|
|
||||||
"reverse_rotation_deg": 0.03211295915781923,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 23,
|
|
||||||
"j": 25,
|
|
||||||
"heldout_inlier_ratio": 0.6881127450980392,
|
|
||||||
"heldout_inlier_rmse_m": 0.12303206700403985,
|
|
||||||
"rotation_invariant_error_deg": 2.3734594630006685,
|
|
||||||
"reverse_translation_m": 0.004939188019097873,
|
|
||||||
"reverse_rotation_deg": 0.12964064637099942,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio",
|
|
||||||
"rotation_conjugacy_invariant"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 23,
|
|
||||||
"j": 26,
|
|
||||||
"heldout_inlier_ratio": 0.6852618757612667,
|
|
||||||
"heldout_inlier_rmse_m": 0.12040544972155913,
|
|
||||||
"rotation_invariant_error_deg": 0.15748268923838538,
|
|
||||||
"reverse_translation_m": 0.006032445250251169,
|
|
||||||
"reverse_rotation_deg": 0.14039335223022864,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 25,
|
|
||||||
"heldout_inlier_ratio": 0.677667493796526,
|
|
||||||
"heldout_inlier_rmse_m": 0.1234631580581415,
|
|
||||||
"rotation_invariant_error_deg": 0.10001483417664048,
|
|
||||||
"reverse_translation_m": 0.023479226891170584,
|
|
||||||
"reverse_rotation_deg": 0.16273859629355136,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 26,
|
|
||||||
"heldout_inlier_ratio": 0.6530209617755857,
|
|
||||||
"heldout_inlier_rmse_m": 0.12710147612984235,
|
|
||||||
"rotation_invariant_error_deg": 0.7149536911782421,
|
|
||||||
"reverse_translation_m": 0.013311199903813952,
|
|
||||||
"reverse_rotation_deg": 0.18193579850150715,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 27,
|
|
||||||
"heldout_inlier_ratio": 0.6649014778325123,
|
|
||||||
"heldout_inlier_rmse_m": 0.12481159614427853,
|
|
||||||
"rotation_invariant_error_deg": 0.5126580224867077,
|
|
||||||
"reverse_translation_m": 0.01787282120544869,
|
|
||||||
"reverse_rotation_deg": 0.1680556511623414,
|
|
||||||
"accepted": false,
|
|
||||||
"rejection_reasons": [
|
|
||||||
"overlap_ratio"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 26,
|
|
||||||
"heldout_inlier_ratio": 0.9127837514934289,
|
|
||||||
"heldout_inlier_rmse_m": 0.09581429165893823,
|
|
||||||
"rotation_invariant_error_deg": 0.0540123863768045,
|
|
||||||
"reverse_translation_m": 0.0035341488401767693,
|
|
||||||
"reverse_rotation_deg": 0.018440169300164816,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 27,
|
|
||||||
"heldout_inlier_ratio": 0.8510739856801909,
|
|
||||||
"heldout_inlier_rmse_m": 0.10418150333394123,
|
|
||||||
"rotation_invariant_error_deg": 0.24124343828050598,
|
|
||||||
"reverse_translation_m": 0.006103232696003387,
|
|
||||||
"reverse_rotation_deg": 0.13366555345654282,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 28,
|
|
||||||
"heldout_inlier_ratio": 0.8853518429870751,
|
|
||||||
"heldout_inlier_rmse_m": 0.10761251229651997,
|
|
||||||
"rotation_invariant_error_deg": 0.31032271091986274,
|
|
||||||
"reverse_translation_m": 0.0034832316659127254,
|
|
||||||
"reverse_rotation_deg": 0.03697744201993421,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 27,
|
|
||||||
"heldout_inlier_ratio": 0.9183867141162515,
|
|
||||||
"heldout_inlier_rmse_m": 0.09074909570650827,
|
|
||||||
"rotation_invariant_error_deg": 0.1549242409220426,
|
|
||||||
"reverse_translation_m": 0.00040199505815399084,
|
|
||||||
"reverse_rotation_deg": 0.011554314408123986,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 28,
|
|
||||||
"heldout_inlier_ratio": 0.8853200095170116,
|
|
||||||
"heldout_inlier_rmse_m": 0.1060418493965508,
|
|
||||||
"rotation_invariant_error_deg": 0.2535175622408232,
|
|
||||||
"reverse_translation_m": 0.007320240476184076,
|
|
||||||
"reverse_rotation_deg": 0.12930335868606002,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 29,
|
|
||||||
"heldout_inlier_ratio": 0.7880466815984911,
|
|
||||||
"heldout_inlier_rmse_m": 0.10871274240898261,
|
|
||||||
"rotation_invariant_error_deg": 0.6273264323501451,
|
|
||||||
"reverse_translation_m": 0.006395243061058376,
|
|
||||||
"reverse_rotation_deg": 0.039517035902872893,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 28,
|
|
||||||
"heldout_inlier_ratio": 0.9289448669201521,
|
|
||||||
"heldout_inlier_rmse_m": 0.08505095515326752,
|
|
||||||
"rotation_invariant_error_deg": 0.06642597183531862,
|
|
||||||
"reverse_translation_m": 0.005157655135593023,
|
|
||||||
"reverse_rotation_deg": 0.02266680787731125,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 29,
|
|
||||||
"heldout_inlier_ratio": 0.7872365477452019,
|
|
||||||
"heldout_inlier_rmse_m": 0.1045551346483567,
|
|
||||||
"rotation_invariant_error_deg": 0.46617350279205994,
|
|
||||||
"reverse_translation_m": 0.011650639607012715,
|
|
||||||
"reverse_rotation_deg": 0.16247191340775555,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 28,
|
|
||||||
"j": 29,
|
|
||||||
"heldout_inlier_ratio": 0.8000944621560987,
|
|
||||||
"heldout_inlier_rmse_m": 0.10865475473581254,
|
|
||||||
"rotation_invariant_error_deg": 0.3806425437120282,
|
|
||||||
"reverse_translation_m": 0.001139416496730335,
|
|
||||||
"reverse_rotation_deg": 0.02119627748722763,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 28,
|
|
||||||
"j": 30,
|
|
||||||
"heldout_inlier_ratio": 0.7721000935453695,
|
|
||||||
"heldout_inlier_rmse_m": 0.10815845248211022,
|
|
||||||
"rotation_invariant_error_deg": 0.37744105649966286,
|
|
||||||
"reverse_translation_m": 0.004462522533166182,
|
|
||||||
"reverse_rotation_deg": 0.03583195432667579,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 28,
|
|
||||||
"j": 31,
|
|
||||||
"heldout_inlier_ratio": 0.7465330381074466,
|
|
||||||
"heldout_inlier_rmse_m": 0.10669666534392452,
|
|
||||||
"rotation_invariant_error_deg": 0.22580828656680296,
|
|
||||||
"reverse_translation_m": 0.0063576490595817345,
|
|
||||||
"reverse_rotation_deg": 0.04001661521442668,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 31,
|
|
||||||
"heldout_inlier_ratio": 0.8185562292643862,
|
|
||||||
"heldout_inlier_rmse_m": 0.09615795732951272,
|
|
||||||
"rotation_invariant_error_deg": 0.608920703223724,
|
|
||||||
"reverse_translation_m": 0.0049991634555749285,
|
|
||||||
"reverse_rotation_deg": 0.021384795873435915,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 32,
|
|
||||||
"heldout_inlier_ratio": 0.8041343079031521,
|
|
||||||
"heldout_inlier_rmse_m": 0.09953343153684206,
|
|
||||||
"rotation_invariant_error_deg": 0.7410181887106262,
|
|
||||||
"reverse_translation_m": 0.0031713764951448154,
|
|
||||||
"reverse_rotation_deg": 0.023767377748422268,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 31,
|
|
||||||
"j": 32,
|
|
||||||
"heldout_inlier_ratio": 0.8157085941946499,
|
|
||||||
"heldout_inlier_rmse_m": 0.09925944770258255,
|
|
||||||
"rotation_invariant_error_deg": 0.1261576421594981,
|
|
||||||
"reverse_translation_m": 0.004719596234885736,
|
|
||||||
"reverse_rotation_deg": 0.06146741991683861,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 31,
|
|
||||||
"j": 33,
|
|
||||||
"heldout_inlier_ratio": 0.760345235280208,
|
|
||||||
"heldout_inlier_rmse_m": 0.09698073659477859,
|
|
||||||
"rotation_invariant_error_deg": 0.11718235773108177,
|
|
||||||
"reverse_translation_m": 0.0019366659831763946,
|
|
||||||
"reverse_rotation_deg": 0.026538404789073603,
|
|
||||||
"accepted": true,
|
|
||||||
"rejection_reasons": []
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
@@ -1,346 +0,0 @@
|
|||||||
{
|
|
||||||
"schema_version": 1,
|
|
||||||
"success": true,
|
|
||||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
|
||||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
|
||||||
"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",
|
|
||||||
"y_axis": "left",
|
|
||||||
"z_axis": "up",
|
|
||||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
|
||||||
},
|
|
||||||
"LiDAR": "raw LiDAR sensor frame"
|
|
||||||
},
|
|
||||||
"backend": "open3d_gicp",
|
|
||||||
"measured_lidar_extrinsic_used_as_initial": false,
|
|
||||||
"body_heading_offset_used": false,
|
|
||||||
"body_antenna_lever_xy_used": false,
|
|
||||||
"translation_m": [
|
|
||||||
1.6368478986289665,
|
|
||||||
-0.2424214446279586,
|
|
||||||
0.08408840390370595
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.8529068506201997,
|
|
||||||
1.3285763243955087,
|
|
||||||
-22.150129507458946
|
|
||||||
],
|
|
||||||
"quaternion_xyzw": [
|
|
||||||
-0.005076802391609619,
|
|
||||||
0.012807178841486907,
|
|
||||||
-0.19199197181185435,
|
|
||||||
0.9812997936448346
|
|
||||||
],
|
|
||||||
"matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.9259501178598366,
|
|
||||||
0.3766733256085306,
|
|
||||||
0.027084774511975725,
|
|
||||||
1.6368478986289665
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.3769334036732196,
|
|
||||||
0.9262266176745453,
|
|
||||||
0.005045979240275979,
|
|
||||||
-0.2424214446279586
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.023185953305318648,
|
|
||||||
-0.014881481316772506,
|
|
||||||
0.9996204044951974,
|
|
||||||
0.08408840390370595
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
],
|
|
||||||
"quality": {
|
|
||||||
"stations": 34,
|
|
||||||
"pairs": 41,
|
|
||||||
"residuals": {
|
|
||||||
"pairs": 41,
|
|
||||||
"translation_m": {
|
|
||||||
"rms": 0.11160362192890017,
|
|
||||||
"median": 0.06165368731012592,
|
|
||||||
"p90": 0.12167778728549836,
|
|
||||||
"p95": 0.12315870952465978,
|
|
||||||
"max": 0.45797693508387505
|
|
||||||
},
|
|
||||||
"rotation_deg": {
|
|
||||||
"rms": 1.082945057501201,
|
|
||||||
"median": 0.744916302248719,
|
|
||||||
"p90": 1.523342607477641,
|
|
||||||
"p95": 1.864505235125524,
|
|
||||||
"max": 4.332008025818138
|
|
||||||
},
|
|
||||||
"per_pair": [
|
|
||||||
{
|
|
||||||
"pair_index": 0,
|
|
||||||
"translation_m": 0.03696060713715898,
|
|
||||||
"rotation_deg": 0.7820683861381595
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 1,
|
|
||||||
"translation_m": 0.039028552931951836,
|
|
||||||
"rotation_deg": 0.6949685743890126
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 2,
|
|
||||||
"translation_m": 0.01732676228513485,
|
|
||||||
"rotation_deg": 1.0438060827136844
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 3,
|
|
||||||
"translation_m": 0.0574978038017053,
|
|
||||||
"rotation_deg": 0.394946532754576
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 4,
|
|
||||||
"translation_m": 0.10801668349596734,
|
|
||||||
"rotation_deg": 0.29336064351197005
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 5,
|
|
||||||
"translation_m": 0.06577881887454434,
|
|
||||||
"rotation_deg": 0.5542732613092561
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 6,
|
|
||||||
"translation_m": 0.06363940755916121,
|
|
||||||
"rotation_deg": 0.5890545170693674
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 7,
|
|
||||||
"translation_m": 0.0640679503495012,
|
|
||||||
"rotation_deg": 0.746120063911557
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 8,
|
|
||||||
"translation_m": 0.021009631897429052,
|
|
||||||
"rotation_deg": 0.6663196620233134
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 9,
|
|
||||||
"translation_m": 0.08609573827254784,
|
|
||||||
"rotation_deg": 0.3687772861174946
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 10,
|
|
||||||
"translation_m": 0.02732304440983888,
|
|
||||||
"rotation_deg": 0.5222061823280917
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 11,
|
|
||||||
"translation_m": 0.04489317903985948,
|
|
||||||
"rotation_deg": 0.8700136243621963
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 12,
|
|
||||||
"translation_m": 0.07260882213085644,
|
|
||||||
"rotation_deg": 0.463182802330913
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 13,
|
|
||||||
"translation_m": 0.08831772542234817,
|
|
||||||
"rotation_deg": 0.659009021415606
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 14,
|
|
||||||
"translation_m": 0.053804778572191785,
|
|
||||||
"rotation_deg": 1.864505235125524
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 15,
|
|
||||||
"translation_m": 0.12315870952465978,
|
|
||||||
"rotation_deg": 1.9856861904193648
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 16,
|
|
||||||
"translation_m": 0.06880647181572622,
|
|
||||||
"rotation_deg": 0.27331219092746517
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 17,
|
|
||||||
"translation_m": 0.059582300078982915,
|
|
||||||
"rotation_deg": 0.3410200031868246
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 18,
|
|
||||||
"translation_m": 0.034895300030885576,
|
|
||||||
"rotation_deg": 0.5283410202190479
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 19,
|
|
||||||
"translation_m": 0.002366788176226253,
|
|
||||||
"rotation_deg": 0.3852038090000981
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 20,
|
|
||||||
"translation_m": 0.06233663305653708,
|
|
||||||
"rotation_deg": 0.6208000110952433
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 21,
|
|
||||||
"translation_m": 0.012626372544105475,
|
|
||||||
"rotation_deg": 0.38292733656142575
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 22,
|
|
||||||
"translation_m": 0.047494831014178666,
|
|
||||||
"rotation_deg": 1.7712490998072288
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 23,
|
|
||||||
"translation_m": 0.06165368731012592,
|
|
||||||
"rotation_deg": 0.8670376638275039
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 24,
|
|
||||||
"translation_m": 0.12172160687143056,
|
|
||||||
"rotation_deg": 4.332008025818138
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 25,
|
|
||||||
"translation_m": 0.06185808030991236,
|
|
||||||
"rotation_deg": 1.523342607477641
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 26,
|
|
||||||
"translation_m": 0.049450564075379046,
|
|
||||||
"rotation_deg": 0.9499016191505171
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 27,
|
|
||||||
"translation_m": 0.016599648000378425,
|
|
||||||
"rotation_deg": 0.7685476074882036
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 28,
|
|
||||||
"translation_m": 0.028501035078080036,
|
|
||||||
"rotation_deg": 0.9107236146504971
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 29,
|
|
||||||
"translation_m": 0.05418062512497841,
|
|
||||||
"rotation_deg": 0.49604339332524483
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 30,
|
|
||||||
"translation_m": 0.02534272431328254,
|
|
||||||
"rotation_deg": 0.2798196345939902
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 31,
|
|
||||||
"translation_m": 0.1006162178003358,
|
|
||||||
"rotation_deg": 0.9066113005741254
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 32,
|
|
||||||
"translation_m": 0.023348232889945957,
|
|
||||||
"rotation_deg": 0.29316028754024664
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 33,
|
|
||||||
"translation_m": 0.06352851658442724,
|
|
||||||
"rotation_deg": 1.123276625138155
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 34,
|
|
||||||
"translation_m": 0.06685972566531029,
|
|
||||||
"rotation_deg": 0.7607492800980359
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 35,
|
|
||||||
"translation_m": 0.3534873096430149,
|
|
||||||
"rotation_deg": 0.744916302248719
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 36,
|
|
||||||
"translation_m": 0.45797693508387505,
|
|
||||||
"rotation_deg": 0.7684054290628693
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 37,
|
|
||||||
"translation_m": 0.10414608850682983,
|
|
||||||
"rotation_deg": 0.8325040370564112
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 38,
|
|
||||||
"translation_m": 0.11058068261532474,
|
|
||||||
"rotation_deg": 0.8695456609442757
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 39,
|
|
||||||
"translation_m": 0.03405996353456899,
|
|
||||||
"rotation_deg": 0.7913171839743647
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 40,
|
|
||||||
"translation_m": 0.12167778728549836,
|
|
||||||
"rotation_deg": 0.16247416554150054
|
|
||||||
}
|
|
||||||
]
|
|
||||||
},
|
|
||||||
"weighted_jacobian_condition_number": 7.006062559882809,
|
|
||||||
"linearized_one_sigma": {
|
|
||||||
"translation_m": [
|
|
||||||
0.007129000573825617,
|
|
||||||
0.007213998960440989,
|
|
||||||
0.0046717381822970125
|
|
||||||
],
|
|
||||||
"rotation_deg": [
|
|
||||||
0.06695429523124989,
|
|
||||||
0.06503891988826449,
|
|
||||||
0.14115095959507304
|
|
||||||
],
|
|
||||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
|
||||||
},
|
|
||||||
"bootstrap": {
|
|
||||||
"runs": 100,
|
|
||||||
"order": [
|
|
||||||
"x_m",
|
|
||||||
"y_m",
|
|
||||||
"z_m",
|
|
||||||
"roll_deg",
|
|
||||||
"pitch_deg",
|
|
||||||
"yaw_deg"
|
|
||||||
],
|
|
||||||
"std": [
|
|
||||||
0.002608526149658222,
|
|
||||||
0.0028276716521100863,
|
|
||||||
0.0023368456176917846,
|
|
||||||
0.0795227773764157,
|
|
||||||
0.07300189168975164,
|
|
||||||
0.10947642324382982
|
|
||||||
],
|
|
||||||
"p025": [
|
|
||||||
1.6327841703535608,
|
|
||||||
-0.24653273132861378,
|
|
||||||
0.07991981050403614,
|
|
||||||
-0.9872072161381827,
|
|
||||||
1.1864367848117605,
|
|
||||||
-22.364052444549895
|
|
||||||
],
|
|
||||||
"p975": [
|
|
||||||
1.6431149050874008,
|
|
||||||
-0.2355422823589496,
|
|
||||||
0.08894657837563841,
|
|
||||||
-0.6759368885907463,
|
|
||||||
1.4669998309298398,
|
|
||||||
-21.953029139092944
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"z_constraint": {
|
|
||||||
"observable_from_planar_AX_XB": false,
|
|
||||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
|
||||||
"rtk_reference_height_above_ground_m": 0.8535,
|
|
||||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
|
||||||
},
|
|
||||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification"
|
|
||||||
}
|
|
||||||
@@ -1,156 +0,0 @@
|
|||||||
i,j,rtk_translation_m,rtk_rotation_deg,heldout_inlier_ratio,heldout_inlier_rmse_m,hessian_rank,hessian_condition,reverse_translation_m,reverse_rotation_deg,multistart_success_rate,accepted,rejection_reasons
|
|
||||||
0,1,1.6721594124489447,24.171297449440814,0.8193962748876044,0.11049675306366954,6,14.013392649694936,0.026679665410762447,0.12399936190197167,1.0,True,
|
|
||||||
0,2,2.0412175279332088,80.09074797031303,0.7525388867463684,0.11492533799491883,6,14.962108606929117,0.0018464756299783867,0.03093241101186373,1.0,True,
|
|
||||||
0,3,6.30529961936688,79.9158388329924,0.628093901505486,0.12365050970311502,6,23.490589415548122,0.010524333328230958,0.23898233567764737,0.5,True,
|
|
||||||
0,4,2.238422471863255,130.25781950514033,0.693351593625498,0.11485738628517483,6,16.305124521706706,0.003627491377787396,0.0495829610363469,1.0,True,
|
|
||||||
0,5,7.5269579262553155,88.82606603101335,0.6015065913370998,0.12959859333012305,6,26.27293180169831,0.013710015050868782,0.26025123892797375,1.0,True,
|
|
||||||
1,2,1.2843386040405174,55.9194505208722,0.7981310803891449,0.11167434332282765,6,13.484382276710306,0.0507423684848027,0.517099810570953,1.0,False,forward_reverse_rotation
|
|
||||||
1,3,5.433888607887495,55.744541383551606,0.678820988438572,0.12109867185657658,6,27.478714016210855,0.008324315958294127,0.2568985217684905,1.0,True,
|
|
||||||
1,4,1.5299424710613851,106.08652205569952,0.6772473651580905,0.1115749218038809,6,13.250326799303036,3.109603769112268,6.665689673243039,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
1,5,7.072560501394692,64.65476858157254,0.618779694923731,0.1278696355679149,6,21.376356908960656,0.012044684321696756,0.5185219918090542,1.0,False,forward_reverse_rotation
|
|
||||||
1,6,8.049825623399226,8.10898363252498,0.02911760982402836,0.16814699881028172,6,51.15739724954147,2.3526085660101135,12.54864815902537,0.0,False,backend_not_converged;heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
2,3,4.340252832276203,0.1749091373206093,0.7609663064208518,0.11583878636902087,6,13.299472485717942,0.007842434748069874,0.017111535671962216,1.0,True,
|
|
||||||
2,4,0.2520257253555564,50.1670715348273,0.796748976299789,0.1148434235904791,6,12.041666425070192,0.011210942709506708,0.11462542679279858,1.0,True,
|
|
||||||
2,5,5.8286926576588955,8.735318060700322,0.7112112112112112,0.12001318292058727,6,15.618628103418056,0.011299298862678088,0.02158353743310138,1.0,True,
|
|
||||||
2,6,6.928094074716812,47.810466888347236,0.058659571772456606,0.1689308471089219,6,22.346184538451386,2.376212271528816,4.191297741726165,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
2,7,6.405228405426323,73.1220331951562,0.05777324320877439,0.16763553481687163,6,11.228331216247241,2.3764975144091216,1.7142235592052535,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
3,4,4.1070657333447205,50.34198067214791,0.6957378664695738,0.11619002437046365,6,17.76967737811838,3.222808271854619,15.590678196925943,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
3,5,2.2886212019715484,8.910227198020936,0.7989069680784996,0.10775905778143813,6,12.151555874812614,0.013532537604982006,0.06485728954506689,1.0,True,
|
|
||||||
3,6,2.618668779147775,47.63555775102663,0.8435613682092555,0.11222309863990189,6,13.914901775514537,0.00951276519885504,0.09743636874086448,1.0,True,
|
|
||||||
3,7,2.7963089245830335,72.9471240578356,0.8651898734177215,0.11184568072686091,6,12.975777248765162,0.010827690226297664,0.12520280777371842,1.0,True,
|
|
||||||
3,8,2.6983089912812726,163.5692883655715,0.825590155700653,0.1078798726707026,6,12.959410142765158,0.005608807919239624,0.021536601402144962,1.0,True,
|
|
||||||
4,5,5.5767898078953735,41.431753474126985,0.6652516676773802,0.12444359189600171,6,17.028714587649738,0.0031552835887398907,0.05824661275042354,1.0,True,
|
|
||||||
4,6,6.68811709459501,97.97753842317455,0.03891480481217775,0.16130442983218543,6,53.12725754116488,5.386834276571879,3.248443974085464,1.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
4,7,6.153247042777442,123.28910472998353,0.01717321472695824,0.16597934102353687,6,197.50040966862915,1.8946664283973234,13.38395621317346,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
4,8,6.802787549686365,146.08873096228075,0.7129198332924737,0.11349036082807593,6,18.616925126379055,4.094017521116411,1.0439112418969816,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
4,9,3.018117089902297,158.11607430100386,0.3323991714390155,0.13218238008205907,6,82.91588951856485,0.17404817666776692,0.7491904391974432,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
5,6,2.0562758992403367,56.545784949047565,0.7604901596732269,0.11101745020162715,6,16.790595774247244,0.01028831511886355,0.031120237309440944,1.0,True,
|
|
||||||
5,7,0.5812996709001959,81.85735125585653,0.8092687180764918,0.10777871969807898,6,15.203386410549202,0.010579733674272045,0.033334626234333836,1.0,True,
|
|
||||||
5,8,2.6887856969568644,172.47951556359513,0.40909652700531457,0.13021927164196687,6,88.03642906190348,2.282274682790372,1.1841285568948536,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
5,9,7.501939677383991,160.45217222486954,0.3361179361179361,0.13367797847230906,6,83.15270070161475,5.255898884183195,7.177742907146106,0.5,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
5,10,7.507648391453363,143.54232919109307,0.5442391832766165,0.13173165083201846,6,26.337222871823244,4.175333583746659,14.054946454556381,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
6,7,1.9723544820714844,25.311566306808967,0.8539354187689203,0.10462253085152279,6,12.50291076661203,0.0028433142784691904,0.028424505435071433,1.0,True,
|
|
||||||
6,8,0.7288530799256238,115.93373061454484,0.7757757757757757,0.11316400028465075,6,15.521747346102574,0.007224674181692249,0.10395594559619384,1.0,True,
|
|
||||||
6,9,7.898758206937296,103.90638727582184,0.6009202835468226,0.1259448851291812,6,28.795189977892598,4.270523553799638,1.1342776748452013,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
6,10,8.382165572419371,159.91188585985944,0.048837495386886455,0.16976022756819517,6,33.03889916791793,8.842801960353667,9.008466157678757,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
6,11,11.12084001821623,138.54043874549885,0.026144624410151765,0.1725705028585339,6,71.57462790292871,2.4153309235424008,10.273490365819672,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
7,8,2.6747845281541447,90.62216430773587,0.8340050377833753,0.11132245152738934,6,16.17035077702852,0.004806949653405576,0.020340398656013788,1.0,True,
|
|
||||||
7,9,8.06955581661561,78.59482096901287,0.6160971335586432,0.12637042722943573,6,24.178664977065605,4.367245914434154,12.796681469446304,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
7,10,8.088675434881791,134.6003195530505,0.04890429614956048,0.17569950505457485,6,17.12714594442953,9.4426170895353,17.952544168886714,0.0,False,backend_not_converged;heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
7,11,10.488793105910844,113.2288724386899,0.03723199383746309,0.16334679446297104,6,88.61274561425562,7.5280305092863244,55.909263646297305,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
7,12,13.79542539595065,94.95104818613552,0.023342903507676944,0.1706723033660903,6,71.17833596146563,5.060797550499477,27.755426788516992,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
8,9,7.73033999229505,12.027343338722995,0.6407549981373402,0.12189583104066957,6,28.06713348388492,2.3076573667891433,3.487290408239611,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
8,10,8.378411682322204,43.97815524531458,0.6075420709986488,0.12933255488441212,6,20.429633989572718,4.865121579871581,2.927465091074779,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
8,11,11.214115106391473,22.60670813095403,0.03922067999490641,0.16246238376196148,6,54.700772476792416,2.594876837596059,7.9735312612345846,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
8,12,14.373410961212507,4.328883878399634,0.023529411764705882,0.1707990278738782,6,65.92814703722017,0.7170106443431138,1.1046762967347212,0.5,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
8,13,18.141079267476005,28.43334478424675,0.020491803278688523,0.1759614106055459,6,128.98300559552638,1.9608884223852157,1.569223825596656,0.0,False,backend_not_converged;heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
9,10,1.9264207261313253,56.00549858403757,0.6576312576312576,0.1069384415347781,6,15.95767235456931,1.6379095873833018,4.808769928108965,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
9,11,4.747620352849464,34.63405146967702,0.5883320678309288,0.11815838246331258,6,21.526672921842795,2.059598786777497,11.82652375960602,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
9,12,7.208566899019793,16.356227217122623,0.5413589364844904,0.12469816852190199,6,38.03547459177405,1.1412129496099401,4.794166723137469,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
9,13,10.706998136562337,16.406001445523756,0.015145729922362225,0.16400783300033744,6,309.6688821904962,3.999785287368469,12.657822343539058,0.5,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
9,14,7.911071381405571,14.085282580602351,0.5416463116756228,0.12859551377809345,6,26.721469573230685,0.011377143482079926,0.04588334980819398,1.0,True,
|
|
||||||
10,11,3.0731501091601263,21.371447114360553,0.7131414267834794,0.12095106901516853,6,13.404202373771934,0.006897671932216771,0.18540056788520015,1.0,True,
|
|
||||||
10,12,6.011368280631378,39.649271366914945,0.6117876278616659,0.1250022412120491,6,19.806559061723252,0.006509808900810373,0.0674238161099294,1.0,True,
|
|
||||||
10,13,9.76425648692991,72.41150002956132,0.5244808055380743,0.1368926377190841,6,36.182352720806286,0.004651842966001154,0.17931102925270942,1.0,True,
|
|
||||||
10,14,6.554187411792988,41.92021600343522,0.5996858385693572,0.12955691635611982,6,17.916357473627126,0.009593578345611885,0.0575378323241282,1.0,True,
|
|
||||||
10,15,10.1706917218607,65.51298166784419,0.5048970366649924,0.13966622273060353,6,30.173625507677606,0.006074999657627903,0.048675594115713976,1.0,True,
|
|
||||||
11,12,3.3258193587172027,18.277824252554396,0.6508076728924785,0.12017753597340275,6,15.211760062254436,0.016302173123952872,0.05141023051579196,1.0,True,
|
|
||||||
11,13,7.195203402213438,51.04005291520078,0.5666710199817161,0.12966061077144855,6,26.1420797484443,0.010250004424021028,0.06307533415790957,1.0,True,
|
|
||||||
11,14,3.634158560526847,20.548768889074672,0.64271407110666,0.12109534664165013,6,14.045896850674039,0.0076106764286992265,0.053501024445426364,1.0,True,
|
|
||||||
11,15,7.446972831184529,44.14153455348363,0.570479416362689,0.12836105648415896,6,18.857231663145765,0.006898635427401595,0.028532809464236104,1.0,True,
|
|
||||||
11,16,10.651790397923806,69.98495270981525,0.47336531178995206,0.13436934972977357,6,42.57049752296035,0.03569532774909474,0.4066994385898772,1.0,True,
|
|
||||||
12,13,3.8697507070400543,32.762228662646386,0.7056733087955325,0.1168985924651875,6,14.509757354686162,0.0020816591736723326,0.08889045468170857,1.0,True,
|
|
||||||
12,14,0.9871080784265185,2.2709446365202766,0.8745432399512789,0.09766070609034913,6,11.857755748379178,0.0024736851957500175,0.01806957610594206,1.0,True,
|
|
||||||
12,15,4.171948395051693,25.86371030092923,0.7033426183844012,0.1210390118956012,6,11.859397045728281,0.02278023379659275,0.04927694758545221,1.0,True,
|
|
||||||
12,16,7.331699780686257,51.70712845726085,0.5820235756385069,0.1245813395619955,6,32.351864267271324,0.009898398498331785,0.03808519306435069,1.0,True,
|
|
||||||
12,17,6.344245780495681,1.4248238883471156,0.6542219994988725,0.12658997017247905,6,11.229930872030522,0.019742666743374927,0.07899239993213694,1.0,True,
|
|
||||||
13,14,3.7992314627329202,30.491284026126113,0.6984766461034874,0.11406275275916469,6,13.924716870947337,0.012533601308866885,0.10861598809330086,1.0,True,
|
|
||||||
13,15,0.9105848166450461,6.898518361717151,0.8794391298650243,0.0990320912382964,6,10.514819201987361,0.006638809913640662,0.04266354358349535,1.0,True,
|
|
||||||
13,16,3.4957081323467,18.94489979461447,0.7273073505141552,0.10958532316684444,6,19.405056504086563,0.005441055528599314,0.12519365495729431,1.0,True,
|
|
||||||
13,17,2.9366461487378266,31.337404774299262,0.7130265716137395,0.11779895987026272,6,12.816390530620648,0.013263327701592529,0.16221305155705523,1.0,True,
|
|
||||||
13,18,5.248032013425982,3.445996452937746,0.7019876443728176,0.11940768524727288,6,25.985486690964827,0.008995185112868311,0.05198334816510605,1.0,True,
|
|
||||||
14,15,3.8816793634199405,23.592765664408958,0.7089927153981411,0.11692319124843933,6,10.742828632896593,0.09073157715426228,0.8466374858545868,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
14,16,7.29101265002769,49.43618382074057,0.5923489278752436,0.12241125802320883,6,33.653658158107916,0.003104270324855819,0.13386021564092,1.0,True,
|
|
||||||
14,17,5.915950814087913,0.8461207481731609,0.6687795177728063,0.12452036369781098,6,8.953051466023156,0.011592867275090568,0.10132037554601482,1.0,True,
|
|
||||||
14,18,8.433581718971825,27.04528757318836,0.6196476790536196,0.12845389972151766,6,17.805501350543512,0.010071755472376367,0.13041952825792016,1.0,True,
|
|
||||||
14,19,9.056542482242314,79.47127430600666,0.5845660749506904,0.12584132662356765,6,30.335272352492893,0.016830803029543952,0.25747616717579747,1.0,True,
|
|
||||||
15,16,3.579287497246505,25.843418156331627,0.7032674772036475,0.1160221689285562,6,22.696890954829914,0.0009988867448377137,0.0903616813411077,1.0,True,
|
|
||||||
15,17,2.2400625117908257,24.438886412582114,0.7489009568140678,0.11923375870790395,6,6.944925131742929,0.02582458487951321,0.09503968088936432,1.0,True,
|
|
||||||
15,18,4.6956742726068,3.452521908779405,0.7165438713998661,0.1178392296206422,6,19.334165264362177,0.009354386824150452,0.1747840133793935,1.0,True,
|
|
||||||
15,19,5.176369821469625,55.87850864159772,0.6924358974358974,0.11506895717743917,6,20.417126084614868,0.012497900854286311,0.3255615001296683,1.0,True,
|
|
||||||
15,20,1.1877416357686716,128.54504202158432,0.6751867872591427,0.11901811643083532,6,27.381712286843683,2.670174543103617,1.9448445706434312,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
16,17,2.956793995513645,50.28230456891372,0.6284461152882206,0.11136186275509914,6,25.18428705575399,0.015372505619716304,0.0751987172187524,1.0,True,
|
|
||||||
16,18,3.369822545690391,22.390896247552213,0.6921281286473868,0.10919235768364494,6,39.42922894015897,0.005355462743716019,0.036543030274398446,1.0,True,
|
|
||||||
16,19,2.313038703742191,30.035090485266096,0.8880188913745961,0.09683468613705355,6,11.709094307553842,0.006745004702224827,0.04851926363284082,1.0,True,
|
|
||||||
16,20,4.242518045976368,102.70162386525263,0.36698412698412697,0.13430062225326117,6,102.05441227788522,1.6194077849835204,3.5461435258834046,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
16,21,9.189957911290794,153.31148960700713,0.23764328854924197,0.14863424676851908,6,75.99451631707726,0.01721606559516444,0.561012451825117,0.5,False,heldout_inlier_ratio;forward_reverse_rotation
|
|
||||||
17,18,2.517968959880004,27.89140832136152,0.7665916015366274,0.11462271017395576,6,9.759734204828526,0.003677259621955875,0.03455865038654393,1.0,True,
|
|
||||||
17,19,3.518310045065406,80.31739505417983,0.645738203957382,0.11467441399973677,6,29.935027493013713,0.002657916871055147,0.03996637099866608,1.0,True,
|
|
||||||
17,20,3.4199679241812992,152.98392843416642,0.2749902761571373,0.1378092692558536,6,36.98799912625142,0.9962499249666478,0.8583377913930432,0.5,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
17,21,8.339875108212771,156.4062058240796,0.4672368255565338,0.13751533768592306,6,32.19730139309397,3.7101311942981123,2.4525134629880094,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
17,22,10.944066586184825,150.8296621811644,0.4255952380952381,0.14178517078123323,6,38.71138397815585,1.8838258929727458,2.643696557684808,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
18,19,2.0416624616211574,52.42598673281832,0.6999343401181878,0.10876407223187459,6,36.91910298018587,0.0029722527816906435,0.028553700929610345,1.0,True,
|
|
||||||
18,20,5.836864764777489,125.0925201128049,0.5796614723267061,0.12778176089815013,6,32.46060078153021,0.08496447343578362,0.24110896992060832,0.5,False,forward_reverse_translation
|
|
||||||
18,21,10.84206419743439,175.70238585457497,0.23118979432439468,0.15492240575842026,6,95.57603256530417,0.3127879752995947,1.8359225701448998,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
18,22,13.461930516546937,178.72107050264088,0.20872354073123797,0.1539528469442395,6,158.96979855906298,5.509854679183967,5.400016950581102,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
18,23,10.787990019880349,164.1156966348418,0.3967277486910995,0.14073241205403234,6,67.81571155690442,0.037998014107336324,0.09667096210109852,1.0,True,
|
|
||||||
19,20,6.120105723142265,72.6665333799865,0.6260444787247719,0.1266849163783949,6,25.979821491778758,0.02278172414929769,0.1983479736758361,1.0,True,
|
|
||||||
19,21,11.201089261967727,123.27639912174082,0.22215292503430212,0.15090955425182226,6,96.8936157485445,2.289236102137041,0.9775110057344923,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
19,22,13.962230939038237,128.85294276465584,0.1883148831488315,0.1538013519108862,6,137.2212791172918,1.512900854675742,1.7779489198947585,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
19,23,10.877481155608614,143.45831663234054,0.37768025078369905,0.1432975121556297,6,38.93505210462377,2.6484748594978824,0.9762972275431752,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
19,24,10.384369483528566,177.87107120381796,0.43504761904761907,0.1392444656116938,6,41.9366060613255,0.04258350776081601,0.7879276702809289,1.0,False,forward_reverse_rotation
|
|
||||||
20,21,5.091648376409225,50.60986574175432,0.6607188376242672,0.12214671257866083,6,14.082798146569486,0.014717307768564562,0.07480162341419787,1.0,True,
|
|
||||||
20,22,7.842914217245693,56.186409384669375,0.576328684508104,0.13245799460807808,6,16.034011252152304,0.021651469263481868,0.4612054468064915,1.0,True,
|
|
||||||
20,23,4.953146569484805,70.79178325235414,0.6451819579702717,0.1210226317867539,6,12.51146899612653,0.018249896901540018,0.12115759970964086,1.0,True,
|
|
||||||
20,24,4.806112840058592,105.20453782384023,0.7383177570093458,0.11441422046802803,6,12.152737785424252,0.009011280440944078,0.06430292826078875,1.0,True,
|
|
||||||
20,25,7.7432318481763245,68.57831319871164,0.6182822702159718,0.12791257682460658,6,29.104161441720713,0.01470353026057929,0.3205553574328468,1.0,True,
|
|
||||||
21,22,2.836151129858731,5.576543642915048,0.7740636818348177,0.11564789267022943,6,12.209471497858695,0.006518431057241462,0.06991931948827856,1.0,True,
|
|
||||||
21,23,1.4635995041292513,20.181917510599828,0.862223327530465,0.10307112004551743,6,11.124777032517395,0.002678668765812511,0.01879593375546071,1.0,True,
|
|
||||||
21,24,2.7001854883863183,54.59467208208592,0.7795265676152102,0.11182158240581809,6,15.934013426145448,0.003491995501354943,0.037425651095358885,1.0,True,
|
|
||||||
21,25,3.6513937480713023,17.968447456957325,0.7340892465252378,0.12002239056891176,6,16.572162980052227,0.03773466739435234,0.28781074838303833,1.0,True,
|
|
||||||
21,26,4.368847767445087,60.912845043424156,0.7147358216190014,0.11997311573294335,6,17.3723156532691,0.01216183417643751,0.10312122071404906,1.0,True,
|
|
||||||
22,23,3.806551883906871,14.605373867684776,0.7408951563458002,0.1172672510975272,6,12.610213323576234,0.009206244303296198,0.0960198600148152,1.0,True,
|
|
||||||
22,24,4.999470711928798,49.01812843917085,0.6936064556176288,0.11914624513148228,6,13.20338761495324,0.006090737153725476,0.02755713841749006,1.0,True,
|
|
||||||
22,25,2.281002791409386,12.391903814042275,0.7356584485868911,0.11474070552638106,6,16.673633938013026,0.001961709159757097,0.011643770804742994,1.0,True,
|
|
||||||
22,26,1.990287035474152,55.33630140050909,0.7422594142259414,0.11765221626900067,6,18.880420396501982,0.007814937371704422,0.03934255356487579,1.0,True,
|
|
||||||
22,27,2.5532406896142295,80.6206767037528,0.7254925373134329,0.11870181965154772,6,21.309389349405333,0.018241823604788293,0.08279236858581901,1.0,True,
|
|
||||||
23,24,1.2663665558774873,34.41275457148608,0.7884810126582279,0.10662565692629541,6,10.611199681165658,0.0018823381482244372,0.020239211377623904,1.0,True,
|
|
||||||
23,25,5.050865141821003,2.213470053642503,0.6843137254901961,0.1217109193489842,6,15.15080906413716,0.01463985673592735,0.20460048921133533,1.0,True,
|
|
||||||
23,26,5.595299803053147,40.730927532824325,0.6772228989037758,0.12237954766026346,6,16.382334072569808,0.04518647006837217,0.20367965681897174,1.0,True,
|
|
||||||
23,27,6.240759831138249,66.01530283606803,0.6637469586374696,0.12340951265232125,6,20.64383986204704,0.010626193556947943,1.1817079481882706,1.0,False,forward_reverse_rotation
|
|
||||||
23,28,6.598772106458927,85.87759904182478,0.6720351390922401,0.12122778564083768,6,19.206570679040215,0.043540468662592216,0.2605147805386839,0.5,True,
|
|
||||||
24,25,6.316196707646632,36.626224625128586,0.6764267990074442,0.12367636388755723,6,21.50176872604184,0.03581640576644142,0.20465043373191275,1.0,True,
|
|
||||||
24,26,6.840027061556236,6.318172961338242,0.6524044389642417,0.12740222503880924,6,21.75476709306229,0.0620474433075452,0.12014838295938772,1.0,True,
|
|
||||||
24,27,7.477875812552711,31.60254826458195,0.6546798029556651,0.12425657449629156,6,26.295019203914542,0.05403266260961897,0.2126374478532295,1.0,True,
|
|
||||||
24,28,7.81303641449596,51.464844470338676,0.6614377470355731,0.12010155595807544,6,20.985000954928537,0.04949067679791638,0.25138526875322614,0.5,True,
|
|
||||||
24,29,7.4949096209288655,93.73390958258972,0.047106325706594884,0.16491171897379944,6,28.651559198797667,0.8785910837751835,4.6775880176920355,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
25,26,1.433673687807028,42.944397586466835,0.9137395459976105,0.08148335762110498,6,16.56285514245561,0.003236736725553251,0.006670817258604747,1.0,True,
|
|
||||||
25,27,1.973107678535245,68.22877288971054,0.8596658711217183,0.10961458820309757,6,22.188153380460914,0.010624789877375612,0.04475651255675051,1.0,True,
|
|
||||||
25,28,2.578633669986193,88.09106909546726,0.8839157491622786,0.10490699814192775,6,16.172179483480598,0.0028341913749953818,0.01585308444597317,1.0,True,
|
|
||||||
25,29,1.4869736566208207,130.3601342077183,0.7857227558401518,0.10954358768244278,6,20.132087187715292,0.0032477187428175502,0.016542353808297643,1.0,True,
|
|
||||||
25,30,5.843711828111692,139.6773015481418,0.05061061531235322,0.15827452276344428,6,186.52424080569762,2.4401001990983646,2.630476332375907,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
26,27,0.6711664435459649,25.284375303243706,0.9188612099644128,0.07520973241900301,6,20.799051531446725,0.00099208733077853,0.005485007138530622,1.0,True,
|
|
||||||
26,28,1.202247282991071,45.146671509000434,0.9182726623840114,0.07611768518866213,6,21.995419584098137,0.004984978187528897,0.011735070988964648,1.0,True,
|
|
||||||
26,29,0.8313560685788559,87.41573662125148,0.7856890251090416,0.1085139306178217,6,24.280602674778585,0.002586732426994246,0.12205883610742861,1.0,True,
|
|
||||||
26,30,5.554376083336843,177.37830086539105,0.7634835395750642,0.10495724850674515,6,34.80647378548566,0.008489213184549637,0.021733210949119494,1.0,True,
|
|
||||||
26,31,6.5424104132673175,156.01119868987084,0.7374054682955207,0.10737521663044328,6,38.310555401782864,0.007807741115783734,0.04372865220115068,0.5,True,
|
|
||||||
27,28,0.6147316450225401,19.862296205756735,0.9281131178707225,0.07220941715117642,6,20.092791877654474,0.002300778353404822,0.001589714984734129,1.0,True,
|
|
||||||
27,29,0.7540837235696874,62.13136131800778,0.7871188037207112,0.10966659884725233,6,24.009650087098127,0.005134403698946511,0.024348440992038003,1.0,True,
|
|
||||||
27,30,5.07252652550922,152.0939255621477,0.7922108208955224,0.10147035321534549,6,32.52026042532519,0.007488026390185518,0.03860522829819172,1.0,True,
|
|
||||||
27,31,6.285022243874859,178.7044260068885,0.7592097617664149,0.10539218018667616,6,48.70669958381935,0.0020937297862771895,0.027401753528281184,1.0,True,
|
|
||||||
27,32,5.778763736289045,138.47370648510574,0.7480278422273782,0.10408734715846861,6,42.21510199826032,0.004407463145301957,0.03490543724835993,0.5,True,
|
|
||||||
28,29,1.3242039147826599,42.26906511225104,0.787814381863266,0.10901449493832827,6,25.9136254196751,0.01570175790237293,0.035696604981368125,1.0,True,
|
|
||||||
28,30,5.091487440029177,132.23162935639098,0.7791159962581852,0.1043856075500982,6,36.50167792578953,0.011701496185342901,0.047047156803733815,1.0,True,
|
|
||||||
28,31,6.538757407908195,158.84212980112872,0.7449015266285981,0.10668967013687278,6,48.04636971250768,0.02442137437725171,0.6810283060803413,1.0,False,forward_reverse_rotation
|
|
||||||
28,32,5.963604730984572,158.33600269086236,0.7252210330386226,0.11213192172123396,6,59.78207706090093,0.01795092616246746,0.05811390678775167,0.0,False,multistart_instability
|
|
||||||
28,33,5.80807626014008,67.05379893079936,0.6692465836255895,0.10871806386783625,6,54.67376255043743,0.132593515882677,0.6593919290562373,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
|
||||||
29,30,4.767831371266539,89.96256424413991,0.7320662880982732,0.10929719051930432,6,30.745355096911858,0.005463604154020641,0.01858348348785812,1.0,True,
|
|
||||||
29,31,5.715598450796842,116.57306468887764,0.7254562254562255,0.11323336570178014,6,49.09678922599784,0.005077799585122041,0.07265894091769737,1.0,True,
|
|
||||||
29,32,5.281749147864957,159.39493219688646,0.33356393404819557,0.1449978595558761,6,111.7227195582132,0.3588603464467423,0.15715476414253352,1.0,False,heldout_inlier_ratio;forward_reverse_translation
|
|
||||||
29,33,4.779166013738703,109.3228640430504,0.2850467289719626,0.13990657628540273,6,178.4302458902832,0.39012209563155037,0.27153598748575447,0.0,False,backend_not_converged;heldout_inlier_ratio;forward_reverse_translation;multistart_instability
|
|
||||||
30,31,2.604154101624297,26.610500444737717,0.8286237272623269,0.09646596945131831,6,41.03513305025278,0.018905314487389895,0.08632503464138006,1.0,True,
|
|
||||||
30,32,1.69105635082049,69.43236795274656,0.8065326633165829,0.09815063400019147,6,35.25635856167186,0.007341509941520377,0.023973741904830165,1.0,True,
|
|
||||||
30,33,3.2411277385703925,160.7145717128097,0.09253766757622493,0.15464713396746754,6,31.157195533215592,1.0417523955000538,7.5233256615684665,1.0,False,backend_not_converged;heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
|
||||||
31,32,0.9137201114724802,42.82186750800884,0.8146841206602162,0.09914628416022428,6,34.3317318149268,0.0693687705829607,0.45703051311645604,1.0,True,
|
|
||||||
31,33,1.4965271484681508,134.10407126807198,0.7551430598250177,0.1003246191461096,6,28.537349904719445,0.005559091155809675,0.033005318250111694,1.0,True,
|
|
||||||
32,33,1.9169426499676907,91.28220376006315,0.7585270860380031,0.0987597723287551,6,32.396623704761396,2.8694889670610495,5.798276157700327,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
|
||||||
|
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -1,416 +0,0 @@
|
|||||||
{
|
|
||||||
"schema_version": 1,
|
|
||||||
"success": true,
|
|
||||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
|
||||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
|
||||||
"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",
|
|
||||||
"y_axis": "left",
|
|
||||||
"z_axis": "up",
|
|
||||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
|
||||||
},
|
|
||||||
"LiDAR": "raw LiDAR sensor frame"
|
|
||||||
},
|
|
||||||
"backend": "small_gicp",
|
|
||||||
"measured_lidar_extrinsic_used_as_initial": false,
|
|
||||||
"body_heading_offset_used": false,
|
|
||||||
"body_antenna_lever_xy_used": false,
|
|
||||||
"translation_m": [
|
|
||||||
1.6362863962126635,
|
|
||||||
-0.2461267175734333,
|
|
||||||
0.0851285837804841
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.7337275596198988,
|
|
||||||
1.3206501867905613,
|
|
||||||
-22.293138670917568
|
|
||||||
],
|
|
||||||
"quaternion_xyzw": [
|
|
||||||
-0.004053851502643108,
|
|
||||||
0.012544688899275593,
|
|
||||||
-0.19323028574532955,
|
|
||||||
0.9810648570503344
|
|
||||||
],
|
|
||||||
"matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.9250093749023974,
|
|
||||||
0.37904117671318505,
|
|
||||||
0.026180960611867178,
|
|
||||||
1.6362863962126635
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.3792445939369631,
|
|
||||||
0.9252912459175456,
|
|
||||||
0.0031061548487011197,
|
|
||||||
-0.2461267175734333
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.023047653074967735,
|
|
||||||
-0.01280221013107426,
|
|
||||||
0.9996523941368298,
|
|
||||||
0.0851285837804841
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
],
|
|
||||||
"quality": {
|
|
||||||
"stations": 34,
|
|
||||||
"pairs": 55,
|
|
||||||
"residuals": {
|
|
||||||
"pairs": 55,
|
|
||||||
"translation_m": {
|
|
||||||
"rms": 0.17789650760700804,
|
|
||||||
"median": 0.07286630775287825,
|
|
||||||
"p90": 0.3720138200025907,
|
|
||||||
"p95": 0.3991391086271845,
|
|
||||||
"max": 0.5307044931342686
|
|
||||||
},
|
|
||||||
"rotation_deg": {
|
|
||||||
"rms": 1.6408745761259504,
|
|
||||||
"median": 0.7722797079864339,
|
|
||||||
"p90": 2.06911907015371,
|
|
||||||
"p95": 4.199030922414834,
|
|
||||||
"max": 4.936245135060849
|
|
||||||
},
|
|
||||||
"per_pair": [
|
|
||||||
{
|
|
||||||
"pair_index": 0,
|
|
||||||
"translation_m": 0.0467393979562504,
|
|
||||||
"rotation_deg": 0.7657431692138433
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 1,
|
|
||||||
"translation_m": 0.05689017243056396,
|
|
||||||
"rotation_deg": 0.808177090866613
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 2,
|
|
||||||
"translation_m": 0.06397107958463491,
|
|
||||||
"rotation_deg": 0.3801068497009622
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 3,
|
|
||||||
"translation_m": 0.1107952950892596,
|
|
||||||
"rotation_deg": 0.22993541870282783
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 4,
|
|
||||||
"translation_m": 0.025928748934271984,
|
|
||||||
"rotation_deg": 0.6046131588115704
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 5,
|
|
||||||
"translation_m": 0.07562027851221909,
|
|
||||||
"rotation_deg": 0.6309936647228139
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 6,
|
|
||||||
"translation_m": 0.18916050340904586,
|
|
||||||
"rotation_deg": 0.26477854089271946
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 7,
|
|
||||||
"translation_m": 0.043365771415170416,
|
|
||||||
"rotation_deg": 0.10315887875456749
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 8,
|
|
||||||
"translation_m": 0.06454458096453627,
|
|
||||||
"rotation_deg": 0.8075022372885624
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 9,
|
|
||||||
"translation_m": 0.05200877185535076,
|
|
||||||
"rotation_deg": 0.5226003188288877
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 10,
|
|
||||||
"translation_m": 0.08066933230596428,
|
|
||||||
"rotation_deg": 0.6774188851574047
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 11,
|
|
||||||
"translation_m": 0.02679009789021938,
|
|
||||||
"rotation_deg": 0.5921424571930466
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 12,
|
|
||||||
"translation_m": 0.033564310488394096,
|
|
||||||
"rotation_deg": 1.0085274567011704
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 13,
|
|
||||||
"translation_m": 0.11139219509163403,
|
|
||||||
"rotation_deg": 0.596821136461582
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 14,
|
|
||||||
"translation_m": 0.06985048714143455,
|
|
||||||
"rotation_deg": 0.8031764622169965
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 15,
|
|
||||||
"translation_m": 0.05926582195242711,
|
|
||||||
"rotation_deg": 2.1146215228832355
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 16,
|
|
||||||
"translation_m": 0.053822686491612724,
|
|
||||||
"rotation_deg": 0.5853967911734774
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 17,
|
|
||||||
"translation_m": 0.06095128937580921,
|
|
||||||
"rotation_deg": 0.44465865069314414
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 18,
|
|
||||||
"translation_m": 0.1465573557185661,
|
|
||||||
"rotation_deg": 2.0008653910594214
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 19,
|
|
||||||
"translation_m": 0.10402727477620027,
|
|
||||||
"rotation_deg": 0.1402969578428506
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 20,
|
|
||||||
"translation_m": 0.06790583241641802,
|
|
||||||
"rotation_deg": 0.31916987926311186
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 21,
|
|
||||||
"translation_m": 0.037917260718891004,
|
|
||||||
"rotation_deg": 0.6351484564817018
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 22,
|
|
||||||
"translation_m": 0.03171219066433979,
|
|
||||||
"rotation_deg": 0.7611907317100989
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 23,
|
|
||||||
"translation_m": 0.08117084986905364,
|
|
||||||
"rotation_deg": 1.5477927872017732
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 24,
|
|
||||||
"translation_m": 0.07160138896826257,
|
|
||||||
"rotation_deg": 0.47596695875013556
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 25,
|
|
||||||
"translation_m": 0.01792823137987938,
|
|
||||||
"rotation_deg": 0.3736004835939083
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 26,
|
|
||||||
"translation_m": 0.07286630775287825,
|
|
||||||
"rotation_deg": 1.800095203725142
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 27,
|
|
||||||
"translation_m": 0.18109166588563658,
|
|
||||||
"rotation_deg": 3.9699177938767174
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 28,
|
|
||||||
"translation_m": 0.24825286214410192,
|
|
||||||
"rotation_deg": 4.619204763488191
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 29,
|
|
||||||
"translation_m": 0.06588464658827484,
|
|
||||||
"rotation_deg": 0.8156362703186536
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 30,
|
|
||||||
"translation_m": 0.07851282552634996,
|
|
||||||
"rotation_deg": 4.018956419097684
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 31,
|
|
||||||
"translation_m": 0.16064262686816105,
|
|
||||||
"rotation_deg": 4.8460088207546645
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 32,
|
|
||||||
"translation_m": 0.1352330525899733,
|
|
||||||
"rotation_deg": 4.936245135060849
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 33,
|
|
||||||
"translation_m": 0.0649121457474399,
|
|
||||||
"rotation_deg": 1.99675397761927
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 34,
|
|
||||||
"translation_m": 0.0460266060768811,
|
|
||||||
"rotation_deg": 1.2713317047431354
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 35,
|
|
||||||
"translation_m": 0.11269690515103371,
|
|
||||||
"rotation_deg": 0.5444667992697017
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 36,
|
|
||||||
"translation_m": 0.05570004024342942,
|
|
||||||
"rotation_deg": 0.6200055232178192
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 37,
|
|
||||||
"translation_m": 0.006720345812259607,
|
|
||||||
"rotation_deg": 1.1919322914717754
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 38,
|
|
||||||
"translation_m": 0.05571822171452726,
|
|
||||||
"rotation_deg": 0.8100733260702963
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 39,
|
|
||||||
"translation_m": 0.03156268585174415,
|
|
||||||
"rotation_deg": 1.1994412303651207
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 40,
|
|
||||||
"translation_m": 0.1396566575693052,
|
|
||||||
"rotation_deg": 1.219665033277036
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 41,
|
|
||||||
"translation_m": 0.42067987248571403,
|
|
||||||
"rotation_deg": 1.1901837024442368
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 42,
|
|
||||||
"translation_m": 0.5307044931342686,
|
|
||||||
"rotation_deg": 1.3118186035265211
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 43,
|
|
||||||
"translation_m": 0.0278193588287066,
|
|
||||||
"rotation_deg": 0.5463366974423033
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 44,
|
|
||||||
"translation_m": 0.08083543060888142,
|
|
||||||
"rotation_deg": 1.597876847929754
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 45,
|
|
||||||
"translation_m": 0.3628770836511988,
|
|
||||||
"rotation_deg": 0.7452934588576191
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 46,
|
|
||||||
"translation_m": 0.40429181316256,
|
|
||||||
"rotation_deg": 1.3531101551442906
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 47,
|
|
||||||
"translation_m": 0.3857777439966788,
|
|
||||||
"rotation_deg": 0.596976162782461
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 48,
|
|
||||||
"translation_m": 0.18867103421064077,
|
|
||||||
"rotation_deg": 0.5750764247339077
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 49,
|
|
||||||
"translation_m": 0.3781049775701853,
|
|
||||||
"rotation_deg": 1.067294104551723
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 50,
|
|
||||||
"translation_m": 0.33164608192922734,
|
|
||||||
"rotation_deg": 0.4502803262827393
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 51,
|
|
||||||
"translation_m": 0.39693080668345215,
|
|
||||||
"rotation_deg": 0.7722797079864339
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 52,
|
|
||||||
"translation_m": 0.05933929949217937,
|
|
||||||
"rotation_deg": 0.9106743636918464
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 53,
|
|
||||||
"translation_m": 0.09592936224298491,
|
|
||||||
"rotation_deg": 0.8770147335571411
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 54,
|
|
||||||
"translation_m": 0.10907903367697035,
|
|
||||||
"rotation_deg": 0.527094410495204
|
|
||||||
}
|
|
||||||
]
|
|
||||||
},
|
|
||||||
"weighted_jacobian_condition_number": 5.739204995062189,
|
|
||||||
"linearized_one_sigma": {
|
|
||||||
"translation_m": [
|
|
||||||
0.006273860396200483,
|
|
||||||
0.006271739194739563,
|
|
||||||
0.005766938056265489
|
|
||||||
],
|
|
||||||
"rotation_deg": [
|
|
||||||
0.06648607826531162,
|
|
||||||
0.07036185324485841,
|
|
||||||
0.1461990497815168
|
|
||||||
],
|
|
||||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
|
||||||
},
|
|
||||||
"bootstrap": {
|
|
||||||
"runs": 100,
|
|
||||||
"order": [
|
|
||||||
"x_m",
|
|
||||||
"y_m",
|
|
||||||
"z_m",
|
|
||||||
"roll_deg",
|
|
||||||
"pitch_deg",
|
|
||||||
"yaw_deg"
|
|
||||||
],
|
|
||||||
"std": [
|
|
||||||
0.002418391924231912,
|
|
||||||
0.0018782458573924745,
|
|
||||||
0.0024162629680468473,
|
|
||||||
0.09973116386354614,
|
|
||||||
0.0717005619911583,
|
|
||||||
0.09954619224880032
|
|
||||||
],
|
|
||||||
"p025": [
|
|
||||||
1.6323740665381714,
|
|
||||||
-0.24963297512571514,
|
|
||||||
0.08157494287763181,
|
|
||||||
-0.9175278935375253,
|
|
||||||
1.1732724086153574,
|
|
||||||
-22.44062221365902
|
|
||||||
],
|
|
||||||
"p975": [
|
|
||||||
1.6411098578257144,
|
|
||||||
-0.24215579438024784,
|
|
||||||
0.09016027720271011,
|
|
||||||
-0.5264292351234366,
|
|
||||||
1.4226725074325792,
|
|
||||||
-22.072092788653528
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"z_constraint": {
|
|
||||||
"observable_from_planar_AX_XB": false,
|
|
||||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
|
||||||
"rtk_reference_height_above_ground_m": 0.8535,
|
|
||||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
|
||||||
},
|
|
||||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification"
|
|
||||||
}
|
|
||||||
@@ -1,48 +0,0 @@
|
|||||||
{
|
|
||||||
"final": {
|
|
||||||
"translation_m": [
|
|
||||||
1.6381793500373911,
|
|
||||||
-0.24084479868828831,
|
|
||||||
0.08448123595331278
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.8171674587248069,
|
|
||||||
1.323288118779805,
|
|
||||||
-22.104163317857477
|
|
||||||
],
|
|
||||||
"pairs": 25,
|
|
||||||
"translation_rms_m": 0.10020667268070801,
|
|
||||||
"rotation_rms_deg": 1.2527941187072538,
|
|
||||||
"condition_number": 7.739413195936781
|
|
||||||
},
|
|
||||||
"backend_difference": {
|
|
||||||
"translation_m": 0.003889255293759414,
|
|
||||||
"rotation_deg": 0.1884307130161592,
|
|
||||||
"delta_matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.9999968771376496,
|
|
||||||
0.0024968749848742764,
|
|
||||||
-0.00010644368722136346,
|
|
||||||
0.0008526003522697501
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.0024970968314049877,
|
|
||||||
0.9999945974960792,
|
|
||||||
-0.0021376356258303525,
|
|
||||||
-0.003658904827736509
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.00010110570323801577,
|
|
||||||
0.0021378947504826257,
|
|
||||||
0.9999977095892133,
|
|
||||||
0.0010058801324768218
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,337 @@
|
|||||||
|
{
|
||||||
|
"schema_version": 1,
|
||||||
|
"success": true,
|
||||||
|
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
||||||
|
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
||||||
|
"frames": {
|
||||||
|
"RTK": {
|
||||||
|
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||||
|
"x_axis": "vehicle forward after applying the configured G90 heading offset",
|
||||||
|
"y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
|
||||||
|
"z_axis": "up",
|
||||||
|
"yaw_enu_deg": "90 - (rawHeadingDeg + -90)",
|
||||||
|
"frame_mode": "vehicle_forward_heading_offset"
|
||||||
|
},
|
||||||
|
"LiDAR": {
|
||||||
|
"description": "raw Helios sensor frame from points_raw polar decode",
|
||||||
|
"x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
|
||||||
|
"y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
|
||||||
|
"z_axis": "up",
|
||||||
|
"origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"backend": "consensus",
|
||||||
|
"measured_lidar_extrinsic_used_as_initial": true,
|
||||||
|
"solver_initial_extrinsic": "D:\\First-dev-dept\\calibration-rtk-run\\run\\rtk_lidar_mechanical_initial.json",
|
||||||
|
"body_heading_offset_deg": -90.0,
|
||||||
|
"body_heading_offset_used": true,
|
||||||
|
"body_antenna_lever_xy_used": false,
|
||||||
|
"translation_m": [
|
||||||
|
0.21782224963960972,
|
||||||
|
-0.41134780227275347,
|
||||||
|
0.1065423366878719
|
||||||
|
],
|
||||||
|
"rotation_rpy_deg_xyz": [
|
||||||
|
0.06623859235262805,
|
||||||
|
0.8096624730962496,
|
||||||
|
-0.5513220563826681
|
||||||
|
],
|
||||||
|
"quaternion_xyzw": [
|
||||||
|
0.00061201333859398,
|
||||||
|
0.007062715262034178,
|
||||||
|
-0.004815137247499642,
|
||||||
|
0.9999632782988027
|
||||||
|
],
|
||||||
|
"coordinate_contract_audit": {
|
||||||
|
"status": "no_near_180_degree_axis_conflict",
|
||||||
|
"requires_physical_axis_confirmation": false,
|
||||||
|
"mechanical_initial_path": "D:\\First-dev-dept\\calibration-rtk-run\\run\\rtk_lidar_mechanical_initial.json",
|
||||||
|
"mechanical_self_consistency": {
|
||||||
|
"baseline_points": "vehicle_right",
|
||||||
|
"frame_mode": "vehicle_forward_heading_offset",
|
||||||
|
"heading_offset_deg": -90.0,
|
||||||
|
"consistent": true,
|
||||||
|
"issues": []
|
||||||
|
},
|
||||||
|
"solution_vs_declared_baseline_side": {
|
||||||
|
"baseline_points": "vehicle_right",
|
||||||
|
"solution_yaw_deg": -0.5513220563826681,
|
||||||
|
"expected_yaw_deg": 0.0,
|
||||||
|
"yaw_error_deg": 0.5513220563826735,
|
||||||
|
"xy_error_m": 0.007516661287052368,
|
||||||
|
"z_error_m": 0.0280423356878719,
|
||||||
|
"mixed_translation_rotation_inheritance": false,
|
||||||
|
"near_expected_pose": true
|
||||||
|
},
|
||||||
|
"solution_relative_to_mechanical_initial": {
|
||||||
|
"translation_m": 0.029032271487700816,
|
||||||
|
"rotation_deg": 0.9820426535863694,
|
||||||
|
"delta_matrix_4x4": [
|
||||||
|
[
|
||||||
|
0.9998538650128304,
|
||||||
|
0.009638565806830942,
|
||||||
|
0.014119017957784103,
|
||||||
|
0.006962889639609726
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.009621275903042714,
|
||||||
|
0.9999528797859223,
|
||||||
|
-0.0012919976154994906,
|
||||||
|
0.002831671727246521
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.014130805670674627,
|
||||||
|
0.0011559658421926345,
|
||||||
|
0.9998994869856016,
|
||||||
|
0.0280423356878719
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"note": "No automatic 180-degree correction was applied. Confirm static GNHPR left/right vs vehicle heading and Helios +X vs vehicle forward before deployment."
|
||||||
|
},
|
||||||
|
"matrix_4x4": [
|
||||||
|
[
|
||||||
|
0.9998538650128304,
|
||||||
|
0.009638565806830942,
|
||||||
|
0.014119017957784103,
|
||||||
|
0.21782224963960972
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.009621275903042714,
|
||||||
|
0.9999528797859223,
|
||||||
|
-0.0012919976154994906,
|
||||||
|
-0.41134780227275347
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.014130805670674627,
|
||||||
|
0.0011559658421926345,
|
||||||
|
0.9998994869856016,
|
||||||
|
0.1065423366878719
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"quality": {
|
||||||
|
"stations": 27,
|
||||||
|
"pairs": 20,
|
||||||
|
"residuals": {
|
||||||
|
"pairs": 20,
|
||||||
|
"translation_m": {
|
||||||
|
"rms": 0.07116027693011169,
|
||||||
|
"median": 0.048258124379040895,
|
||||||
|
"p90": 0.10707349630410129,
|
||||||
|
"p95": 0.12100345665884242,
|
||||||
|
"max": 0.1859157912711514
|
||||||
|
},
|
||||||
|
"rotation_deg": {
|
||||||
|
"rms": 0.9820870524210346,
|
||||||
|
"median": 0.6015253089423158,
|
||||||
|
"p90": 1.626986719630708,
|
||||||
|
"p95": 1.7485896823567941,
|
||||||
|
"max": 2.6898815510329674
|
||||||
|
},
|
||||||
|
"per_pair": [
|
||||||
|
{
|
||||||
|
"pair_index": 0,
|
||||||
|
"translation_m": 0.10590532722733068,
|
||||||
|
"rotation_deg": 0.5565962122599133
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 1,
|
||||||
|
"translation_m": 0.11758701799503664,
|
||||||
|
"rotation_deg": 0.36543717366192036
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 2,
|
||||||
|
"translation_m": 0.07568813061789224,
|
||||||
|
"rotation_deg": 1.6990480050580474
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 3,
|
||||||
|
"translation_m": 0.07918615709103373,
|
||||||
|
"rotation_deg": 0.33723851079591255
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 4,
|
||||||
|
"translation_m": 0.04618587731787657,
|
||||||
|
"rotation_deg": 0.21044665580544528
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 5,
|
||||||
|
"translation_m": 0.1859157912711514,
|
||||||
|
"rotation_deg": 2.6898815510329674
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 6,
|
||||||
|
"translation_m": 0.015032558607699278,
|
||||||
|
"rotation_deg": 0.2283712127949818
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 7,
|
||||||
|
"translation_m": 0.013237326674396453,
|
||||||
|
"rotation_deg": 1.6189799101387812
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 8,
|
||||||
|
"translation_m": 0.07003654121179845,
|
||||||
|
"rotation_deg": 0.6464544056247182
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 9,
|
||||||
|
"translation_m": 0.093450766078183,
|
||||||
|
"rotation_deg": 0.6476769159531274
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 10,
|
||||||
|
"translation_m": 0.015054656507824072,
|
||||||
|
"rotation_deg": 1.041955302231996
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 11,
|
||||||
|
"translation_m": 0.02338150242985638,
|
||||||
|
"rotation_deg": 0.9212619385214977
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 12,
|
||||||
|
"translation_m": 0.01577916809950981,
|
||||||
|
"rotation_deg": 0.5021648028537498
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 13,
|
||||||
|
"translation_m": 0.05033037144020521,
|
||||||
|
"rotation_deg": 0.31352985458971655
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 14,
|
||||||
|
"translation_m": 0.059761151158357104,
|
||||||
|
"rotation_deg": 0.8078349560587313
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 15,
|
||||||
|
"translation_m": 0.04160156756834453,
|
||||||
|
"rotation_deg": 0.32936437427192145
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 16,
|
||||||
|
"translation_m": 0.02424387832291359,
|
||||||
|
"rotation_deg": 0.430799739443549
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 17,
|
||||||
|
"translation_m": 0.05938006964566194,
|
||||||
|
"rotation_deg": 0.3855347569405268
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 18,
|
||||||
|
"translation_m": 0.003922740066294881,
|
||||||
|
"rotation_deg": 0.8803664894801937
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"pair_index": 19,
|
||||||
|
"translation_m": 0.02258390614293418,
|
||||||
|
"rotation_deg": 0.9489255319885953
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"weighted_jacobian_condition_number": 7.551077537197385,
|
||||||
|
"linearized_one_sigma": {
|
||||||
|
"translation_m": [
|
||||||
|
0.01109415838733884,
|
||||||
|
0.011043247019191056,
|
||||||
|
0.0049617743728656676
|
||||||
|
],
|
||||||
|
"rotation_deg": [
|
||||||
|
0.08809558088476348,
|
||||||
|
0.08691570892756702,
|
||||||
|
0.13552099560800782
|
||||||
|
],
|
||||||
|
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||||
|
},
|
||||||
|
"bootstrap": {
|
||||||
|
"runs": 200,
|
||||||
|
"order": [
|
||||||
|
"x_m",
|
||||||
|
"y_m",
|
||||||
|
"z_m",
|
||||||
|
"roll_deg",
|
||||||
|
"pitch_deg",
|
||||||
|
"yaw_deg"
|
||||||
|
],
|
||||||
|
"std": [
|
||||||
|
0.006235825930578342,
|
||||||
|
0.004851737742140203,
|
||||||
|
0.0004474159829207979,
|
||||||
|
0.07186455153709871,
|
||||||
|
0.11762318545143821,
|
||||||
|
0.061763211598034336
|
||||||
|
],
|
||||||
|
"p025": [
|
||||||
|
0.20955504469154063,
|
||||||
|
-0.4174678620205485,
|
||||||
|
0.10568919463688603,
|
||||||
|
-0.06202286115194262,
|
||||||
|
0.5207464314096704,
|
||||||
|
-0.6639417037512721
|
||||||
|
],
|
||||||
|
"p975": [
|
||||||
|
0.23211472114546824,
|
||||||
|
-0.4001091835786546,
|
||||||
|
0.1074353326322909,
|
||||||
|
0.23746760471402314,
|
||||||
|
1.0462704287205609,
|
||||||
|
-0.43455346485967655
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"z_constraint": {
|
||||||
|
"observable_from_planar_AX_XB": false,
|
||||||
|
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
||||||
|
"rtk_reference_height_above_ground_m": 1.9165,
|
||||||
|
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
||||||
|
},
|
||||||
|
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
|
||||||
|
"selection": {
|
||||||
|
"recommended": true,
|
||||||
|
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
||||||
|
"open3d_vs_small_gicp": {
|
||||||
|
"translation_m": 0.00312519750472982,
|
||||||
|
"rotation_deg": 0.11907006217018351,
|
||||||
|
"delta_matrix_4x4": [
|
||||||
|
[
|
||||||
|
0.9999996872332217,
|
||||||
|
-0.00030752729270551075,
|
||||||
|
-0.0007286703114418758,
|
||||||
|
0.000533303946557151
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.00030892706731572284,
|
||||||
|
0.9999981058814015,
|
||||||
|
0.0019216653392719056,
|
||||||
|
-0.002909732881513971
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0007280779667146176,
|
||||||
|
-0.0019218898442212235,
|
||||||
|
0.9999978881187204,
|
||||||
|
0.001007919095162138
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
{
|
||||||
|
"final": {
|
||||||
|
"translation_m": [
|
||||||
|
0.21782224963960972,
|
||||||
|
-0.41134780227275347,
|
||||||
|
0.1065423366878719
|
||||||
|
],
|
||||||
|
"rotation_rpy_deg_xyz": [
|
||||||
|
0.06623859235262805,
|
||||||
|
0.8096624730962496,
|
||||||
|
-0.5513220563826681
|
||||||
|
],
|
||||||
|
"pairs": 20,
|
||||||
|
"translation_rms_m": 0.07116027693011169,
|
||||||
|
"rotation_rms_deg": 0.9820870524210346,
|
||||||
|
"condition_number": 7.551077537197385,
|
||||||
|
"coordinate_contract_status": "no_near_180_degree_axis_conflict",
|
||||||
|
"recommended_for_deployment": true
|
||||||
|
},
|
||||||
|
"backend_difference": {
|
||||||
|
"translation_m": 0.00312519750472982,
|
||||||
|
"rotation_deg": 0.11907006217018351,
|
||||||
|
"delta_matrix_4x4": [
|
||||||
|
[
|
||||||
|
0.9999996872332217,
|
||||||
|
-0.00030752729270551075,
|
||||||
|
-0.0007286703114418758,
|
||||||
|
0.000533303946557151
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.00030892706731572284,
|
||||||
|
0.9999981058814015,
|
||||||
|
0.0019216653392719056,
|
||||||
|
-0.002909732881513971
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0007280779667146176,
|
||||||
|
-0.0019218898442212235,
|
||||||
|
0.9999978881187204,
|
||||||
|
0.001007919095162138
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
+59
-14
@@ -1,27 +1,72 @@
|
|||||||
# run目录
|
# run目录
|
||||||
|
|
||||||
根README包含完整复现命令；这里仅列入口职责。
|
根 README 含完整复现与本次结果说明；这里只列入口职责。
|
||||||
|
|
||||||
| 脚本 | 用途 |
|
| 脚本 | 用途 |
|
||||||
|---|---|
|
|---|---|
|
||||||
| `run_full_pipeline.ps1` | 调用一步导出得到 `combined/`，再跑到最终 `T_RTK_lidar` |
|
| `run_full_pipeline.ps1` | 站目录导出 `combined/` 后跑到 `T_RTK_lidar` |
|
||||||
| `export_multisensor_stations.ps1` | 薄封装：调用 `tools/export_raw_to_combined.py` |
|
| `export_multisensor_stations.ps1` | 薄封装：`tools/export_raw_to_combined.py` |
|
||||||
| `prepare_multisensor_dataset.ps1` | 每站选一帧，生成yaw-only RTK参考轨迹和`frames_all` |
|
| `prepare_multisensor_dataset.ps1` | 每站一帧 + RTK 位姿（默认含双天线 pitch/roll） |
|
||||||
| `run_direct_rtk_lidar.ps1` | 从combined数据运行RTK直接标定和最终结果封装 |
|
| `run_direct_rtk_lidar.ps1` | 从 `combined/` 标定并封装最终结果（**默认车头向前 -90**） |
|
||||||
| `run_single_dataset.ps1` | 执行地面、两个GICP后端、精筛、共识和AX=XB求解 |
|
| `run_single_dataset.ps1` | 地面、双 GICP、精筛、共识、AX=XB |
|
||||||
| `run_joint_rtk_lidar.ps1` | 合并多个独立批次的批内共识运动对和地面平面，求解共享外参 |
|
| `run_joint_rtk_lidar.ps1` | 多批共识对联合求解 |
|
||||||
| `view_result.ps1` | 打开3D运动对对比并打印数值增量 |
|
| `view_result.ps1` | 3D 运动对对比 |
|
||||||
|
| `rtk_lidar_mechanical_initial.json` | 仅 AX=XB 初值；**禁止**用于 pair |
|
||||||
|
|
||||||
原始→中间包请优先直接用 Python 一步导出（与 Lidar-IMU 用法对齐）：
|
## 默认参数（匹配当前约 2 m 车顶雷达 / 车头向前）
|
||||||
|
|
||||||
|
| 参数 | 默认 |
|
||||||
|
|---|---|
|
||||||
|
| `HeadingOffsetDeg` | `-90`（车头向前；主从装反、基线朝右） |
|
||||||
|
| `GroundZMin/Max` | `-2.5` / `-1.5` |
|
||||||
|
| `ExpectedStations` | `27` |
|
||||||
|
| `MinStations` | `20` |
|
||||||
|
| `RtkReferenceHeightAboveGroundM` | **无默认，必填**（本车 1.9165） |
|
||||||
|
|
||||||
|
pair 注册**不传** `--initial-extrinsic`。
|
||||||
|
|
||||||
|
## 原始 → combined
|
||||||
|
|
||||||
|
站目录：
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu-rscap ... --out ... --overwrite
|
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`）
|
G90 连续录制 + 站时间窗：
|
||||||
- `-TimeBasis device_gnss`（默认）：雷达设备时 ↔ GNSS week/TOW
|
|
||||||
- `-TimeBasis host`：旧 dlog + 主机接收时间关联
|
|
||||||
|
|
||||||
所有路径均为命令行参数。标定入口要求显式传入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_vehicle_h19165" `
|
||||||
|
-OutputRoot "D:\data\rtk_lidar_run\outputs_vehicle_h19165" `
|
||||||
|
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||||
|
-HeadingOffsetDeg -90 `
|
||||||
|
-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_vehicle_h19165\frames_all" `
|
||||||
|
-Pairs "D:\data\rtk_lidar_run\outputs_vehicle_h19165\consensus\B_consensus.npz" `
|
||||||
|
-Extrinsic "D:\data\rtk_lidar_run\outputs_vehicle_h19165\final_T_RTK_lidar.json" `
|
||||||
|
-PairIndex 0
|
||||||
|
```
|
||||||
|
|||||||
@@ -4,7 +4,7 @@ param(
|
|||||||
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
|
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
|
||||||
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
|
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
|
||||||
[string]$PoseName = "rtk_gga_raw_heading",
|
[string]$PoseName = "rtk_gga_raw_heading",
|
||||||
[int]$MinStations = 30,
|
[int]$MinStations = 20,
|
||||||
[int]$ExpectedStations = 0,
|
[int]$ExpectedStations = 0,
|
||||||
[double]$HeadingStdLimitDeg = 0.5,
|
[double]$HeadingStdLimitDeg = 0.5,
|
||||||
[switch]$Overwrite
|
[switch]$Overwrite
|
||||||
|
|||||||
@@ -0,0 +1,41 @@
|
|||||||
|
{
|
||||||
|
"schema_version": 3,
|
||||||
|
"convention": "T_RTK_lidar maps raw LiDAR points into the vehicle-forward RTK body frame (X forward, Y left, Z up) after HeadingOffsetDeg=-90",
|
||||||
|
"frame_mode": "vehicle_forward_heading_offset",
|
||||||
|
"heading_offset_deg": -90.0,
|
||||||
|
"baseline_points": "vehicle_right",
|
||||||
|
"baseline_points_note": "Field-confirmed: master/slave assignment reversed vs G90 diagram, antennas left-right symmetric about rear-axle centerline. Master/GGA on vehicle left, slave on right; rawHeading points vehicle right.",
|
||||||
|
"vehicle_flu_lever_master_to_lidar_m": [
|
||||||
|
0.210859360,
|
||||||
|
-0.414179474,
|
||||||
|
0.078500001
|
||||||
|
],
|
||||||
|
"vehicle_flu_note": "Vehicle FLU: LiDAR origin relative to master/GGA = ahead, right, above. CAD drawing X was opposite vehicle-forward; longitudinal sign is +X in true FLU (solver also converges to +X).",
|
||||||
|
"antenna_symmetry_note": "Master/slave are mirrors about the rear-axle centerline; swap flips baseline 180° and the vehicle-Y sign of the master→LiDAR lever",
|
||||||
|
"translation_m": [
|
||||||
|
0.210859360,
|
||||||
|
-0.414179474,
|
||||||
|
0.078500001
|
||||||
|
],
|
||||||
|
"rotation_rpy_deg_xyz": [
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"matrix_4x4": [
|
||||||
|
[1.0, 0.0, 0.0, 0.210859360],
|
||||||
|
[0.0, 1.0, 0.0, -0.414179474],
|
||||||
|
[0.0, 0.0, 1.0, 0.078500001],
|
||||||
|
[0.0, 0.0, 0.0, 1.0]
|
||||||
|
],
|
||||||
|
"use": "Final AX=XB solver initialization only; never use for LiDAR pair registration",
|
||||||
|
"yaw_note": "In vehicle-forward delivery, LiDAR +X ≈ vehicle forward ⇒ mechanical yaw ≈ 0",
|
||||||
|
"z_note": "78.500001 mm = H_L - H_R with H_L=1994.999879 mm, H_R=1916.499878 mm",
|
||||||
|
"attitude_composition": "R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset); pitch/roll stay in baseline frame",
|
||||||
|
"baseline_frame_equivalent": {
|
||||||
|
"heading_offset_deg": 0.0,
|
||||||
|
"translation_m": [0.414179474, 0.210859360, 0.078500001],
|
||||||
|
"rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
|
||||||
|
"note": "Same physical install expressed in rawHeading baseline frame"
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -3,32 +3,67 @@ param(
|
|||||||
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
||||||
[string]$OutputRoot = "",
|
[string]$OutputRoot = "",
|
||||||
[string]$WorkRoot = "",
|
[string]$WorkRoot = "",
|
||||||
[int]$ExpectedStations = 34,
|
[int]$ExpectedStations = 27,
|
||||||
|
[int]$MinStations = 20,
|
||||||
[int]$MinPairs = 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. Default vehicle-forward for this car: -90
|
||||||
|
# (master/slave swapped, baseline points vehicle-right).
|
||||||
|
[double]$HeadingOffsetDeg = -90.0,
|
||||||
|
[string]$SolverInitialExtrinsic = "",
|
||||||
|
[double]$RefineMinInlierRatio = 0.63,
|
||||||
|
[double]$RefineMaxInlierRmseM = 0.14
|
||||||
)
|
)
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
$ErrorActionPreference = "Stop"
|
||||||
$Repo = Split-Path -Parent $PSScriptRoot
|
$Repo = Split-Path -Parent $PSScriptRoot
|
||||||
if ([string]::IsNullOrWhiteSpace($OutputRoot)) { $OutputRoot = Join-Path $Repo "outputs\rtk_lidar_calibration" }
|
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($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
|
$Prepared = $WorkRoot
|
||||||
|
|
||||||
& (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") `
|
& (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") `
|
||||||
-CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg 0 `
|
-CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg $HeadingOffsetDeg `
|
||||||
-AntennaLever @(0.0,0.0,0.0) -PoseName "rtk_gga_raw_heading" -MinStations 30 -ExpectedStations $ExpectedStations -Overwrite
|
-AntennaLever @(0.0,0.0,0.0) -PoseName $PoseName -MinStations $MinStations `
|
||||||
|
-ExpectedStations $ExpectedStations -Overwrite
|
||||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }
|
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") `
|
& (Join-Path $Repo "run\run_single_dataset.ps1") `
|
||||||
-Prepared $Prepared -OutputRoot $OutputRoot `
|
-Prepared $Prepared -OutputRoot $OutputRoot `
|
||||||
-ReferencePoseFile "reference_poses_rtk_gga_raw_heading.csv" `
|
-ReferencePoseFile $ReferencePoseFile `
|
||||||
-ReferenceHeight $RtkReferenceHeightAboveGroundM -MinPairs $MinPairs -Bootstrap $Bootstrap
|
-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" }
|
if ($LASTEXITCODE -ne 0) { throw "RTK-direct calibration failed" }
|
||||||
|
|
||||||
$Finalize = @(
|
$Finalize = @(
|
||||||
(Join-Path $Repo "code\finalize_direct_rtk_lidar.py"),
|
(Join-Path $Repo "code\finalize_direct_rtk_lidar.py"),
|
||||||
"--result-root", $OutputRoot,
|
"--result-root", $OutputRoot,
|
||||||
"--reference-height", "$RtkReferenceHeightAboveGroundM"
|
"--reference-height", "$RtkReferenceHeightAboveGroundM",
|
||||||
|
"--heading-offset-deg", "$HeadingOffsetDeg"
|
||||||
)
|
)
|
||||||
& python @Finalize
|
& python @Finalize
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
|
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
|
||||||
|
|||||||
@@ -8,9 +8,14 @@
|
|||||||
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
||||||
[string]$Timezone = "+08:00",
|
[string]$Timezone = "+08:00",
|
||||||
[ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss",
|
[ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss",
|
||||||
[int]$ExpectedStations = 34,
|
[int]$ExpectedStations = 27,
|
||||||
|
[int]$MinStations = 20,
|
||||||
[int]$MinPairs = 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 = -90.0
|
||||||
)
|
)
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
$ErrorActionPreference = "Stop"
|
||||||
@@ -28,7 +33,10 @@ if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" }
|
|||||||
-CombinedRoot (Join-Path $ExportRoot "combined") `
|
-CombinedRoot (Join-Path $ExportRoot "combined") `
|
||||||
-WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
|
-WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
|
||||||
-RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM `
|
-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" }
|
if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" }
|
||||||
|
|
||||||
Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')"
|
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)][string]$OutputRoot,
|
||||||
[Parameter(Mandatory = $true)][double]$ReferenceHeight,
|
[Parameter(Mandatory = $true)][double]$ReferenceHeight,
|
||||||
[string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
|
[string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
|
||||||
|
[int]$MinStations = 20,
|
||||||
[int]$MinPairs = 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"
|
$ErrorActionPreference = "Stop"
|
||||||
@@ -32,27 +42,38 @@ foreach ($Path in @($Frames, $ReferencePoses)) {
|
|||||||
New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null
|
New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null
|
||||||
|
|
||||||
$Ground = Join-Path $Common "ground_planes.csv"
|
$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")) {
|
foreach ($Backend in @("small_gicp", "open3d")) {
|
||||||
$Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
|
$Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
|
||||||
$Raw = Join-Path $Directory "B_estimation.npz"
|
$Raw = Join-Path $Directory "B_estimation.npz"
|
||||||
$QualityJson = Join-Path $Directory "B_quality.json"
|
$QualityJson = Join-Path $Directory "B_quality.json"
|
||||||
$QualityCsv = Join-Path $Directory "B_quality.csv"
|
$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,
|
$PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--reference-poses", $ReferencePoses,
|
||||||
"--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv,
|
"--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv,
|
||||||
"--min-pairs", "$MinPairs")
|
"--min-stations", "$MinStations", "--min-pairs", "$MinPairs", "--max-gap", "$MaxGap")
|
||||||
if ($Backend -eq "open3d") { $PairArgs += @("--max-gap", "3", "--multistart", "1", "--iterations", "40") }
|
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 pairs" $PairArgs
|
||||||
Run-Python "$Backend X-independent refinement" @(
|
Run-Python "$Backend X-independent refinement" @(
|
||||||
$Refine, "--pairs", $Raw, "--quality-json", $QualityJson,
|
$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"),
|
$Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
|
||||||
"--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
|
"--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
|
||||||
"--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
|
"--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"
|
$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
|
||||||
@@ -61,10 +82,14 @@ Run-Python "cross-backend consensus" @(
|
|||||||
"--small-pairs", (Join-Path $Small "B_refined.npz"),
|
"--small-pairs", (Join-Path $Small "B_refined.npz"),
|
||||||
"--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
|
"--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
|
||||||
)
|
)
|
||||||
Run-Python "consensus calibration" @(
|
$ConsensusCalibrationArgs = @(
|
||||||
$Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
|
$Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
|
||||||
"--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
|
"--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
|
||||||
"--output", (Join-Path $ConsensusOut "extrinsic.json")
|
"--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"
|
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,153 @@
|
|||||||
|
"""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 ( # noqa: E402
|
||||||
|
coordinate_contract_audit,
|
||||||
|
mechanical_self_consistency,
|
||||||
|
)
|
||||||
|
from prepare_multisensor_station_dataset import heading_to_enu_yaw # noqa: E402
|
||||||
|
from rtk_attitude import attitude_rotation, rtk_body_rotation # 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_attitude_rotation_applies_baseline_pitch_elevation() -> None:
|
||||||
|
_, yaw = heading_to_enu_yaw(0.0, 0.0) # heading north → body X = +North
|
||||||
|
rotation = attitude_rotation(yaw, pitch_deg=10.0, roll_deg=0.0)
|
||||||
|
body_x = rotation @ np.array([1.0, 0.0, 0.0])
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
body_x,
|
||||||
|
[0.0, math.cos(math.radians(10.0)), math.sin(math.radians(10.0))],
|
||||||
|
atol=1e-12,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_vehicle_forward_offset_keeps_pitch_about_baseline() -> None:
|
||||||
|
# Baseline points east (vehicle right if nose north); pitch elevates baseline X.
|
||||||
|
# Vehicle-forward offset -90 must not simply Ry after vehicle yaw.
|
||||||
|
raw_heading = 90.0
|
||||||
|
pitch = 10.0
|
||||||
|
r_correct = rtk_body_rotation(raw_heading, -90.0, pitch_deg=pitch, roll_deg=0.0)
|
||||||
|
_, yaw_raw = heading_to_enu_yaw(raw_heading, 0.0)
|
||||||
|
r_baseline = attitude_rotation(yaw_raw, pitch_deg=pitch, roll_deg=0.0)
|
||||||
|
rz90 = np.array([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
|
||||||
|
np.testing.assert_allclose(r_correct, r_baseline @ rz90, atol=1e-12)
|
||||||
|
# Level vehicle-forward X should point north.
|
||||||
|
r_level = rtk_body_rotation(raw_heading, -90.0, pitch_deg=0.0, roll_deg=0.0)
|
||||||
|
np.testing.assert_allclose(r_level @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-12)
|
||||||
|
|
||||||
|
|
||||||
|
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_is_vehicle_forward_swapped_master() -> None:
|
||||||
|
path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
|
||||||
|
document = __import__("json").loads(path.read_text(encoding="utf-8-sig"))
|
||||||
|
transform = load_extrinsic_matrix(path)
|
||||||
|
np.testing.assert_allclose(transform[:3, 3], [0.210859360, -0.414179474, 0.078500001])
|
||||||
|
np.testing.assert_allclose(transform[:3, :3], np.eye(3))
|
||||||
|
np.testing.assert_allclose(params_transform(transform_params(transform)), transform, atol=1e-12)
|
||||||
|
assert document["baseline_points"] == "vehicle_right"
|
||||||
|
assert document["frame_mode"] == "vehicle_forward_heading_offset"
|
||||||
|
assert document["heading_offset_deg"] == -90.0
|
||||||
|
assert document["rotation_rpy_deg_xyz"][2] == 0.0
|
||||||
|
check = mechanical_self_consistency(document)
|
||||||
|
assert check["consistent"] is True
|
||||||
|
|
||||||
|
|
||||||
|
def test_mixed_left_xy_plus_right_yaw_mechanical_is_rejected() -> None:
|
||||||
|
mixed = {
|
||||||
|
"baseline_points": "vehicle_left",
|
||||||
|
"translation_m": [0.414179474, 0.210859360, 0.078500001],
|
||||||
|
"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.078500001],
|
||||||
|
[0.0, 0.0, 0.0, 1.0],
|
||||||
|
],
|
||||||
|
}
|
||||||
|
check = mechanical_self_consistency(mixed)
|
||||||
|
assert check["consistent"] is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_deprecated_minus_xy_right_baseline_is_rejected_for_swapped_master() -> None:
|
||||||
|
deprecated = {
|
||||||
|
"baseline_points": "vehicle_right",
|
||||||
|
"translation_m": [-0.414179474, -0.210859360, 0.078500001],
|
||||||
|
"rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
|
||||||
|
}
|
||||||
|
check = mechanical_self_consistency(deprecated)
|
||||||
|
assert check["consistent"] is False
|
||||||
|
|
||||||
|
|
||||||
|
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)
|
||||||
|
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
|
||||||
|
|
||||||
|
|
||||||
|
def test_previous_mixed_result_branch_is_not_recommended() -> None:
|
||||||
|
"""Old baseline-frame mixed solution disagrees with vehicle-forward mechanical initial."""
|
||||||
|
initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
|
||||||
|
solution = np.array(
|
||||||
|
[
|
||||||
|
[0.00942353438668686, -0.9999215926659111, 0.00824654595155475, 0.4123055815579212],
|
||||||
|
[0.9998714355322929, 0.009529416150714898, 0.012895837871912157, 0.2173092104098051],
|
||||||
|
[-0.012973411511822136, 0.008123961367132958, 0.9998828390593821, 0.10405760639434848],
|
||||||
|
[0.0, 0.0, 0.0, 1.0],
|
||||||
|
],
|
||||||
|
float,
|
||||||
|
)
|
||||||
|
audit = coordinate_contract_audit({
|
||||||
|
"solver_initial_extrinsic": str(initial_path),
|
||||||
|
"matrix_4x4": solution.tolist(),
|
||||||
|
})
|
||||||
|
assert audit["requires_physical_axis_confirmation"] is True
|
||||||
|
assert audit["status"] in {
|
||||||
|
"near_180_degree_axis_conflict",
|
||||||
|
"solution_disagrees_with_mechanical_baseline_side",
|
||||||
|
}
|
||||||
+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（一般不必单独跑） |
|
| `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/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU（调试用）；一步导出已内嵌同等逻辑 |
|
||||||
| `rscap_v2/h32_msop.py` | H32 MSOP 解码（XYZ / 极坐标 `points_raw`） |
|
| `rscap_v2/h32_msop.py` | H32 MSOP 解码（XYZ / 极坐标 `points_raw`） |
|
||||||
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
|
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
|
||||||
|
|||||||
@@ -25,6 +25,7 @@ import numpy as np
|
|||||||
|
|
||||||
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
|
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
|
||||||
POSITION_TYPES = {"GGA", "PVTSLNA"}
|
POSITION_TYPES = {"GGA", "PVTSLNA"}
|
||||||
|
HEADING_TYPES = {"UNIHEADINGA", "GNHPR"}
|
||||||
|
|
||||||
|
|
||||||
def parse_named_path(text: str) -> tuple[str, Path]:
|
def parse_named_path(text: str) -> tuple[str, Path]:
|
||||||
@@ -182,7 +183,8 @@ def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
|
|||||||
("differential_age_s", np.float64, np.nan),
|
("differential_age_s", np.float64, np.nan),
|
||||||
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
||||||
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
||||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
("pitch_deg", np.float64, np.nan), ("roll_deg", np.float64, np.nan),
|
||||||
|
("heading_stddev_deg", np.float64, np.nan),
|
||||||
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
|
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
|
||||||
("solution_satellites", np.int32, -1),
|
("solution_satellites", np.int32, -1),
|
||||||
):
|
):
|
||||||
@@ -230,7 +232,7 @@ def build_combined(
|
|||||||
|
|
||||||
heading = []
|
heading = []
|
||||||
for row in rtk_rows:
|
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
|
continue
|
||||||
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
|
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
|
||||||
if assoc is None:
|
if assoc is None:
|
||||||
@@ -258,7 +260,14 @@ def build_combined(
|
|||||||
for segment_index, source in enumerate(frame_paths):
|
for segment_index, source in enumerate(frame_paths):
|
||||||
with np.load(source, allow_pickle=False) as frame:
|
with np.load(source, allow_pickle=False) as frame:
|
||||||
values = {key: np.asarray(frame[key]) for key in frame.files}
|
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)
|
position_index = nearest_index(position_times, lidar_time_ns)
|
||||||
heading_index = nearest_index(heading_times, lidar_time_ns)
|
heading_index = nearest_index(heading_times, lidar_time_ns)
|
||||||
@@ -289,11 +298,15 @@ def build_combined(
|
|||||||
for key, dtype, default in (
|
for key, dtype, default in (
|
||||||
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
||||||
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
||||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
("pitch_deg", np.float64, np.nan), ("roll_deg", np.float64, np.nan),
|
||||||
|
("heading_stddev_deg", np.float64, np.nan),
|
||||||
("pitch_stddev_deg", np.float64, np.nan),
|
("pitch_stddev_deg", np.float64, np.nan),
|
||||||
("solution_satellites", np.int32, -1),
|
("solution_satellites", np.int32, -1),
|
||||||
):
|
):
|
||||||
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
|
value = heading_row.get(key, default)
|
||||||
|
if key == "roll_deg" and value is None:
|
||||||
|
value = 0.0
|
||||||
|
values[f"rtk_{key}"] = np.asarray([value], dtype=dtype)
|
||||||
values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
|
values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
|
||||||
values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
|
values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
|
||||||
device_ns = gnss_utc_ns(heading_row, gps_utc_leap_seconds)
|
device_ns = gnss_utc_ns(heading_row, gps_utc_leap_seconds)
|
||||||
@@ -337,6 +350,7 @@ def build_combined(
|
|||||||
"output": str(output.relative_to(out)),
|
"output": str(output.relative_to(out)),
|
||||||
"source_lidar": str(source.resolve()),
|
"source_lidar": str(source.resolve()),
|
||||||
"lidar_time_ns": lidar_time_ns,
|
"lidar_time_ns": lidar_time_ns,
|
||||||
|
"lidar_device_time_ns": lidar_device_time_ns,
|
||||||
"rtk_gga_dt_ns": position_dt,
|
"rtk_gga_dt_ns": position_dt,
|
||||||
"rtk_heading_dt_ns": heading_dt,
|
"rtk_heading_dt_ns": heading_dt,
|
||||||
"rtk_valid": position_ok,
|
"rtk_valid": position_ok,
|
||||||
|
|||||||
@@ -0,0 +1,295 @@
|
|||||||
|
#!/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
|
||||||
|
|
||||||
|
from rtk_attitude import heading_to_enu_yaw, rotation_to_quat_xyzw, rtk_body_rotation
|
||||||
|
|
||||||
|
|
||||||
|
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)
|
||||||
|
p.add_argument(
|
||||||
|
"--heading-offset-deg",
|
||||||
|
type=float,
|
||||||
|
default=None,
|
||||||
|
help="Added to rawHeading before ENU yaw. Default: body_heading_offset_deg from extrinsic JSON, else 0.",
|
||||||
|
)
|
||||||
|
p.add_argument(
|
||||||
|
"--orientation-model",
|
||||||
|
choices=("heading_pitch_roll", "yaw_only"),
|
||||||
|
default="heading_pitch_roll",
|
||||||
|
help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll in T_W_RTK; yaw_only forces pitch=roll=0",
|
||||||
|
)
|
||||||
|
return p.parse_args()
|
||||||
|
|
||||||
|
|
||||||
|
POSITION_TYPES = {"GGA", "PVTSLNA"}
|
||||||
|
HEADING_TYPES = {"UNIHEADINGA", "GNHPR"}
|
||||||
|
|
||||||
|
|
||||||
|
def heading_row_valid(row: dict[str, Any]) -> bool:
|
||||||
|
if row.get("type") == "UNIHEADINGA":
|
||||||
|
return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
|
||||||
|
if row.get("type") == "GNHPR":
|
||||||
|
return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def heading_quality_ok(row: dict[str, Any], std_limit_deg: float) -> list[str]:
|
||||||
|
reasons: list[str] = []
|
||||||
|
if row.get("type") == "UNIHEADINGA":
|
||||||
|
if str(row.get("heading_solution", "")) != "NARROW_INT":
|
||||||
|
reasons.append("HEADING_NOT_NARROW_INT")
|
||||||
|
std = float(row.get("heading_stddev_deg") or math.inf)
|
||||||
|
if std > std_limit_deg:
|
||||||
|
reasons.append("HEADING_STD_EXCEEDED")
|
||||||
|
elif row.get("type") == "GNHPR":
|
||||||
|
quality = int(row.get("heading_quality", -1) or -1)
|
||||||
|
if quality not in {4, 5} and not row.get("heading_valid"):
|
||||||
|
reasons.append("HEADING_QUALITY_NOT_FIXED")
|
||||||
|
return reasons
|
||||||
|
|
||||||
|
|
||||||
|
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 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 linear_lerp(a: float, b: float, u: float) -> float:
|
||||||
|
return (1.0 - u) * a + u * b
|
||||||
|
|
||||||
|
|
||||||
|
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")]
|
||||||
|
positions = sorted(
|
||||||
|
[
|
||||||
|
r for r in rtk
|
||||||
|
if r.get("type") in POSITION_TYPES
|
||||||
|
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") in HEADING_TYPES and heading_row_valid(r)],
|
||||||
|
key=lambda r: int(r["host_receive_utc_ns"]),
|
||||||
|
)
|
||||||
|
if not lidar or len(positions) < 2 or len(heading) < 2:
|
||||||
|
raise RuntimeError("insufficient LiDAR/GGA|PVTSLNA/heading(GNHPR|UNIHEADINGA) 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")
|
||||||
|
heading_offset_deg = (
|
||||||
|
float(a.heading_offset_deg)
|
||||||
|
if a.heading_offset_deg is not None
|
||||||
|
else float(ext.get("body_heading_offset_deg", 0.0) or 0.0)
|
||||||
|
)
|
||||||
|
|
||||||
|
position_times = np.asarray([int(r["host_receive_utc_ns"]) for r in positions], 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 positions if int(r.get("fix_quality", -1)) in {4, 5}),
|
||||||
|
positions[0],
|
||||||
|
)
|
||||||
|
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 = bracket(positions, position_times, t, max_ns)
|
||||||
|
hb = 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("POSITION_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)
|
||||||
|
corrected_heading, yaw = heading_to_enu_yaw(raw_heading, heading_offset_deg)
|
||||||
|
if a.orientation_model == "heading_pitch_roll":
|
||||||
|
pitch = linear_lerp(float(h0.get("pitch_deg") or 0.0), float(h1.get("pitch_deg") or 0.0), hu)
|
||||||
|
roll = linear_lerp(float(h0.get("roll_deg") or 0.0), float(h1.get("roll_deg") or 0.0), hu)
|
||||||
|
else:
|
||||||
|
pitch = 0.0
|
||||||
|
roll = 0.0
|
||||||
|
t_w_r = np.eye(4)
|
||||||
|
t_w_r[:3, :3] = rtk_body_rotation(
|
||||||
|
raw_heading, heading_offset_deg, pitch_deg=pitch, roll_deg=roll
|
||||||
|
)
|
||||||
|
t_w_r[:3, 3] = p_rtk
|
||||||
|
t_w_l = t_w_r @ t_r_l
|
||||||
|
q = rotation_to_quat_xyzw(t_w_l[:3, :3])
|
||||||
|
|
||||||
|
fix0, fix1 = int(g0.get("fix_quality", -1)), int(g1.get("fix_quality", -1))
|
||||||
|
if fix0 not in {4, 5} or fix1 not in {4, 5}:
|
||||||
|
reasons.append("RTK_POSITION_NOT_FIXED")
|
||||||
|
reasons.extend(heading_quality_ok(h0, a.heading_std_limit_deg))
|
||||||
|
reasons.extend(heading_quality_ok(h1, a.heading_std_limit_deg))
|
||||||
|
# Deduplicate while preserving order
|
||||||
|
reasons = list(dict.fromkeys(reasons))
|
||||||
|
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,
|
||||||
|
"corrected_heading_deg": corrected_heading,
|
||||||
|
"heading_offset_deg": heading_offset_deg,
|
||||||
|
"yaw_enu_deg": math.degrees(yaw),
|
||||||
|
"pitch_deg": pitch,
|
||||||
|
"roll_deg": roll,
|
||||||
|
"position_fix_before": fix0, "position_fix_after": fix1,
|
||||||
|
"heading_type_before": h0.get("type"), "heading_type_after": h1.get("type"),
|
||||||
|
"heading_solution_before": h0.get("heading_solution"),
|
||||||
|
"heading_solution_after": h1.get("heading_solution"),
|
||||||
|
"position_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6,
|
||||||
|
"position_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 position sample",
|
||||||
|
"rtk_frame": (
|
||||||
|
"delivered body X follows rawHeading after heading_offset_deg; "
|
||||||
|
"pitch/roll applied in baseline frame before the fixed offset"
|
||||||
|
),
|
||||||
|
"heading_offset_deg": heading_offset_deg,
|
||||||
|
"heading_sources_accepted": sorted(HEADING_TYPES),
|
||||||
|
"position_sources_accepted": sorted(POSITION_TYPES),
|
||||||
|
"orientation_model": a.orientation_model,
|
||||||
|
"orientation_composition": (
|
||||||
|
"R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset)"
|
||||||
|
),
|
||||||
|
"orientation_note": "Uses dual-antenna GNHPR/UNIHEADINGA pitch/roll; 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": (
|
||||||
|
"position endpoints fix_quality in {4,5}; UNIHEADINGA endpoints NARROW_INT with std gate; "
|
||||||
|
"GNHPR endpoints heading_valid/quality 4|5; 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
|
#!/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``.
|
This is an **internal** helper used by ``export_raw_to_combined.py``.
|
||||||
For RTK–LiDAR calibration, prefer the one-shot exporter that writes ``combined/``.
|
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)
|
- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns)
|
||||||
- ``frame_counter``, ``point_count``, optional host receive stamp
|
- ``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
|
from __future__ import annotations
|
||||||
@@ -25,10 +30,16 @@ from typing import Any
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
ROOT = Path(__file__).resolve().parent
|
ROOT = Path(__file__).resolve().parent
|
||||||
|
sys.path.insert(0, str(ROOT))
|
||||||
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
||||||
|
|
||||||
from capture_format_v2 import file_summary, read_capture # noqa: E402
|
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:
|
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(
|
def try_resolve_dlog_root(station_dir: Path) -> Path | None:
|
||||||
station: Path,
|
try:
|
||||||
out: Path,
|
return resolve_dlog_root(station_dir)
|
||||||
*,
|
except FileNotFoundError:
|
||||||
capture_name: str = "h32.rscap",
|
return None
|
||||||
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."""
|
|
||||||
|
|
||||||
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 = out / "frames"
|
||||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
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
|
saver = np.savez_compressed if compress else np.savez
|
||||||
manifest_rows: list[dict[str, Any]] = []
|
manifest_rows: list[dict[str, Any]] = []
|
||||||
written = 0
|
written = 0
|
||||||
@@ -93,7 +115,7 @@ def export_station_h32(
|
|||||||
"device_time_s": np.asarray([frame.t_start_s], dtype=np.float64),
|
"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),
|
"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),
|
"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)
|
saver(destination, **payload)
|
||||||
written += 1
|
written += 1
|
||||||
@@ -108,18 +130,17 @@ def export_station_h32(
|
|||||||
)
|
)
|
||||||
|
|
||||||
metadata: dict[str, Any] = {
|
metadata: dict[str, Any] = {
|
||||||
"source_rscap": str(rscap.resolve()),
|
|
||||||
"capture": file_summary(capture),
|
|
||||||
"frames_decoded": len(frames),
|
"frames_decoded": len(frames),
|
||||||
"frames_written": written,
|
"frames_written": written,
|
||||||
"frames_dir": str(frames_dir.resolve()),
|
"frames_dir": str(frames_dir.resolve()),
|
||||||
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
|
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
|
||||||
"points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
|
"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 / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||||
(out / "README.md").write_text(
|
(out / "README.md").write_text(
|
||||||
"# H32 station export (internal)\n\n"
|
"# H32 station export (internal)\n\n"
|
||||||
f"- source: `{rscap}`\n"
|
f"- source: `{source_label}`\n"
|
||||||
f"- frames: `{frames_dir}`\n"
|
f"- frames: `{frames_dir}`\n"
|
||||||
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
|
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
|
||||||
encoding="utf-8",
|
encoding="utf-8",
|
||||||
@@ -128,18 +149,133 @@ def export_station_h32(
|
|||||||
reports = out / "reports"
|
reports = out / "reports"
|
||||||
reports.mkdir(parents=True, exist_ok=True)
|
reports.mkdir(parents=True, exist_ok=True)
|
||||||
with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
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.writeheader()
|
||||||
writer.writerows(manifest_rows)
|
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
|
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:
|
def parse_args() -> argparse.Namespace:
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
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("--out", type=Path, required=True)
|
||||||
parser.add_argument("--capture-name", default="h32.rscap")
|
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("--stride", type=int, default=1)
|
||||||
parser.add_argument("--min-frame-points", type=int, default=100)
|
parser.add_argument("--min-frame-points", type=int, default=100)
|
||||||
parser.add_argument("--min-range-m", type=float, default=0.3)
|
parser.add_argument("--min-range-m", type=float, default=0.3)
|
||||||
@@ -152,8 +288,38 @@ def parse_args() -> argparse.Namespace:
|
|||||||
|
|
||||||
def main() -> int:
|
def main() -> int:
|
||||||
args = parse_args()
|
args = parse_args()
|
||||||
|
station = args.station
|
||||||
|
if station.is_file() and station.suffix.lower() == ".rscap":
|
||||||
metadata = export_station_h32(
|
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,
|
args.out,
|
||||||
capture_name=args.capture_name,
|
capture_name=args.capture_name,
|
||||||
stride=args.stride,
|
stride=args.stride,
|
||||||
|
|||||||
@@ -1,20 +1,23 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package.
|
"""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/``
|
calibration-ready intermediate out. Downstream prepare/solve consume ``combined/``
|
||||||
only (``manifest.csv`` + associated frame NPZs).
|
only (``manifest.csv`` + associated frame NPZs).
|
||||||
|
|
||||||
Expected raw layout:
|
Expected raw layout (new H32 DLogCapture):
|
||||||
|
|
||||||
stations/
|
stations/
|
||||||
001/h32.rscap
|
001/ # dobject/ + dobject_recording/ (or 001/dlog/...)
|
||||||
002/h32.rscap
|
002/
|
||||||
...
|
...
|
||||||
captures/ (paths passed explicitly)
|
captures/
|
||||||
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
|
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
|
||||||
imu.rscap # N300 (associated only; not used in AX=XB)
|
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``:
|
Output under ``--out``:
|
||||||
|
|
||||||
export/<station>/frames/*.npz # internal LiDAR frames
|
export/<station>/frames/*.npz # internal LiDAR frames
|
||||||
@@ -22,9 +25,6 @@ Output under ``--out``:
|
|||||||
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
||||||
export_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.
|
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 build_multisensor_npz import build_combined # noqa: E402
|
||||||
from capture_format_v2 import file_summary, read_capture # 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
|
from pipeline_common_corrected import ( # noqa: E402
|
||||||
parse_imu_capture,
|
parse_imu_capture,
|
||||||
parse_rtk_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:
|
def is_h32_station(station: Path, capture_name: str) -> bool:
|
||||||
try:
|
try:
|
||||||
resolve_lidar_rscap(station, capture_name)
|
resolve_lidar_rscap(station, capture_name)
|
||||||
return True
|
|
||||||
except FileNotFoundError:
|
except FileNotFoundError:
|
||||||
return False
|
return False
|
||||||
|
return True
|
||||||
|
|
||||||
|
|
||||||
def is_dlog_station(station: Path) -> bool:
|
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:
|
if names:
|
||||||
stations = [stations_root / name for name in names]
|
stations = [stations_root / name for name in names]
|
||||||
missing = [str(path) for path in stations if not path.is_dir()]
|
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
|
path
|
||||||
for path in stations_root.iterdir()
|
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,
|
key=lambda path: path.name,
|
||||||
)
|
)
|
||||||
if not stations:
|
if not stations:
|
||||||
raise FileNotFoundError(
|
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
|
return stations
|
||||||
|
|
||||||
@@ -101,7 +119,7 @@ def export_legacy_dlog_station(
|
|||||||
sys.executable,
|
sys.executable,
|
||||||
str(exporter),
|
str(exporter),
|
||||||
"--dlog",
|
"--dlog",
|
||||||
str(station),
|
str(try_resolve_dlog_root(station) or station),
|
||||||
"--out",
|
"--out",
|
||||||
str(out),
|
str(out),
|
||||||
"--object",
|
"--object",
|
||||||
@@ -154,6 +172,9 @@ def export_raw_to_combined(
|
|||||||
out: Path,
|
out: Path,
|
||||||
station_names: list[str] | None = None,
|
station_names: list[str] | None = None,
|
||||||
lidar_capture_name: str = "h32.rscap",
|
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",
|
lidar_object: str = "frontlidar",
|
||||||
timezone: str = "+08:00",
|
timezone: str = "+08:00",
|
||||||
stride: int = 1,
|
stride: int = 1,
|
||||||
@@ -174,7 +195,6 @@ def export_raw_to_combined(
|
|||||||
if out.exists() and any(out.iterdir()) and not overwrite:
|
if out.exists() and any(out.iterdir()) and not overwrite:
|
||||||
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
||||||
if overwrite and out.exists():
|
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"):
|
for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
|
||||||
target = out / child
|
target = out / child
|
||||||
if target.is_dir():
|
if target.is_dir():
|
||||||
@@ -187,15 +207,29 @@ def export_raw_to_combined(
|
|||||||
parsed_root = out / "parsed"
|
parsed_root = out / "parsed"
|
||||||
combined_root = out / "combined"
|
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)
|
parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root)
|
||||||
|
|
||||||
station_meta: list[dict[str, Any]] = []
|
station_meta: list[dict[str, Any]] = []
|
||||||
lidar_segments: list[tuple[str, Path]] = []
|
lidar_segments: list[tuple[str, Path]] = []
|
||||||
saw_dlog = False
|
saw_legacy_dlog = False
|
||||||
for station in stations:
|
for station in stations:
|
||||||
station_out = export_root / station.name
|
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(
|
meta = export_station_h32(
|
||||||
station,
|
station,
|
||||||
station_out,
|
station_out,
|
||||||
@@ -205,8 +239,10 @@ def export_raw_to_combined(
|
|||||||
resume=False,
|
resume=False,
|
||||||
)
|
)
|
||||||
kind = "h32_rscap"
|
kind = "h32_rscap"
|
||||||
elif is_dlog_station(station):
|
elif is_legacy_pointcloud_dlog_station(station, msop_object=msop_object) or is_dlog_station(
|
||||||
saw_dlog = True
|
station
|
||||||
|
):
|
||||||
|
saw_legacy_dlog = True
|
||||||
export_legacy_dlog_station(
|
export_legacy_dlog_station(
|
||||||
station,
|
station,
|
||||||
station_out,
|
station_out,
|
||||||
@@ -217,16 +253,18 @@ def export_raw_to_combined(
|
|||||||
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
||||||
kind = "legacy_dlog"
|
kind = "legacy_dlog"
|
||||||
else:
|
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"
|
frames_dir = station_out / "frames"
|
||||||
if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")):
|
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}")
|
raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}")
|
||||||
lidar_segments.append((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})
|
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(
|
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,
|
file=sys.stderr,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -260,7 +298,9 @@ def export_raw_to_combined(
|
|||||||
},
|
},
|
||||||
"timestamp_policy": {
|
"timestamp_policy": {
|
||||||
"default_time_basis": time_basis,
|
"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",
|
"rtk": "GNSS week/TOW when time_basis=device_gnss; else host_receive_utc_ns",
|
||||||
"imu": "associated only; host-anchored device deltas in combined window",
|
"imu": "associated only; host-anchored device deltas in combined window",
|
||||||
"host_utc": "kept for audit; not the default calibration timeline for new captures",
|
"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("--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("--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("--station", action="append", default=[], help="Optional station name filter; repeatable")
|
||||||
parser.add_argument("--lidar-capture-name", default="h32.rscap")
|
parser.add_argument("--lidar-capture-name", default="h32.rscap", help="Legacy H32 .rscap filename")
|
||||||
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy dlog DObject name")
|
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("--timezone", default="+08:00", help="Legacy dlog tick timezone")
|
||||||
parser.add_argument("--stride", type=int, default=1)
|
parser.add_argument("--stride", type=int, default=1)
|
||||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
||||||
@@ -304,6 +352,9 @@ def main() -> int:
|
|||||||
out=args.out,
|
out=args.out,
|
||||||
station_names=args.station,
|
station_names=args.station,
|
||||||
lidar_capture_name=args.lidar_capture_name,
|
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,
|
lidar_object=args.lidar_object,
|
||||||
timezone=args.timezone,
|
timezone=args.timezone,
|
||||||
stride=args.stride,
|
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()
|
||||||
@@ -1,5 +1,5 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""Prepare one static LiDAR frame and one yaw-only RTK reference pose per NPZ segment."""
|
"""Prepare one static LiDAR frame and one RTK reference pose per NPZ segment."""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
@@ -14,6 +14,13 @@ from typing import Any
|
|||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
from rtk_attitude import (
|
||||||
|
heading_to_enu_yaw,
|
||||||
|
parse_pitch_roll_from_heading_raw,
|
||||||
|
rotation_to_quat_xyzw,
|
||||||
|
rtk_body_rotation,
|
||||||
|
)
|
||||||
|
|
||||||
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
|
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
|
||||||
|
|
||||||
|
|
||||||
@@ -53,15 +60,30 @@ def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: f
|
|||||||
return rotation @ (ecef - origin)
|
return rotation @ (ecef - origin)
|
||||||
|
|
||||||
|
|
||||||
def yaw_rotation(yaw: float) -> np.ndarray:
|
def scalar(data: np.lib.npyio.NpzFile, name: str, default: float | None = None) -> float:
|
||||||
c, s = math.cos(yaw), math.sin(yaw)
|
if name not in data.files:
|
||||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
|
if default is None:
|
||||||
|
raise KeyError(name)
|
||||||
|
return float(default)
|
||||||
def scalar(data: np.lib.npyio.NpzFile, name: str) -> float:
|
|
||||||
return float(np.asarray(data[name]).reshape(-1)[0])
|
return float(np.asarray(data[name]).reshape(-1)[0])
|
||||||
|
|
||||||
|
|
||||||
|
def frame_pitch_roll(data: np.lib.npyio.NpzFile) -> tuple[float, float]:
|
||||||
|
pitch = scalar(data, "rtk_pitch_deg", math.nan)
|
||||||
|
roll = scalar(data, "rtk_roll_deg", math.nan)
|
||||||
|
if math.isfinite(pitch) and math.isfinite(roll):
|
||||||
|
return pitch, roll
|
||||||
|
raw = None
|
||||||
|
if "rtk_heading_raw_utf8" in data.files:
|
||||||
|
raw = bytes(np.asarray(data["rtk_heading_raw_utf8"]).reshape(-1))
|
||||||
|
parsed_pitch, parsed_roll = parse_pitch_roll_from_heading_raw(raw)
|
||||||
|
if not math.isfinite(pitch):
|
||||||
|
pitch = float(parsed_pitch) if parsed_pitch is not None else 0.0
|
||||||
|
if not math.isfinite(roll):
|
||||||
|
roll = float(parsed_roll) if parsed_roll is not None else 0.0
|
||||||
|
return pitch, roll
|
||||||
|
|
||||||
|
|
||||||
def parse_args() -> argparse.Namespace:
|
def parse_args() -> argparse.Namespace:
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
parser.add_argument("--combined-root", type=Path, required=True)
|
parser.add_argument("--combined-root", type=Path, required=True)
|
||||||
@@ -73,6 +95,12 @@ def parse_args() -> argparse.Namespace:
|
|||||||
parser.add_argument("--heading-std-limit-deg", type=float, default=0.5)
|
parser.add_argument("--heading-std-limit-deg", type=float, default=0.5)
|
||||||
parser.add_argument("--min-stations", type=int, default=30)
|
parser.add_argument("--min-stations", type=int, default=30)
|
||||||
parser.add_argument("--expected-stations", type=int, default=0)
|
parser.add_argument("--expected-stations", type=int, default=0)
|
||||||
|
parser.add_argument(
|
||||||
|
"--orientation-model",
|
||||||
|
choices=("heading_pitch_roll", "yaw_only"),
|
||||||
|
default="heading_pitch_roll",
|
||||||
|
help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll; yaw_only forces roll=pitch=0",
|
||||||
|
)
|
||||||
parser.add_argument("--overwrite", action="store_true")
|
parser.add_argument("--overwrite", action="store_true")
|
||||||
return parser.parse_args()
|
return parser.parse_args()
|
||||||
|
|
||||||
@@ -102,9 +130,10 @@ def main() -> int:
|
|||||||
for row in good:
|
for row in good:
|
||||||
path = args.combined_root / Path(row["output"])
|
path = args.combined_root / Path(row["output"])
|
||||||
with np.load(path, allow_pickle=False) as data:
|
with np.load(path, allow_pickle=False) as data:
|
||||||
|
pitch, roll = frame_pitch_roll(data)
|
||||||
samples.append((scalar(data, "rtk_lat_deg"), scalar(data, "rtk_lon_deg"),
|
samples.append((scalar(data, "rtk_lat_deg"), scalar(data, "rtk_lon_deg"),
|
||||||
scalar(data, "rtk_altitude_m"), scalar(data, "rtk_raw_heading_deg"),
|
scalar(data, "rtk_altitude_m"), scalar(data, "rtk_raw_heading_deg"),
|
||||||
scalar(data, "rtk_pitch_deg"), scalar(data, "rtk_heading_stddev_deg")))
|
pitch, roll, scalar(data, "rtk_heading_stddev_deg", math.nan)))
|
||||||
values = np.asarray(samples, dtype=float)
|
values = np.asarray(samples, dtype=float)
|
||||||
heading_std = circular_std_deg(values[:, 3])
|
heading_std = circular_std_deg(values[:, 3])
|
||||||
if heading_std > args.heading_std_limit_deg:
|
if heading_std > args.heading_std_limit_deg:
|
||||||
@@ -112,14 +141,23 @@ def main() -> int:
|
|||||||
continue
|
continue
|
||||||
frame = good[len(good) // 2]
|
frame = good[len(good) // 2]
|
||||||
source = args.combined_root / Path(frame["output"])
|
source = args.combined_root / Path(frame["output"])
|
||||||
selected.append({"station": segment, "source": source, "time": int(frame["lidar_time_ns"]) / 1e9,
|
reported_std = values[:, 6]
|
||||||
|
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])),
|
"lat": float(np.mean(values[:, 0])), "lon": float(np.mean(values[:, 1])),
|
||||||
"alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3])})
|
"alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3]),
|
||||||
summaries.append({"station": segment, "frames": len(group), "valid_fixed_frames": len(good),
|
"pitch": float(np.mean(values[:, 4])), "roll": float(np.mean(values[:, 5])),
|
||||||
|
})
|
||||||
|
summaries.append({
|
||||||
|
"station": segment, "frames": len(group), "valid_fixed_frames": len(good),
|
||||||
"heading_mean_deg": circular_mean_deg(values[:, 3]),
|
"heading_mean_deg": circular_mean_deg(values[:, 3]),
|
||||||
"heading_circular_std_deg": heading_std, "rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
|
"heading_circular_std_deg": heading_std,
|
||||||
"reported_heading_std_mean_deg": float(np.nanmean(values[:, 5])),
|
"rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
|
||||||
"altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source)})
|
"rtk_roll_mean_deg": float(np.mean(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:
|
if args.expected_stations and len(selected) != args.expected_stations:
|
||||||
raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
||||||
@@ -132,37 +170,62 @@ def main() -> int:
|
|||||||
origin = selected[0]
|
origin = selected[0]
|
||||||
origin_ecef = geodetic_to_ecef(origin["lat"], origin["lon"], origin["alt"])
|
origin_ecef = geodetic_to_ecef(origin["lat"], origin["lon"], origin["alt"])
|
||||||
lever = np.asarray(args.antenna_lever, dtype=float)
|
lever = np.asarray(args.antenna_lever, dtype=float)
|
||||||
|
use_attitude = args.orientation_model == "heading_pitch_roll"
|
||||||
pose_rows = []
|
pose_rows = []
|
||||||
for index, item in enumerate(selected, 1):
|
for index, item in enumerate(selected, 1):
|
||||||
destination = frames / f"station_{index:02d}.npz"
|
destination = frames / f"station_{index:02d}.npz"
|
||||||
shutil.copy2(item["source"], destination)
|
shutil.copy2(item["source"], destination)
|
||||||
antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
|
antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
|
||||||
origin["lat"], origin["lon"])
|
origin["lat"], origin["lon"])
|
||||||
corrected_heading = (item["heading"] + args.heading_offset_deg) % 360.0
|
corrected_heading, yaw = heading_to_enu_yaw(item["heading"], args.heading_offset_deg)
|
||||||
yaw = math.radians(90.0 - corrected_heading)
|
pitch = float(item["pitch"]) if use_attitude else 0.0
|
||||||
reference_position = antenna - yaw_rotation(yaw) @ lever
|
roll = float(item["roll"]) if use_attitude else 0.0
|
||||||
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, 0.0, 0.0,
|
rotation = rtk_body_rotation(
|
||||||
math.sin(yaw / 2.0), math.cos(yaw / 2.0)])))
|
item["heading"], args.heading_offset_deg, pitch_deg=pitch, roll_deg=roll
|
||||||
summaries[index - 1].update({"sequence": index, "prepared_frame": destination.name,
|
)
|
||||||
"corrected_heading_deg": corrected_heading})
|
reference_position = antenna - rotation @ lever
|
||||||
|
quat = rotation_to_quat_xyzw(rotation)
|
||||||
|
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, *quat])))
|
||||||
|
summaries[index - 1].update({
|
||||||
|
"sequence": index, "prepared_frame": destination.name,
|
||||||
|
"corrected_heading_deg": corrected_heading,
|
||||||
|
"pose_yaw_enu_deg": math.degrees(yaw),
|
||||||
|
"pose_pitch_deg": pitch, "pose_roll_deg": roll,
|
||||||
|
})
|
||||||
pose_path = args.output / f"reference_poses_{args.pose_name}.csv"
|
pose_path = args.output / f"reference_poses_{args.pose_name}.csv"
|
||||||
with pose_path.open("w", encoding="utf-8", newline="") as stream:
|
with pose_path.open("w", encoding="utf-8", newline="") as stream:
|
||||||
writer = csv.DictWriter(stream, fieldnames=POSE_FIELDS); writer.writeheader(); writer.writerows(pose_rows)
|
writer = csv.DictWriter(stream, fieldnames=POSE_FIELDS); writer.writeheader(); writer.writerows(pose_rows)
|
||||||
with (args.output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
|
with (args.output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||||
fields = sorted({key for row in summaries for key in row})
|
fields = sorted({key for row in summaries for key in row})
|
||||||
writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
|
writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
|
||||||
document = {"source_combined_root": str(args.combined_root.resolve()), "station_count": len(selected),
|
document = {
|
||||||
|
"source_combined_root": str(args.combined_root.resolve()), "station_count": len(selected),
|
||||||
"rejected": rejected, "pose_csv": pose_path.name,
|
"rejected": rejected, "pose_csv": pose_path.name,
|
||||||
"selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
|
"selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
|
||||||
"reference_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg,
|
"reference_pose_configuration": {
|
||||||
|
"raw_heading_offset_deg": args.heading_offset_deg,
|
||||||
"antenna_lever_body_m": args.antenna_lever,
|
"antenna_lever_body_m": args.antenna_lever,
|
||||||
"orientation_model": "yaw-only, identical to the previous calibration workflow"},
|
"heading_offset_semantics": (
|
||||||
|
"added to clockwise-from-north GNHPR heading before ENU yaw conversion"
|
||||||
|
),
|
||||||
|
"orientation_model": args.orientation_model,
|
||||||
|
"orientation_composition": (
|
||||||
|
"R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset); "
|
||||||
|
"yaw_raw from rawHeading, pitch/roll stay in baseline frame"
|
||||||
|
),
|
||||||
|
"pitch_roll_note": (
|
||||||
|
"pitch/roll come from dual-antenna GNHPR/UNIHEADINGA (baseline elevation / reported roll). "
|
||||||
|
"This is not a fused IMU vehicle attitude; G90 roll is often ~0."
|
||||||
|
),
|
||||||
|
},
|
||||||
"stations": [{"sequence": i + 1, "source_station": item["station"],
|
"stations": [{"sequence": i + 1, "source_station": item["station"],
|
||||||
"source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
|
"source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
|
||||||
for i, item in enumerate(selected)]}
|
for i, item in enumerate(selected)],
|
||||||
|
}
|
||||||
(args.output / "manifest.json").write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
(args.output / "manifest.json").write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||||
print(json.dumps({"prepared": str(args.output.resolve()), "stations": len(selected),
|
print(json.dumps({"prepared": str(args.output.resolve()), "stations": len(selected),
|
||||||
"rejected": rejected, "pose_csv": pose_path.name}, ensure_ascii=False, indent=2))
|
"rejected": rejected, "pose_csv": pose_path.name,
|
||||||
|
"orientation_model": args.orientation_model}, ensure_ascii=False, indent=2))
|
||||||
return 0
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
+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],
|
azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
|
||||||
distance_mm = raw * distance_unit_mm, then:
|
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)
|
y = d_m * cos(alt) * sin(az)
|
||||||
z = d_m * sin(alt)
|
z = d_m * sin(alt)
|
||||||
|
|
||||||
MSOP-only captures do not include DIFOP; vertical angles default to a uniform
|
When DIFOP is unavailable, vertical angles default to a uniform -16°…+16° fan
|
||||||
-16°…+16° fan, horizontal channel offsets default to 0.
|
and horizontal channel offsets default to 0.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
from typing import Iterable, Sequence
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
@@ -192,9 +193,10 @@ def _block_points_raw(
|
|||||||
return np.asarray(rows, dtype=np.float32)
|
return np.asarray(rows, dtype=np.float32)
|
||||||
|
|
||||||
|
|
||||||
def iter_h32_frames_polar(
|
def iter_h32_frames_polar_from_packets(
|
||||||
capture: CaptureFile,
|
packets: Iterable[bytes],
|
||||||
*,
|
*,
|
||||||
|
host_utc_ticks: Sequence[int] | None = None,
|
||||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||||
frame_stride: int = 1,
|
frame_stride: int = 1,
|
||||||
min_range_m: float = 0.3,
|
min_range_m: float = 0.3,
|
||||||
@@ -203,7 +205,7 @@ def iter_h32_frames_polar(
|
|||||||
vertical_deg: np.ndarray | None = None,
|
vertical_deg: np.ndarray | None = None,
|
||||||
horizontal_deg: np.ndarray | None = None,
|
horizontal_deg: np.ndarray | None = None,
|
||||||
) -> list[LidarFramePolarExport]:
|
) -> 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)
|
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)
|
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
|
prev_az: float | None = None
|
||||||
kept = 0
|
kept = 0
|
||||||
stride = max(1, int(frame_stride))
|
stride = max(1, int(frame_stride))
|
||||||
|
host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
|
||||||
|
|
||||||
def emit() -> None:
|
def emit() -> None:
|
||||||
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
||||||
@@ -250,13 +253,15 @@ def iter_h32_frames_polar(
|
|||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
for chunk in capture.chunks:
|
for index, packet in enumerate(packets):
|
||||||
packet = chunk.raw
|
|
||||||
if len(packet) != PACKET_LENGTH:
|
if len(packet) != PACKET_LENGTH:
|
||||||
continue
|
continue
|
||||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||||
unit = distance_unit_mm(packet)
|
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
|
idx = DATA_START
|
||||||
for _block in range(BLOCKS):
|
for _block in range(BLOCKS):
|
||||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
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
|
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(
|
def iter_h32_frames(
|
||||||
capture: CaptureFile,
|
capture: CaptureFile,
|
||||||
*,
|
*,
|
||||||
|
|||||||
@@ -120,6 +120,7 @@ def parse_heading(line: str) -> dict:
|
|||||||
"baseline_length_m": safe_float(fields[2]),
|
"baseline_length_m": safe_float(fields[2]),
|
||||||
"raw_heading_deg": raw_heading,
|
"raw_heading_deg": raw_heading,
|
||||||
"pitch_deg": safe_float(fields[4]),
|
"pitch_deg": safe_float(fields[4]),
|
||||||
|
"roll_deg": 0.0,
|
||||||
"heading_stddev_deg": safe_float(fields[6]),
|
"heading_stddev_deg": safe_float(fields[6]),
|
||||||
"pitch_stddev_deg": safe_float(fields[7]) if len(fields) > 7 else None,
|
"pitch_stddev_deg": safe_float(fields[7]) if len(fields) > 7 else None,
|
||||||
"station_id": fields[8].strip('"') if len(fields) > 8 else "",
|
"station_id": fields[8].strip('"') if len(fields) > 8 else "",
|
||||||
@@ -131,6 +132,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:
|
def parse_pvtslna(line: str) -> dict:
|
||||||
"""Parse Unicore/G90 ``#PVTSLNA`` into GGA-compatible position fields.
|
"""Parse Unicore/G90 ``#PVTSLNA`` into GGA-compatible position fields.
|
||||||
|
|
||||||
@@ -221,6 +246,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
|||||||
row.update(parse_pvtslna(line))
|
row.update(parse_pvtslna(line))
|
||||||
elif line.startswith("#UNIHEADINGA"):
|
elif line.startswith("#UNIHEADINGA"):
|
||||||
row.update(parse_heading(line))
|
row.update(parse_heading(line))
|
||||||
|
elif line.startswith("$GNHPR"):
|
||||||
|
row.update(parse_gnhpr(line))
|
||||||
except ValueError as ex:
|
except ValueError as ex:
|
||||||
row["parse_error"] = str(ex)
|
row["parse_error"] = str(ex)
|
||||||
rows.append(row)
|
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 = []
|
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):
|
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||||
stream = b"".join(chunk.raw for chunk in chunks)
|
stream = b"".join(chunk.raw for chunk in chunks)
|
||||||
cursor = 0
|
cursor = 0
|
||||||
@@ -56,6 +62,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
|||||||
cursor = end
|
cursor = end
|
||||||
if not raw_line:
|
if not raw_line:
|
||||||
continue
|
continue
|
||||||
|
if accepted_prefix_bytes is not None and not raw_line.startswith(accepted_prefix_bytes):
|
||||||
|
continue
|
||||||
line = raw_line.decode("ascii", "replace")
|
line = raw_line.decode("ascii", "replace")
|
||||||
row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
|
row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
|
||||||
row.update(source_for_span(chunks, start, end, segment_id))
|
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))
|
row.update(parse_pvtslna(line))
|
||||||
elif line.startswith("#UNIHEADINGA"):
|
elif line.startswith("#UNIHEADINGA"):
|
||||||
row.update(parse_heading(line))
|
row.update(parse_heading(line))
|
||||||
|
elif line.startswith("$GNHPR"):
|
||||||
|
row.update(parse_gnhpr(line))
|
||||||
except ValueError as ex:
|
except ValueError as ex:
|
||||||
row["parse_error"] = str(ex)
|
row["parse_error"] = str(ex)
|
||||||
rows.append(row)
|
rows.append(row)
|
||||||
|
|||||||
@@ -0,0 +1,127 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""RTK dual-antenna attitude helpers shared by prepare and SLAM delivery."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
import re
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
# GNHPR / UNIHEADINGA pitch is baseline elevation (far antenna higher ⇒ +pitch).
|
||||||
|
# Build the baseline-frame attitude first, then apply the fixed body yaw offset:
|
||||||
|
# R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset)
|
||||||
|
# so pitch/roll stay about the physical baseline, even when delivering vehicle-forward.
|
||||||
|
|
||||||
|
|
||||||
|
def heading_to_enu_yaw(raw_heading_deg: float, heading_offset_deg: float = 0.0) -> tuple[float, float]:
|
||||||
|
"""Convert clockwise-from-north heading to mathematical ENU yaw (rad)."""
|
||||||
|
corrected_heading = (raw_heading_deg + heading_offset_deg) % 360.0
|
||||||
|
return corrected_heading, math.radians(90.0 - corrected_heading)
|
||||||
|
|
||||||
|
|
||||||
|
def _rz(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 _ry(pitch_rad: float) -> np.ndarray:
|
||||||
|
c, s = math.cos(pitch_rad), math.sin(pitch_rad)
|
||||||
|
return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def _rx(roll_rad: float) -> np.ndarray:
|
||||||
|
c, s = math.cos(roll_rad), math.sin(roll_rad)
|
||||||
|
return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def attitude_rotation(
|
||||||
|
yaw_rad: float,
|
||||||
|
pitch_deg: float = 0.0,
|
||||||
|
roll_deg: float = 0.0,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""ENU←baseline rotation: Rz(yaw) Ry(-pitch) Rx(roll).
|
||||||
|
|
||||||
|
Positive ``pitch_deg`` elevates baseline X (slave higher than master).
|
||||||
|
"""
|
||||||
|
return _rz(float(yaw_rad)) @ _ry(-math.radians(float(pitch_deg))) @ _rx(math.radians(float(roll_deg)))
|
||||||
|
|
||||||
|
|
||||||
|
def rtk_body_rotation(
|
||||||
|
raw_heading_deg: float,
|
||||||
|
heading_offset_deg: float = 0.0,
|
||||||
|
pitch_deg: float = 0.0,
|
||||||
|
roll_deg: float = 0.0,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""ENU←delivered RTK body frame.
|
||||||
|
|
||||||
|
Pitch/roll are applied in the raw baseline frame; ``heading_offset_deg`` then
|
||||||
|
rotates that frame into the delivered body (0 = baseline X, -90 = vehicle
|
||||||
|
forward when baseline points vehicle-right on this vehicle).
|
||||||
|
"""
|
||||||
|
_, yaw_baseline = heading_to_enu_yaw(raw_heading_deg, 0.0)
|
||||||
|
return attitude_rotation(yaw_baseline, pitch_deg, roll_deg) @ _rz(-math.radians(float(heading_offset_deg)))
|
||||||
|
|
||||||
|
|
||||||
|
def rotation_to_quat_xyzw(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
r = np.asarray(rotation, dtype=float)
|
||||||
|
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],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
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],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
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],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
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],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
if q[3] < 0.0:
|
||||||
|
q = -q
|
||||||
|
return q / np.linalg.norm(q)
|
||||||
|
|
||||||
|
|
||||||
|
def parse_pitch_roll_from_heading_raw(raw_utf8: bytes | str | None) -> tuple[float | None, float | None]:
|
||||||
|
"""Best-effort pitch/roll from a stored GNHPR/UNIHEADINGA raw line."""
|
||||||
|
if raw_utf8 is None:
|
||||||
|
return None, None
|
||||||
|
text = raw_utf8.decode("ascii", "ignore") if isinstance(raw_utf8, (bytes, bytearray)) else str(raw_utf8)
|
||||||
|
text = text.strip()
|
||||||
|
if "GNHPR" in text:
|
||||||
|
parts = text.split(",")
|
||||||
|
if len(parts) >= 5:
|
||||||
|
try:
|
||||||
|
return float(parts[3]), float(parts[4])
|
||||||
|
except ValueError:
|
||||||
|
return None, None
|
||||||
|
if "UNIHEADINGA" in text.upper() or "HEADINGA" in text.upper():
|
||||||
|
payload = text.split(";", 1)[-1]
|
||||||
|
fields = payload.split(",")
|
||||||
|
if len(fields) >= 5:
|
||||||
|
try:
|
||||||
|
return float(fields[4]), 0.0
|
||||||
|
except ValueError:
|
||||||
|
return None, None
|
||||||
|
match = re.search(r",(-?\d+(?:\.\d+)?),(-?\d+(?:\.\d+)?),\d,", text)
|
||||||
|
if match:
|
||||||
|
try:
|
||||||
|
return float(match.group(1)), float(match.group(2))
|
||||||
|
except ValueError:
|
||||||
|
return None, None
|
||||||
|
return None, None
|
||||||
+69
-132
@@ -1,13 +1,12 @@
|
|||||||
# 雷达与 RTK 标定说明书
|
# 雷达与 RTK 标定说明书
|
||||||
|
|
||||||
本文说明如何用本仓库完成 **双天线 RTK ↔ 3D 激光雷达** 外参标定，得到可直接使用的 `T_RTK_lidar`。
|
本文说明如何用本仓库完成 **双天线 RTK ↔ 3D 激光雷达** 外参标定，得到可直接使用的 `T_RTK_lidar`。
|
||||||
|
默认交付坐标系为 **车头向前**（`HeadingOffsetDeg = -90`）。更完整的指标与本次结果见根目录 [`README.md`](README.md)。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 1. 标定目标
|
## 1. 标定目标
|
||||||
|
|
||||||
求解外参 `T_RTK_lidar`，把雷达点变换到 RTK 导航系：
|
|
||||||
|
|
||||||
```text
|
```text
|
||||||
p_RTK = T_RTK_lidar · p_lidar
|
p_RTK = T_RTK_lidar · p_lidar
|
||||||
```
|
```
|
||||||
@@ -15,11 +14,15 @@ p_RTK = T_RTK_lidar · p_lidar
|
|||||||
| 项目 | 说明 |
|
| 项目 | 说明 |
|
||||||
|---|---|
|
|---|---|
|
||||||
| 输出文件 | `final_T_RTK_lidar.json` |
|
| 输出文件 | `final_T_RTK_lidar.json` |
|
||||||
| 坐标系 | RTK 导航系（GGA 原点 + 双天线航向），**不是**车体后轮轴系 |
|
| 坐标系 | **车头向前**：GGA 原点 + 车头 X（本车 `HeadingOffsetDeg=-90`） |
|
||||||
| 不用到的量 | 车体航向偏置、天线 XY 杆臂、IMU 姿态 |
|
| 不用到的量 | 车体航向偏置、天线 XY 杆臂、IMU 融合姿态（双天线 pitch/roll 默认进入参考位姿） |
|
||||||
| 必须提供 | 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
|
```text
|
||||||
T_body_lidar = T_body_rtk · T_RTK_lidar
|
T_body_lidar = T_body_rtk · T_RTK_lidar
|
||||||
@@ -31,59 +34,41 @@ T_body_lidar = T_body_rtk · T_RTK_lidar
|
|||||||
|
|
||||||
- 系统：Windows + PowerShell
|
- 系统：Windows + PowerShell
|
||||||
- Python：3.11
|
- Python：3.11
|
||||||
- 安装依赖：
|
- `python -m pip install -r requirements.txt`（NumPy、SciPy、Open3D、small_gicp）
|
||||||
|
|
||||||
```powershell
|
|
||||||
python -m pip install -r requirements.txt
|
|
||||||
```
|
|
||||||
|
|
||||||
依赖：NumPy、SciPy、Open3D、small_gicp。完整流程需要 **Open3D 与 small_gicp 两个配准后端**；若 Windows 无 small_gicp wheel，可改用 WSL2。
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 3. 数据采集
|
## 3. 数据采集
|
||||||
|
|
||||||
### 3.1 目录结构
|
### 3.1 每站独立目录
|
||||||
|
|
||||||
**新车（默认）**：每站一段 H32 雷达 `.rscap`，RTK/IMU 全程各一条：
|
|
||||||
|
|
||||||
```text
|
```text
|
||||||
raw_dataset/
|
raw_dataset/
|
||||||
├── stations/
|
├── stations/001|002|.../
|
||||||
│ ├── 001/h32.rscap
|
└── captures/rtk.rscap , imu.rscap
|
||||||
│ ├── 002/h32.rscap
|
|
||||||
│ └── ...
|
|
||||||
└── captures/
|
|
||||||
├── rtk.rscap # G90：#PVTSLNA 位置 + #UNIHEADINGA 航向
|
|
||||||
└── imu.rscap # N300；仅关联保存，不参与外参求解
|
|
||||||
```
|
```
|
||||||
|
|
||||||
旧车 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 | 固定解，航向有效；站内航向圆标准差 ≤ 0.5° |
|
||||||
| RTK 质量 | 固定解（质量 4/5），航向有效 |
|
| 必测量 | ANT1 相位中心离地高度 |
|
||||||
| 站内航向稳定 | 圆标准差 ≤ 0.5° |
|
|
||||||
| 必测量 | **ANT1（GGA 参考点）离地高度**，含天线相位中心修正 |
|
|
||||||
|
|
||||||
### 3.3 现场确认（标定前必做）
|
现场确认：GGA 对应哪根天线、`rawHeading` 方向、离地高度测法（改高度必须重跑求解）。
|
||||||
|
|
||||||
1. **哪根天线是 GGA 原点**（通常 ANT1）
|
|
||||||
2. **`rawHeading` 方向**：ANT1→ANT2 还是相反（搞反会导致 yaw 差约 180°）
|
|
||||||
3. **离地高度测法**：例如安装底面高度 + 天线 PCO，写入求解参数，不要事后只改 JSON 里的 z
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 4. 一键标定
|
## 4. 一键标定
|
||||||
|
|
||||||
### 4.1 仅导出标定中间包（推荐先跑通）
|
### 4.1 站目录 → combined
|
||||||
|
|
||||||
与 Lidar-IMU 的 `export_rscap_to_v1` 同级：原始数据 → `combined/`。
|
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
python tools\export_raw_to_combined.py `
|
python tools\export_raw_to_combined.py `
|
||||||
@@ -94,13 +79,11 @@ python tools\export_raw_to_combined.py `
|
|||||||
--overwrite
|
--overwrite
|
||||||
```
|
```
|
||||||
|
|
||||||
### 4.2 导出 + 求解到最终外参
|
### 4.2 导出 + 求解
|
||||||
|
|
||||||
在仓库根目录执行（路径按本机修改）：
|
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
$Repo = (Resolve-Path ".").Path
|
$Repo = (Resolve-Path ".").Path
|
||||||
$Raw = "E:\calibration_data\data4"
|
$Raw = "E:\calibration_data\stations_batch"
|
||||||
$Out = "E:\calibration_output\rtk_lidar"
|
$Out = "E:\calibration_output\rtk_lidar"
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" `
|
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" `
|
-RtkCapture "$Raw\captures\rtk.rscap" `
|
||||||
-ImuCapture "$Raw\captures\imu.rscap" `
|
-ImuCapture "$Raw\captures\imu.rscap" `
|
||||||
-OutputRoot $Out `
|
-OutputRoot $Out `
|
||||||
-RtkReferenceHeightAboveGroundM 0.758 `
|
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||||
-ExpectedStations 34
|
-HeadingOffsetDeg -90 `
|
||||||
|
-ExpectedStations 27 `
|
||||||
|
-GroundZMin -2.5 `
|
||||||
|
-GroundZMax -1.5
|
||||||
```
|
```
|
||||||
|
|
||||||
| 关键参数 | 含义 |
|
| 参数 | 含义 |
|
||||||
|---|---|
|
|---|---|
|
||||||
| `-RtkReferenceHeightAboveGroundM` | RTK 参考点离地高度（米），**必填** |
|
| `-RtkReferenceHeightAboveGroundM` | 相位中心离地高（m），**必填**；本车 1.9165 |
|
||||||
| `-ExpectedStations` | 期望站点数 |
|
| `-HeadingOffsetDeg` | 默认 **-90** = 车头向前（本车主从装反） |
|
||||||
| `-MinPairs` | 最少共识运动对，默认 20 |
|
| `-GroundZMin/Max` | 约 2 m 雷达用 `[-2.5,-1.5]` |
|
||||||
| `-Bootstrap` | bootstrap 次数，默认 200 |
|
| `-ExpectedStations` / `-MinStations` | 本批 27 / 20 |
|
||||||
|
|
||||||
### 已有 combined 数据时
|
### 4.3 已有 combined
|
||||||
|
|
||||||
可跳过原始导出，直接标定：
|
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
|
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
|
||||||
-CombinedRoot "...\exported\combined" `
|
-CombinedRoot "D:\data\rtk_lidar_run\combined" `
|
||||||
-WorkRoot "...\prepared_rtk_direct" `
|
-WorkRoot "D:\data\rtk_lidar_run\prepared_vehicle_h19165" `
|
||||||
-OutputRoot "...\calibration" `
|
-OutputRoot "D:\data\rtk_lidar_run\outputs_vehicle_h19165" `
|
||||||
-RtkReferenceHeightAboveGroundM 0.758 `
|
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||||
-ExpectedStations 34
|
-HeadingOffsetDeg -90 `
|
||||||
|
-ExpectedStations 27 `
|
||||||
|
-GroundZMin -2.5 `
|
||||||
|
-GroundZMax -1.5
|
||||||
```
|
```
|
||||||
|
|
||||||
|
> 不得复用其他车辆或历史采集的天线离地高度、站点数量与外参结果。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 5. 输出说明
|
## 5. 输出说明
|
||||||
|
|
||||||
```text
|
```text
|
||||||
$Out/
|
$OutputRoot/
|
||||||
├── exported/ # 解析与关联中间结果
|
├── open3d_gicp/ small_gicp/ consensus/
|
||||||
├── prepared_rtk_direct/ # 每站一帧 + RTK 位姿表
|
├── common/ground_planes.csv
|
||||||
└── calibration/
|
├── summary.json
|
||||||
├── open3d_gicp/ # 后端 1
|
└── final_T_RTK_lidar.json
|
||||||
├── small_gicp/ # 后端 2
|
|
||||||
├── consensus/ # 双后端共识运动对
|
|
||||||
├── summary.json # 质量汇总
|
|
||||||
└── final_T_RTK_lidar.json ← 最终交付物
|
|
||||||
```
|
```
|
||||||
|
|
||||||
`final_T_RTK_lidar.json` 主要字段：
|
看 `summary.json` 的共识对数、AX 残差、bootstrap、双后端差。内部一致性 ≠ ±3 cm 绝对真值。
|
||||||
|
|
||||||
- `translation_m`：平移 (x, y, z)，单位米
|
|
||||||
- `rotation_rpy_deg_xyz`：滚转 / 俯仰 / 偏航，单位度
|
|
||||||
- `matrix_4x4`：4×4 齐次变换矩阵
|
|
||||||
|
|
||||||
质量指标看 `summary.json`：共识对数、AX 残差 RMS/中位数/P95、bootstrap 标准差、双后端差异。
|
|
||||||
|
|
||||||
> 内部一致性好 ≠ 已达到 ±3 cm 绝对真值；正式部署前建议再做独立轨迹验证。
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 6. 结果检查（可视化）
|
## 6. 可视化
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
|
$Repo = "D:\First-dev-dept\calibration-rtk-run"
|
||||||
|
$Out = "D:\data\rtk_lidar_run\outputs_vehicle_h19165"
|
||||||
|
$Work = "D:\data\rtk_lidar_run\prepared_vehicle_h19165"
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||||||
-Frames "$Out\prepared_rtk_direct\frames_all" `
|
-Frames "$Work\frames_all" `
|
||||||
-Pairs "$Out\calibration\consensus\B_consensus.npz" `
|
-Pairs "$Out\consensus\B_consensus.npz" `
|
||||||
-Extrinsic "$Out\calibration\final_T_RTK_lidar.json" `
|
-Extrinsic "$Out\final_T_RTK_lidar.json" `
|
||||||
-PairIndex 0
|
-PairIndex 0
|
||||||
```
|
```
|
||||||
|
|
||||||
| 按键 | 含义 |
|
重点看模式 **3 与 4**；用 `N`/`P` 多看**大转角**运动对。
|
||||||
|---|---|
|
|
||||||
| `1` | 原始点云 |
|
|
||||||
| `2` | 仅用 RTK 运动作初值 |
|
|
||||||
| `3` | GICP 测得的 B |
|
|
||||||
| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） |
|
|
||||||
| `N` / `]` | 下一运动对 |
|
|
||||||
| `P` / `[` | 上一运动对 |
|
|
||||||
| `Q` / `Esc` | 退出 |
|
|
||||||
|
|
||||||
蓝 = 目标站 i，橙 = 源站 j。重点看模式 **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
|
更细算法与本次数值结果见 [`README.md`](README.md)。
|
||||||
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)。
|
|
||||||
|
|||||||
Reference in New Issue
Block a user