Compare commits
12
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 lichun.qu
|
1233f8aafd | ||
 lichun.quandCursor
|
5ac50ad71f | ||
|
lichun.qu
|
3b3790ca2d | ||
|
lichun.quandCursor
|
6b44a495fb | ||
|
lichun.quandCursor
|
03fcee7e32 | ||
|
lichun.quandCursor
|
c2f99b94a2 | ||
|
lichun.quandCursor
|
2237be77a4 | ||
|
lichun.quandCursor
|
30f7e66db3 | ||
|
lichun.quandCursor
|
4ff176d184 | ||
|
lichun.quandCursor
|
ea06a3a523 | ||
|
lichun.quandCursor
|
e50a79b114 | ||
|
lichun.quandCursor
|
cf1fad7594 |
+5
-19
@@ -1,24 +1,10 @@
|
|||||||
# Python
|
|
||||||
__pycache__/
|
__pycache__/
|
||||||
*.py[cod]
|
*.py[cod]
|
||||||
.pytest_cache/
|
.pytest_cache/
|
||||||
|
*.egg-info/
|
||||||
|
.eggs/
|
||||||
|
dist/
|
||||||
|
build/
|
||||||
|
examples/synthetic_session/
|
||||||
.venv/
|
.venv/
|
||||||
venv/
|
venv/
|
||||||
|
|
||||||
# IDE / OS
|
|
||||||
.idea/
|
|
||||||
.vscode/
|
|
||||||
.DS_Store
|
|
||||||
Thumbs.db
|
|
||||||
|
|
||||||
# Raw data and generated outputs
|
|
||||||
data/raw/
|
|
||||||
work/
|
|
||||||
outputs/
|
|
||||||
*.rscap
|
|
||||||
*.dorec
|
|
||||||
*.log
|
|
||||||
|
|
||||||
# Large generated point clouds outside the archived reference result
|
|
||||||
**/frames/
|
|
||||||
**/frames_all/
|
|
||||||
|
|||||||
@@ -1,251 +1,184 @@
|
|||||||
# 双天线RTK—3D LiDAR直接手眼标定
|
# LiDAR–IMU 外参标定
|
||||||
|
|
||||||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达坐标转换到RTK导航坐标系。它**不是** `base_link` 车体外参，也不会在求解阶段使用车体航向偏置或RTK到后轮轴的XY杆臂。
|
用连续行驶中的 LiDAR 与 IMU 相对运动，估计安装外参与时间偏置：
|
||||||
数据下载地址：https://fs.fairylandtech.com:5001/FRLD/#file_id=963272954246902180 账号：lichun.qu@fairylandtech.com 密码：lichun.qu
|
|
||||||
|
|
||||||
## 1. 输出坐标约定
|
|
||||||
|
|
||||||
统一约定 `T_A_B` 把B系点变换到A系：
|
|
||||||
|
|
||||||
```text
|
```text
|
||||||
p_RTK = T_RTK_lidar · p_lidar
|
p_IMU = T_IMU_lidar · p_lidar
|
||||||
```
|
```
|
||||||
|
|
||||||
RTK导航系在本仓库中定义为：
|
**当前阶段：** 算法与合成自检已闭环；已提供 `tools/export_rscap_to_v1.py`（N300 `.rscap` + H32 dlog/MSOP → 中间格式）；**合格实车验收尚未完成**，故正式外参尚未对实车落盘交付。
|
||||||
|
|
||||||
- 原点：GGA位置参考点（通常为ANT1相位中心，必须结合接收机配置确认）；
|
---
|
||||||
- X轴：`rawHeading`所表示的双天线基线在水平面的投影；
|
|
||||||
- Y轴：左；
|
|
||||||
- Z轴：上；
|
|
||||||
- ENU航向：`yaw = 90° - rawHeading`；
|
|
||||||
- roll、pitch：当前轨迹中固定为0。
|
|
||||||
|
|
||||||
如果下游需要 `T_body_lidar`，必须另有经过确认的 `T_body_rtk`：
|
## 先看什么（对外三份就够）
|
||||||
|
|
||||||
```text
|
|
||||||
T_body_lidar = T_body_rtk · T_RTK_lidar
|
|
||||||
```
|
|
||||||
|
|
||||||
## 2. 算法流程
|
| 顺序 | 文档 | 用途 |
|
||||||
|
| --- | -------------------------------------- | -------------------- |
|
||||||
|
| 1 | **本 README** | 做什么、怎么跑、结果怎么判 |
|
||||||
|
| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + 原始数据导出命令 |
|
||||||
|
| 3 | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采（合格数据要求） |
|
||||||
|
|
||||||
```text
|
|
||||||
逐站 H32.rscap + 全程 RTK.rscap + IMU.rscap
|
|
||||||
→ tools/export_raw_to_combined.py（一步导出标定中间包 combined/）
|
|
||||||
→ 每站选择一帧静态点云，位置转局部ENU，rawHeading构造yaw-only RTK pose
|
|
||||||
→ Open3D GICP和small_gicp分别求 B_ij = T_Li_Lj
|
|
||||||
→ 留出点、Hessian、正反向、多初值和旋转共轭不变量筛选
|
|
||||||
→ 两后端共同认可的边形成consensus B
|
|
||||||
→ A_ij X = X B_ij + 地面法向/高度约束求 X = T_RTK_lidar
|
|
||||||
→ bootstrap、双后端差异、逐对残差和3D可视化检查
|
|
||||||
```
|
|
||||||
|
|
||||||
代码实际使用：
|
其余（方法细述、测试说明、源码职责、CHANGELOG）给深入阅读 / 改代码时用，见文末。
|
||||||
|
|
||||||
```text
|
---
|
||||||
A_ij = inv(T_W_Ri) · T_W_Rj = T_Ri_Rj
|
|
||||||
B_ij = T_Li_Lj # 将站点j点云变换到站点i
|
|
||||||
A_ij · X = X · B_ij
|
|
||||||
X = T_RTK_lidar
|
|
||||||
```
|
|
||||||
|
|
||||||
## 3. 原始数据目录
|
|
||||||
|
|
||||||
大体积数据不提交Git。**新车默认布局**（H32 / G90 / N300 新插件，不再出 dlog）：
|
|
||||||
|
|
||||||
```text
|
## 1. 输入 / 输出
|
||||||
raw_dataset/
|
|
||||||
├── stations/ # 每站停稳后单独录一段雷达
|
|
||||||
│ ├── 001/h32.rscap
|
|
||||||
│ ├── 002/h32.rscap
|
|
||||||
│ └── ...
|
|
||||||
└── captures/
|
|
||||||
├── rtk.rscap # 进场到收工连续录（G90：#PVTSLNA + #UNIHEADINGA）
|
|
||||||
└── imu.rscap # 连续录（N300；仅关联，不参与外参求解）
|
|
||||||
```
|
|
||||||
|
|
||||||
一键导出默认：雷达用 **MSOP 设备时间**，RTK 用 **GNSS week/TOW** 做最近邻关联（`-TimeBasis device_gnss`）。旧 dlog 数据集可继续放在同结构的 `dobject/` + `dobject_recording/` 下，并用 `-TimeBasis host`。
|
|
||||||
|
|
||||||
每个站点应在车辆完全静止后记录点云；建议不少于30站，并包含充足的直行、左转、右转和大角度转向姿态变化。
|
| 输入 | 说明 |
|
||||||
|
| --------- | ----------------------------------------------------------- |
|
||||||
|
| `imu.csv` | `t,gx,gy,gz,ax,ay,az`（秒；rad/s；m/s²）；`t` 用设备时间 |
|
||||||
|
| 雷达会话目录 | `frames_index.csv` + `frames/*.npz`（米制 XYZ） |
|
||||||
|
| 车辆 YAML | 轴向与时间语义；外参真值可空（`config/vehicle_installation.template.yaml`） |
|
||||||
|
|
||||||
## 4. 环境安装
|
|
||||||
|
|
||||||
已验证环境为Windows、PowerShell、Python 3.11。安装依赖：
|
|
||||||
|
| 输出 | 说明 |
|
||||||
|
| ------------------ | ---------------------- |
|
||||||
|
| `T_IMU_lidar.json` | 外参 |
|
||||||
|
| `time_offset.json` | `t_imu = t_lidar + δt` |
|
||||||
|
| `summary.json` | 状态、残差、可观性 |
|
||||||
|
|
||||||
|
|
||||||
|
新车原始数据导出（H32 dlog/zip + HI13 rscap）：
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
python -m pip install -r requirements.txt
|
python tools\export_rscap_to_v1.py `
|
||||||
|
--imu-rscap path\to\hi13r4-imu.rscap `
|
||||||
|
--imu-kind hi13 `
|
||||||
|
--lidar-dlog path\to\session_or_recovered.zip `
|
||||||
|
--host-start 2026-08-08T17:40:05 `
|
||||||
|
--host-end 2026-08-08T17:45:15 `
|
||||||
|
--out path\to\session_v1 `
|
||||||
|
--require-difop
|
||||||
```
|
```
|
||||||
|
|
||||||
依赖包括NumPy、SciPy、Open3D和small_gicp。若small_gicp没有对应Windows wheel，可在WSL2中安装后运行Python核心命令，或先只运行Open3D后端；完整共识流程需要两个后端都可用。
|
---
|
||||||
|
|
||||||
## 5. 从原始数据一键复现
|
|
||||||
|
|
||||||
导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级：**一条命令**把原始 rscap 变成标定可直接使用的 `combined/`。
|
|
||||||
|
|
||||||
仅导出中间包：
|
## 2. 一键复现（合成，不需实车）
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
python tools\export_raw_to_combined.py `
|
cd <本仓库根目录>
|
||||||
--stations-root "$Raw\stations" `
|
python -m pip install -e ".[dev]"
|
||||||
--rtk-rscap "$Raw\captures\rtk.rscap" `
|
python -m pip install -e ".[open3d]" # 推荐
|
||||||
--imu-rscap "$Raw\captures\imu.rscap" `
|
powershell -File tools\reproduce_synthetic.ps1
|
||||||
--out "$Out\exported" `
|
|
||||||
--overwrite
|
|
||||||
```
|
```
|
||||||
|
|
||||||
产物：
|
证明：链路可跑通，能收回已知 yaw / δt。
|
||||||
|
不证明：实车安装精度、平移可交付。
|
||||||
|
|
||||||
|
产物在 `examples/synthetic_session/out/`（含 `summary.json` 与 `motion_pairs.json`）。叠点查看：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
# 优先读取 summary 同目录的 motion_pairs.json，按需加载点云（无需重算配准）
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar examples\synthetic_session\lidar `
|
||||||
|
--summary examples\synthetic_session\out\summary.json `
|
||||||
|
--pair-index 0
|
||||||
|
```
|
||||||
|
|
||||||
|
旧标定目录若缺少缓存，可只补导出运动对（不重求解外参）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\export_motion_pairs_for_viz.py `
|
||||||
|
--lidar path\to\lidar `
|
||||||
|
--imu path\to\imu.csv `
|
||||||
|
--summary path\to\out\summary.json
|
||||||
|
```
|
||||||
|
|
||||||
|
键 `1`–`4` 切换叠点模式；`N`/`P` 切换运动对。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 3. 真实数据怎么跑
|
||||||
|
|
||||||
|
1. 按采集清单录制（设备时间；静止 + 低速转弯；有结构场景）
|
||||||
|
2. 导出中间格式（第1节命令）
|
||||||
|
3. 填写车辆 YAML 的轴向与时间语义
|
||||||
|
4. 标定：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m imu_lidar.cli run `
|
||||||
|
--vehicle-config config\vehicle_installation.template.yaml `
|
||||||
|
--imu path\to\session_v1\imu.csv `
|
||||||
|
--lidar path\to\session_v1\lidar `
|
||||||
|
--output path\to\out `
|
||||||
|
--mode rotation_only `
|
||||||
|
--time-offset-search-s 2.0
|
||||||
|
```
|
||||||
|
|
||||||
|
1. 看 `summary.json`，再叠点 / 用验证会话复核后才交付
|
||||||
|
|
||||||
|
|
||||||
|
| 模式 | 交付 | 成功标志 |
|
||||||
|
| ------------------- | ------- | --------------------------- |
|
||||||
|
| `rotation_only`（先做） | 旋转 + δt | `rotation_only_accepted` |
|
||||||
|
| `full_se3`（激励够再试） | + 可观平移 | `full_se3_accepted`（否则平移拒绝） |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
| `summary.json` 状态 | 含义 |
|
||||||
|
| ---------------------------------------------- | ----------- |
|
||||||
|
| `rotation_only_accepted` / `full_se3_accepted` | 可进入验证 |
|
||||||
|
| `full_se3_rejected_due_to_observability` | 旋转可用，平移不交 |
|
||||||
|
| `blocked` | **不可作安装参数** |
|
||||||
|
|
||||||
|
|
||||||
|
预期量级：旋转约 0.5°–2°；水平平移数厘米～十几厘米；无坡时竖直常不可观。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 4. 方法（一句话）
|
||||||
|
|
||||||
|
关键帧雷达配准得 **B**，同区间 IMU 预积分得 **A**，解 `R_A R_X ≈ R_X R_B`；再估 δt。可观时才在 `full_se3` 下交平移。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 5. 试验边界（勿误读）
|
||||||
|
|
||||||
|
|
||||||
|
| | 合成 pytest | 旧车 S2 线下 |
|
||||||
|
| ------ | ------------------------ | ----------------- |
|
||||||
|
| 目的 | 回归算法 | 验证旧主机时间数据上链路能跑完 |
|
||||||
|
| 期望 | `rotation_only_accepted` | `blocked`**（预期）** |
|
||||||
|
| 当安装参数？ | 否 | **否** |
|
||||||
|
|
||||||
|
|
||||||
|
细节：[tests/README.md](tests/README.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 6. 仓库结构与其余文档
|
||||||
|
|
||||||
```text
|
```text
|
||||||
$Out\exported\
|
imu_lidar/ 算法与 CLI
|
||||||
├── export/ # 内部：各站雷达帧（调试用）
|
config/ 车辆配置模板
|
||||||
├── parsed/ # 内部：RTK/IMU JSONL
|
docs/ 采集清单、数据格式、方法细述
|
||||||
├── combined/ # ★ 标定入口：关联后的多传感器 NPZ + manifest.csv
|
tools/ 导出、合成复现、可视化
|
||||||
└── export_summary.json
|
tests/ 自动化测试
|
||||||
```
|
```
|
||||||
|
|
||||||
完整求解（导出 + prepare + AX=XB）在仓库根目录执行：
|
|
||||||
|
|
||||||
```powershell
|
| 文档 | 何时看 |
|
||||||
$Repo = (Resolve-Path ".").Path
|
| ------------------------------------------------ | ---------------- |
|
||||||
$Raw = "E:\calibration_data\data4"
|
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 要看方法约定与实现状态表 |
|
||||||
$Out = "E:\calibration_output\rtk_lidar"
|
| [tests/README.md](tests/README.md) | 要看合成用例 / S2 烟测记录 |
|
||||||
|
| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 要改源码 |
|
||||||
|
| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 要查改动史 |
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" `
|
|
||||||
-DataRoot "$Raw\stations" `
|
|
||||||
-RtkCapture "$Raw\captures\rtk.rscap" `
|
|
||||||
-ImuCapture "$Raw\captures\imu.rscap" `
|
|
||||||
-OutputRoot $Out `
|
|
||||||
-RtkReferenceHeightAboveGroundM 0.758 `
|
|
||||||
-ExpectedStations 34
|
|
||||||
```
|
|
||||||
|
|
||||||
主要输出：
|
改算法请同步职责说明与 CHANGELOG；改对外用法请更新本 README。
|
||||||
|
|
||||||
```text
|
|
||||||
$Out/
|
|
||||||
├── exported/
|
|
||||||
│ ├── export/ # 内部各站LiDAR帧
|
|
||||||
│ ├── parsed/ # 内部 RTK/IMU JSONL
|
|
||||||
│ └── combined/ # ★ 按LiDAR帧关联后的多传感器NPZ
|
|
||||||
├── prepared_rtk_direct/
|
|
||||||
│ ├── frames_all/ # 每站选中的静态帧
|
|
||||||
│ └── reference_poses_rtk_gga_raw_heading.csv
|
|
||||||
└── calibration/
|
|
||||||
├── open3d_gicp/
|
|
||||||
├── small_gicp/
|
|
||||||
├── consensus/
|
|
||||||
├── summary.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可视化
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
|
||||||
-Frames "$Out\prepared_rtk_direct\frames_all" `
|
|
||||||
-Pairs "$Out\calibration\consensus\B_consensus.npz" `
|
|
||||||
-Extrinsic "$Out\calibration\final_T_RTK_lidar.json" `
|
|
||||||
-PairIndex 0
|
|
||||||
```
|
|
||||||
|
|
||||||
窗口中：
|
|
||||||
|
|
||||||
- 蓝色：目标站点i；橙色：站点j；
|
|
||||||
- `1`：原始点云；
|
|
||||||
- `2`：RTK运动A直接作为初值；
|
|
||||||
- `3`：GICP测得的B；
|
|
||||||
- `4`：最终外参预测的 `X^-1 A X`；
|
|
||||||
- `N` / `]`：下一运动对；
|
|
||||||
- `P` / `[`：上一运动对；
|
|
||||||
- `Q` / `Esc`：退出。
|
|
||||||
|
|
||||||
模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应用 `N`/`P` 多看几对，不能只挑视觉效果最好的一对。
|
|
||||||
|
|
||||||
## 7. data4与data4+data5结果对比
|
|
||||||
|
|
||||||
data5补充了30个有效静态站点及两个法向方向不同的固定平面板。当前联合流程只合并data4、data5各自的批内运动对，不构造跨批次运动，因此两次采集的时间、ENU原点和绝对位置不同不会直接影响共享外参；前提是传感器安装未改变，并且两批数据使用相同的RTK坐标定义和LiDAR原始坐标定义。
|
|
||||||
|
|
||||||
两块平面板在现有代码中作为点云场景结构参与GICP配准，但没有作为已知RTK/ENU平面方程单独加入优化；若后续能测得板面方程，才可新增绝对平面约束。
|
|
||||||
|
|
||||||
为公平比较，下面两组结果都使用ANT1参考点离地高度`0.758 m`重新求解：
|
|
||||||
|
|
||||||
| 指标 | data4单独 | data4+data5联合 | 变化 |
|
|
||||||
|---|---:|---:|---:|
|
|
||||||
| 有效站点 | 34 | 64 | +30 |
|
|
||||||
| 共识运动对 | 25 | 36 | +11 |
|
|
||||||
| 平移残差RMS | 0.100394 m | 0.086902 m | -13.4% |
|
|
||||||
| 平移残差中位数 | 0.062075 m | 0.053647 m | -13.6% |
|
|
||||||
| 平移残差P95 | 0.123039 m | 0.125166 m | +1.7% |
|
|
||||||
| 平移残差最大值 | 0.353438 m | 0.355864 m | +0.7% |
|
|
||||||
| 旋转残差RMS | 1.252391° | 1.115207° | -11.0% |
|
|
||||||
| 旋转残差中位数 | 0.747183° | 0.685164° | -8.3% |
|
|
||||||
| 旋转残差P90 | 1.825297° | 1.481369° | -18.8% |
|
|
||||||
| 旋转残差P95 | 1.965290° | 1.875860° | -4.6% |
|
|
||||||
| Weighted Jacobian condition | 7.713973 | 8.379217 | +8.6% |
|
|
||||||
| bootstrap z标准差 | 0.003147 m | 0.001957 m | -37.8% |
|
|
||||||
| bootstrap roll标准差 | 0.099319° | 0.062245° | -37.3% |
|
|
||||||
| bootstrap pitch标准差 | 0.096049° | 0.064370° | -33.0% |
|
|
||||||
|
|
||||||
联合结果为：
|
|
||||||
|
|
||||||
```text
|
|
||||||
translation_m = [1.642932528, -0.242302311, 0.180599708]
|
|
||||||
RPY_deg_xyz = [-0.886210651, 1.372780243, -22.112054052]
|
|
||||||
|
|
||||||
T_RTK_lidar =
|
|
||||||
0.926183553 0.376030869 0.028014474 1.642932528
|
|
||||||
-0.376311142 0.926478119 0.005312208 -0.242302311
|
|
||||||
-0.023957243 -0.015462238 0.999593402 0.180599708
|
|
||||||
0.000000000 0.000000000 0.000000000 1.000000000
|
|
||||||
```
|
|
||||||
|
|
||||||
与相同高度下的data4单独结果相比，联合外参相差`5.25 mm / 0.064°`。data5使RMS、中位数、旋转P90和bootstrap稳定性改善，但平移P95及最大值没有改善，说明少数高残差运动对仍然存在；不应仅为降低最大值而按最终外参残差删边。
|
|
||||||
|
|
||||||
已经分别得到各批次的共识运动对和地面平面时，可运行：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
$Names = @("data4", "data5")
|
|
||||||
$Pairs = @("E:\data4\consensus\B_consensus.npz", "E:\data5\consensus\B_consensus.npz")
|
|
||||||
$Planes = @("E:\data4\common\ground_planes.csv", "E:\data5\common\ground_planes.csv")
|
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" `
|
|
||||||
-BatchNames $Names -Pairs $Pairs -GroundPlanes $Planes `
|
|
||||||
-OutputRoot "E:\calibration_output\data4_data5_joint" `
|
|
||||||
-RtkReferenceHeightAboveGroundM 0.758 -Bootstrap 200
|
|
||||||
```
|
|
||||||
|
|
||||||
脚本会先分别拟合各批次外参；任一批与首批相差超过`0.25 m`或`5°`时中止，提示检查RTK航向/坐标定义和传感器安装。阈值可通过`-MaxBatchTranslationDifferenceM`和`-MaxBatchRotationDifferenceDeg`显式调整。
|
|
||||||
|
|
||||||
仓库内[`results/reference_data4`](results/reference_data4/README.md)是历史data4参考产物，使用旧高度配置，不应与上表直接比较，也不应继续作为当前联合外参下发。
|
|
||||||
|
|
||||||
## 8. z与精度限制
|
|
||||||
|
|
||||||
平面阿克曼运动不能独立观测z。当前联合结果使用64站地面平面和ANT1参考点离地`0.758 m`约束z；其中`0.758 m`来自本次现场粗测的天线底部安装参考高度`0.710 m`，加上天线标签给出的L1/L2 PCO高度`46/50 mm`的中值`48 mm`。该值仍是测量输入，不是手眼运动方程自行估计出来的量。
|
|
||||||
|
|
||||||
旧联合结果曾使用`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。
|
|
||||||
|
|
||||||
AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。
|
|
||||||
|
|
||||||
此外，代码无法单独证明GGA对应哪根物理天线、`rawHeading`是ANT1→ANT2还是ANT2→ANT1；必须用接收机配置、接线和现场运动实验确认。方向错误会导致RTK坐标系yaw相差约180°。
|
|
||||||
|
|
||||||
## 9. 仓库目录
|
|
||||||
|
|
||||||
| 目录 | 职责 |
|
|
||||||
|---|---|
|
|
||||||
| [`code/`](code/) | GICP、运动对质量评价、AX=XB求解、结果封装和3D可视化 |
|
|
||||||
| [`tools/`](tools/) | 原始dlog/rscap解析、按LiDAR帧关联及静态站点prepared生成 |
|
|
||||||
| [`run/`](run/) | PowerShell入口；所有数据和输出路径都通过参数传入 |
|
|
||||||
| [`results/reference_data4/`](results/reference_data4/) | 历史data4精简参考结果，不包含点云和本机过程目录 |
|
|
||||||
| `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 |
|
|
||||||
|
|
||||||
各代码文件职责见[`code/README.md`](code/README.md)，命令索引见[`run/README.md`](run/README.md)，工具说明见[`tools/README.md`](tools/README.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 可视化方法。
|
|
||||||
|
|
||||||
@@ -1,13 +0,0 @@
|
|||||||
# code目录
|
|
||||||
|
|
||||||
| 文件 | 职责 |
|
|
||||||
|---|---|
|
|
||||||
| `rigorous_calibration.py` | 核心CLI：读取静态点云/RTK位姿，Open3D或small_gicp求B，拟合地面，求解/验证AX=XB |
|
|
||||||
| `refine_pairs.py` | 不使用最终X，按留出点重叠率、RMSE、旋转共轭不变量和正反向一致性精筛运动对 |
|
|
||||||
| `cross_backend_filter.py` | 保留Open3D与small_gicp共同认可且变换接近的边；共识B数值取Open3D结果 |
|
|
||||||
| `finalize_direct_rtk_lidar.py` | 将三路求解结果封装为明确方向的`T_RTK_lidar`，选择consensus为最终结果 |
|
|
||||||
| `visualize_pair_3d.py` | 交互显示原始、RTK初值、GICP B和`X^-1AX`，并打印增量 |
|
|
||||||
| `compare_extrinsics.py` | 计算两套外参的SE(3)平移/旋转差异 |
|
|
||||||
| `build_joint_rtk_lidar_inputs.py` | 合并多个独立批次的批内A/B运动对和地面平面，并保留批次索引与汇总信息 |
|
|
||||||
|
|
||||||
核心约定：`A=T_Ri_Rj`、`B=T_Li_Lj`、`X=T_RTK_lidar`，满足`A X = X B`。点云配准以i为target、j为source，B将j帧点云变换到i帧。
|
|
||||||
@@ -1,119 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Combine independent RTK-direct hand-eye batches for a shared extrinsic.
|
|
||||||
|
|
||||||
Each batch contributes only its within-batch A/B motion pairs and LiDAR ground
|
|
||||||
planes. No cross-batch motion pair is created, so different ENU origins and
|
|
||||||
capture locations are valid as long as every batch uses the same RTK-direct
|
|
||||||
frame definition and unchanged physical sensor installation.
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import csv
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
def parse_args() -> argparse.Namespace:
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--batch-name", action="append", required=True)
|
|
||||||
parser.add_argument("--pairs", action="append", required=True, type=Path)
|
|
||||||
parser.add_argument("--ground-planes", action="append", required=True, type=Path)
|
|
||||||
parser.add_argument("--output-pairs", required=True, type=Path)
|
|
||||||
parser.add_argument("--output-ground-planes", required=True, type=Path)
|
|
||||||
parser.add_argument("--summary", required=True, type=Path)
|
|
||||||
return parser.parse_args()
|
|
||||||
|
|
||||||
|
|
||||||
def load_planes(path: Path, batch_name: str) -> list[dict[str, str]]:
|
|
||||||
with path.open(encoding="utf-8-sig", newline="") as stream:
|
|
||||||
rows = list(csv.DictReader(stream))
|
|
||||||
if not rows:
|
|
||||||
raise ValueError(f"no ground planes in {path}")
|
|
||||||
for row in rows:
|
|
||||||
for key in ("nx", "ny", "nz", "d"):
|
|
||||||
if key not in row or row[key] in (None, ""):
|
|
||||||
raise ValueError(f"missing {key} in {path}")
|
|
||||||
row["source_batch"] = batch_name
|
|
||||||
return rows
|
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
|
||||||
args = parse_args()
|
|
||||||
count = len(args.batch_name)
|
|
||||||
if count < 2 or len(args.pairs) != count or len(args.ground_planes) != count:
|
|
||||||
raise ValueError("provide the same number of --batch-name, --pairs, and --ground-planes (at least two)")
|
|
||||||
|
|
||||||
pair_parts: list[dict[str, np.ndarray]] = []
|
|
||||||
plane_rows: list[dict[str, str]] = []
|
|
||||||
batch_summaries: list[dict[str, object]] = []
|
|
||||||
for index, (name, pairs_path, planes_path) in enumerate(zip(args.batch_name, args.pairs, args.ground_planes)):
|
|
||||||
with np.load(pairs_path, allow_pickle=False) as source:
|
|
||||||
required = ("A", "B", "meta", "station_times", "rtk_nearest_dt_s")
|
|
||||||
missing = [key for key in required if key not in source]
|
|
||||||
if missing:
|
|
||||||
raise ValueError(f"{pairs_path} missing {missing}")
|
|
||||||
a = np.asarray(source["A"], float)
|
|
||||||
b = np.asarray(source["B"], float)
|
|
||||||
meta = np.asarray(source["meta"], float)
|
|
||||||
times = np.asarray(source["station_times"], float)
|
|
||||||
rtk_dt = np.asarray(source["rtk_nearest_dt_s"], float)
|
|
||||||
if len(a) == 0 or len(a) != len(b) or len(a) != len(meta):
|
|
||||||
raise ValueError(f"invalid A/B/meta sizes in {pairs_path}")
|
|
||||||
pair_parts.append({"A": a, "B": b, "meta": meta, "station_times": times, "rtk_dt": rtk_dt})
|
|
||||||
rows = load_planes(planes_path, name)
|
|
||||||
plane_rows.extend(rows)
|
|
||||||
batch_summaries.append({
|
|
||||||
"name": name,
|
|
||||||
"pairs_path": str(pairs_path.resolve()),
|
|
||||||
"ground_planes_path": str(planes_path.resolve()),
|
|
||||||
"pairs": len(a),
|
|
||||||
"stations": len(times),
|
|
||||||
"ground_planes": len(rows),
|
|
||||||
"pair_offset": sum(item["A"].shape[0] for item in pair_parts[:-1]),
|
|
||||||
})
|
|
||||||
|
|
||||||
output_pairs = args.output_pairs
|
|
||||||
output_pairs.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
batch_index = np.concatenate([np.full(len(part["A"]), index, np.int32) for index, part in enumerate(pair_parts)])
|
|
||||||
np.savez_compressed(
|
|
||||||
output_pairs,
|
|
||||||
A=np.concatenate([part["A"] for part in pair_parts]),
|
|
||||||
B=np.concatenate([part["B"] for part in pair_parts]),
|
|
||||||
meta=np.concatenate([part["meta"] for part in pair_parts]),
|
|
||||||
station_times=np.concatenate([part["station_times"] for part in pair_parts]),
|
|
||||||
rtk_nearest_dt_s=np.concatenate([part["rtk_dt"] for part in pair_parts]),
|
|
||||||
batch_index=batch_index,
|
|
||||||
batch_names=np.asarray(args.batch_name),
|
|
||||||
backend=np.asarray("independent_batch_consensus"),
|
|
||||||
)
|
|
||||||
|
|
||||||
output_planes = args.output_ground_planes
|
|
||||||
output_planes.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
fieldnames = ["nx", "ny", "nz", "d", "source_batch"]
|
|
||||||
with output_planes.open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
writer = csv.DictWriter(stream, fieldnames=fieldnames)
|
|
||||||
writer.writeheader()
|
|
||||||
for row in plane_rows:
|
|
||||||
writer.writerow({key: row[key] for key in fieldnames})
|
|
||||||
|
|
||||||
summary = {
|
|
||||||
"schema_version": 1,
|
|
||||||
"convention": "Shared T_RTK_lidar; only within-batch A_ij and B_ij are combined.",
|
|
||||||
"batches": batch_summaries,
|
|
||||||
"total_pairs": int(len(batch_index)),
|
|
||||||
"total_ground_planes": len(plane_rows),
|
|
||||||
"output_pairs": str(output_pairs.resolve()),
|
|
||||||
"output_ground_planes": str(output_planes.resolve()),
|
|
||||||
}
|
|
||||||
args.summary.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
args.summary.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())
|
|
||||||
@@ -1,43 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Compare two homogeneous-extrinsic JSON files in parameter space and on SE(3)."""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--reference", type=Path, required=True)
|
|
||||||
parser.add_argument("--candidate", type=Path, required=True)
|
|
||||||
parser.add_argument("--output", type=Path, required=True)
|
|
||||||
args = parser.parse_args()
|
|
||||||
reference = json.loads(args.reference.read_text(encoding="utf-8-sig"))
|
|
||||||
candidate = json.loads(args.candidate.read_text(encoding="utf-8-sig"))
|
|
||||||
a = np.asarray(reference["matrix_4x4"], dtype=float)
|
|
||||||
b = np.asarray(candidate["matrix_4x4"], dtype=float)
|
|
||||||
delta = np.linalg.inv(a) @ b
|
|
||||||
result = {
|
|
||||||
"convention": "delta = inverse(reference) @ candidate",
|
|
||||||
"reference": str(args.reference.resolve()),
|
|
||||||
"candidate": str(args.candidate.resolve()),
|
|
||||||
"candidate_minus_reference_translation_xyz_m": (b[:3, 3] - a[:3, 3]).tolist(),
|
|
||||||
"candidate_minus_reference_rpy_xyz_deg": (
|
|
||||||
np.asarray(candidate["rotation_rpy_deg_xyz"], float)
|
|
||||||
- np.asarray(reference["rotation_rpy_deg_xyz"], float)
|
|
||||||
).tolist(),
|
|
||||||
"relative_translation_norm_m": float(np.linalg.norm(delta[:3, 3])),
|
|
||||||
"relative_rotation_deg": float(np.degrees(Rotation.from_matrix(delta[:3, :3]).magnitude())),
|
|
||||||
"relative_matrix_4x4": delta.tolist(),
|
|
||||||
}
|
|
||||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
args.output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
print(json.dumps(result, ensure_ascii=False, indent=2))
|
|
||||||
return 0
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
raise SystemExit(main())
|
|
||||||
@@ -1,81 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Keep common A/B edges on which Open3D and small_gicp agree, without using X."""
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
|
|
||||||
def key(meta):
|
|
||||||
return int(meta[0]), int(meta[1])
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--open3d-pairs", required=True)
|
|
||||||
parser.add_argument("--small-pairs", required=True)
|
|
||||||
parser.add_argument("--output", required=True)
|
|
||||||
parser.add_argument("--audit")
|
|
||||||
parser.add_argument("--max-translation", type=float, default=0.05)
|
|
||||||
parser.add_argument("--max-rotation", type=float, default=0.50)
|
|
||||||
parser.add_argument("--min-pairs", type=int, default=25)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
with np.load(args.open3d_pairs, allow_pickle=False) as source:
|
|
||||||
open_a = np.asarray(source["A"], float)
|
|
||||||
open_b = np.asarray(source["B"], float)
|
|
||||||
open_meta = np.asarray(source["meta"], float)
|
|
||||||
station_times = np.asarray(source["station_times"])
|
|
||||||
rtk_dt = np.asarray(source["rtk_nearest_dt_s"])
|
|
||||||
with np.load(args.small_pairs, allow_pickle=False) as source:
|
|
||||||
small = {key(meta): np.asarray(b, float)
|
|
||||||
for meta, b in zip(source["meta"], source["B"])}
|
|
||||||
|
|
||||||
keep, audit = [], []
|
|
||||||
for meta, b_open in zip(open_meta, open_b):
|
|
||||||
edge = key(meta)
|
|
||||||
if edge not in small:
|
|
||||||
audit.append({"i": edge[0], "j": edge[1], "accepted": False,
|
|
||||||
"reason": "not_in_small_gicp_refined"})
|
|
||||||
keep.append(False)
|
|
||||||
continue
|
|
||||||
delta = np.linalg.inv(b_open) @ small[edge]
|
|
||||||
translation = float(np.linalg.norm(delta[:3, 3]))
|
|
||||||
rotation = float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude()))
|
|
||||||
accepted = translation <= args.max_translation and rotation <= args.max_rotation
|
|
||||||
keep.append(accepted)
|
|
||||||
audit.append({
|
|
||||||
"i": edge[0], "j": edge[1],
|
|
||||||
"open3d_small_translation_m": translation,
|
|
||||||
"open3d_small_rotation_deg": rotation,
|
|
||||||
"accepted": accepted,
|
|
||||||
"reason": "" if accepted else "backend_disagreement",
|
|
||||||
})
|
|
||||||
keep = np.asarray(keep, bool)
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
np.savez_compressed(
|
|
||||||
output, A=open_a[keep], B=open_b[keep], meta=open_meta[keep],
|
|
||||||
station_times=station_times, rtk_nearest_dt_s=rtk_dt,
|
|
||||||
backend=np.asarray("open3d_gicp_cross_backend_consensus"),
|
|
||||||
)
|
|
||||||
audit_path = Path(args.audit or output.with_suffix(".consensus.json"))
|
|
||||||
audit_path.write_text(json.dumps({
|
|
||||||
"selection_is_X_independent": True,
|
|
||||||
"B_source": "Open3D; small_gicp is used only as an agreement gate",
|
|
||||||
"max_translation_m": args.max_translation,
|
|
||||||
"max_rotation_deg": args.max_rotation,
|
|
||||||
"input_open3d_pairs": len(open_b),
|
|
||||||
"accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"pairs": audit,
|
|
||||||
}, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
if np.count_nonzero(keep) < args.min_pairs:
|
|
||||||
raise RuntimeError(f"only {np.count_nonzero(keep)} consensus pairs")
|
|
||||||
print(json.dumps({"accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,128 +0,0 @@
|
|||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
|
|
||||||
def load(path: Path) -> dict:
|
|
||||||
return json.loads(path.read_text(encoding="utf-8-sig"))
|
|
||||||
|
|
||||||
|
|
||||||
def write(path: Path, document: dict) -> None:
|
|
||||||
path.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
path.write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
|
|
||||||
|
|
||||||
def inverse(t: np.ndarray) -> np.ndarray:
|
|
||||||
result = np.eye(4)
|
|
||||||
result[:3, :3] = t[:3, :3].T
|
|
||||||
result[:3, 3] = -result[:3, :3] @ t[:3, 3]
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def delta(a: np.ndarray, b: np.ndarray) -> dict:
|
|
||||||
d = inverse(a) @ b
|
|
||||||
return {
|
|
||||||
"translation_m": float(np.linalg.norm(d[:3, 3])),
|
|
||||||
"rotation_deg": float(np.linalg.norm(Rotation.from_matrix(d[:3, :3]).as_rotvec()) * 180.0 / math.pi),
|
|
||||||
"delta_matrix_4x4": d.tolist(),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def corrected(raw: dict, backend: str, reference_height: float) -> dict:
|
|
||||||
return {
|
|
||||||
"schema_version": 1,
|
|
||||||
"success": bool(raw["success"]),
|
|
||||||
"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": backend,
|
|
||||||
"measured_lidar_extrinsic_used_as_initial": False,
|
|
||||||
"body_heading_offset_used": False,
|
|
||||||
"body_antenna_lever_xy_used": False,
|
|
||||||
"translation_m": raw["translation_m"],
|
|
||||||
"rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
|
|
||||||
"quaternion_xyzw": raw["quaternion_xyzw"],
|
|
||||||
"matrix_4x4": raw["matrix_4x4"],
|
|
||||||
"quality": {
|
|
||||||
"stations": raw["estimation"]["stations"],
|
|
||||||
"pairs": raw["estimation"]["pairs"],
|
|
||||||
"residuals": raw["estimation"]["residuals"],
|
|
||||||
"weighted_jacobian_condition_number": raw["weighted_jacobian_condition_number"],
|
|
||||||
"linearized_one_sigma": raw["linearized_one_sigma"],
|
|
||||||
"bootstrap": raw["bootstrap"],
|
|
||||||
},
|
|
||||||
"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": reference_height,
|
|
||||||
"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",
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def main() -> None:
|
|
||||||
parser = argparse.ArgumentParser()
|
|
||||||
parser.add_argument("--result-root", type=Path, required=True)
|
|
||||||
parser.add_argument("--reference-height", type=float, required=True)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
def solver_output(directory: str) -> Path:
|
|
||||||
raw = args.result_root / directory / "extrinsic_raw.json"
|
|
||||||
standard = args.result_root / directory / "extrinsic.json"
|
|
||||||
return raw if raw.exists() else standard
|
|
||||||
|
|
||||||
paths = {
|
|
||||||
"open3d_gicp": solver_output("open3d_gicp"),
|
|
||||||
"small_gicp": solver_output("small_gicp"),
|
|
||||||
"consensus": solver_output("consensus"),
|
|
||||||
}
|
|
||||||
docs = {}
|
|
||||||
for backend, path in paths.items():
|
|
||||||
document = corrected(load(path), backend, args.reference_height)
|
|
||||||
write(path.with_name("extrinsic_rtk_lidar.json"), document)
|
|
||||||
docs[backend] = document
|
|
||||||
|
|
||||||
open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
|
|
||||||
small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
|
|
||||||
final = dict(docs["consensus"])
|
|
||||||
final["selection"] = {
|
|
||||||
"recommended": True,
|
|
||||||
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
|
||||||
"open3d_vs_small_gicp": delta(open_t, small_t),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
write(args.result_root / "final_T_RTK_lidar.json", final)
|
|
||||||
summary = {
|
|
||||||
"final": {
|
|
||||||
"translation_m": final["translation_m"],
|
|
||||||
"rotation_rpy_deg_xyz": final["rotation_rpy_deg_xyz"],
|
|
||||||
"pairs": final["quality"]["pairs"],
|
|
||||||
"translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"],
|
|
||||||
"rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"],
|
|
||||||
"condition_number": final["quality"]["weighted_jacobian_condition_number"],
|
|
||||||
},
|
|
||||||
"backend_difference": delta(open_t, small_t),
|
|
||||||
}
|
|
||||||
write(args.result_root / "summary.json", summary)
|
|
||||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,87 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""X-independent second-stage filter for stationary A/B pairs."""
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from rigorous_calibration import read_pairs, rotation_angle_deg
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--pairs", required=True)
|
|
||||||
parser.add_argument("--quality-json", required=True)
|
|
||||||
parser.add_argument("--output", required=True)
|
|
||||||
parser.add_argument("--audit")
|
|
||||||
parser.add_argument("--min-pairs", type=int, default=25)
|
|
||||||
parser.add_argument("--min-inlier-ratio", type=float, default=0.70)
|
|
||||||
parser.add_argument("--max-inlier-rmse", type=float, default=0.13)
|
|
||||||
parser.add_argument("--max-rotation-invariant-error", type=float, default=0.75)
|
|
||||||
parser.add_argument("--reverse-translation-tolerance", type=float, default=0.05)
|
|
||||||
parser.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
a_array, b_array, meta, _ = read_pairs(args.pairs)
|
|
||||||
quality = json.loads(Path(args.quality_json).read_text(encoding="utf-8-sig"))
|
|
||||||
reports = {(int(item["i"]), int(item["j"])): item for item in quality["pairs"]}
|
|
||||||
keep, audit = [], []
|
|
||||||
for a_ij, b_ij, item_meta in zip(a_array, b_array, meta):
|
|
||||||
key = (int(item_meta[0]), int(item_meta[1]))
|
|
||||||
report = reports[key]
|
|
||||||
heldout = report["heldout_symmetric"]
|
|
||||||
reverse = report["forward_reverse"]
|
|
||||||
invariant = abs(rotation_angle_deg(a_ij[:3, :3]) - rotation_angle_deg(b_ij[:3, :3]))
|
|
||||||
reasons = []
|
|
||||||
if heldout["inlier_ratio"] < args.min_inlier_ratio:
|
|
||||||
reasons.append("overlap_ratio")
|
|
||||||
if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
|
|
||||||
reasons.append("heldout_rmse")
|
|
||||||
if invariant > args.max_rotation_invariant_error:
|
|
||||||
reasons.append("rotation_conjugacy_invariant")
|
|
||||||
if reverse["translation_m"] > args.reverse_translation_tolerance:
|
|
||||||
reasons.append("forward_reverse_translation")
|
|
||||||
if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
|
|
||||||
reasons.append("forward_reverse_rotation")
|
|
||||||
accepted = not reasons
|
|
||||||
keep.append(accepted)
|
|
||||||
audit.append({
|
|
||||||
"i": key[0], "j": key[1], "heldout_inlier_ratio": heldout["inlier_ratio"],
|
|
||||||
"heldout_inlier_rmse_m": heldout["inlier_rmse_m"],
|
|
||||||
"rotation_invariant_error_deg": invariant,
|
|
||||||
"reverse_translation_m": reverse["translation_m"],
|
|
||||||
"reverse_rotation_deg": reverse["rotation_deg"],
|
|
||||||
"accepted": accepted, "rejection_reasons": reasons,
|
|
||||||
})
|
|
||||||
keep = np.asarray(keep, bool)
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
with np.load(args.pairs, allow_pickle=False) as source:
|
|
||||||
np.savez_compressed(
|
|
||||||
output, A=a_array[keep], B=b_array[keep], meta=meta[keep],
|
|
||||||
station_times=np.asarray(source["station_times"]),
|
|
||||||
rtk_nearest_dt_s=np.asarray(source["rtk_nearest_dt_s"]),
|
|
||||||
backend=np.asarray(source["backend"]),
|
|
||||||
)
|
|
||||||
audit_path = Path(args.audit or output.with_suffix(".refinement.json"))
|
|
||||||
audit_path.write_text(json.dumps({
|
|
||||||
"selection_is_X_independent": True,
|
|
||||||
"criteria": {
|
|
||||||
"min_inlier_ratio": args.min_inlier_ratio,
|
|
||||||
"max_inlier_rmse_m": args.max_inlier_rmse,
|
|
||||||
"max_rotation_invariant_error_deg": args.max_rotation_invariant_error,
|
|
||||||
"reverse_translation_tolerance_m": args.reverse_translation_tolerance,
|
|
||||||
"reverse_rotation_tolerance_deg": args.reverse_rotation_tolerance,
|
|
||||||
},
|
|
||||||
"input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"pairs": audit,
|
|
||||||
}, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
if np.count_nonzero(keep) < args.min_pairs:
|
|
||||||
raise RuntimeError(f"only {np.count_nonzero(keep)} refined pairs; need {args.min_pairs}")
|
|
||||||
print(json.dumps({"input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,775 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Rigorous stationary LiDAR / reference-trajectory hand-eye calibration.
|
|
||||||
|
|
||||||
Convention: T_A_B maps points from frame B into frame A.
|
|
||||||
For this repository the reference frame is the RTK navigation frame.
|
|
||||||
X = T_RTK_lidar, A_ij = T_W_Ri^-1 T_W_Rj, B_ij = T_Li_Lj,
|
|
||||||
therefore A_ij X = X B_ij. Raw sensor-frame points_raw are used.
|
|
||||||
"""
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import csv
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
import time
|
|
||||||
from dataclasses import dataclass
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.optimize import least_squares
|
|
||||||
from scipy.spatial import cKDTree
|
|
||||||
|
|
||||||
|
|
||||||
def skew(v):
|
|
||||||
x, y, z = v
|
|
||||||
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
|
|
||||||
|
|
||||||
|
|
||||||
def so3_exp(v):
|
|
||||||
angle = float(np.linalg.norm(v))
|
|
||||||
if angle < 1e-12:
|
|
||||||
return np.eye(3) + skew(v)
|
|
||||||
k = skew(np.asarray(v, float) / angle)
|
|
||||||
return np.eye(3) + math.sin(angle) * k + (1.0 - math.cos(angle)) * k @ k
|
|
||||||
|
|
||||||
|
|
||||||
def so3_log(rotation):
|
|
||||||
cosine = float(np.clip((np.trace(rotation) - 1.0) / 2.0, -1.0, 1.0))
|
|
||||||
angle = math.acos(cosine)
|
|
||||||
vee = np.array([
|
|
||||||
rotation[2, 1] - rotation[1, 2],
|
|
||||||
rotation[0, 2] - rotation[2, 0],
|
|
||||||
rotation[1, 0] - rotation[0, 1],
|
|
||||||
])
|
|
||||||
if angle < 1e-9:
|
|
||||||
return vee / 2.0
|
|
||||||
if abs(math.pi - angle) < 1e-5:
|
|
||||||
values, vectors = np.linalg.eigh((rotation + np.eye(3)) / 2.0)
|
|
||||||
return vectors[:, int(np.argmax(values))] * angle
|
|
||||||
return vee * angle / (2.0 * math.sin(angle))
|
|
||||||
|
|
||||||
|
|
||||||
def quat_to_rotation(q):
|
|
||||||
x, y, z, w = np.asarray(q, float) / np.linalg.norm(q)
|
|
||||||
return np.array([
|
|
||||||
[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
|
|
||||||
[2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
|
|
||||||
[2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)],
|
|
||||||
])
|
|
||||||
|
|
||||||
|
|
||||||
def rotation_to_quat(rotation):
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
return Rotation.from_matrix(rotation).as_quat()
|
|
||||||
|
|
||||||
|
|
||||||
def rpy_deg(rotation):
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
return Rotation.from_matrix(rotation).as_euler("xyz", degrees=True).tolist()
|
|
||||||
|
|
||||||
|
|
||||||
def make_transform(translation, rotation):
|
|
||||||
transform = np.eye(4)
|
|
||||||
transform[:3, :3] = rotation
|
|
||||||
transform[:3, 3] = translation
|
|
||||||
return transform
|
|
||||||
|
|
||||||
|
|
||||||
def params_transform(params):
|
|
||||||
return make_transform(params[:3], so3_exp(params[3:]))
|
|
||||||
|
|
||||||
|
|
||||||
def inverse_transform(transform):
|
|
||||||
answer = np.eye(4)
|
|
||||||
answer[:3, :3] = transform[:3, :3].T
|
|
||||||
answer[:3, 3] = -answer[:3, :3] @ transform[:3, 3]
|
|
||||||
return answer
|
|
||||||
|
|
||||||
|
|
||||||
def transform_points(points, transform):
|
|
||||||
return points @ transform[:3, :3].T + transform[:3, 3]
|
|
||||||
|
|
||||||
|
|
||||||
def rotation_angle_deg(rotation):
|
|
||||||
return math.degrees(np.linalg.norm(so3_log(rotation)))
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class PoseSeries:
|
|
||||||
time: np.ndarray
|
|
||||||
transforms: np.ndarray
|
|
||||||
|
|
||||||
|
|
||||||
def read_poses(path):
|
|
||||||
timestamps, transforms = [], []
|
|
||||||
with Path(path).open(encoding="utf-8-sig", newline="") as stream:
|
|
||||||
reader = csv.DictReader(stream)
|
|
||||||
required = ("time", "x", "y", "z", "qx", "qy", "qz", "qw")
|
|
||||||
missing = [key for key in required if key not in (reader.fieldnames or [])]
|
|
||||||
if missing:
|
|
||||||
raise ValueError(f"{path}: missing pose fields {missing}")
|
|
||||||
for row in reader:
|
|
||||||
timestamps.append(float(row["time"]))
|
|
||||||
translation = np.array([float(row[k]) for k in ("x", "y", "z")])
|
|
||||||
quaternion = np.array([float(row[k]) for k in ("qx", "qy", "qz", "qw")])
|
|
||||||
transforms.append(make_transform(translation, quat_to_rotation(quaternion)))
|
|
||||||
order = np.argsort(timestamps)
|
|
||||||
return PoseSeries(np.asarray(timestamps)[order], np.asarray(transforms)[order])
|
|
||||||
|
|
||||||
|
|
||||||
def nearest_pose(series, timestamp):
|
|
||||||
index = int(np.argmin(np.abs(series.time - timestamp)))
|
|
||||||
return series.transforms[index], float(abs(series.time[index] - timestamp))
|
|
||||||
|
|
||||||
|
|
||||||
def npz_files(root):
|
|
||||||
files = sorted(Path(root).rglob("*.npz"))
|
|
||||||
if not files:
|
|
||||||
raise FileNotFoundError(f"no NPZ files under {root}")
|
|
||||||
return files
|
|
||||||
|
|
||||||
|
|
||||||
def load_npz_xyz(path, min_range=1.0, max_range=50.0):
|
|
||||||
with np.load(path, allow_pickle=False) as data:
|
|
||||||
if "points_raw" not in data:
|
|
||||||
raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden")
|
|
||||||
raw = np.asarray(data["points_raw"], dtype=np.float64)
|
|
||||||
timestamp = float(np.ravel(data["unix_time_ns"])[0]) / 1e9
|
|
||||||
counter = int(np.ravel(data["frame_counter"])[0])
|
|
||||||
distance = raw[:, 0] * 0.001
|
|
||||||
azimuth = np.deg2rad(raw[:, 1])
|
|
||||||
altitude = np.deg2rad(raw[:, 2])
|
|
||||||
valid = (
|
|
||||||
np.isfinite(distance + azimuth + altitude)
|
|
||||||
& (distance >= min_range)
|
|
||||||
& (distance <= max_range)
|
|
||||||
)
|
|
||||||
distance, azimuth, altitude = distance[valid], azimuth[valid], altitude[valid]
|
|
||||||
xyz = np.column_stack((
|
|
||||||
distance * np.cos(altitude) * np.cos(azimuth),
|
|
||||||
distance * np.cos(altitude) * np.sin(azimuth),
|
|
||||||
distance * np.sin(altitude),
|
|
||||||
))
|
|
||||||
return timestamp, counter, xyz
|
|
||||||
|
|
||||||
|
|
||||||
def load_stations(root, min_range, max_range, z_min=None, z_max=None):
|
|
||||||
stations = []
|
|
||||||
for path in npz_files(root):
|
|
||||||
timestamp, counter, xyz = load_npz_xyz(path, min_range, max_range)
|
|
||||||
if z_min is not None:
|
|
||||||
xyz = xyz[(xyz[:, 2] >= z_min) & (xyz[:, 2] <= z_max)]
|
|
||||||
stations.append((timestamp, counter, path, xyz))
|
|
||||||
stations.sort(key=lambda item: item[0])
|
|
||||||
return stations
|
|
||||||
|
|
||||||
|
|
||||||
def split_holdout(points, fraction, phase):
|
|
||||||
stride = max(int(round(1.0 / fraction)), 2)
|
|
||||||
index = np.arange(len(points))
|
|
||||||
holdout = ((index + phase) % stride) == 0
|
|
||||||
return points[~holdout], points[holdout]
|
|
||||||
|
|
||||||
|
|
||||||
def make_o3d_cloud(points, voxel):
|
|
||||||
import open3d as o3d
|
|
||||||
cloud = o3d.geometry.PointCloud()
|
|
||||||
cloud.points = o3d.utility.Vector3dVector(np.asarray(points, float))
|
|
||||||
return cloud.voxel_down_sample(voxel)
|
|
||||||
|
|
||||||
|
|
||||||
def align_open3d(target, source, initial, voxels, correspondences, iterations):
|
|
||||||
import open3d as o3d
|
|
||||||
registration = o3d.pipelines.registration
|
|
||||||
estimate = registration.TransformationEstimationForGeneralizedICP()
|
|
||||||
criteria = registration.ICPConvergenceCriteria(max_iteration=iterations)
|
|
||||||
transform, stages = np.asarray(initial, float), []
|
|
||||||
final_target = final_source = final_answer = None
|
|
||||||
started = time.perf_counter()
|
|
||||||
for voxel, correspondence in zip(voxels, correspondences):
|
|
||||||
target_cloud = make_o3d_cloud(target, voxel)
|
|
||||||
source_cloud = make_o3d_cloud(source, voxel)
|
|
||||||
answer = registration.registration_generalized_icp(
|
|
||||||
source_cloud, target_cloud, correspondence, transform, estimate, criteria
|
|
||||||
)
|
|
||||||
transform = np.asarray(answer.transformation, float)
|
|
||||||
stages.append({
|
|
||||||
"voxel_m": voxel,
|
|
||||||
"max_correspondence_m": correspondence,
|
|
||||||
"fitness": float(answer.fitness),
|
|
||||||
"inlier_rmse_m": float(answer.inlier_rmse),
|
|
||||||
"target_points": len(target_cloud.points),
|
|
||||||
"source_points": len(source_cloud.points),
|
|
||||||
})
|
|
||||||
final_target, final_source, final_answer = target_cloud, source_cloud, answer
|
|
||||||
information = registration.get_information_matrix_from_point_clouds(
|
|
||||||
final_source, final_target, correspondences[-1], transform
|
|
||||||
)
|
|
||||||
inliers = int(round(float(final_answer.fitness) * len(final_source.points)))
|
|
||||||
return {
|
|
||||||
"transform": transform,
|
|
||||||
"hessian": np.asarray(information, float),
|
|
||||||
"converged": None,
|
|
||||||
"iterations": None,
|
|
||||||
"num_inliers": inliers,
|
|
||||||
"objective": float(final_answer.inlier_rmse ** 2 * max(inliers, 1)),
|
|
||||||
"elapsed_sec": time.perf_counter() - started,
|
|
||||||
"stages": stages,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def align_small_gicp(target, source, initial, voxels, correspondences, iterations, threads):
|
|
||||||
import small_gicp
|
|
||||||
transform, stages, result = np.asarray(initial, float), [], None
|
|
||||||
started = time.perf_counter()
|
|
||||||
for voxel, correspondence in zip(voxels, correspondences):
|
|
||||||
result = small_gicp.align(
|
|
||||||
np.ascontiguousarray(target),
|
|
||||||
np.ascontiguousarray(source),
|
|
||||||
transform,
|
|
||||||
registration_type="GICP",
|
|
||||||
downsampling_resolution=voxel,
|
|
||||||
max_correspondence_distance=correspondence,
|
|
||||||
num_threads=threads,
|
|
||||||
max_iterations=iterations,
|
|
||||||
rotation_epsilon=math.radians(0.005),
|
|
||||||
translation_epsilon=0.0005,
|
|
||||||
verbose=False,
|
|
||||||
)
|
|
||||||
transform = np.asarray(result.T_target_source, float)
|
|
||||||
stages.append({
|
|
||||||
"voxel_m": voxel,
|
|
||||||
"max_correspondence_m": correspondence,
|
|
||||||
"converged": bool(result.converged),
|
|
||||||
"iterations": int(result.iterations),
|
|
||||||
"num_inliers": int(result.num_inliers),
|
|
||||||
"objective": float(result.error),
|
|
||||||
})
|
|
||||||
return {
|
|
||||||
"transform": transform,
|
|
||||||
"hessian": np.asarray(result.H, float),
|
|
||||||
"converged": bool(result.converged),
|
|
||||||
"iterations": int(result.iterations),
|
|
||||||
"num_inliers": int(result.num_inliers),
|
|
||||||
"objective": float(result.error),
|
|
||||||
"elapsed_sec": time.perf_counter() - started,
|
|
||||||
"stages": stages,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def align_backend(backend, target, source, initial, args):
|
|
||||||
if backend == "open3d":
|
|
||||||
return align_open3d(
|
|
||||||
target, source, initial, args.voxels, args.correspondences, args.iterations
|
|
||||||
)
|
|
||||||
return align_small_gicp(
|
|
||||||
target, source, initial, args.voxels, args.correspondences,
|
|
||||||
args.iterations, args.threads
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def symmetric_heldout_metrics(target_fit, target_holdout, source_fit, source_holdout,
|
|
||||||
transform, threshold):
|
|
||||||
transformed_source_fit = transform_points(source_fit, transform)
|
|
||||||
transformed_source_holdout = transform_points(source_holdout, transform)
|
|
||||||
forward = cKDTree(target_fit).query(transformed_source_holdout, workers=-1)[0]
|
|
||||||
reverse = cKDTree(transformed_source_fit).query(target_holdout, workers=-1)[0]
|
|
||||||
distances = np.concatenate((forward, reverse))
|
|
||||||
inliers = distances[distances <= threshold]
|
|
||||||
return {
|
|
||||||
"evaluated": int(len(distances)),
|
|
||||||
"inliers": int(len(inliers)),
|
|
||||||
"inlier_ratio": float(len(inliers) / max(len(distances), 1)),
|
|
||||||
"inlier_rmse_m": float(np.sqrt(np.mean(inliers**2))) if len(inliers) else None,
|
|
||||||
"median_m": float(np.median(distances)),
|
|
||||||
"p90_m": float(np.quantile(distances, 0.90)),
|
|
||||||
"p95_m": float(np.quantile(distances, 0.95)),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def hessian_metrics(hessian, characteristic_length=10.0):
|
|
||||||
hessian = 0.5 * (np.asarray(hessian, float) + np.asarray(hessian, float).T)
|
|
||||||
scale = np.diag([1.0 / characteristic_length] * 3 + [1.0] * 3)
|
|
||||||
scaled = scale.T @ hessian @ scale
|
|
||||||
values, vectors = np.linalg.eigh(scaled)
|
|
||||||
largest = max(float(np.max(np.abs(values))), np.finfo(float).eps)
|
|
||||||
positive = values[values > largest * 1e-9]
|
|
||||||
condition = float(positive[-1] / positive[0]) if len(positive) else float("inf")
|
|
||||||
return {
|
|
||||||
"native_order": ["rx_rad", "ry_rad", "rz_rad", "tx_m", "ty_m", "tz_m"],
|
|
||||||
"scaled_eigenvalues": values.tolist(),
|
|
||||||
"effective_rank": int(len(positive)),
|
|
||||||
"scaled_condition_number": condition,
|
|
||||||
"weakest_scaled_direction": vectors[:, int(np.argmin(values))].tolist(),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def transform_difference(reference, candidate):
|
|
||||||
delta = inverse_transform(reference) @ candidate
|
|
||||||
return {
|
|
||||||
"translation_m": float(np.linalg.norm(delta[:3, 3])),
|
|
||||||
"rotation_deg": rotation_angle_deg(delta[:3, :3]),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def loop_metrics(transforms):
|
|
||||||
loops = []
|
|
||||||
for (i, j), b_ij in transforms.items():
|
|
||||||
for (j2, k), b_jk in transforms.items():
|
|
||||||
if j2 != j or (i, k) not in transforms:
|
|
||||||
continue
|
|
||||||
loops.append(transform_difference(transforms[(i, k)], b_ij @ b_jk))
|
|
||||||
if not loops:
|
|
||||||
return {"count": 0}
|
|
||||||
translation = np.array([item["translation_m"] for item in loops])
|
|
||||||
rotation = np.array([item["rotation_deg"] for item in loops])
|
|
||||||
return {
|
|
||||||
"count": len(loops),
|
|
||||||
"translation_rms_m": float(np.sqrt(np.mean(translation**2))),
|
|
||||||
"translation_p95_m": float(np.quantile(translation, 0.95)),
|
|
||||||
"rotation_rms_deg": float(np.sqrt(np.mean(rotation**2))),
|
|
||||||
"rotation_p95_deg": float(np.quantile(rotation, 0.95)),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_ground(args):
|
|
||||||
stations = load_stations(args.frames, args.min_range, args.max_range)
|
|
||||||
rows = []
|
|
||||||
for timestamp, counter, _, xyz in stations:
|
|
||||||
roi = xyz[(xyz[:, 2] >= args.z_min) & (xyz[:, 2] <= args.z_max)]
|
|
||||||
if len(roi) < args.min_inliers:
|
|
||||||
continue
|
|
||||||
cloud = make_o3d_cloud(roi, args.voxel)
|
|
||||||
plane, indexes = cloud.segment_plane(
|
|
||||||
args.distance_threshold, 3, args.ransac_iterations
|
|
||||||
)
|
|
||||||
normal = np.asarray(plane[:3], float)
|
|
||||||
norm = np.linalg.norm(normal)
|
|
||||||
normal, distance = normal / norm, float(plane[3] / norm)
|
|
||||||
if distance < 0:
|
|
||||||
normal, distance = -normal, -distance
|
|
||||||
points = np.asarray(cloud.points)[indexes]
|
|
||||||
rms = float(np.sqrt(np.mean((points @ normal + distance) ** 2)))
|
|
||||||
if len(indexes) >= args.min_inliers and rms <= args.max_rms:
|
|
||||||
rows.append([timestamp, *normal, distance, len(indexes), rms, counter])
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
with output.open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
writer = csv.writer(stream)
|
|
||||||
writer.writerow(["time", "nx", "ny", "nz", "d", "inliers", "rms_m", "frame_counter"])
|
|
||||||
writer.writerows(rows)
|
|
||||||
print(json.dumps({"planes": len(rows), "output": str(output.resolve())}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_pairs(args):
|
|
||||||
if len(args.voxels) != len(args.correspondences):
|
|
||||||
raise ValueError("--voxels and --correspondences must have equal lengths")
|
|
||||||
stations = load_stations(
|
|
||||||
args.frames, args.min_range, args.max_range, args.z_min, args.z_max
|
|
||||||
)
|
|
||||||
reference = read_poses(args.reference_poses)
|
|
||||||
if len(stations) < args.min_stations:
|
|
||||||
raise ValueError(f"need at least {args.min_stations} stations, got {len(stations)}")
|
|
||||||
reference_poses, reference_dt = [], []
|
|
||||||
for timestamp, _, _, xyz in stations:
|
|
||||||
if len(xyz) < args.min_roi_points:
|
|
||||||
raise ValueError(f"station at {timestamp} has only {len(xyz)} ROI points")
|
|
||||||
pose, dt = nearest_pose(reference, timestamp + args.time_offset)
|
|
||||||
reference_poses.append(pose)
|
|
||||||
reference_dt.append(dt)
|
|
||||||
reference_poses = np.asarray(reference_poses)
|
|
||||||
split = [split_holdout(station[3], args.holdout_fraction, i)
|
|
||||||
for i, station in enumerate(stations)]
|
|
||||||
rng = np.random.default_rng(args.seed)
|
|
||||||
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
|
|
||||||
accepted_transforms = {}
|
|
||||||
for i in range(len(stations)):
|
|
||||||
for j in range(i + args.min_gap, min(len(stations), i + args.max_gap + 1)):
|
|
||||||
a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j]
|
|
||||||
translation = float(np.linalg.norm(a_ij[:2, 3]))
|
|
||||||
rotation = rotation_angle_deg(a_ij[:3, :3])
|
|
||||||
if translation < args.min_translation and rotation < args.min_rotation:
|
|
||||||
continue
|
|
||||||
initial_b = a_ij.copy() # X0=I; no measured extrinsic.
|
|
||||||
target_fit, target_holdout = split[i]
|
|
||||||
source_fit, source_holdout = split[j]
|
|
||||||
forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
|
|
||||||
heldout = symmetric_heldout_metrics(
|
|
||||||
target_fit, target_holdout, source_fit, source_holdout,
|
|
||||||
forward["transform"], args.evaluation_distance
|
|
||||||
)
|
|
||||||
hessian = hessian_metrics(forward["hessian"])
|
|
||||||
reverse_answer = align_backend(
|
|
||||||
args.backend, source_fit, target_fit, inverse_transform(initial_b), args
|
|
||||||
)
|
|
||||||
reverse = transform_difference(
|
|
||||||
forward["transform"], inverse_transform(reverse_answer["transform"])
|
|
||||||
)
|
|
||||||
multistart = []
|
|
||||||
for _ in range(args.multistart):
|
|
||||||
perturb = np.r_[
|
|
||||||
rng.normal(0.0, args.multistart_translation_sigma, 3),
|
|
||||||
np.deg2rad(rng.normal(0.0, args.multistart_rotation_sigma, 3)),
|
|
||||||
]
|
|
||||||
candidate = align_backend(
|
|
||||||
args.backend, target_fit, source_fit,
|
|
||||||
params_transform(perturb) @ initial_b, args
|
|
||||||
)
|
|
||||||
multistart.append(transform_difference(forward["transform"], candidate["transform"]))
|
|
||||||
stable = [
|
|
||||||
item["translation_m"] <= args.multistart_translation_tolerance
|
|
||||||
and item["rotation_deg"] <= args.multistart_rotation_tolerance
|
|
||||||
for item in multistart
|
|
||||||
]
|
|
||||||
success_rate = float(np.mean(stable)) if stable else 1.0
|
|
||||||
reasons = []
|
|
||||||
if forward["converged"] is False:
|
|
||||||
reasons.append("backend_not_converged")
|
|
||||||
if heldout["inlier_ratio"] < args.min_inlier_ratio:
|
|
||||||
reasons.append("heldout_inlier_ratio")
|
|
||||||
if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
|
|
||||||
reasons.append("heldout_inlier_rmse")
|
|
||||||
if hessian["effective_rank"] < 6:
|
|
||||||
reasons.append("hessian_rank")
|
|
||||||
if hessian["scaled_condition_number"] > args.max_hessian_condition:
|
|
||||||
reasons.append("hessian_condition")
|
|
||||||
if reverse["translation_m"] > args.reverse_translation_tolerance:
|
|
||||||
reasons.append("forward_reverse_translation")
|
|
||||||
if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
|
|
||||||
reasons.append("forward_reverse_rotation")
|
|
||||||
if success_rate < args.min_multistart_success:
|
|
||||||
reasons.append("multistart_instability")
|
|
||||||
accepted = not reasons
|
|
||||||
report = {
|
|
||||||
"i": i, "j": j,
|
|
||||||
"lidar_time_i": stations[i][0], "lidar_time_j": stations[j][0],
|
|
||||||
"frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1],
|
|
||||||
"rtk_translation_m": translation, "rtk_rotation_deg": rotation,
|
|
||||||
"nearest_rtk_dt_i_s": reference_dt[i], "nearest_rtk_dt_j_s": reference_dt[j],
|
|
||||||
"initial_B_source": "X0=identity; B0=A (no measured extrinsic)",
|
|
||||||
"B_ij_4x4": forward["transform"].tolist(),
|
|
||||||
"backend": args.backend, "backend_converged": forward["converged"],
|
|
||||||
"backend_iterations": forward["iterations"],
|
|
||||||
"backend_num_inliers": forward["num_inliers"],
|
|
||||||
"backend_objective": forward["objective"],
|
|
||||||
"backend_elapsed_sec": forward["elapsed_sec"],
|
|
||||||
"multiscale_stages": forward["stages"],
|
|
||||||
"heldout_symmetric": heldout, "hessian": hessian,
|
|
||||||
"forward_reverse": reverse,
|
|
||||||
"multistart": {"runs": len(multistart), "success_rate": success_rate,
|
|
||||||
"deltas": multistart},
|
|
||||||
"accepted": accepted, "rejection_reasons": reasons,
|
|
||||||
}
|
|
||||||
reports.append(report)
|
|
||||||
print(f"{args.backend} {i:02d}->{j:02d} rmse={heldout['inlier_rmse_m']} "
|
|
||||||
f"ratio={heldout['inlier_ratio']:.3f} accepted={accepted}")
|
|
||||||
if accepted:
|
|
||||||
accepted_a.append(a_ij)
|
|
||||||
accepted_b.append(forward["transform"])
|
|
||||||
accepted_meta.append([i, j, stations[i][0], stations[j][0]])
|
|
||||||
accepted_transforms[(i, j)] = forward["transform"]
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
np.savez_compressed(
|
|
||||||
output, A=np.asarray(accepted_a), B=np.asarray(accepted_b),
|
|
||||||
meta=np.asarray(accepted_meta),
|
|
||||||
station_times=np.asarray([item[0] for item in stations]),
|
|
||||||
rtk_nearest_dt_s=np.asarray(reference_dt), backend=np.asarray(args.backend),
|
|
||||||
)
|
|
||||||
quality = {
|
|
||||||
"schema_version": 2,
|
|
||||||
"backend": args.backend,
|
|
||||||
"transform_convention": "B_ij=T_Li_Lj maps station j points into station i",
|
|
||||||
"raw_point_field": "points_raw",
|
|
||||||
"measured_extrinsic_used_as_initial": False,
|
|
||||||
"stations": len(stations), "candidate_pairs": len(reports),
|
|
||||||
"accepted_pairs": len(accepted_a),
|
|
||||||
"parameters": vars(args),
|
|
||||||
"accepted_loop_closure": loop_metrics(accepted_transforms),
|
|
||||||
"pairs": reports,
|
|
||||||
}
|
|
||||||
quality["parameters"].pop("func", None)
|
|
||||||
quality_path = Path(args.quality_json or output.with_suffix(".quality.json"))
|
|
||||||
quality_path.write_text(json.dumps(quality, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
csv_path = Path(args.quality_csv or output.with_suffix(".quality.csv"))
|
|
||||||
with csv_path.open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
fields = ["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"]
|
|
||||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
|
||||||
writer.writeheader()
|
|
||||||
for item in reports:
|
|
||||||
writer.writerow({
|
|
||||||
"i": item["i"], "j": item["j"],
|
|
||||||
"rtk_translation_m": item["rtk_translation_m"],
|
|
||||||
"rtk_rotation_deg": item["rtk_rotation_deg"],
|
|
||||||
"heldout_inlier_ratio": item["heldout_symmetric"]["inlier_ratio"],
|
|
||||||
"heldout_inlier_rmse_m": item["heldout_symmetric"]["inlier_rmse_m"],
|
|
||||||
"hessian_rank": item["hessian"]["effective_rank"],
|
|
||||||
"hessian_condition": item["hessian"]["scaled_condition_number"],
|
|
||||||
"reverse_translation_m": item["forward_reverse"]["translation_m"],
|
|
||||||
"reverse_rotation_deg": item["forward_reverse"]["rotation_deg"],
|
|
||||||
"multistart_success_rate": item["multistart"]["success_rate"],
|
|
||||||
"accepted": item["accepted"],
|
|
||||||
"rejection_reasons": ";".join(item["rejection_reasons"]),
|
|
||||||
})
|
|
||||||
if len(accepted_a) < args.min_pairs:
|
|
||||||
raise RuntimeError(f"only {len(accepted_a)} accepted pairs; need {args.min_pairs}")
|
|
||||||
print(json.dumps({
|
|
||||||
"backend": args.backend, "stations": len(stations),
|
|
||||||
"candidate_pairs": len(reports), "accepted_pairs": len(accepted_a),
|
|
||||||
"output": str(output.resolve()), "quality_json": str(quality_path.resolve()),
|
|
||||||
"loop": quality["accepted_loop_closure"],
|
|
||||||
}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def read_planes(path):
|
|
||||||
planes = []
|
|
||||||
with Path(path).open(encoding="utf-8-sig", newline="") as stream:
|
|
||||||
for row in csv.DictReader(stream):
|
|
||||||
normal = np.array([float(row[k]) for k in ("nx", "ny", "nz")])
|
|
||||||
norm = np.linalg.norm(normal)
|
|
||||||
normal, distance = normal / norm, float(row["d"]) / norm
|
|
||||||
if distance < 0:
|
|
||||||
normal, distance = -normal, -distance
|
|
||||||
planes.append([*normal, distance])
|
|
||||||
return np.asarray(planes)
|
|
||||||
|
|
||||||
|
|
||||||
def read_pairs(path):
|
|
||||||
with np.load(path, allow_pickle=False) as data:
|
|
||||||
return (np.asarray(data["A"], float), np.asarray(data["B"], float),
|
|
||||||
np.asarray(data["meta"], float), len(data["station_times"]))
|
|
||||||
|
|
||||||
|
|
||||||
def calibration_residual(params, a_array, b_array, planes, args):
|
|
||||||
x = params_transform(params)
|
|
||||||
values = []
|
|
||||||
for a_ij, b_ij in zip(a_array, b_array):
|
|
||||||
error = inverse_transform(a_ij @ x) @ x @ b_ij
|
|
||||||
values.extend((error[:3, 3] / args.translation_sigma).tolist())
|
|
||||||
values.extend((so3_log(error[:3, :3]) / math.radians(args.rotation_sigma)).tolist())
|
|
||||||
body_up = np.array([0.0, 0.0, 1.0])
|
|
||||||
for plane in planes:
|
|
||||||
normal_body = x[:3, :3] @ plane[:3]
|
|
||||||
values.extend((np.cross(normal_body, body_up) / args.plane_normal_sigma).tolist())
|
|
||||||
body_distance = plane[3] - float(normal_body @ x[:3, 3])
|
|
||||||
values.append((body_distance - args.reference_height) / args.plane_height_sigma)
|
|
||||||
return np.asarray(values)
|
|
||||||
|
|
||||||
|
|
||||||
def pair_metrics(a_array, b_array, x):
|
|
||||||
translation, rotation, rows = [], [], []
|
|
||||||
for index, (a_ij, b_ij) in enumerate(zip(a_array, b_array)):
|
|
||||||
predicted = inverse_transform(x) @ a_ij @ x
|
|
||||||
delta = inverse_transform(b_ij) @ predicted
|
|
||||||
t = float(np.linalg.norm(delta[:3, 3]))
|
|
||||||
r = rotation_angle_deg(delta[:3, :3])
|
|
||||||
translation.append(t); rotation.append(r)
|
|
||||||
rows.append({"pair_index": index, "translation_m": t, "rotation_deg": r})
|
|
||||||
translation, rotation = np.asarray(translation), np.asarray(rotation)
|
|
||||||
def stats(values):
|
|
||||||
return {
|
|
||||||
"rms": float(np.sqrt(np.mean(values**2))),
|
|
||||||
"median": float(np.median(values)),
|
|
||||||
"p90": float(np.quantile(values, 0.90)),
|
|
||||||
"p95": float(np.quantile(values, 0.95)),
|
|
||||||
"max": float(np.max(values)),
|
|
||||||
}
|
|
||||||
return {"pairs": len(rows), "translation_m": stats(translation),
|
|
||||||
"rotation_deg": stats(rotation), "per_pair": rows}
|
|
||||||
|
|
||||||
|
|
||||||
def solve_extrinsic(a_array, b_array, planes, args):
|
|
||||||
rng = np.random.default_rng(args.seed)
|
|
||||||
starts = [np.zeros(6)]
|
|
||||||
for _ in range(args.solver_multistart - 1):
|
|
||||||
starts.append(np.r_[
|
|
||||||
rng.normal(0.0, args.start_translation_sigma, 3),
|
|
||||||
np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
|
|
||||||
])
|
|
||||||
candidates = []
|
|
||||||
lower = np.r_[[-5.0] * 3, [-math.pi] * 3]
|
|
||||||
upper = np.r_[[5.0] * 3, [math.pi] * 3]
|
|
||||||
for start in starts:
|
|
||||||
answer = least_squares(
|
|
||||||
calibration_residual, np.clip(start, lower, upper),
|
|
||||||
args=(a_array, b_array, planes, args),
|
|
||||||
bounds=(lower, upper), loss="huber", f_scale=1.5,
|
|
||||||
x_scale="jac", max_nfev=args.max_nfev,
|
|
||||||
)
|
|
||||||
candidates.append(answer)
|
|
||||||
best = min(candidates, key=lambda item: item.cost)
|
|
||||||
return best, candidates
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_calibrate(args):
|
|
||||||
a_array, b_array, meta, stations = read_pairs(args.pairs)
|
|
||||||
planes = read_planes(args.ground_planes)
|
|
||||||
best, candidates = solve_extrinsic(a_array, b_array, planes, args)
|
|
||||||
x = params_transform(best.x)
|
|
||||||
residual = calibration_residual(best.x, a_array, b_array, planes, args)
|
|
||||||
absolute = np.abs(residual)
|
|
||||||
weights = np.ones_like(residual)
|
|
||||||
weights[absolute > 1.5] = 1.5 / absolute[absolute > 1.5]
|
|
||||||
weighted_jacobian = best.jac * np.sqrt(weights)[:, None]
|
|
||||||
singular = np.linalg.svd(weighted_jacobian, compute_uv=False)
|
|
||||||
condition = float(singular[0] / max(singular[-1], 1e-15))
|
|
||||||
dof = max(len(residual) - 6, 1)
|
|
||||||
covariance = np.linalg.pinv(weighted_jacobian.T @ weighted_jacobian) * float(
|
|
||||||
np.sum(weights * residual**2) / dof
|
|
||||||
)
|
|
||||||
sigma = np.sqrt(np.maximum(np.diag(covariance), 0.0))
|
|
||||||
candidate_summary = []
|
|
||||||
for item in candidates:
|
|
||||||
candidate_x = params_transform(item.x)
|
|
||||||
candidate_summary.append({
|
|
||||||
"cost": float(item.cost), "success": bool(item.success),
|
|
||||||
**transform_difference(x, candidate_x),
|
|
||||||
})
|
|
||||||
bootstrap = []
|
|
||||||
rng = np.random.default_rng(args.seed + 1)
|
|
||||||
for _ in range(args.bootstrap):
|
|
||||||
indexes = rng.integers(0, len(a_array), len(a_array))
|
|
||||||
answer = least_squares(
|
|
||||||
calibration_residual, best.x,
|
|
||||||
args=(a_array[indexes], b_array[indexes], planes, args),
|
|
||||||
loss="huber", f_scale=1.5, x_scale="jac", max_nfev=args.max_nfev,
|
|
||||||
)
|
|
||||||
bootstrap.append(np.r_[answer.x[:3], rpy_deg(so3_exp(answer.x[3:]))])
|
|
||||||
bootstrap = np.asarray(bootstrap)
|
|
||||||
result = {
|
|
||||||
"schema_version": 2,
|
|
||||||
"success": bool(best.success),
|
|
||||||
"message": best.message,
|
|
||||||
"convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame",
|
|
||||||
"equation": "A_ij X = X B_ij",
|
|
||||||
"measured_extrinsic_used_as_initial": False,
|
|
||||||
"translation_m": x[:3, 3].tolist(),
|
|
||||||
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
|
|
||||||
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
|
|
||||||
"matrix_4x4": x.tolist(),
|
|
||||||
"estimation": {"stations": stations, "pairs": len(a_array),
|
|
||||||
"residuals": pair_metrics(a_array, b_array, x)},
|
|
||||||
"ground": {
|
|
||||||
"planes": len(planes),
|
|
||||||
"reference_origin_height_above_ground_m": args.reference_height,
|
|
||||||
"formula": "d_lidar - (R_X n_lidar)^T t_X - reference_height",
|
|
||||||
},
|
|
||||||
"linearized_one_sigma": {
|
|
||||||
"translation_m": sigma[:3].tolist(),
|
|
||||||
"rotation_deg": np.rad2deg(sigma[3:]).tolist(),
|
|
||||||
"warning": "conditional local estimate; bootstrap is the primary stability check",
|
|
||||||
},
|
|
||||||
"weighted_jacobian_condition_number": condition,
|
|
||||||
"solver_multistart": {
|
|
||||||
"runs": len(candidates), "candidates_relative_to_best": candidate_summary,
|
|
||||||
},
|
|
||||||
"bootstrap": {
|
|
||||||
"runs": len(bootstrap),
|
|
||||||
"order": ["x_m", "y_m", "z_m", "roll_deg", "pitch_deg", "yaw_deg"],
|
|
||||||
"std": np.std(bootstrap, axis=0, ddof=1).tolist() if len(bootstrap) > 1 else None,
|
|
||||||
"p025": np.quantile(bootstrap, 0.025, axis=0).tolist() if len(bootstrap) else None,
|
|
||||||
"p975": np.quantile(bootstrap, 0.975, axis=0).tolist() if len(bootstrap) else None,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
print(json.dumps(result, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_validate(args):
|
|
||||||
result = json.loads(Path(args.extrinsic).read_text(encoding="utf-8-sig"))
|
|
||||||
x = np.asarray(result["matrix_4x4"], float)
|
|
||||||
a_array, b_array, meta, stations = read_pairs(args.pairs)
|
|
||||||
metrics = pair_metrics(a_array, b_array, x)
|
|
||||||
for row, pair_meta in zip(metrics["per_pair"], meta):
|
|
||||||
row.update({"i": int(pair_meta[0]), "j": int(pair_meta[1])})
|
|
||||||
report = {
|
|
||||||
"role": "auxiliary check only; first-batch RTK is sparse",
|
|
||||||
"blind_with_respect_to_X": True,
|
|
||||||
"note": "No AX residual was used to select these pairs",
|
|
||||||
"stations": stations, "metrics": metrics,
|
|
||||||
}
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
output.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
print(json.dumps(report, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def build_parser():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
commands = parser.add_subparsers(dest="command", required=True)
|
|
||||||
ground = commands.add_parser("ground")
|
|
||||||
ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True)
|
|
||||||
ground.add_argument("--min-range", type=float, default=1.0); ground.add_argument("--max-range", type=float, default=30.0)
|
|
||||||
ground.add_argument("--z-min", type=float, default=-1.4); ground.add_argument("--z-max", type=float, default=-0.4)
|
|
||||||
ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025)
|
|
||||||
ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500)
|
|
||||||
ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground)
|
|
||||||
|
|
||||||
pairs = commands.add_parser("pairs")
|
|
||||||
pairs.add_argument("--backend", choices=["open3d", "small_gicp"], required=True)
|
|
||||||
pairs.add_argument("--frames", required=True)
|
|
||||||
pairs.add_argument("--reference-poses", "--body", dest="reference_poses", required=True)
|
|
||||||
pairs.add_argument("--output", required=True); pairs.add_argument("--quality-json"); pairs.add_argument("--quality-csv")
|
|
||||||
pairs.add_argument("--time-offset", type=float, default=0.0)
|
|
||||||
pairs.add_argument("--min-stations", type=int, default=30); pairs.add_argument("--min-pairs", type=int, default=25)
|
|
||||||
pairs.add_argument("--min-gap", type=int, default=1); pairs.add_argument("--max-gap", type=int, default=5)
|
|
||||||
pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0)
|
|
||||||
pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0)
|
|
||||||
pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0)
|
|
||||||
pairs.add_argument("--min-roi-points", type=int, default=1000)
|
|
||||||
pairs.add_argument("--holdout-fraction", type=float, default=0.20)
|
|
||||||
pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08])
|
|
||||||
pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25])
|
|
||||||
pairs.add_argument("--iterations", type=int, default=60); pairs.add_argument("--threads", type=int, default=8)
|
|
||||||
pairs.add_argument("--evaluation-distance", type=float, default=0.25)
|
|
||||||
pairs.add_argument("--min-inlier-ratio", type=float, default=0.35); pairs.add_argument("--max-inlier-rmse", type=float, default=0.16)
|
|
||||||
pairs.add_argument("--max-hessian-condition", type=float, default=1e8)
|
|
||||||
pairs.add_argument("--reverse-translation-tolerance", type=float, default=0.08)
|
|
||||||
pairs.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
|
|
||||||
pairs.add_argument("--multistart", type=int, default=2)
|
|
||||||
pairs.add_argument("--multistart-translation-sigma", type=float, default=0.30)
|
|
||||||
pairs.add_argument("--multistart-rotation-sigma", type=float, default=3.0)
|
|
||||||
pairs.add_argument("--multistart-translation-tolerance", type=float, default=0.08)
|
|
||||||
pairs.add_argument("--multistart-rotation-tolerance", type=float, default=0.50)
|
|
||||||
pairs.add_argument("--min-multistart-success", type=float, default=0.50)
|
|
||||||
pairs.add_argument("--seed", type=int, default=20260721); pairs.set_defaults(func=cmd_pairs)
|
|
||||||
|
|
||||||
calibrate = commands.add_parser("calibrate")
|
|
||||||
calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True)
|
|
||||||
calibrate.add_argument("--output", required=True)
|
|
||||||
calibrate.add_argument("--translation-sigma", type=float, default=0.05)
|
|
||||||
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
|
|
||||||
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
|
|
||||||
calibrate.add_argument("--plane-height-sigma", type=float, default=0.03)
|
|
||||||
calibrate.add_argument(
|
|
||||||
"--reference-height", "--body-height", dest="reference_height",
|
|
||||||
type=float, required=True,
|
|
||||||
help="measured RTK/GGA reference-origin height above the local ground in metres",
|
|
||||||
)
|
|
||||||
calibrate.add_argument("--solver-multistart", type=int, default=12)
|
|
||||||
calibrate.add_argument("--start-translation-sigma", type=float, default=1.0)
|
|
||||||
calibrate.add_argument("--start-rotation-sigma", type=float, default=20.0)
|
|
||||||
calibrate.add_argument("--bootstrap", type=int, default=100)
|
|
||||||
calibrate.add_argument("--max-nfev", type=int, default=1000)
|
|
||||||
calibrate.add_argument("--seed", type=int, default=20260721); calibrate.set_defaults(func=cmd_calibrate)
|
|
||||||
|
|
||||||
validate = commands.add_parser("validate")
|
|
||||||
validate.add_argument("--pairs", required=True); validate.add_argument("--extrinsic", required=True)
|
|
||||||
validate.add_argument("--output", required=True); validate.set_defaults(func=cmd_validate)
|
|
||||||
return parser
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
args = build_parser().parse_args()
|
|
||||||
args.func(args)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,294 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion.
|
|
||||||
|
|
||||||
Modes (keyboard), aligned with the LiDAR–IMU viewer:
|
|
||||||
1 raw source (no transform)
|
|
||||||
2 RTK prediction with X=I (B_pred = A)
|
|
||||||
3 LiDAR registration B (reference)
|
|
||||||
4 calibrated prediction B_pred = X^{-1} A X
|
|
||||||
5 optional body-left RPY test (only if --left-rpy-deg is non-zero)
|
|
||||||
N / ] next motion pair
|
|
||||||
P / [ previous motion pair
|
|
||||||
Q / Esc exit
|
|
||||||
|
|
||||||
Blue = target station i; orange = source station j after the selected transform.
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from rigorous_calibration import (
|
|
||||||
inverse_transform,
|
|
||||||
load_stations,
|
|
||||||
rotation_angle_deg,
|
|
||||||
rpy_deg,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
COLORS = {
|
|
||||||
"target": [0.10, 0.65, 1.00],
|
|
||||||
"source": [1.00, 0.35, 0.05],
|
|
||||||
}
|
|
||||||
|
|
||||||
MODE_NAMES = (
|
|
||||||
"1 raw",
|
|
||||||
"2 RTK initial (X=I)",
|
|
||||||
"3 GICP B",
|
|
||||||
"4 calibrated X^-1 A X",
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def cloud(o3d, points, color, voxel):
|
|
||||||
item = o3d.geometry.PointCloud()
|
|
||||||
item.points = o3d.utility.Vector3dVector(points)
|
|
||||||
if voxel > 0:
|
|
||||||
item = item.voxel_down_sample(voxel)
|
|
||||||
item.paint_uniform_color(color)
|
|
||||||
return item
|
|
||||||
|
|
||||||
|
|
||||||
def set_cloud_points(cloud_geom, points, color, voxel, o3d) -> None:
|
|
||||||
tmp = cloud(o3d, points, color, voxel)
|
|
||||||
cloud_geom.points = tmp.points
|
|
||||||
cloud_geom.colors = tmp.colors
|
|
||||||
|
|
||||||
|
|
||||||
def delta_components(reference, candidate):
|
|
||||||
"""Components of reference^-1*candidate, plus coordinate-invariant norms."""
|
|
||||||
delta = inverse_transform(reference) @ candidate
|
|
||||||
translation = np.asarray(delta[:3, 3], float)
|
|
||||||
return {
|
|
||||||
"translation_xyz_cm": (translation * 100.0).tolist(),
|
|
||||||
"translation_norm_cm": float(np.linalg.norm(translation) * 100.0),
|
|
||||||
"rotation_rpy_deg_xyz": rpy_deg(delta[:3, :3]),
|
|
||||||
"rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def body_left_rpy(x, rpy_correction_deg):
|
|
||||||
correction = np.eye(4)
|
|
||||||
correction[:3, :3] = Rotation.from_euler(
|
|
||||||
"xyz", np.asarray(rpy_correction_deg, float), degrees=True
|
|
||||||
).as_matrix()
|
|
||||||
return correction @ x
|
|
||||||
|
|
||||||
|
|
||||||
def print_delta(name, reference, candidate):
|
|
||||||
item = delta_components(reference, candidate)
|
|
||||||
tx, ty, tz = item["translation_xyz_cm"]
|
|
||||||
roll, pitch, yaw = item["rotation_rpy_deg_xyz"]
|
|
||||||
print(
|
|
||||||
f"{name}: B^-1*motion "
|
|
||||||
f"t_xyz=[{tx:+.3f}, {ty:+.3f}, {tz:+.3f}] cm "
|
|
||||||
f"rpy=[{roll:+.3f}, {pitch:+.3f}, {yaw:+.3f}] deg "
|
|
||||||
f"|t|={item['translation_norm_cm']:.3f} cm "
|
|
||||||
f"|R|={item['rotation_angle_deg']:.4f} deg"
|
|
||||||
)
|
|
||||||
return item
|
|
||||||
|
|
||||||
|
|
||||||
def transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg):
|
|
||||||
b_calibrated = inverse_transform(x) @ a_ij @ x
|
|
||||||
transforms = {
|
|
||||||
MODE_NAMES[0]: np.eye(4),
|
|
||||||
MODE_NAMES[1]: a_ij.copy(),
|
|
||||||
MODE_NAMES[2]: b_gicp.copy(),
|
|
||||||
MODE_NAMES[3]: b_calibrated,
|
|
||||||
}
|
|
||||||
correction = np.asarray(left_rpy_deg, float)
|
|
||||||
test_name = None
|
|
||||||
if np.any(np.abs(correction) > 0.0):
|
|
||||||
x_test = body_left_rpy(x, correction)
|
|
||||||
test_name = f"5 test body-left RPY {correction.tolist()} deg"
|
|
||||||
transforms[test_name] = inverse_transform(x_test) @ a_ij @ x_test
|
|
||||||
return transforms, test_name
|
|
||||||
|
|
||||||
|
|
||||||
def resolve_pair(stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy_deg):
|
|
||||||
a_ij = np.asarray(pairs_a[pair_index], float)
|
|
||||||
b_gicp = np.asarray(pairs_b[pair_index], float)
|
|
||||||
i, j = np.asarray(pairs_meta[pair_index, :2], int)
|
|
||||||
transforms, test_name = transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg)
|
|
||||||
label = (
|
|
||||||
f"pair {pair_index + 1}/{len(pairs_a)} "
|
|
||||||
f"station {i} <- {j} "
|
|
||||||
f"rotB={rotation_angle_deg(b_gicp[:3, :3]):.2f} deg "
|
|
||||||
f"|tB|={float(np.linalg.norm(b_gicp[:3, 3])):.3f} m"
|
|
||||||
)
|
|
||||||
return i, j, a_ij, b_gicp, transforms, test_name, label
|
|
||||||
|
|
||||||
|
|
||||||
def print_pair_header(label, b_gicp, transforms, test_name, a_ij):
|
|
||||||
print("-" * 72)
|
|
||||||
print(label)
|
|
||||||
print("blue=target i | orange=source j")
|
|
||||||
mode_hint = "1-4"
|
|
||||||
if test_name is not None:
|
|
||||||
mode_hint = "1-5"
|
|
||||||
print(f"{mode_hint}: overlay mode | N/]: next pair | P/[: prev pair | Q/Esc: exit")
|
|
||||||
print(
|
|
||||||
"IMPORTANT: delta xyz/rpy are components of B^-1*(X^-1*A*X), expressed "
|
|
||||||
"in station-j LiDAR coordinates; screen-left/right depends on the 3D camera view."
|
|
||||||
)
|
|
||||||
baseline = print_delta("mode4 minus mode3", b_gicp, transforms[MODE_NAMES[3]])
|
|
||||||
roll, pitch, yaw = np.abs(baseline["rotation_rpy_deg_xyz"])
|
|
||||||
if max(roll, pitch) > max(0.10, 2.0 * yaw):
|
|
||||||
print("note: roll/pitch dominate yaw on this pair.")
|
|
||||||
tx, ty, tz = np.abs(baseline["translation_xyz_cm"])
|
|
||||||
if tz > max(tx, ty):
|
|
||||||
print("note: largest translation component is Z for this pair.")
|
|
||||||
body_up = np.array([0.0, 0.0, 1.0])
|
|
||||||
if np.linalg.norm(a_ij[:3, :3] @ body_up - body_up) < 1e-8:
|
|
||||||
print(
|
|
||||||
"observability: this A preserves the body Z axis, so body-left X.z "
|
|
||||||
"translation is unobservable from this pair; use ground/external height constraints."
|
|
||||||
)
|
|
||||||
if test_name is not None:
|
|
||||||
print_delta("mode5 minus mode3", b_gicp, transforms[test_name])
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
import open3d as o3d
|
|
||||||
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--frames", required=True)
|
|
||||||
parser.add_argument("--pairs", required=True)
|
|
||||||
parser.add_argument("--extrinsic", required=True)
|
|
||||||
parser.add_argument("--pair-index", type=int, default=0, help="Starting motion-pair index")
|
|
||||||
parser.add_argument("--voxel", type=float, default=0.10)
|
|
||||||
parser.add_argument(
|
|
||||||
"--left-rpy-deg",
|
|
||||||
nargs=3,
|
|
||||||
type=float,
|
|
||||||
default=[0.0, 0.0, 0.0],
|
|
||||||
metavar=("ROLL", "PITCH", "YAW"),
|
|
||||||
help="optional body-frame left correction applied as DeltaR_body * X",
|
|
||||||
)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
stations = load_stations(args.frames, 1.0, 60.0)
|
|
||||||
with np.load(args.pairs, allow_pickle=False) as data:
|
|
||||||
if len(stations) != len(data["station_times"]):
|
|
||||||
raise ValueError(
|
|
||||||
f"frames contain {len(stations)} stations but pair file records "
|
|
||||||
f"{len(data['station_times'])}"
|
|
||||||
)
|
|
||||||
pairs_a = np.asarray(data["A"], float)
|
|
||||||
pairs_b = np.asarray(data["B"], float)
|
|
||||||
pairs_meta = np.asarray(data["meta"])
|
|
||||||
n_pairs = len(pairs_a)
|
|
||||||
if not 0 <= args.pair_index < n_pairs:
|
|
||||||
raise IndexError(f"pair-index {args.pair_index} outside [0,{n_pairs - 1}]")
|
|
||||||
|
|
||||||
with open(args.extrinsic, encoding="utf-8-sig") as stream:
|
|
||||||
result = json.load(stream)
|
|
||||||
x = np.asarray(result["matrix_4x4"], float)
|
|
||||||
left_rpy = np.asarray(args.left_rpy_deg, float)
|
|
||||||
|
|
||||||
pair_index = int(args.pair_index)
|
|
||||||
i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair(
|
|
||||||
stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy
|
|
||||||
)
|
|
||||||
|
|
||||||
viewer = o3d.visualization.VisualizerWithKeyCallback()
|
|
||||||
viewer.create_window("RTK–LiDAR registration inspection", 1400, 900)
|
|
||||||
target_cloud = cloud(o3d, stations[i][3], COLORS["target"], args.voxel)
|
|
||||||
source_cloud = cloud(o3d, stations[j][3], COLORS["source"], args.voxel)
|
|
||||||
viewer.add_geometry(target_cloud)
|
|
||||||
viewer.add_geometry(source_cloud)
|
|
||||||
viewer.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0))
|
|
||||||
viewer.get_render_option().background_color = np.array([0.02, 0.02, 0.02])
|
|
||||||
viewer.get_render_option().point_size = 2.0
|
|
||||||
|
|
||||||
state = {
|
|
||||||
"pair_index": pair_index,
|
|
||||||
"mode_name": MODE_NAMES[3],
|
|
||||||
"current": np.eye(4),
|
|
||||||
"transforms": transforms,
|
|
||||||
"b_gicp": b_gicp,
|
|
||||||
"a_ij": a_ij,
|
|
||||||
"test_name": test_name,
|
|
||||||
}
|
|
||||||
|
|
||||||
def apply_mode(vis, mode_name: str, *, announce: bool = True) -> None:
|
|
||||||
desired = state["transforms"][mode_name]
|
|
||||||
source_cloud.transform(desired @ inverse_transform(state["current"]))
|
|
||||||
state["current"] = desired
|
|
||||||
state["mode_name"] = mode_name
|
|
||||||
vis.update_geometry(source_cloud)
|
|
||||||
if announce:
|
|
||||||
if mode_name == MODE_NAMES[2]:
|
|
||||||
print(f"{mode_name}: registration reference; delta = 0")
|
|
||||||
else:
|
|
||||||
print_delta(mode_name + " minus mode3", state["b_gicp"], desired)
|
|
||||||
|
|
||||||
def load_pair(vis, new_index: int) -> None:
|
|
||||||
new_index = int(new_index) % n_pairs
|
|
||||||
i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair(
|
|
||||||
stations, pairs_a, pairs_b, pairs_meta, new_index, x, left_rpy
|
|
||||||
)
|
|
||||||
state["pair_index"] = new_index
|
|
||||||
state["transforms"] = transforms
|
|
||||||
state["b_gicp"] = b_gicp
|
|
||||||
state["a_ij"] = a_ij
|
|
||||||
state["test_name"] = test_name
|
|
||||||
state["current"] = np.eye(4)
|
|
||||||
set_cloud_points(target_cloud, stations[i][3], COLORS["target"], args.voxel, o3d)
|
|
||||||
set_cloud_points(source_cloud, stations[j][3], COLORS["source"], args.voxel, o3d)
|
|
||||||
vis.update_geometry(target_cloud)
|
|
||||||
vis.update_geometry(source_cloud)
|
|
||||||
# Keep current mode if still available (mode 5 may vanish when correction is zero).
|
|
||||||
mode_name = state["mode_name"]
|
|
||||||
if mode_name not in transforms:
|
|
||||||
mode_name = MODE_NAMES[3]
|
|
||||||
print_pair_header(label, b_gicp, transforms, test_name, a_ij)
|
|
||||||
apply_mode(vis, mode_name, announce=True)
|
|
||||||
|
|
||||||
def make_mode_cb(mode_name: str):
|
|
||||||
def callback(vis):
|
|
||||||
if mode_name not in state["transforms"]:
|
|
||||||
print(f"{mode_name}: unavailable (pass non-zero --left-rpy-deg for mode 5)")
|
|
||||||
return False
|
|
||||||
apply_mode(vis, mode_name, announce=True)
|
|
||||||
return False
|
|
||||||
|
|
||||||
return callback
|
|
||||||
|
|
||||||
def next_pair(vis):
|
|
||||||
load_pair(vis, state["pair_index"] + 1)
|
|
||||||
return False
|
|
||||||
|
|
||||||
def prev_pair(vis):
|
|
||||||
load_pair(vis, state["pair_index"] - 1)
|
|
||||||
return False
|
|
||||||
|
|
||||||
print_pair_header(label, b_gicp, transforms, test_name, a_ij)
|
|
||||||
for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4")), MODE_NAMES):
|
|
||||||
viewer.register_key_callback(key, make_mode_cb(name))
|
|
||||||
|
|
||||||
def mode5(vis):
|
|
||||||
name = state["test_name"]
|
|
||||||
if name is None or name not in state["transforms"]:
|
|
||||||
print("5: unavailable (pass non-zero --left-rpy-deg for mode 5)")
|
|
||||||
return False
|
|
||||||
apply_mode(vis, name, announce=True)
|
|
||||||
return False
|
|
||||||
|
|
||||||
viewer.register_key_callback(ord("5"), mode5)
|
|
||||||
for key in (ord("N"), ord("n"), ord("]")):
|
|
||||||
viewer.register_key_callback(key, next_pair)
|
|
||||||
for key in (ord("P"), ord("p"), ord("[")):
|
|
||||||
viewer.register_key_callback(key, prev_pair)
|
|
||||||
|
|
||||||
apply_mode(viewer, MODE_NAMES[3], announce=False)
|
|
||||||
viewer.run()
|
|
||||||
viewer.destroy_window()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -0,0 +1,51 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "S2_old_validation"
|
||||||
|
body_frame:
|
||||||
|
name: "rear_axle_center"
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "S2_old_smoke"
|
||||||
|
installed_at: "unknown"
|
||||||
|
notes: "Smoke-test on old S2 host-time data only. Not for delivery."
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "HI13_old_S2"
|
||||||
|
raw_frame:
|
||||||
|
axes: "as exported HI91"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "frontlidar"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "Cartesian metres from points_raw spherical conversion"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "host_utc_receive_of_serial_chunk"
|
||||||
|
lidar_timestamp_source: "unix_time_ns_from_dlog_export"
|
||||||
|
lidar_frame_time_definition: "frame midpoint approx from unix_time_ns"
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_m: null
|
||||||
|
rotation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_deg: null
|
||||||
@@ -0,0 +1,86 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "outdoor_usable_20260808"
|
||||||
|
body_frame:
|
||||||
|
name: "base_link"
|
||||||
|
# 车体约定：后轮轴中心在地面投影为原点附近参考；X 前 / Y 左 / Z 上
|
||||||
|
# translation_m 的 Z 使用「离地高度」；后轮轴中心离地 294 mm
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
reference_point: "rear_axle_center_xy__z_above_ground"
|
||||||
|
rear_axle_height_above_ground_m: 0.294
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "20260808_priority_windows"
|
||||||
|
installed_at: "2026-08-08"
|
||||||
|
notes: >
|
||||||
|
HI13R4 + H32 DLogCapture. Body +X forward: LiDAR and IMU at positive X.
|
||||||
|
CAD sheet may draw +X rearward; numbers below are body-frame.
|
||||||
|
Z is height above ground = CAD height at axle + 0.294 m (axle AGL).
|
||||||
|
LiDAR CAD dZ is 1637.499879 mm relative to the axle reference. Phase-center
|
||||||
|
AGL adds rear-axle height 294 mm and the 63.5 mm phase-center offset.
|
||||||
|
IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up).
|
||||||
|
LiDAR Cartesian in NPZ assumed body-aligned (X forward).
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "HI13R4"
|
||||||
|
raw_frame:
|
||||||
|
# HI13R4 用户手册 2.4：右-前-上 (RFU)
|
||||||
|
axes: "X right, Y forward, Z up (RFU)"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
# X/Y：后轮轴中心 → IMU；Z：离地 = CAD 0.8925 + 0.294
|
||||||
|
translation_m: [2.574126255, 0.0365, 1.1865]
|
||||||
|
# body <- imu : p_body = R_body_imu * p_imu
|
||||||
|
# R_body_imu = [[0,1,0],[-1,0,0],[0,0,1]] (fwd=imu_y, left=-imu_x, up=imu_z)
|
||||||
|
rotation_matrix_body_imu: [[0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
source: "CAD X/Y in body (+X forward); Z = CAD axle-height + 294mm AGL + HI13R4 RFU"
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "RSLidarH32"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
# X/Y：后轮轴中心 → 雷达
|
||||||
|
# Z离地 = CAD dZ 1.637499879 + 后轮轴离地 0.294 + 相位中心偏移 0.0635
|
||||||
|
translation_m: [2.522276859, 0.000020526, 1.994999879]
|
||||||
|
rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
source: "CAD X/Y in body (+X forward); Z AGL = CAD dZ 1.637499879 + axle AGL 0.294 + phase-center offset 0.0635; attitude = body"
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "hi13_device_timestamp_ms_seconds"
|
||||||
|
lidar_timestamp_source: "h32_msop_device_timestamp_seconds"
|
||||||
|
lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint"
|
||||||
|
host_bridge: "MSOP HostReceiveUtcTicks + IMU receive_utc_ticks"
|
||||||
|
|
||||||
|
# Derived prior for p_IMU = R_IMU_lidar * p_lidar + t_IMU_lidar
|
||||||
|
# t_body = t_lidar_body - t_imu_body
|
||||||
|
# t_IMU_lidar = R_IMU_body * t_body, R_IMU_lidar = R_IMU_body * R_body_lidar
|
||||||
|
derived_T_IMU_lidar_prior:
|
||||||
|
R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||||
|
t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
|
||||||
|
notes: >
|
||||||
|
Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd).
|
||||||
|
Relative Z = 1.994999879 - 1.1865 = 0.808499879 m.
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: true
|
||||||
|
sigma_m: [0.05, 0.05, 0.05]
|
||||||
|
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||||
|
rotation_prior:
|
||||||
|
enabled: true
|
||||||
|
sigma_deg: 15.0
|
||||||
|
R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
@@ -0,0 +1,51 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "example_vehicle"
|
||||||
|
body_frame:
|
||||||
|
name: "base_link"
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "example_install"
|
||||||
|
installed_at: "unknown"
|
||||||
|
notes: "V1 example config. Mount translations may stay null."
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "unknown_imu"
|
||||||
|
raw_frame:
|
||||||
|
axes: "declare after black-box tests: e.g. out_x=forward, out_y=left, out_z=up"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "unknown_lidar"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "device_or_file_clock_seconds"
|
||||||
|
lidar_timestamp_source: "frame_midpoint_seconds"
|
||||||
|
lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint"
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_m: null
|
||||||
|
rotation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_deg: null
|
||||||
@@ -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 参考点离地高度及其测量方法。
|
|
||||||
@@ -0,0 +1,209 @@
|
|||||||
|
# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题
|
||||||
|
|
||||||
|
> 数据：`D:\data\calibration_usable_20260808`
|
||||||
|
> 可用会话：`sessions_v1_host_aligned`（三优先窗）
|
||||||
|
> 当前结果目录：各窗 `out_fixed_dt0/`
|
||||||
|
> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`
|
||||||
|
> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml`
|
||||||
|
> 约定外参：`p_IMU = T_IMU_lidar · p_lidar`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 一句话结论
|
||||||
|
|
||||||
|
**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。**
|
||||||
|
三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 当前可用结果（`out_fixed_dt0`）
|
||||||
|
|
||||||
|
约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。
|
||||||
|
|
||||||
|
| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
|
||||||
|
|----|------|----|---------------------|--------------|----------|--------------|
|
||||||
|
| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
|
||||||
|
| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° |
|
||||||
|
| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° |
|
||||||
|
|
||||||
|
- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。
|
||||||
|
- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。
|
||||||
|
- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。
|
||||||
|
|
||||||
|
### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
|
||||||
|
- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
|
||||||
|
[ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ],
|
||||||
|
[-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
|
||||||
|
- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
|
||||||
|
[ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ],
|
||||||
|
[ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
|
||||||
|
- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
|
||||||
|
[ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
|
||||||
|
[ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 相对历史失败轮次
|
||||||
|
|
||||||
|
| 轮次 | 问题 | 结果 |
|
||||||
|
|------|------|------|
|
||||||
|
| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° |
|
||||||
|
| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） |
|
||||||
|
| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 已澄清并写入配置的坐标系 / 先验
|
||||||
|
|
||||||
|
### 3.1 车体与传感器
|
||||||
|
|
||||||
|
- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。
|
||||||
|
- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。
|
||||||
|
- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。
|
||||||
|
- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。
|
||||||
|
|
||||||
|
### 3.2 安装量（`translation_m`）
|
||||||
|
|
||||||
|
| 传感器 | X / Y（后轮轴中心） | Z（离地） |
|
||||||
|
|--------|---------------------|-----------|
|
||||||
|
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
|
||||||
|
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
|
||||||
|
|
||||||
|
- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
|
||||||
|
- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。
|
||||||
|
|
||||||
|
### 3.3 导出外参先验
|
||||||
|
|
||||||
|
- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。
|
||||||
|
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
|
||||||
|
- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 现存问题清单
|
||||||
|
|
||||||
|
### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移）
|
||||||
|
|
||||||
|
- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。
|
||||||
|
- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。
|
||||||
|
- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。
|
||||||
|
- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。
|
||||||
|
- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。
|
||||||
|
|
||||||
|
### P2. 平面运动导致竖直平移弱可观
|
||||||
|
|
||||||
|
- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。
|
||||||
|
- 流水线门控已给出 `translation_accepted=false`。
|
||||||
|
- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。
|
||||||
|
|
||||||
|
### P3. 时间偏移若再自由估计会被带偏（已规避，需保持）
|
||||||
|
|
||||||
|
- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。
|
||||||
|
- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。
|
||||||
|
- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。
|
||||||
|
|
||||||
|
### P4. 单窗低残差 ≠ 外参正确（历史教训）
|
||||||
|
|
||||||
|
- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。
|
||||||
|
- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
|
||||||
|
- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。
|
||||||
|
|
||||||
|
### P5. 旋转软先验尚未做无先验对照
|
||||||
|
|
||||||
|
- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。
|
||||||
|
- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。
|
||||||
|
|
||||||
|
### P6. 文档与操作约定未完全同步（工程）
|
||||||
|
|
||||||
|
- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
|
||||||
|
- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 不该做 / 可以做
|
||||||
|
|
||||||
|
| 动作 | 建议 |
|
||||||
|
|------|------|
|
||||||
|
| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） |
|
||||||
|
| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
|
||||||
|
| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 |
|
||||||
|
| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** |
|
||||||
|
| 无先验 / 大 σ 旋转对照 | **建议做** |
|
||||||
|
| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
|
||||||
|
| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 建议下一步顺序
|
||||||
|
|
||||||
|
1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。
|
||||||
|
2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。
|
||||||
|
3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。
|
||||||
|
4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 常用路径与命令
|
||||||
|
|
||||||
|
```text
|
||||||
|
数据根：
|
||||||
|
D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
|
||||||
|
|
||||||
|
结果：
|
||||||
|
...\priority_XXXX\out_fixed_dt0\summary.json
|
||||||
|
...\priority_XXXX\out_fixed_dt0\motion_pairs.json
|
||||||
|
...\calibration_manifest_fixed_dt0.json
|
||||||
|
```
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
# 可视化（rotation_only 默认模式4用 GICP 平移）
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
|
||||||
|
--summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
|
||||||
|
--pair-index 0
|
||||||
|
|
||||||
|
# 若要看「坏 Δp」导致的错层效果：
|
||||||
|
# --mode4-translation imu
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. 问题优先级（跟踪用）
|
||||||
|
|
||||||
|
| ID | 严重度 | 状态 | 标题 |
|
||||||
|
|----|--------|------|------|
|
||||||
|
| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 |
|
||||||
|
| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 |
|
||||||
|
| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） |
|
||||||
|
| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
|
||||||
|
| P5 | 中 | 待做 | 无旋转先验对照 |
|
||||||
|
| P6 | 低 | 待做 | README/交付物同步 |
|
||||||
@@ -0,0 +1,195 @@
|
|||||||
|
# LiDAR–IMU 外参标定说明
|
||||||
|
|
||||||
|
**用途：** 方法约定与实现状态（深入阅读）。日常使用请先看根目录 [`README.md`](../README.md)。
|
||||||
|
|
||||||
|
本文说明标定目标、约定、流水线与实现状态。代码在 `imu_lidar/`。场地采集见 [`标定流程与采集清单.md`](标定流程与采集清单.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 目标与约定
|
||||||
|
|
||||||
|
估计安装外参：
|
||||||
|
|
||||||
|
```text
|
||||||
|
p_IMU = T_IMU_lidar · p_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
约定：`T_A_B` 表示把 **B 系点**变换到 **A 系**。
|
||||||
|
|
||||||
|
相对运动手眼模型：
|
||||||
|
|
||||||
|
```text
|
||||||
|
A_ij ≈ IMU 在 [t_i, t_j] 的相对运动（预积分）
|
||||||
|
B_ij ≈ LiDAR 在同时间段的相对运动（关键帧配准）
|
||||||
|
A X ≈ X B
|
||||||
|
X = T_IMU_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
旋转子问题（常规主交付）：
|
||||||
|
|
||||||
|
```text
|
||||||
|
R_A R_X = R_X R_B
|
||||||
|
→ R_IMU_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
若已有完整六自由度外参，且另有 `T_RTK_lidar`，可链式得到：
|
||||||
|
|
||||||
|
```text
|
||||||
|
T_lidar_IMU = inverse(T_IMU_lidar)
|
||||||
|
T_RTK_IMU = T_RTK_lidar @ T_lidar_IMU
|
||||||
|
```
|
||||||
|
|
||||||
|
**不要**把 IMU 加速度二次积分成轨迹，再当作绝对位姿去做完整六自由度手眼。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 交付分层
|
||||||
|
|
||||||
|
| 层级 | 交付 | 数据最低要求 |
|
||||||
|
|---|---|---|
|
||||||
|
| 第一步 | 旋转 + 时间偏置 δt | **设备时间戳**；静止 + 低速转弯 /「8」字；结构化场景 |
|
||||||
|
| 第二步 | 上一步 + 可观的水平平移 | 更多转弯半径与加减速 |
|
||||||
|
| 第三步 | 完整六自由度（含可靠竖直分量） | 缓坡俯仰激励，或外测垂直杆臂先验 |
|
||||||
|
|
||||||
|
可观性不过关 → 只交旋转，不强交“假精确”六自由度。
|
||||||
|
仅主机接收时间、或残差未过门控的结果 → **不要当作正式安装参数**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 总体原则
|
||||||
|
|
||||||
|
1. **时间同步优先于外参**：δt 未对齐时，旋转与平移都不可信。使用设备时间戳。
|
||||||
|
2. **先求旋转，再求平移**：旋转通常更稳；平面运动下竖直平移常常不可观。
|
||||||
|
3. **用连续运动标定**：停车多站、再靠站间长积分，不适合作为纯 IMU 外参主流程。
|
||||||
|
4. **可观性门控**：过不了就降级交付。
|
||||||
|
5. **残差小 ≠ 标定对**：需叠点云 / 跨会话等独立验证。
|
||||||
|
6. **首轮建议低速**：先保证配准与时间对齐；点云去畸变可选。
|
||||||
|
7. **安装参数不写死在源码**：轴向与时间语义进 YAML；机械尺寸可作检查，不能伪装成已标定平移。
|
||||||
|
8. **注意耦合**：时间相关峰很弱时，δt、航向角与陀螺零偏可能互相补偿，结果不可当真。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 流水线（现行实现）
|
||||||
|
|
||||||
|
```text
|
||||||
|
vehicle_config
|
||||||
|
→ timestamp_audit
|
||||||
|
→ imu_audit（静止零偏等）
|
||||||
|
→ time_offset：粗估 δt
|
||||||
|
→ keyframes / [可选] deskew
|
||||||
|
→ motion_pairs：完整 IMU 预积分 + 配准 B
|
||||||
|
→ rotation_handeye：加权求解 R
|
||||||
|
→ （有候选 R 时）精修 δt，必要时交替重建运动对
|
||||||
|
→ joint_optimizer：精修旋转与陀螺零偏；可观且 full_se3 时再估平移等
|
||||||
|
→ finalize
|
||||||
|
```
|
||||||
|
|
||||||
|
| 模块 | 文件 | 职责 |
|
||||||
|
|---|---|---|
|
||||||
|
| 配置 | `vehicle_config.py` | 读安装 YAML |
|
||||||
|
| IO | `imu_io.py` / `lidar_io.py` | 标准 CSV / 帧目录 |
|
||||||
|
| 质检 | `timestamp_audit.py` / `imu_audit.py` | 时间域、静止零偏 |
|
||||||
|
| δt | `time_offset.py` | 粗估 + 有符号精修 |
|
||||||
|
| 运动 | `keyframes.py` / `registration.py` / `lidar_deskew.py` | 关键帧、配准、可选去畸变 |
|
||||||
|
| IMU 侧 | `imu_preintegration.py` / `motion_pairs.py` | 预积分与运动对 |
|
||||||
|
| 求解 | `rotation_handeye.py` / `joint_optimizer.py` / `observability.py` | 手眼、联合精修、门控 |
|
||||||
|
| 编排 | `pipeline.py` / `cli.py` / `finalize.py` | 入口与落盘 |
|
||||||
|
|
||||||
|
输入中间格式见 [V1_数据格式.md](V1_数据格式.md)。改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||||
|
|
||||||
|
### 运行示例
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m pip install -e ".[dev]"
|
||||||
|
python -m pip install -e ".[open3d]" # 可选
|
||||||
|
|
||||||
|
python -m imu_lidar.cli plan --mode rotation_only
|
||||||
|
|
||||||
|
python -m imu_lidar.cli run `
|
||||||
|
--vehicle-config config\vehicle_installation.template.yaml `
|
||||||
|
--imu path\to\imu.csv `
|
||||||
|
--lidar path\to\lidar_session `
|
||||||
|
--output path\to\output `
|
||||||
|
--mode rotation_only `
|
||||||
|
--time-offset-search-s 2.0
|
||||||
|
```
|
||||||
|
|
||||||
|
合成自检：`python tools\generate_synthetic_session.py` 后跑 CLI，再 `python -m pytest -q`。
|
||||||
|
|
||||||
|
### 结果状态
|
||||||
|
|
||||||
|
| status | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| `rotation_only_accepted` | 旋转过门，可交旋转与报告中的 δt |
|
||||||
|
| `full_se3_accepted` | 可观且联合优化通过，可交完整 `T` |
|
||||||
|
| `full_se3_rejected_due_to_observability` | 旋转可用，平移未接受 |
|
||||||
|
| `blocked` | 质检 / δt / 手眼残差等硬门失败，**不交付** |
|
||||||
|
|
||||||
|
`rotation_only` 模式下：不要把未标定的机械平移拼进 4×4 伪装成完整标定。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 采集要点
|
||||||
|
|
||||||
|
单趟动态会话：
|
||||||
|
|
||||||
|
```text
|
||||||
|
[静止 20–30 s] → [低速激励 3–8 min] → [再静止 10–20 s]
|
||||||
|
```
|
||||||
|
|
||||||
|
优先激励：
|
||||||
|
|
||||||
|
- 低速「8」字 / 左右圆（旋转主激励）
|
||||||
|
- 直线加减速（有助于时间对齐与水平平移）
|
||||||
|
- 缓坡（仅完整六自由度需要）：约 3°～8°，连续长度优先 ≥ 20～30 m
|
||||||
|
|
||||||
|
硬条件：IMU / LiDAR **设备时间戳**；结构化场景；标定全程安装不得改动。
|
||||||
|
|
||||||
|
更完整的现场清单见 [标定流程与采集清单.md](标定流程与采集清单.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 数学上允许与禁止
|
||||||
|
|
||||||
|
禁止：加速度二次积分当真值轨迹；把只有旋转的相对运动硬补成完整六自由度；用最终外参反向筛边掩盖失败。
|
||||||
|
|
||||||
|
允许：预积分旋转手眼求旋转；在可观时用预积分残差联合估计平移，并精修零偏等辅助量。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 车辆配置
|
||||||
|
|
||||||
|
使用 `--vehicle-config vehicle_installation.yaml`。
|
||||||
|
模板中安装平移/旋转保持未标定状态，直至实测确认；禁止写死某车杆臂冒充结果。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. 实现状态（当前阶段）
|
||||||
|
|
||||||
|
本仓库**仅此一条**标定路径：连续运动关键帧 + IMU 预积分。对外总览与「合成 / 旧车 / 合格数据预期」见根目录 [`README.md`](../README.md) §0。
|
||||||
|
|
||||||
|
| 项 | 状态 |
|
||||||
|
|---|---|
|
||||||
|
| 连续运动关键帧标定流水线 | 已实现（现行唯一路径） |
|
||||||
|
| 加权预积分 / 加权手眼 | 已实现 |
|
||||||
|
| 旋转预积分因子 + 有符号 δt 精修 | 已实现 |
|
||||||
|
| 完整预积分（含速度/位移增量）与可观时的平移优化 | 已实现 |
|
||||||
|
| 可观性门控 / `rotation_only` | 已实现 |
|
||||||
|
| 合成数据 pytest / 一键复现 | 已实现（证明链路与已知 yaw/δt，不证明实车精度） |
|
||||||
|
| 旧车主机时间烟测（S2） | 线下可跑；预期 `blocked`，不当交付 |
|
||||||
|
| 设备时间新车数据正式验收 | **待做** |
|
||||||
|
| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`（N300 rscap + H32 dlog/MSOP → V1；dlog 用 DIFOP 通道角） |
|
||||||
|
|
||||||
|
细项见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 9. 相关文档
|
||||||
|
|
||||||
|
| 内容 | 路径 | 备注 |
|
||||||
|
|---|---|---|
|
||||||
|
| 对外总览（优先） | 根目录 [`README.md`](../README.md) | 日常入口 |
|
||||||
|
| 采集清单 | [`标定流程与采集清单.md`](标定流程与采集清单.md) | 现场 |
|
||||||
|
| 数据格式 / 导出 | [`V1_数据格式.md`](V1_数据格式.md) | 中间格式 |
|
||||||
|
| 文件职责说明 | [`imu_lidar/文件职责说明.md`](../imu_lidar/文件职责说明.md) | 改代码 |
|
||||||
|
| 改动史 | [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md) | 改代码 |
|
||||||
+109
@@ -0,0 +1,109 @@
|
|||||||
|
# V1 标准中间数据格式
|
||||||
|
|
||||||
|
**用途：** 标定程序读入的 CSV/NPZ 约定，以及新车原始数据如何导出。总览见根目录 [README](../README.md)。
|
||||||
|
|
||||||
|
## 从原始数据导出
|
||||||
|
|
||||||
|
**推荐（新 H32 + HI13）：** HI13 `.rscap` + 雷达 Medulla dlog / recovered zip（raw MSOP + DIFOP）。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\export_rscap_to_v1.py `
|
||||||
|
--imu-rscap path\to\hi13r4-imu.rscap `
|
||||||
|
--imu-kind hi13 `
|
||||||
|
--lidar-dlog path\to\session_or_dlog_or_recovered.zip `
|
||||||
|
--host-start 2026-08-08T17:40:05 `
|
||||||
|
--host-end 2026-08-08T17:45:15 `
|
||||||
|
--out path\to\session_v1 `
|
||||||
|
--frame-stride 5 `
|
||||||
|
--require-difop
|
||||||
|
```
|
||||||
|
|
||||||
|
`--lidar-dlog` 可为：标准 `dobject/`+`dobject_recording/` 目录，或 recovered zip（`indices.log` + `data.bin`）。
|
||||||
|
`--imu-kind`：`hi13` / `n300` / `auto`（默认按文件名推断）。
|
||||||
|
`--host-start/end`：按本地墙钟切窗（仅裁剪；标定主轴仍是设备时间）。
|
||||||
|
默认 DObject：`frontlidar-msop-raw`、`frontlidar-difop-raw`。
|
||||||
|
|
||||||
|
**兼容旧 MSOP-only `.rscap`：**
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\export_rscap_to_v1.py `
|
||||||
|
--imu-rscap path\to\n300.rscap `
|
||||||
|
--lidar-rscap path\to\h32_msop.rscap `
|
||||||
|
--out path\to\session_v1 `
|
||||||
|
--frame-stride 1
|
||||||
|
```
|
||||||
|
|
||||||
|
产出：`imu.csv`、`lidar/`（含 `frames_index.csv`）、`export_summary.json`。
|
||||||
|
标定主轴仍是**设备时间**；同时写出**主机 UTC 接收时间**，用于把雷达帧桥接到 IMU 设备钟（禁止把两边设备时间第一帧强行重合）。
|
||||||
|
|
||||||
|
## IMU
|
||||||
|
|
||||||
|
文件：`imu.csv` 或 `imu.npz`
|
||||||
|
|
||||||
|
### CSV
|
||||||
|
|
||||||
|
```text
|
||||||
|
t,gx,gy,gz,ax,ay,az,t_host_utc_s,receive_utc_ticks
|
||||||
|
0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81,1754646005.123,6389...
|
||||||
|
...
|
||||||
|
```
|
||||||
|
|
||||||
|
| 列 | 含义 | 单位 |
|
||||||
|
|---|---|---|
|
||||||
|
| t | IMU 设备时钟时间 | s |
|
||||||
|
| gx,gy,gz | 角速度 | rad/s |
|
||||||
|
| ax,ay,az | 比力/加速度 | m/s² |
|
||||||
|
| t_host_utc_s | 主机 UTC 接收时间（Unix） | s |
|
||||||
|
| receive_utc_ticks | 同上，.NET UTC ticks | — |
|
||||||
|
|
||||||
|
### NPZ
|
||||||
|
|
||||||
|
数组：`t (N,)`, `gyro (N,3)`, `acc (N,3)`，含义同上。
|
||||||
|
|
||||||
|
> IMU 与 LiDAR 的时间原点可以不同。流水线会估计常值偏置：`t_imu = t_lidar + delta_t`。
|
||||||
|
|
||||||
|
## LiDAR
|
||||||
|
|
||||||
|
目录结构：
|
||||||
|
|
||||||
|
```text
|
||||||
|
lidar_session/
|
||||||
|
├── frames_index.csv
|
||||||
|
└── frames/
|
||||||
|
├── frame_00000.npz
|
||||||
|
├── frame_00001.npz
|
||||||
|
└── ...
|
||||||
|
```
|
||||||
|
|
||||||
|
### frames_index.csv
|
||||||
|
|
||||||
|
```text
|
||||||
|
frame_id,filename,t_start,t_end,host_receive_utc_ticks,t_host_utc_s,host_receive_utc_end_ticks,t_host_utc_end_s
|
||||||
|
0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22
|
||||||
|
```
|
||||||
|
|
||||||
|
| 列 | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| t_start / t_end | H32 MSOP **设备时间**（秒） |
|
||||||
|
| t_host_utc_s / t_host_utc_end_s | 帧首/末包 **HostReceiveUtcTicks** → Unix 秒 |
|
||||||
|
|
||||||
|
也兼容旧列名 `file`。对齐脚本用主机 UTC 把 `t_*` 重写到 IMU 设备钟后再跑标定。
|
||||||
|
|
||||||
|
### 每帧 NPZ
|
||||||
|
|
||||||
|
- `points`: `float64/float32`，形状 `(N, 3)`，LiDAR 直角坐标系，单位米
|
||||||
|
|
||||||
|
## 时间不同步能不能用？
|
||||||
|
|
||||||
|
可以，前提是：
|
||||||
|
|
||||||
|
1. 两边都覆盖同一段**有角速度激励**的物理运动（尤其是转弯）；
|
||||||
|
2. 偏置近似为**常数**（短会话）；
|
||||||
|
3. `--time-offset-search-s` 足够覆盖可能的偏移（默认 ±1 s，可加大）。
|
||||||
|
|
||||||
|
若两段数据完全不是同一趟行驶，或只有静止 IMU、没有重叠运动，则无法估 δt，标定会被 `blocked`。
|
||||||
|
|
||||||
|
## 最小可用会话
|
||||||
|
|
||||||
|
- IMU：建议含静止段 + 运动段，采样率稳定
|
||||||
|
- LiDAR：建议 ≥ 20 帧，场景有墙/柱等结构，含转弯
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
# 纯 LiDAR–IMU：标定流程与采集清单
|
||||||
|
|
||||||
|
**用途：** 现场怎么采合格数据（看完根目录 [README](../README.md) 后再看本文即可）。
|
||||||
|
算法命令与结果判读以 README 为准；改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||||
|
|
||||||
|
仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐采集与流程要点。
|
||||||
|
目标外参：`p_IMU = T_IMU_lidar · p_lidar`（`T_A_B` 表示把 B 系点变到 A 系）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 原则（先读）
|
||||||
|
|
||||||
|
1. **时间对齐优先**：时钟偏置未对准时，旋转与平移都不可信；正式数据用**设备时间戳**。
|
||||||
|
2. **先旋转，再平移**：旋转通常更稳；平面低速时竖直方向平移常常不可观。
|
||||||
|
3. **用连续运动**：停车多站适合 RTK 手眼，不适合作为纯 IMU 外参主流程。
|
||||||
|
4. **可观才交平移**：激励不够就只交旋转，不强交“假精确”六自由度。
|
||||||
|
5. **残差小 ≠ 标定对**：需叠点云、跨会话等独立验证。
|
||||||
|
|
||||||
|
相对运动模型：
|
||||||
|
|
||||||
|
```text
|
||||||
|
A ≈ IMU 预积分相对运动（关键帧区间）
|
||||||
|
B ≈ 雷达关键帧配准相对运动
|
||||||
|
R_A · R_X ≈ R_X · R_B → 先求旋转
|
||||||
|
完整模式且可观时再求平移 t
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 端到端流程（与现行代码一致）
|
||||||
|
|
||||||
|
```text
|
||||||
|
确认轴向 / 单位 / 时间语义
|
||||||
|
→ 现场采集（首尾静止 + 低速多转弯；建议 ≥2 段独立会话）
|
||||||
|
→ 导出标准中间格式（imu.csv + lidar 会话目录）
|
||||||
|
→ 质检（时间 / IMU）不通过则停
|
||||||
|
→ 粗估时间偏置 δt
|
||||||
|
→ 关键帧 → 配准得 B；完整 IMU 预积分得 A（旋转/速度/位移增量）
|
||||||
|
→ 加权旋转手眼得 R
|
||||||
|
→ 用 R 精修 δt，必要时重新组对再解 R（可交替数轮）
|
||||||
|
→ 联合精修 R 与常值陀螺零偏
|
||||||
|
→ full_se3 且可观：再估重力、关键帧速度、时变零偏与平移 t
|
||||||
|
→ 写出 T / δt / summary → 叠点云 / 跨会话验证后交付
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
| 步骤 | 现行模块 | 说明 |
|
||||||
|
| --- | -------------------------------------------------------------------- | --------------------------------------- |
|
||||||
|
| 质检 | `timestamp_audit` / `imu_audit` | 含静止段陀螺零偏初值 |
|
||||||
|
| 时间 | `time_offset` | 模长相关粗估 + 有符号三轴精修 |
|
||||||
|
| 运动对 | `keyframes` / `registration` / `imu_preintegration` / `motion_pairs` | 预积分始终算满；手眼先用旋转 |
|
||||||
|
| 旋转 | `rotation_handeye` | 加权手眼 |
|
||||||
|
| 精修 | `joint_optimizer` / `observability` | `rotation_only` 到旋转为止；`full_se3` 可观才碰平移 |
|
||||||
|
|
||||||
|
|
||||||
|
点云去畸变（`lidar_deskew`）可选；低速首轮可不依赖。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 3. 采集设计
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 3.1 单趟会话结构
|
||||||
|
|
||||||
|
```text
|
||||||
|
静止 20–30 s → 连续运动 3–8 min → 再静止 10–20 s
|
||||||
|
```
|
||||||
|
|
||||||
|
运动优先：低速「8」字 / 左右圆；再补加减速直线。
|
||||||
|
要可靠竖直方向外参时，另加缓坡（约 3°～8°，有效长度优先 ≥20～30 m），或改用外测垂直尺寸先验。
|
||||||
|
|
||||||
|
### 3.2 会话安排
|
||||||
|
|
||||||
|
|
||||||
|
| 会话 | 作用 |
|
||||||
|
| ----- | ---------------------------------- |
|
||||||
|
| A | 主标定 |
|
||||||
|
| B | 独立验证（**同一场地**换一条不完全相同的路线即可，不参与求外参） |
|
||||||
|
| C（可选） | 不同速度/路线，测稳定性 |
|
||||||
|
|
||||||
|
|
||||||
|
安装全程不得改动。多会话不要跨会话拼运动对。
|
||||||
|
|
||||||
|
### 3.3 录制字段（原始，勿先做姿态融合）
|
||||||
|
|
||||||
|
- IMU：设备时间、陀螺、加速度（建议同时留主机接收时间便于排查）
|
||||||
|
- LiDAR：每帧起止时间（最好有包级/逐点时间）、原始点云
|
||||||
|
|
||||||
|
中间格式见 `[V1_数据格式.md](V1_数据格式.md)`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 4. 现场 Checklist
|
||||||
|
|
||||||
|
**出发前**
|
||||||
|
|
||||||
|
- [ ] 安装固定；草图/卷尺粗测仅作参考，不当真值
|
||||||
|
- [ ] 单位与轴向确认；设备时间可写盘
|
||||||
|
- [ ] 结构化路线（墙/杆/路缘），避开空旷无特征区
|
||||||
|
- [ ] 存储与供电充足
|
||||||
|
|
||||||
|
**录制中**
|
||||||
|
|
||||||
|
- [ ] 首尾静止；中间有明显左右转与加减速
|
||||||
|
- [ ] 不改安装、不切换时间源
|
||||||
|
- [ ] 记录会话 ID、天气、异常（急刹、掉包等）
|
||||||
|
|
||||||
|
**当场快查**
|
||||||
|
|
||||||
|
- [ ] IMU 静止段平稳，转弯时角速度明显
|
||||||
|
- [ ] 点云帧数/点数正常，无明显大面积丢帧
|
||||||
|
- [ ] 雷达与 IMU 时间覆盖同一时段
|
||||||
|
|
||||||
|
**回实验室**
|
||||||
|
|
||||||
|
- [ ] 已导出中间格式并通过质检
|
||||||
|
- [ ] 本次目标：`rotation_only` 还是尝试 `full_se3`
|
||||||
|
- [ ] 若要竖直方向：确认真有俯仰/高度激励，否则降级交付
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 5. 精度预期
|
||||||
|
|
||||||
|
|
||||||
|
| 量 | 较现实范围 | 说明 |
|
||||||
|
| ---------- | --------- | ---------------- |
|
||||||
|
| 旋转 | 约 0.5°–2° | 最精确部分 |
|
||||||
|
| 水平平移 | 数厘米～十几厘米 | 强依赖配准、激励与同步 |
|
||||||
|
| 竖直 / 部分杠杆臂 | 往往更差甚至不可观 | 无高度激励时无法得出“精确 z” |
|
||||||
|
|
||||||
|
|
||||||
|
如果条件有限，优先交付：**可靠旋转 + δt + 可观的平移分量（若有）+ 明确限制说明**。
|
||||||
|
|
||||||
|
满足条件后的模式与成功标志见根目录 [`README.md`](../README.md) §0。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 6. 交付物建议
|
||||||
|
|
||||||
|
程序默认写出：`T_IMU_lidar.json`、`time_offset.json`、`summary.json`。
|
||||||
|
完整报告目录还可补充：可观性结论、跨会话对比、运动对质量表、限制说明（尤其竖直方向与时间同步方式）。
|
||||||
|
|
||||||
|
```text
|
||||||
|
静止 + 激励录制（多会话）
|
||||||
|
→ 质检 → 估 δt → 关键帧 A/B → 先解 R
|
||||||
|
→ 精修 δt 与 R → 可观则求 t → 验证后交付
|
||||||
|
```
|
||||||
|
|
||||||
+209
@@ -0,0 +1,209 @@
|
|||||||
|
# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题
|
||||||
|
|
||||||
|
> 数据：`D:\data\calibration_usable_20260808`
|
||||||
|
> 可用会话：`sessions_v1_host_aligned`（三优先窗）
|
||||||
|
> 当前结果目录：各窗 `out_fixed_dt0/`
|
||||||
|
> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`
|
||||||
|
> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml`
|
||||||
|
> 约定外参：`p_IMU = T_IMU_lidar · p_lidar`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 一句话结论
|
||||||
|
|
||||||
|
**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。**
|
||||||
|
三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 当前可用结果（`out_fixed_dt0`）
|
||||||
|
|
||||||
|
约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。
|
||||||
|
|
||||||
|
| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
|
||||||
|
|----|------|----|---------------------|--------------|----------|--------------|
|
||||||
|
| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
|
||||||
|
| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° |
|
||||||
|
| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° |
|
||||||
|
|
||||||
|
- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。
|
||||||
|
- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。
|
||||||
|
- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。
|
||||||
|
|
||||||
|
### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
|
||||||
|
- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
|
||||||
|
[ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ],
|
||||||
|
[-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
|
||||||
|
- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
|
||||||
|
[ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ],
|
||||||
|
[ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
|
||||||
|
- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
|
||||||
|
[ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
|
||||||
|
[ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 相对历史失败轮次
|
||||||
|
|
||||||
|
| 轮次 | 问题 | 结果 |
|
||||||
|
|------|------|------|
|
||||||
|
| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° |
|
||||||
|
| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） |
|
||||||
|
| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 已澄清并写入配置的坐标系 / 先验
|
||||||
|
|
||||||
|
### 3.1 车体与传感器
|
||||||
|
|
||||||
|
- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。
|
||||||
|
- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。
|
||||||
|
- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。
|
||||||
|
- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。
|
||||||
|
|
||||||
|
### 3.2 安装量（`translation_m`）
|
||||||
|
|
||||||
|
| 传感器 | X / Y（后轮轴中心） | Z（离地） |
|
||||||
|
|--------|---------------------|-----------|
|
||||||
|
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
|
||||||
|
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
|
||||||
|
|
||||||
|
- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
|
||||||
|
- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。
|
||||||
|
|
||||||
|
### 3.3 导出外参先验
|
||||||
|
|
||||||
|
- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。
|
||||||
|
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
|
||||||
|
- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 现存问题清单
|
||||||
|
|
||||||
|
### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移）
|
||||||
|
|
||||||
|
- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。
|
||||||
|
- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。
|
||||||
|
- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。
|
||||||
|
- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。
|
||||||
|
- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。
|
||||||
|
|
||||||
|
### P2. 平面运动导致竖直平移弱可观
|
||||||
|
|
||||||
|
- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。
|
||||||
|
- 流水线门控已给出 `translation_accepted=false`。
|
||||||
|
- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。
|
||||||
|
|
||||||
|
### P3. 时间偏移若再自由估计会被带偏（已规避，需保持）
|
||||||
|
|
||||||
|
- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。
|
||||||
|
- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。
|
||||||
|
- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。
|
||||||
|
|
||||||
|
### P4. 单窗低残差 ≠ 外参正确（历史教训）
|
||||||
|
|
||||||
|
- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。
|
||||||
|
- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
|
||||||
|
- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。
|
||||||
|
|
||||||
|
### P5. 旋转软先验尚未做无先验对照
|
||||||
|
|
||||||
|
- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。
|
||||||
|
- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。
|
||||||
|
|
||||||
|
### P6. 文档与操作约定未完全同步（工程）
|
||||||
|
|
||||||
|
- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
|
||||||
|
- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 不该做 / 可以做
|
||||||
|
|
||||||
|
| 动作 | 建议 |
|
||||||
|
|------|------|
|
||||||
|
| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） |
|
||||||
|
| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
|
||||||
|
| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 |
|
||||||
|
| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** |
|
||||||
|
| 无先验 / 大 σ 旋转对照 | **建议做** |
|
||||||
|
| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
|
||||||
|
| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 建议下一步顺序
|
||||||
|
|
||||||
|
1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。
|
||||||
|
2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。
|
||||||
|
3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。
|
||||||
|
4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 常用路径与命令
|
||||||
|
|
||||||
|
```text
|
||||||
|
数据根：
|
||||||
|
D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
|
||||||
|
|
||||||
|
结果：
|
||||||
|
...\priority_XXXX\out_fixed_dt0\summary.json
|
||||||
|
...\priority_XXXX\out_fixed_dt0\motion_pairs.json
|
||||||
|
...\calibration_manifest_fixed_dt0.json
|
||||||
|
```
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
# 可视化（rotation_only 默认模式4用 GICP 平移）
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
|
||||||
|
--summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
|
||||||
|
--pair-index 0
|
||||||
|
|
||||||
|
# 若要看「坏 Δp」导致的错层效果：
|
||||||
|
# --mode4-translation imu
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. 问题优先级（跟踪用）
|
||||||
|
|
||||||
|
| ID | 严重度 | 状态 | 标题 |
|
||||||
|
|----|--------|------|------|
|
||||||
|
| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 |
|
||||||
|
| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 |
|
||||||
|
| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） |
|
||||||
|
| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
|
||||||
|
| P5 | 中 | 待做 | 无旋转先验对照 |
|
||||||
|
| P6 | 低 | 待做 | README/交付物同步 |
|
||||||
@@ -0,0 +1,270 @@
|
|||||||
|
# `imu_lidar` 改动记录
|
||||||
|
|
||||||
|
本文件专门记录 `imu_lidar` 目录内的实现改动。
|
||||||
|
每条包含：**时间戳**、**改动内容**（以「原本怎么做 → 改成怎么做」书写）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-11 10:55 (UTC+8)
|
||||||
|
|
||||||
|
### 运动对缓存：标定落盘，可视化直读
|
||||||
|
|
||||||
|
- **原本**：`visualize_pair_3d` 每次启动都重新关键帧+配准+预积分，等同半次标定。
|
||||||
|
- **改成**：
|
||||||
|
- 标定成功后写出 `motion_pairs.json`（`motion_pairs_io.py` / `finalize`）。
|
||||||
|
- 可视化优先读缓存并对点云懒加载；`--rebuild-pairs` 可回退旧路径。
|
||||||
|
- 旧结果可用 `tools/export_motion_pairs_for_viz.py` 只补导出运动对，无需重求解外参。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-11 08:55 (UTC+8)
|
||||||
|
|
||||||
|
### 主机桥接后冻结 δt + 旋转先验软约束
|
||||||
|
|
||||||
|
- **原本**：手眼后 signed δt 精修可在弱 MSE 下降下连走数步（最远约 0.5 s）；旋转手眼无 CAD 先验，平面运动下 yaw 易掉进低残差错解。
|
||||||
|
- **改成**：
|
||||||
|
- CLI：`--fixed-time-offset-s`、`--no-signed-time-refine`、`--max-signed-refine-shift-s`。
|
||||||
|
- signed refine：默认 `|Δδt|≤0.05 s`，且要求 MSE 至少降约 2%。
|
||||||
|
- `rotation_handeye` 读取配置 `rotation_prior` 作初值/软约束。
|
||||||
|
- 主机 UTC 桥接会话建议：`--fixed-time-offset-s 0 --no-signed-time-refine`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-09 14:30 (UTC+8)
|
||||||
|
|
||||||
|
### 导出：HI13 IMU + recovered dlog zip + 墙钟切窗
|
||||||
|
|
||||||
|
- **原本**：IMU 只解 N300 FDILink；dlog 只认标准 `*.dorec`；无法按图上时段切窗。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/rscap_v2/hi13_imu.py`（HI91：g→m/s²、°/s→rad/s、设备 ms）。
|
||||||
|
- `h32_dlog` 支持 recovered zip（`indices.log` + `data.bin`），ZIP_STORED 成员按文件绝对 offset 直读。
|
||||||
|
- `export_rscap_to_v1.py`：`--imu-kind hi13|n300|auto`、多段 `--imu-rscap`、`--host-start/end` 切窗。
|
||||||
|
- 辅助脚本 `tools/export_usable_20260808_windows.py` 导出优先运动段。
|
||||||
|
- **未推送**（按用户要求本地改完即可）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-05 09:00 (UTC+8)
|
||||||
|
|
||||||
|
### 导出：支持 H32 DLogCapture（MSOP+DIFOP）→ V1
|
||||||
|
|
||||||
|
- **原本**：导出只读 H32 MSOP V2 `.rscap`，无 DIFOP，垂直角用默认 −16°…+16°。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/h32_dlog/`（dobject 索引、MSOP/DIFOP payload V1、DIFOP 通道角）。
|
||||||
|
- `export_rscap_to_v1.py` 增加 `--lidar-dlog`（与 `--lidar-rscap` 二选一）；默认用 DIFOP 角做 XYZ。
|
||||||
|
- `h32_msop.iter_h32_frames_from_packets` 供 dlog/rscap 共用拼帧。
|
||||||
|
- 单测 `tests/test_h32_dlog_export.py`；文档改为推荐 dlog 导出命令。
|
||||||
|
- **标定核心**（`imu_lidar/` 读 V1）未改。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-03 17:30 (UTC+8)
|
||||||
|
|
||||||
|
### 文档：精简对外阅读路径
|
||||||
|
|
||||||
|
- **原本**：README 很长，多份文档职责不清，外人易觉复杂。
|
||||||
|
- **改成**：README 改为短入口 +「对外三份就够」；采集清单 / 方法说明 / 测试说明 / 职责说明文首标明用途；细节仍保留在原文件。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-03 11:40 (UTC+8)
|
||||||
|
|
||||||
|
### 新增：N300/H32 `.rscap` → V1 中间格式导出
|
||||||
|
|
||||||
|
- **原本**：标定只接受 CSV/NPZ；新车原始录制需手工转换，无仓库内导出器。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/rscap_v2/`（V2 读取、N300 IMU、H32 MSOP 拼帧）与 `tools/export_rscap_to_v1.py`。
|
||||||
|
- 导出写入设备时间轴的 `imu.csv` + `lidar/`；支持 `--frame-stride` / `--max-points-per-frame`。
|
||||||
|
- 单元测试 `tests/test_export_rscap_helpers.py`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-08-01 11:40 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档：现状一览补充「合格数据」定义
|
||||||
|
|
||||||
|
- **原本**：§0 只写「合格数据拿到后」怎么跑，未写清何为合格。
|
||||||
|
- **改成**：根 `[README.md](../README.md)` §0 增加「什么叫合格数据」表（时间戳 / 会话 / 场景 / 格式 / 反例）及拿到后的模式与预期。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-08-01 11:30 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档：现状一览 + 去掉「方案」二分表述
|
||||||
|
|
||||||
|
- **原本**：对外说明仍偶发「方案二」等旧称呼；根 README 缺少一眼可读的阶段 / 合成 vs 旧车 / 合格数据预期；烟测配置与对比脚本文件名带 `scheme2`。
|
||||||
|
- **改成**：
|
||||||
|
- 根 `[README.md](../README.md)` 增加 §0「现状一览」；明确仓库只有一条连续运动标定路径。
|
||||||
|
- `[tests/README.md](../tests/README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明同步边界与阶段。
|
||||||
|
- `config/s2_old_smoke.yaml`、`tools/compare_s2_runs.py` 替换旧 `*scheme2*` 命名。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 18:10 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 配准可视化工具 + tests 说明（含 S2 线下记录）
|
||||||
|
|
||||||
|
- **原本**：无类似 RTK 仓库的运动对叠点 3D 查看；`tests/` 未说明合成 pytest 与 S2 旧数据线下试验的区别与结果。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/visualize_pair_3d.py` / `view_pair.ps1`（键 1–4：原始 / IMU(X=I) / 雷达 B / `X⁻¹AX`；可 `--save-png`）。
|
||||||
|
- 新增 `[tests/README.md](../tests/README.md)`：自动化用例表 + S2 主机时间数据做了什么、结果为何 `blocked`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 17:20 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档同步 + 合成数据一键复现
|
||||||
|
|
||||||
|
- **原本**：`docs/标定流程与采集清单.md` 仍偏旧版「待写代码 / 因子图设想」；根 README 缺少清晰的一键复现入口与输入输出总表。
|
||||||
|
- **改成**：
|
||||||
|
- 采集清单与现行流水线对齐（完整预积分、δt↔R 交替、可观时再估平移）。
|
||||||
|
- 新增 `tools/reproduce_synthetic.py` / `.ps1`、`tools/show_calibration_report.py`；合成生成写入 `meta.json`；根 README 增加「系统输入输出 + 一键复现」。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 16:30 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档：移除已删除的静站路径表述，对外 README 重写
|
||||||
|
|
||||||
|
- **原本**：根 README / `docs` / 包说明仍对照已删除的静站路径与内部阶段黑话；`pyproject` 仍声明已删除的 `static_station` 包。
|
||||||
|
- **改成**：
|
||||||
|
- 删除旧静站文档；采集清单定为 `[docs/标定流程与采集清单.md](../docs/标定流程与采集清单.md)`。
|
||||||
|
- 根 `[README.md](../README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明改为对外可读，只保留连续运动标定路径。
|
||||||
|
- `pyproject.toml` 仅保留 `imu_lidar` / `tools`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 14:00 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### Phase-C：完整 IMU 预积分 + 重力/速度/动态零偏（full_se3）
|
||||||
|
|
||||||
|
- **原本**：
|
||||||
|
- 运动对仅陀螺旋转预积分（`ΔR/Σ/J_bg`）；`t_A` 为空。
|
||||||
|
- 联合精修只估常值陀螺零偏修正；SE(3) 平移用经典手眼式 `(R_A-I)t ≈ R_X t_B`，无重力/速度/`b_a`。
|
||||||
|
- **改成**：
|
||||||
|
- `imu_preintegration.preintegrate_imu`：中值法积分 `ΔR/Δv/Δp`，传播 15 维误差态后输出 9×9 `Σ`（含 bias RW 过程噪声）与 9×3 `J_bg/J_ba`；保留 `preintegrate_gyro`。
|
||||||
|
- `motion_pairs` 始终调用完整预积分，写入 `delta_v/delta_p/cov9/J_bg9/J_ba`，`t_A_m=Δp`。
|
||||||
|
- `joint_optimizer`：`rotation_only` 仍 Phase-A；`full_se3` 可观时 Phase-C 联合估 `R_X,t_X,g,v_k,b_g,k,b_a,k`（关键帧 RW 先验）。
|
||||||
|
- `pipeline` 用静止加速度推重力初值；`summary.joint` 增加 `gravity_m_s2` / `accel_bias_m_s2`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 11:20 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档维护约定 + README 与现行实现对齐
|
||||||
|
|
||||||
|
- **原本**：根 README 与已删除的静站目录说明仍按「双路径并行」表述；部分模块说明未写明有符号 δt；改代码时 README 更新不完整。
|
||||||
|
- **改成**：
|
||||||
|
- 对外说明统一为**唯一连续运动标定路径**；流水线描述对齐有符号 δt 与联合精修。
|
||||||
|
- 根 README 增加「文档维护」表：每次改代码必须同步涉及的 README / 本 CHANGELOG。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 09:40 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 流水线：手眼未过门时仍尝试有符号 δt 精修
|
||||||
|
|
||||||
|
- **原本**：`rotation_handeye.ok=false`（如 RMS>5°）时立即 `blocked` 返回，阶段 A 的有符号 δt 精修根本不会执行。
|
||||||
|
- **改成**：只要可用运动对数 ≥3，即使用当前候选 `R` 做最多 2 轮有符号 δt 精修并重建运动对；精修后再按手眼门控决定是否 `blocked`。保证阶段 A 在困难数据上也能完整参与。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 09:20 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 阶段 A：标准旋转预积分因子 + 精确时间边界 + 有符号 δt 精修
|
||||||
|
|
||||||
|
- **原本**：
|
||||||
|
- 预积分只输出 `ΔR` 与启发式标量 weight/`σ`，区间端点用邻近 IMU 样本，无 `Σ`、无 `J_bg`。
|
||||||
|
- δt 仅靠角速度模长互相关粗估；手眼得到 `R` 后不再回头精修时间。
|
||||||
|
- 联合精修对零偏多用重积分或 `Exp(-δbΔt)` 近似，残差未按协方差白化，也无 `δb` 先验。
|
||||||
|
- **改成**：
|
||||||
|
- `imu_preintegration.preintegrate_gyro`：区间端点 **线性插值** 到精确 `t0/t1`；离散中值更新同时传播 `cov(Σ)` 与 `J_bg`（`ΔR(b+δb)≈ΔR Exp(J_bg δb)`）；weight 由 `trace(Σ)` + 激励/时长构造。
|
||||||
|
- `motion_pairs` metadata 增加 `cov`、`J_bg`，modeling 标记为 `gyro_preintegration_factor_phase_a`。
|
||||||
|
- `time_offset.refine_time_offset_signed`：用当前 `R_IMU_lidar` 把 LiDAR 角速度变到 IMU 系，在粗 δt 邻域做 **三轴有符号 MSE 精修**；仅当 MSE 下降且 **模长相关不劣化** 时才接受，避免 ICP 噪声带偏；`pipeline` 在手眼后与构对交替最多 2 轮。
|
||||||
|
- `joint_optimizer`：残差按 `Σ` **信息白化**；零偏用 `J_bg` 一阶修正；增加弱 `δb` 先验。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-30 17:50 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 第 1 步：帧间 IMU 轻量加强（加权预积分手眼）
|
||||||
|
|
||||||
|
- **原本**：`motion_pairs` 用 `integrate_gyro_rotation` 直接得到 `R_A`，各运动对等权进入 `rotation_handeye`；手眼残差不区分长短间隔与激励强弱。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `imu_preintegration.py`：对 `[t_i, t_j]` 做中值陀螺预积分，估计 `σ` 与 **pair weight**（偏短间隔、有角速度、低不确定度）。
|
||||||
|
- `motion_pairs` 改为调用 `preintegrate_gyro`，在 `metadata` 写入 `weight/duration_s/mean_gyro_norm/preint_sigma_rad/t_*_imu_s`，并增加 A/B 转角粗一致性过滤。
|
||||||
|
- `rotation_handeye` 改为 **√weight 加权** 的 Tsai 初值与 Huber 非线性精修；报告仍给未加权 RMS/中位数便于解读。
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 第 2 步：预积分残差联合精修（外参 + 陀螺零偏）
|
||||||
|
|
||||||
|
- **原本**：`joint_optimizer` 在手眼 `R_X` 基础上，仅在可观时用离散手眼平移式尝试 SE(3)；旋转侧不再用 IMU 过程模型，也不联合估零偏。
|
||||||
|
- **改成**：
|
||||||
|
- `joint_optimizer.solve_joint_extrinsic` 增加预积分旋转残差：`log(ΔRᵀ · R_X R_B R_Xᵀ)`，按 weight 加权。
|
||||||
|
- 联合变量增加陀螺零偏修正 `δb`：有 `imu` 时按区间 **重预积分**；否则用一阶修正 `ΔR(b+δb)≈ΔR Exp(-δbΔt)`。
|
||||||
|
- `pipeline` 把 `imu`、静止零偏、`δt` 传入 joint；summary 增加 `gyro_bias_rad_s`。
|
||||||
|
- 平移仍受可观性门控；`rotation_only` 时不交付平移。
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档
|
||||||
|
|
||||||
|
- **原本**：`imu_lidar/README.md` 仅模块列表，无逐次改动史。
|
||||||
|
- **改成**：新增本文件 `CHANGELOG.md`；模块说明中补充 `imu_preintegration.py` 与建模步骤描述。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 模板（以后追加用）
|
||||||
|
|
||||||
|
```markdown
|
||||||
|
## YYYY-MM-DD HH:MM (UTC+8)
|
||||||
|
|
||||||
|
### 标题
|
||||||
|
|
||||||
|
- **原本**：...
|
||||||
|
- **改成**：...
|
||||||
|
```
|
||||||
|
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""LiDAR–IMU calibration package (V1 runnable pipeline)."""
|
||||||
|
|
||||||
|
from .contracts import CalibrationMode, CalibrationStatus, TransformConvention
|
||||||
|
|
||||||
|
__all__ = ["CalibrationMode", "CalibrationStatus", "TransformConvention"]
|
||||||
@@ -0,0 +1,345 @@
|
|||||||
|
"""Command-line entry point for LiDAR–IMU calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
from datetime import datetime
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from .contracts import CalibrationMode, CalibrationRequest, CalibrationStatus, SessionInput
|
||||||
|
from .phase_a_replay import run_phase_a_replay
|
||||||
|
from .pipeline import describe_pipeline, run_calibration
|
||||||
|
|
||||||
|
|
||||||
|
def _format_progress_value(value: Any) -> str:
|
||||||
|
if isinstance(value, float):
|
||||||
|
return f"{value:.3f}"
|
||||||
|
if isinstance(value, (list, tuple, set)):
|
||||||
|
return "[" + ",".join(str(item) for item in value) + "]"
|
||||||
|
return str(value)
|
||||||
|
|
||||||
|
|
||||||
|
def _print_progress(event: dict[str, Any]) -> None:
|
||||||
|
"""Print one compact, immediately flushed progress line."""
|
||||||
|
|
||||||
|
timestamp = datetime.now().strftime("%H:%M:%S")
|
||||||
|
stage_index = event.get("stage_index", "?")
|
||||||
|
stage_total = event.get("stage_total", "?")
|
||||||
|
stage_name = event.get("stage", "unknown")
|
||||||
|
message = event.get("event", "progress")
|
||||||
|
fields = " ".join(
|
||||||
|
f"{key}={_format_progress_value(value)}"
|
||||||
|
for key, value in event.items()
|
||||||
|
if key not in {"stage_index", "stage_total", "stage", "event"}
|
||||||
|
and value is not None
|
||||||
|
)
|
||||||
|
suffix = f" | {fields}" if fields else ""
|
||||||
|
print(
|
||||||
|
f"[{timestamp}] [stage {stage_index}/{stage_total} {stage_name}] {message}{suffix}",
|
||||||
|
flush=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _parse_session_imu_specs(
|
||||||
|
specs: list[str] | None,
|
||||||
|
) -> dict[str, Path]:
|
||||||
|
result: dict[str, Path] = {}
|
||||||
|
for spec in specs or []:
|
||||||
|
if "=" not in spec:
|
||||||
|
raise SystemExit(
|
||||||
|
"--session-imu must use SESSION_ID=PATH syntax"
|
||||||
|
)
|
||||||
|
session_id, raw_path = spec.split("=", 1)
|
||||||
|
session_id = session_id.strip()
|
||||||
|
if not session_id or not raw_path.strip():
|
||||||
|
raise SystemExit(
|
||||||
|
"--session-imu must use non-empty SESSION_ID=PATH"
|
||||||
|
)
|
||||||
|
if session_id in result:
|
||||||
|
raise SystemExit(
|
||||||
|
f"duplicate --session-imu for {session_id}"
|
||||||
|
)
|
||||||
|
result[session_id] = Path(raw_path.strip())
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def _print_phase_a_progress(
|
||||||
|
event: str,
|
||||||
|
fields: dict[str, Any],
|
||||||
|
) -> None:
|
||||||
|
_print_progress(
|
||||||
|
{
|
||||||
|
"stage_index": "A",
|
||||||
|
"stage_total": "A",
|
||||||
|
"stage": "phase_a_replay",
|
||||||
|
"event": event,
|
||||||
|
**fields,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def build_parser() -> argparse.ArgumentParser:
|
||||||
|
parser = argparse.ArgumentParser(description="LiDAR–IMU extrinsic calibration (V1)")
|
||||||
|
subcommands = parser.add_subparsers(dest="command", required=True)
|
||||||
|
|
||||||
|
plan = subcommands.add_parser("plan", help="显示标定阶段，不读取数据")
|
||||||
|
plan.add_argument("--vehicle-config", help="车辆配置路径（仅展示，plan 不读取）")
|
||||||
|
plan.add_argument(
|
||||||
|
"--mode",
|
||||||
|
choices=[mode.value for mode in CalibrationMode],
|
||||||
|
default=CalibrationMode.ROTATION_ONLY.value,
|
||||||
|
)
|
||||||
|
|
||||||
|
run = subcommands.add_parser(
|
||||||
|
"run",
|
||||||
|
help="执行 V1 标定流水线（可重复 --imu/--lidar/--session-id 做多会话联合）",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--session-id",
|
||||||
|
action="append",
|
||||||
|
default=None,
|
||||||
|
help="会话 ID（可重复；与 --imu/--lidar 一一对应）",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--imu",
|
||||||
|
action="append",
|
||||||
|
required=True,
|
||||||
|
help="IMU CSV/NPZ 路径（可重复）",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--lidar",
|
||||||
|
action="append",
|
||||||
|
required=True,
|
||||||
|
help="LiDAR 会话目录（可重复）",
|
||||||
|
)
|
||||||
|
run.add_argument("--vehicle-config", required=True, help="车辆配置 YAML")
|
||||||
|
run.add_argument("--output", required=True, help="输出目录")
|
||||||
|
run.add_argument(
|
||||||
|
"--mode",
|
||||||
|
choices=[mode.value for mode in CalibrationMode],
|
||||||
|
default=CalibrationMode.ROTATION_ONLY.value,
|
||||||
|
)
|
||||||
|
run.add_argument("--max-iterations", type=int, default=2)
|
||||||
|
run.add_argument("--time-offset-search-s", type=float, default=1.0)
|
||||||
|
run.add_argument(
|
||||||
|
"--fixed-time-offset-s",
|
||||||
|
type=float,
|
||||||
|
default=None,
|
||||||
|
help="Skip |ω| δt search and use this constant (use 0 after host-UTC bridge)",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--session-time-offset-s",
|
||||||
|
action="append",
|
||||||
|
type=float,
|
||||||
|
default=None,
|
||||||
|
help="Per-session fixed time offset; repeat once per --imu/--lidar input",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--no-signed-time-refine",
|
||||||
|
action="store_true",
|
||||||
|
help="Disable signed 3-axis δt refine after hand-eye (recommended for host-bridged data)",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--max-signed-refine-shift-s",
|
||||||
|
type=float,
|
||||||
|
default=0.05,
|
||||||
|
help="Max |Δδt| accepted by signed refine from the coarse estimate",
|
||||||
|
)
|
||||||
|
run.add_argument("--min-pair-rotation-deg", type=float, default=3.0)
|
||||||
|
run.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
||||||
|
run.add_argument("--min-registration-fitness", type=float, default=0.5)
|
||||||
|
run.add_argument("--max-imu-gap-s", type=float, default=0.05)
|
||||||
|
run.add_argument("--max-lidar-gap-s", type=float, default=1.0)
|
||||||
|
|
||||||
|
replay = subcommands.add_parser(
|
||||||
|
"phase-a-replay",
|
||||||
|
help="Replay Phase-A from cached motion pairs without rerunning GICP",
|
||||||
|
)
|
||||||
|
replay.add_argument("--motion-pairs", type=Path, required=True)
|
||||||
|
replay.add_argument("--vehicle-config", type=Path, required=True)
|
||||||
|
replay.add_argument("--output", type=Path, required=True)
|
||||||
|
replay.add_argument(
|
||||||
|
"--session-imu",
|
||||||
|
action="append",
|
||||||
|
default=None,
|
||||||
|
metavar="SESSION_ID=PATH",
|
||||||
|
help="Raw IMU mapping used only when cache lacks J_bg/cov",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--exclude-session",
|
||||||
|
action="append",
|
||||||
|
default=None,
|
||||||
|
help="Session ID to exclude; may be repeated",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--strong-rotation-min-deg",
|
||||||
|
type=float,
|
||||||
|
default=1.0,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--decorrelation-block-s",
|
||||||
|
type=float,
|
||||||
|
default=3.0,
|
||||||
|
help="Per-session time-block length used to decorrelate factors",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--max-pairs-per-block",
|
||||||
|
type=int,
|
||||||
|
default=1,
|
||||||
|
help="Maximum factors kept in each decorrelation block",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--bias-prior-sigma-rad-s",
|
||||||
|
type=float,
|
||||||
|
default=0.002,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--yaw-std-max-deg",
|
||||||
|
type=float,
|
||||||
|
default=0.5,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--loo-yaw-range-max-deg",
|
||||||
|
type=float,
|
||||||
|
default=1.0,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--data-prior-difference-max-deg",
|
||||||
|
type=float,
|
||||||
|
default=1.0,
|
||||||
|
)
|
||||||
|
replay.add_argument("--max-nfev", type=int, default=200)
|
||||||
|
|
||||||
|
return parser
|
||||||
|
|
||||||
|
|
||||||
|
def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]:
|
||||||
|
imus = [Path(p) for p in args.imu]
|
||||||
|
lidars = [Path(p) for p in args.lidar]
|
||||||
|
if len(imus) != len(lidars):
|
||||||
|
raise SystemExit(f"--imu count ({len(imus)}) must match --lidar count ({len(lidars)})")
|
||||||
|
if args.session_id is None:
|
||||||
|
session_ids = [f"session{i}" for i in range(len(imus))]
|
||||||
|
else:
|
||||||
|
session_ids = list(args.session_id)
|
||||||
|
if len(session_ids) != len(imus):
|
||||||
|
raise SystemExit(
|
||||||
|
f"--session-id count ({len(session_ids)}) must match --imu/--lidar ({len(imus)})"
|
||||||
|
)
|
||||||
|
if args.session_time_offset_s is None:
|
||||||
|
session_offsets: list[float | None] = [None] * len(imus)
|
||||||
|
else:
|
||||||
|
session_offsets = list(args.session_time_offset_s)
|
||||||
|
if len(session_offsets) != len(imus):
|
||||||
|
raise SystemExit(
|
||||||
|
f"--session-time-offset-s count ({len(session_offsets)}) must match "
|
||||||
|
f"--imu/--lidar ({len(imus)})"
|
||||||
|
)
|
||||||
|
return tuple(
|
||||||
|
SessionInput(
|
||||||
|
session_id=sid,
|
||||||
|
imu_source=imu,
|
||||||
|
lidar_source=lidar,
|
||||||
|
fixed_time_offset_s=offset,
|
||||||
|
)
|
||||||
|
for sid, imu, lidar, offset in zip(session_ids, imus, lidars, session_offsets)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def main(argv: list[str] | None = None) -> int:
|
||||||
|
parser = build_parser()
|
||||||
|
args = parser.parse_args(argv)
|
||||||
|
|
||||||
|
if args.command == "plan":
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=Path(args.vehicle_config) if args.vehicle_config else None,
|
||||||
|
requested_mode=CalibrationMode(args.mode),
|
||||||
|
)
|
||||||
|
print("LiDAR–IMU calibration stages:")
|
||||||
|
print(f"requested mode: {request.requested_mode.value}")
|
||||||
|
for index, stage in enumerate(describe_pipeline(request), start=1):
|
||||||
|
print(f"{index}. {stage.name}: {stage.responsibility}")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
if args.command == "phase-a-replay":
|
||||||
|
summary = run_phase_a_replay(
|
||||||
|
motion_pairs_path=args.motion_pairs,
|
||||||
|
vehicle_config_path=args.vehicle_config,
|
||||||
|
output_directory=args.output,
|
||||||
|
imu_paths_by_session=_parse_session_imu_specs(
|
||||||
|
args.session_imu
|
||||||
|
),
|
||||||
|
excluded_sessions=set(args.exclude_session or []),
|
||||||
|
strong_rotation_min_deg=args.strong_rotation_min_deg,
|
||||||
|
decorrelation_block_s=args.decorrelation_block_s,
|
||||||
|
max_pairs_per_block=args.max_pairs_per_block,
|
||||||
|
bias_prior_sigma_rad_s=args.bias_prior_sigma_rad_s,
|
||||||
|
yaw_std_max_deg=args.yaw_std_max_deg,
|
||||||
|
leave_one_out_yaw_range_max_deg=(
|
||||||
|
args.loo_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
data_prior_difference_max_deg=(
|
||||||
|
args.data_prior_difference_max_deg
|
||||||
|
),
|
||||||
|
max_nfev=args.max_nfev,
|
||||||
|
progress_callback=_print_phase_a_progress,
|
||||||
|
)
|
||||||
|
print(f"status: {summary['status']}")
|
||||||
|
print(f"acceptance_checks: {summary['acceptance_checks']}")
|
||||||
|
for name, variant in summary["variants"].items():
|
||||||
|
print(
|
||||||
|
f"{name}: rpy_deg_xyz={variant['rpy_deg_xyz']} "
|
||||||
|
f"RMS={variant['residual_rms_deg']:.6f} "
|
||||||
|
f"P95={variant['residual_p95_deg']:.6f}"
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
"A1 marginalized yaw_std_deg: "
|
||||||
|
f"{summary['marginal_observability_A1']['yaw_std_deg']}"
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
"leave_one_out_yaw_range_deg: "
|
||||||
|
f"{summary['leave_one_out_yaw_range_deg']}"
|
||||||
|
)
|
||||||
|
print(f"report directory: {args.output}")
|
||||||
|
return 0 if (summary["accepted"] or summary.get("partial_accepted")) else 2
|
||||||
|
|
||||||
|
if args.command == "run":
|
||||||
|
sessions = _build_sessions(args)
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=Path(args.vehicle_config),
|
||||||
|
sessions=sessions,
|
||||||
|
requested_mode=CalibrationMode(args.mode),
|
||||||
|
output_directory=Path(args.output),
|
||||||
|
max_iterations=args.max_iterations,
|
||||||
|
min_pair_rotation_deg=args.min_pair_rotation_deg,
|
||||||
|
min_pair_translation_m=args.min_pair_translation_m,
|
||||||
|
min_registration_fitness=args.min_registration_fitness,
|
||||||
|
max_imu_gap_s=args.max_imu_gap_s,
|
||||||
|
max_lidar_gap_s=args.max_lidar_gap_s,
|
||||||
|
time_offset_search_s=args.time_offset_search_s,
|
||||||
|
fixed_time_offset_s=args.fixed_time_offset_s,
|
||||||
|
enable_signed_time_refine=not args.no_signed_time_refine,
|
||||||
|
max_signed_refine_shift_s=args.max_signed_refine_shift_s,
|
||||||
|
)
|
||||||
|
result = run_calibration(request, progress_callback=_print_progress)
|
||||||
|
print(f"status: {result.status.value}")
|
||||||
|
print(f"message: {result.message}")
|
||||||
|
if result.time_offset_s is not None:
|
||||||
|
print(f"time_offset_s (first session; t_imu = t_lidar + dt): {result.time_offset_s:.6f}")
|
||||||
|
joint = (result.details or {}).get("joint") or {}
|
||||||
|
if joint:
|
||||||
|
print(f"merged_pair_count: {joint.get('merged_pair_count')}")
|
||||||
|
print(f"pair_counts_per_session: {joint.get('pair_counts_per_session')}")
|
||||||
|
if result.T_IMU_lidar is not None:
|
||||||
|
print("T_IMU_lidar:")
|
||||||
|
print(result.T_IMU_lidar)
|
||||||
|
print(f"report directory: {args.output}")
|
||||||
|
return 0 if result.status != CalibrationStatus.BLOCKED else 2
|
||||||
|
|
||||||
|
parser.error(f"unknown command {args.command}")
|
||||||
|
return 2
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,130 @@
|
|||||||
|
"""Shared contracts for the LiDAR–IMU calibration pipeline."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from enum import Enum
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
class TransformConvention(str, Enum):
|
||||||
|
"""The only transform convention used by this project."""
|
||||||
|
|
||||||
|
T_A_B = "T_A_B maps points from frame B into frame A"
|
||||||
|
|
||||||
|
|
||||||
|
class CalibrationMode(str, Enum):
|
||||||
|
ROTATION_ONLY = "rotation_only"
|
||||||
|
FULL_SE3 = "full_se3"
|
||||||
|
|
||||||
|
|
||||||
|
class CalibrationStatus(str, Enum):
|
||||||
|
NOT_RUN = "not_run"
|
||||||
|
BLOCKED = "blocked"
|
||||||
|
ROTATION_ONLY_ACCEPTED = "rotation_only_accepted"
|
||||||
|
ROTATION_ONLY_PRIOR_CONSTRAINED = "rotation_only_prior_constrained"
|
||||||
|
FULL_SE3_ACCEPTED = "full_se3_accepted"
|
||||||
|
FULL_SE3_REJECTED = "full_se3_rejected_due_to_observability"
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class SessionInput:
|
||||||
|
"""Input paths for one independently recorded session."""
|
||||||
|
|
||||||
|
session_id: str
|
||||||
|
imu_source: Path
|
||||||
|
lidar_source: Path
|
||||||
|
board_configuration_id: str | None = None
|
||||||
|
# Optional session-local override. The request-level value remains a
|
||||||
|
# backward-compatible fallback for batches whose timelines are all aligned.
|
||||||
|
fixed_time_offset_s: float | None = None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CalibrationRequest:
|
||||||
|
"""Top-level calibration request."""
|
||||||
|
|
||||||
|
vehicle_config: Path | None
|
||||||
|
sessions: tuple[SessionInput, ...] = ()
|
||||||
|
requested_mode: CalibrationMode = CalibrationMode.ROTATION_ONLY
|
||||||
|
output_directory: Path | None = None
|
||||||
|
max_iterations: int = 2
|
||||||
|
min_pair_rotation_deg: float = 3.0
|
||||||
|
min_pair_translation_m: float = 0.3
|
||||||
|
min_registration_fitness: float = 0.5
|
||||||
|
max_imu_gap_s: float = 0.05
|
||||||
|
max_lidar_gap_s: float = 1.0
|
||||||
|
time_offset_search_s: float = 1.0
|
||||||
|
# If set, skip |ω| search and use this constant (host-UTC-bridged sessions: 0).
|
||||||
|
fixed_time_offset_s: float | None = None
|
||||||
|
# Signed 3-axis refine after hand-eye; disable for already-bridged timelines.
|
||||||
|
enable_signed_time_refine: bool = True
|
||||||
|
# Reject signed refine steps that walk farther than this from the coarse δt.
|
||||||
|
max_signed_refine_shift_s: float = 0.05
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class CalibrationResult:
|
||||||
|
"""Result envelope written by finalize after pipeline gates."""
|
||||||
|
|
||||||
|
status: CalibrationStatus = CalibrationStatus.NOT_RUN
|
||||||
|
message: str = "Calibration has not been executed."
|
||||||
|
details: dict[str, Any] = field(default_factory=dict)
|
||||||
|
T_IMU_lidar: np.ndarray | None = None
|
||||||
|
time_offset_s: float | None = None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuSeries:
|
||||||
|
"""Normalized IMU samples.
|
||||||
|
|
||||||
|
``t_s`` is the native IMU clock in seconds (need not match LiDAR epoch).
|
||||||
|
Gyro must be rad/s; accelerometer must be m/s^2.
|
||||||
|
"""
|
||||||
|
|
||||||
|
t_s: np.ndarray
|
||||||
|
gyro_rad_s: np.ndarray
|
||||||
|
acc_m_s2: np.ndarray
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
object.__setattr__(self, "t_s", np.asarray(self.t_s, dtype=float).reshape(-1))
|
||||||
|
object.__setattr__(self, "gyro_rad_s", np.asarray(self.gyro_rad_s, dtype=float).reshape(-1, 3))
|
||||||
|
object.__setattr__(self, "acc_m_s2", np.asarray(self.acc_m_s2, dtype=float).reshape(-1, 3))
|
||||||
|
n = self.t_s.size
|
||||||
|
if self.gyro_rad_s.shape != (n, 3) or self.acc_m_s2.shape != (n, 3):
|
||||||
|
raise ValueError("IMU arrays must share the same length and have shape (N, 3)")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class LidarFrame:
|
||||||
|
"""One LiDAR sweep in Cartesian sensor coordinates."""
|
||||||
|
|
||||||
|
frame_id: str
|
||||||
|
t_start_s: float
|
||||||
|
t_end_s: float
|
||||||
|
points_xyz: np.ndarray
|
||||||
|
path: Path | None = None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def t_mid_s(self) -> float:
|
||||||
|
return 0.5 * (self.t_start_s + self.t_end_s)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MotionPair:
|
||||||
|
"""One relative-motion observation between keyframes i and j."""
|
||||||
|
|
||||||
|
session_id: str
|
||||||
|
i: int
|
||||||
|
j: int
|
||||||
|
t_i_s: float
|
||||||
|
t_j_s: float
|
||||||
|
R_A: np.ndarray
|
||||||
|
R_B: np.ndarray
|
||||||
|
t_A_m: np.ndarray | None = None
|
||||||
|
t_B_m: np.ndarray | None = None
|
||||||
|
fitness: float = 0.0
|
||||||
|
metadata: dict[str, Any] = field(default_factory=dict)
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
"""Package calibration outputs as JSON-friendly artifacts."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import CalibrationResult, CalibrationStatus
|
||||||
|
from .geometry import rotation_matrix_to_quaternion_xyzw, rpy_deg_xyz
|
||||||
|
|
||||||
|
|
||||||
|
def _to_serializable(value: Any) -> Any:
|
||||||
|
if isinstance(value, np.ndarray):
|
||||||
|
return value.tolist()
|
||||||
|
if isinstance(value, (np.floating, np.integer, np.bool_)):
|
||||||
|
return value.item()
|
||||||
|
if isinstance(value, Path):
|
||||||
|
return str(value)
|
||||||
|
if isinstance(value, dict):
|
||||||
|
return {str(k): _to_serializable(v) for k, v in value.items()}
|
||||||
|
if isinstance(value, (list, tuple)):
|
||||||
|
return [_to_serializable(v) for v in value]
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def finalize_result(
|
||||||
|
*,
|
||||||
|
status: CalibrationStatus,
|
||||||
|
message: str,
|
||||||
|
details: dict[str, Any],
|
||||||
|
T_IMU_lidar: np.ndarray | None = None,
|
||||||
|
time_offset_s: float | None = None,
|
||||||
|
output_directory: Path | None = None,
|
||||||
|
motion_pairs_payload: dict[str, Any] | None = None,
|
||||||
|
) -> CalibrationResult:
|
||||||
|
"""Build the result envelope and optionally write report files."""
|
||||||
|
|
||||||
|
result = CalibrationResult(
|
||||||
|
status=status,
|
||||||
|
message=message,
|
||||||
|
details=_to_serializable(details),
|
||||||
|
T_IMU_lidar=None if T_IMU_lidar is None else np.asarray(T_IMU_lidar, dtype=float),
|
||||||
|
time_offset_s=time_offset_s,
|
||||||
|
)
|
||||||
|
|
||||||
|
if output_directory is not None:
|
||||||
|
output_directory = Path(output_directory)
|
||||||
|
output_directory.mkdir(parents=True, exist_ok=True)
|
||||||
|
summary = {
|
||||||
|
"status": status.value,
|
||||||
|
"message": message,
|
||||||
|
"time_offset_s": time_offset_s,
|
||||||
|
"details": result.details,
|
||||||
|
}
|
||||||
|
if result.T_IMU_lidar is not None:
|
||||||
|
t = result.T_IMU_lidar
|
||||||
|
summary["T_IMU_lidar"] = {
|
||||||
|
"matrix": t.tolist(),
|
||||||
|
"translation_m": t[:3, 3].tolist(),
|
||||||
|
"rotation_quaternion_xyzw": rotation_matrix_to_quaternion_xyzw(t[:3, :3]).tolist(),
|
||||||
|
"rpy_deg_xyz": rpy_deg_xyz(t[:3, :3]).tolist(),
|
||||||
|
"convention": "p_IMU = T_IMU_lidar * p_lidar",
|
||||||
|
}
|
||||||
|
(output_directory / "T_IMU_lidar.json").write_text(
|
||||||
|
json.dumps(summary["T_IMU_lidar"], indent=2),
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
if time_offset_s is not None:
|
||||||
|
(output_directory / "time_offset.json").write_text(
|
||||||
|
json.dumps({"delta_t_s": time_offset_s, "definition": "t_imu = t_lidar + delta_t"}, indent=2),
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
if motion_pairs_payload is not None:
|
||||||
|
from .motion_pairs_io import save_motion_pairs
|
||||||
|
|
||||||
|
save_motion_pairs(output_directory / "motion_pairs.json", motion_pairs_payload)
|
||||||
|
summary["motion_pairs_file"] = "motion_pairs.json"
|
||||||
|
(output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
|
||||||
|
return result
|
||||||
@@ -0,0 +1,207 @@
|
|||||||
|
"""SE(3)/SO(3) utilities for LiDAR–IMU calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
def skew(vector: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return the skew-symmetric matrix such that ``skew(v) @ w == v x w``."""
|
||||||
|
|
||||||
|
x, y, z = np.asarray(vector, dtype=float).reshape(3)
|
||||||
|
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def so3_exp(rotation_vector: np.ndarray) -> np.ndarray:
|
||||||
|
"""Map a rotation vector in radians onto SO(3)."""
|
||||||
|
|
||||||
|
vector = np.asarray(rotation_vector, dtype=float).reshape(3)
|
||||||
|
angle = float(np.linalg.norm(vector))
|
||||||
|
if angle < 1e-12:
|
||||||
|
return np.eye(3) + skew(vector)
|
||||||
|
axis_cross = skew(vector / angle)
|
||||||
|
return np.eye(3) + math.sin(angle) * axis_cross + (1.0 - math.cos(angle)) * axis_cross @ axis_cross
|
||||||
|
|
||||||
|
|
||||||
|
def so3_log(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Map an SO(3) matrix to a rotation vector in radians."""
|
||||||
|
|
||||||
|
rotation = np.asarray(rotation, dtype=float).reshape(3, 3)
|
||||||
|
cos_angle = float(np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0))
|
||||||
|
angle = math.acos(cos_angle)
|
||||||
|
if angle < 1e-12:
|
||||||
|
return 0.5 * np.array(
|
||||||
|
[
|
||||||
|
rotation[2, 1] - rotation[1, 2],
|
||||||
|
rotation[0, 2] - rotation[2, 0],
|
||||||
|
rotation[1, 0] - rotation[0, 1],
|
||||||
|
],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
if abs(angle - math.pi) < 1e-6:
|
||||||
|
# Near 180°: use eigenvector of the +1 eigenvalue.
|
||||||
|
eigvals, eigvecs = np.linalg.eigh(0.5 * (rotation + rotation.T))
|
||||||
|
axis = eigvecs[:, int(np.argmax(eigvals))]
|
||||||
|
return axis * angle
|
||||||
|
return (
|
||||||
|
0.5
|
||||||
|
* angle
|
||||||
|
/ math.sin(angle)
|
||||||
|
* np.array(
|
||||||
|
[
|
||||||
|
rotation[2, 1] - rotation[1, 2],
|
||||||
|
rotation[0, 2] - rotation[2, 0],
|
||||||
|
rotation[1, 0] - rotation[0, 1],
|
||||||
|
],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def rotation_angle_deg(rotation: np.ndarray) -> float:
|
||||||
|
"""Return the rotation angle in degrees."""
|
||||||
|
|
||||||
|
return float(np.degrees(np.linalg.norm(so3_log(rotation))))
|
||||||
|
|
||||||
|
|
||||||
|
def inverse_transform(transform: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return the inverse of a rigid 4x4 transform."""
|
||||||
|
|
||||||
|
transform = np.asarray(transform, dtype=float)
|
||||||
|
if transform.shape != (4, 4):
|
||||||
|
raise ValueError("a rigid transform must have shape (4, 4)")
|
||||||
|
result = np.eye(4)
|
||||||
|
result[:3, :3] = transform[:3, :3].T
|
||||||
|
result[:3, 3] = -result[:3, :3] @ transform[:3, 3]
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def make_transform(translation_m: np.ndarray, rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Build ``T_A_B`` from its translation and rotation components."""
|
||||||
|
|
||||||
|
translation_m = np.asarray(translation_m, dtype=float).reshape(3)
|
||||||
|
rotation = np.asarray(rotation, dtype=float)
|
||||||
|
if rotation.shape != (3, 3):
|
||||||
|
raise ValueError("a rotation matrix must have shape (3, 3)")
|
||||||
|
result = np.eye(4)
|
||||||
|
result[:3, :3] = rotation
|
||||||
|
result[:3, 3] = translation_m
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def transform_points(points: np.ndarray, transform: np.ndarray) -> np.ndarray:
|
||||||
|
"""Apply ``T_A_B`` to an ``(N, 3)`` point array expressed in frame B."""
|
||||||
|
|
||||||
|
points = np.asarray(points, dtype=float)
|
||||||
|
if points.ndim != 2 or points.shape[1] != 3:
|
||||||
|
raise ValueError("points must have shape (N, 3)")
|
||||||
|
return points @ transform[:3, :3].T + transform[:3, 3]
|
||||||
|
|
||||||
|
|
||||||
|
def orthonormalize_rotation(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Project a near-rotation matrix onto SO(3)."""
|
||||||
|
|
||||||
|
u, _, vt = np.linalg.svd(np.asarray(rotation, dtype=float).reshape(3, 3))
|
||||||
|
result = u @ vt
|
||||||
|
if np.linalg.det(result) < 0:
|
||||||
|
u[:, -1] *= -1
|
||||||
|
result = u @ vt
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def integrate_gyro_rotation(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
bias_rad_s: np.ndarray | None = None,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Integrate gyroscope samples on ``[t0, t1]`` and return ``R(t0<-t1)`` wait.
|
||||||
|
|
||||||
|
Returns ``R_i_j`` that maps vectors from the IMU frame at ``t1`` into the
|
||||||
|
IMU frame at ``t0`` using right-invariant discrete integration:
|
||||||
|
|
||||||
|
R <- R @ Exp(omega * dt)
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
if times_s.size < 2:
|
||||||
|
return np.eye(3)
|
||||||
|
bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
if t1 < t0:
|
||||||
|
raise ValueError("t1 must be >= t0")
|
||||||
|
|
||||||
|
# Include one sample before t0 and after t1 when possible for interpolation.
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return np.eye(3)
|
||||||
|
|
||||||
|
rotation = np.eye(3)
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
omega = 0.5 * (gyro_rad_s[index] + gyro_rad_s[index + 1]) - bias
|
||||||
|
rotation = rotation @ so3_exp(omega * dt)
|
||||||
|
return orthonormalize_rotation(rotation)
|
||||||
|
|
||||||
|
|
||||||
|
def rotation_matrix_to_quaternion_xyzw(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Convert SO(3) to quaternion ``[x, y, z, w]``."""
|
||||||
|
|
||||||
|
rotation = orthonormalize_rotation(rotation)
|
||||||
|
trace = float(np.trace(rotation))
|
||||||
|
if trace > 0:
|
||||||
|
s = math.sqrt(trace + 1.0) * 2.0
|
||||||
|
w = 0.25 * s
|
||||||
|
x = (rotation[2, 1] - rotation[1, 2]) / s
|
||||||
|
y = (rotation[0, 2] - rotation[2, 0]) / s
|
||||||
|
z = (rotation[1, 0] - rotation[0, 1]) / s
|
||||||
|
elif rotation[0, 0] > rotation[1, 1] and rotation[0, 0] > rotation[2, 2]:
|
||||||
|
s = math.sqrt(1.0 + rotation[0, 0] - rotation[1, 1] - rotation[2, 2]) * 2.0
|
||||||
|
w = (rotation[2, 1] - rotation[1, 2]) / s
|
||||||
|
x = 0.25 * s
|
||||||
|
y = (rotation[0, 1] + rotation[1, 0]) / s
|
||||||
|
z = (rotation[0, 2] + rotation[2, 0]) / s
|
||||||
|
elif rotation[1, 1] > rotation[2, 2]:
|
||||||
|
s = math.sqrt(1.0 + rotation[1, 1] - rotation[0, 0] - rotation[2, 2]) * 2.0
|
||||||
|
w = (rotation[0, 2] - rotation[2, 0]) / s
|
||||||
|
x = (rotation[0, 1] + rotation[1, 0]) / s
|
||||||
|
y = 0.25 * s
|
||||||
|
z = (rotation[1, 2] + rotation[2, 1]) / s
|
||||||
|
else:
|
||||||
|
s = math.sqrt(1.0 + rotation[2, 2] - rotation[0, 0] - rotation[1, 1]) * 2.0
|
||||||
|
w = (rotation[1, 0] - rotation[0, 1]) / s
|
||||||
|
x = (rotation[0, 2] + rotation[2, 0]) / s
|
||||||
|
y = (rotation[1, 2] + rotation[2, 1]) / s
|
||||||
|
z = 0.25 * s
|
||||||
|
return np.array([x, y, z, w], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def rpy_deg_xyz(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Intrinsic XYZ Euler angles in degrees from a rotation matrix."""
|
||||||
|
|
||||||
|
rotation = orthonormalize_rotation(rotation)
|
||||||
|
sy = math.sqrt(rotation[0, 0] ** 2 + rotation[1, 0] ** 2)
|
||||||
|
if sy > 1e-8:
|
||||||
|
roll = math.atan2(rotation[2, 1], rotation[2, 2])
|
||||||
|
pitch = math.atan2(-rotation[2, 0], sy)
|
||||||
|
yaw = math.atan2(rotation[1, 0], rotation[0, 0])
|
||||||
|
else:
|
||||||
|
roll = math.atan2(-rotation[1, 2], rotation[1, 1])
|
||||||
|
pitch = math.atan2(-rotation[2, 0], sy)
|
||||||
|
yaw = 0.0
|
||||||
|
return np.degrees(np.array([roll, pitch, yaw], dtype=float))
|
||||||
@@ -0,0 +1,86 @@
|
|||||||
|
"""IMU unit, axis, bias, and saturation audit."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries
|
||||||
|
|
||||||
|
G = 9.80665
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuAuditReport:
|
||||||
|
ok: bool
|
||||||
|
gyro_bias_rad_s: np.ndarray
|
||||||
|
static_acc_mean_m_s2: np.ndarray
|
||||||
|
static_acc_norm_m_s2: float
|
||||||
|
suggested_up_axis: int
|
||||||
|
suggested_up_sign: float
|
||||||
|
static_ratio: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _static_mask(gyro: np.ndarray, acc: np.ndarray) -> np.ndarray:
|
||||||
|
gyro_norm = np.linalg.norm(gyro, axis=1)
|
||||||
|
acc_norm = np.linalg.norm(acc, axis=1)
|
||||||
|
gyro_thr = max(0.02, float(np.percentile(gyro_norm, 20)) * 1.5)
|
||||||
|
acc_thr_low = 0.7 * G
|
||||||
|
acc_thr_high = 1.3 * G
|
||||||
|
return (gyro_norm < gyro_thr) & (acc_norm > acc_thr_low) & (acc_norm < acc_thr_high)
|
||||||
|
|
||||||
|
|
||||||
|
def audit_imu(imu: ImuSeries) -> ImuAuditReport:
|
||||||
|
"""Audit normalized IMU samples and estimate a static gyro bias."""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
mask = _static_mask(imu.gyro_rad_s, imu.acc_m_s2)
|
||||||
|
static_ratio = float(np.mean(mask)) if mask.size else 0.0
|
||||||
|
if static_ratio < 0.02:
|
||||||
|
# Fall back to lowest-gyro percentile window.
|
||||||
|
gyro_norm = np.linalg.norm(imu.gyro_rad_s, axis=1)
|
||||||
|
cutoff = float(np.percentile(gyro_norm, 10))
|
||||||
|
mask = gyro_norm <= cutoff
|
||||||
|
notes.append("few gravity-consistent static samples; using lowest-gyro percentile")
|
||||||
|
static_ratio = float(np.mean(mask))
|
||||||
|
|
||||||
|
if not np.any(mask):
|
||||||
|
notes.append("no static samples found")
|
||||||
|
bias = np.zeros(3)
|
||||||
|
acc_mean = np.zeros(3)
|
||||||
|
acc_norm = 0.0
|
||||||
|
up_axis = 2
|
||||||
|
up_sign = 1.0
|
||||||
|
ok = False
|
||||||
|
else:
|
||||||
|
bias = np.mean(imu.gyro_rad_s[mask], axis=0)
|
||||||
|
acc_mean = np.mean(imu.acc_m_s2[mask], axis=0)
|
||||||
|
acc_norm = float(np.linalg.norm(acc_mean))
|
||||||
|
up_axis = int(np.argmax(np.abs(acc_mean)))
|
||||||
|
up_sign = float(np.sign(acc_mean[up_axis]) or 1.0)
|
||||||
|
if abs(acc_norm - G) > 2.5:
|
||||||
|
notes.append(
|
||||||
|
f"static |acc|={acc_norm:.3f} differs from g={G}; check units (expect m/s^2)"
|
||||||
|
)
|
||||||
|
gyro_peak = float(np.max(np.linalg.norm(imu.gyro_rad_s, axis=1)))
|
||||||
|
if gyro_peak > 20.0:
|
||||||
|
notes.append(
|
||||||
|
f"peak |gyro|={gyro_peak:.1f} rad/s looks extreme; check whether data is deg/s"
|
||||||
|
)
|
||||||
|
ok = abs(acc_norm - G) < 3.5 or static_ratio > 0.05
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"suggested up axis index={up_axis} sign={up_sign:+.0f} (0=x,1=y,2=z)"
|
||||||
|
)
|
||||||
|
return ImuAuditReport(
|
||||||
|
ok=ok,
|
||||||
|
gyro_bias_rad_s=np.asarray(bias, dtype=float),
|
||||||
|
static_acc_mean_m_s2=np.asarray(acc_mean, dtype=float),
|
||||||
|
static_acc_norm_m_s2=float(acc_norm),
|
||||||
|
suggested_up_axis=up_axis,
|
||||||
|
suggested_up_sign=up_sign,
|
||||||
|
static_ratio=static_ratio,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,72 @@
|
|||||||
|
"""IMU adapters for the V1 standard intermediate format.
|
||||||
|
|
||||||
|
Accepted inputs
|
||||||
|
---------------
|
||||||
|
1. CSV with header:
|
||||||
|
t,gx,gy,gz,ax,ay,az
|
||||||
|
- ``t`` in seconds on the IMU clock
|
||||||
|
- gyro in rad/s
|
||||||
|
- accel in m/s^2
|
||||||
|
|
||||||
|
2. NPZ with arrays:
|
||||||
|
t, gyro, acc
|
||||||
|
shapes: (N,), (N,3), (N,3)
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries
|
||||||
|
|
||||||
|
|
||||||
|
def load_imu_samples(path: Path | str) -> ImuSeries:
|
||||||
|
"""Load normalized IMU samples from CSV or NPZ."""
|
||||||
|
|
||||||
|
source = Path(path)
|
||||||
|
if not source.exists():
|
||||||
|
raise FileNotFoundError(source)
|
||||||
|
if source.suffix.lower() == ".csv":
|
||||||
|
return _load_imu_csv(source)
|
||||||
|
if source.suffix.lower() == ".npz":
|
||||||
|
return _load_imu_npz(source)
|
||||||
|
raise ValueError(f"unsupported IMU format '{source.suffix}' (use .csv or .npz)")
|
||||||
|
|
||||||
|
|
||||||
|
def _load_imu_csv(path: Path) -> ImuSeries:
|
||||||
|
data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
|
||||||
|
if data.ndim == 0:
|
||||||
|
data = np.array([data])
|
||||||
|
names = set(data.dtype.names or ())
|
||||||
|
required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
|
||||||
|
if not required.issubset(names):
|
||||||
|
raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
|
||||||
|
t = np.asarray(data["t"], dtype=float).reshape(-1)
|
||||||
|
gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
|
||||||
|
acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
|
||||||
|
order = np.argsort(t)
|
||||||
|
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||||
|
|
||||||
|
|
||||||
|
def _load_imu_npz(path: Path) -> ImuSeries:
|
||||||
|
with np.load(path) as payload:
|
||||||
|
keys = set(payload.files)
|
||||||
|
if not {"t", "gyro", "acc"}.issubset(keys):
|
||||||
|
raise ValueError(f"IMU NPZ must contain t, gyro, acc; got {sorted(keys)}")
|
||||||
|
t = np.asarray(payload["t"], dtype=float).reshape(-1)
|
||||||
|
gyro = np.asarray(payload["gyro"], dtype=float).reshape(-1, 3)
|
||||||
|
acc = np.asarray(payload["acc"], dtype=float).reshape(-1, 3)
|
||||||
|
order = np.argsort(t)
|
||||||
|
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||||
|
|
||||||
|
|
||||||
|
def save_imu_csv(path: Path | str, imu: ImuSeries) -> None:
|
||||||
|
"""Write IMU samples to the standard CSV format."""
|
||||||
|
|
||||||
|
destination = Path(path)
|
||||||
|
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
array = np.column_stack([imu.t_s, imu.gyro_rad_s, imu.acc_m_s2])
|
||||||
|
header = "t,gx,gy,gz,ax,ay,az"
|
||||||
|
np.savetxt(destination, array, delimiter=",", header=header, comments="")
|
||||||
@@ -0,0 +1,452 @@
|
|||||||
|
"""Frame-to-frame IMU preintegration (Phase-A rotation + Phase-C full factor).
|
||||||
|
|
||||||
|
Phase-A: ``ΔR``, 3×3 ``Σ``, ``J_bg``.
|
||||||
|
Phase-C: ``ΔR/Δv/Δp``, 9×9 ``Σ`` (with bias RW process noise), ``J_bg``/``J_ba``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .geometry import orthonormalize_rotation, so3_exp, so3_log, skew
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class GyroPreintegration:
|
||||||
|
"""Rotation-only preintegration on ``[t0, t1]`` (IMU clock)."""
|
||||||
|
|
||||||
|
delta_R: np.ndarray
|
||||||
|
duration_s: float
|
||||||
|
mean_gyro_norm: float
|
||||||
|
sigma_rad: float
|
||||||
|
weight: float
|
||||||
|
bias_rad_s: np.ndarray
|
||||||
|
cov: np.ndarray
|
||||||
|
J_bg: np.ndarray
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuPreintegration:
|
||||||
|
"""Full IMU preintegration on ``[t0, t1]`` (IMU clock).
|
||||||
|
|
||||||
|
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``.
|
||||||
|
``delta_v`` / ``delta_p`` are body-frame increments (no gravity).
|
||||||
|
Error-state order in ``cov`` / Jacobians: ``[δθ, δv, δp]`` (9).
|
||||||
|
``J_bg`` / ``J_ba`` are 9×3: first-order correction w.r.t. constant bias deltas.
|
||||||
|
"""
|
||||||
|
|
||||||
|
delta_R: np.ndarray
|
||||||
|
delta_v: np.ndarray
|
||||||
|
delta_p: np.ndarray
|
||||||
|
duration_s: float
|
||||||
|
mean_gyro_norm: float
|
||||||
|
sigma_rad: float
|
||||||
|
weight: float
|
||||||
|
gyro_bias_rad_s: np.ndarray
|
||||||
|
acc_bias_m_s2: np.ndarray
|
||||||
|
cov: np.ndarray
|
||||||
|
J_bg: np.ndarray
|
||||||
|
J_ba: np.ndarray
|
||||||
|
|
||||||
|
|
||||||
|
def _right_jacobian(phi: np.ndarray) -> np.ndarray:
|
||||||
|
"""SO(3) right Jacobian ``Jr(φ)`` with ``Exp(φ+δ)≈Exp(φ)Exp(Jr δ)``."""
|
||||||
|
|
||||||
|
phi = np.asarray(phi, dtype=float).reshape(3)
|
||||||
|
angle = float(np.linalg.norm(phi))
|
||||||
|
if angle < 1e-8:
|
||||||
|
return np.eye(3) - 0.5 * skew(phi)
|
||||||
|
axis = phi / angle
|
||||||
|
s = skew(axis)
|
||||||
|
return (
|
||||||
|
np.eye(3)
|
||||||
|
- ((1.0 - np.cos(angle)) / angle) * s
|
||||||
|
+ ((angle - np.sin(angle)) / angle) * (s @ s)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _interp_vec(times_s: np.ndarray, values: np.ndarray, t: float) -> np.ndarray:
|
||||||
|
"""Linear interpolate a 3-vector series at an exact time."""
|
||||||
|
|
||||||
|
return np.array(
|
||||||
|
[float(np.interp(t, times_s, values[:, axis])) for axis in range(3)],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _interp_gyro(times_s: np.ndarray, gyro_rad_s: np.ndarray, t: float) -> np.ndarray:
|
||||||
|
"""Linear interpolate gyro at an exact time."""
|
||||||
|
|
||||||
|
return _interp_vec(times_s, gyro_rad_s, t)
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_weight(duration_s: float, mean_gyro_norm: float, cov_trace: float) -> float:
|
||||||
|
"""Larger weight for short, excited, low-covariance intervals."""
|
||||||
|
|
||||||
|
duration_term = 1.0 / max(duration_s, 0.05)
|
||||||
|
excite_term = min(max(mean_gyro_norm, 1e-3), 1.0)
|
||||||
|
avg_var = max(cov_trace / 3.0, 1e-8)
|
||||||
|
return float(duration_term * excite_term / avg_var)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegrate_gyro(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
bias_rad_s: np.ndarray | None = None,
|
||||||
|
*,
|
||||||
|
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||||
|
) -> GyroPreintegration:
|
||||||
|
"""Discrete mid-point gyro preintegration with exact endpoints.
|
||||||
|
|
||||||
|
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``
|
||||||
|
via right-invariant updates ``ΔR ← ΔR Exp((ω-b) dt)``.
|
||||||
|
|
||||||
|
Also returns:
|
||||||
|
- ``cov``: 3×3 covariance of the right tangent noise on ``ΔR``
|
||||||
|
- ``J_bg``: ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
|
||||||
|
empty = GyroPreintegration(
|
||||||
|
delta_R=np.eye(3),
|
||||||
|
duration_s=0.0,
|
||||||
|
mean_gyro_norm=0.0,
|
||||||
|
sigma_rad=1e3,
|
||||||
|
weight=1e-6,
|
||||||
|
bias_rad_s=bias.copy(),
|
||||||
|
cov=np.eye(3) * 1e6,
|
||||||
|
J_bg=np.zeros((3, 3)),
|
||||||
|
)
|
||||||
|
if times_s.size < 2 or duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||||
|
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
if duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
delta_r = np.eye(3)
|
||||||
|
j_bg = np.zeros((3, 3))
|
||||||
|
cov = np.zeros((3, 3))
|
||||||
|
sigma2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||||
|
gyro_norms: list[float] = []
|
||||||
|
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Exact endpoint gyro via linear interpolation inside the sample interval.
|
||||||
|
g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
|
||||||
|
g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
|
||||||
|
omega = 0.5 * (g_a + g_b) - bias
|
||||||
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|
||||||
|
theta = omega * dt
|
||||||
|
jr = _right_jacobian(theta)
|
||||||
|
a_mat = so3_exp(-theta)
|
||||||
|
j_bg = a_mat @ j_bg - jr * dt
|
||||||
|
cov = a_mat @ cov @ a_mat.T + jr @ (sigma2 * dt * np.eye(3)) @ jr.T
|
||||||
|
delta_r = delta_r @ so3_exp(theta)
|
||||||
|
|
||||||
|
delta_r = orthonormalize_rotation(delta_r)
|
||||||
|
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||||
|
cov = 0.5 * (cov + cov.T)
|
||||||
|
cov = cov + np.eye(3) * 1e-12
|
||||||
|
if mean_gyro_norm < 0.02:
|
||||||
|
cov = cov * 4.0
|
||||||
|
cov_trace = float(np.trace(cov))
|
||||||
|
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||||
|
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||||
|
return GyroPreintegration(
|
||||||
|
delta_R=delta_r,
|
||||||
|
duration_s=duration,
|
||||||
|
mean_gyro_norm=mean_gyro_norm,
|
||||||
|
sigma_rad=sigma_rad,
|
||||||
|
weight=weight,
|
||||||
|
bias_rad_s=bias.copy(),
|
||||||
|
cov=cov,
|
||||||
|
J_bg=np.asarray(j_bg, dtype=float),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegrate_imu(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
acc_m_s2: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
acc_bias_m_s2: np.ndarray | None = None,
|
||||||
|
*,
|
||||||
|
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||||
|
sigma_a_m_s2_sqrt_hz: float = 2.0e-2,
|
||||||
|
sigma_bg_rw_rad_s_sqrt_hz: float = 1.0e-5,
|
||||||
|
sigma_ba_rw_m_s2_sqrt_hz: float = 1.0e-3,
|
||||||
|
) -> ImuPreintegration:
|
||||||
|
"""Mid-point IMU preintegration with exact endpoints and bias-RW noise.
|
||||||
|
|
||||||
|
Discrete updates (right-invariant)::
|
||||||
|
|
||||||
|
ΔR ← ΔR Exp((ω-bg) dt)
|
||||||
|
Δv ← Δv + ΔR (a-ba) dt
|
||||||
|
Δp ← Δp + Δv_old dt + 0.5 ΔR (a-ba) dt²
|
||||||
|
|
||||||
|
Propagates a 15-DoF error state ``[δθ, δv, δp, δbg, δba]`` then returns the
|
||||||
|
top-left 9×9 covariance (bias RW already folded in) and 9×3 Jacobians.
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
acc_m_s2 = np.asarray(acc_m_s2, dtype=float).reshape(-1, 3)
|
||||||
|
bg = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
ba = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
empty = ImuPreintegration(
|
||||||
|
delta_R=np.eye(3),
|
||||||
|
delta_v=np.zeros(3),
|
||||||
|
delta_p=np.zeros(3),
|
||||||
|
duration_s=0.0,
|
||||||
|
mean_gyro_norm=0.0,
|
||||||
|
sigma_rad=1e3,
|
||||||
|
weight=1e-6,
|
||||||
|
gyro_bias_rad_s=bg.copy(),
|
||||||
|
acc_bias_m_s2=ba.copy(),
|
||||||
|
cov=np.eye(9) * 1e6,
|
||||||
|
J_bg=np.zeros((9, 3)),
|
||||||
|
J_ba=np.zeros((9, 3)),
|
||||||
|
)
|
||||||
|
if times_s.size < 2 or acc_m_s2.shape != gyro_rad_s.shape:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||||
|
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
if duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
delta_r = np.eye(3)
|
||||||
|
delta_v = np.zeros(3)
|
||||||
|
delta_p = np.zeros(3)
|
||||||
|
# Jacobians of [δθ, δv, δp] w.r.t. constant bias (accumulated analytically).
|
||||||
|
j_bg = np.zeros((9, 3))
|
||||||
|
j_ba = np.zeros((9, 3))
|
||||||
|
# 15×15 covariance: [θ, v, p, bg, ba]
|
||||||
|
cov15 = np.zeros((15, 15))
|
||||||
|
sg2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||||
|
sa2 = float(sigma_a_m_s2_sqrt_hz) ** 2
|
||||||
|
sbg2 = float(sigma_bg_rw_rad_s_sqrt_hz) ** 2
|
||||||
|
sba2 = float(sigma_ba_rw_m_s2_sqrt_hz) ** 2
|
||||||
|
gyro_norms: list[float] = []
|
||||||
|
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
g_a = _interp_vec(times_s, gyro_rad_s, seg0)
|
||||||
|
g_b = _interp_vec(times_s, gyro_rad_s, seg1)
|
||||||
|
a_a = _interp_vec(times_s, acc_m_s2, seg0)
|
||||||
|
a_b = _interp_vec(times_s, acc_m_s2, seg1)
|
||||||
|
omega = 0.5 * (g_a + g_b) - bg
|
||||||
|
acc = 0.5 * (a_a + a_b) - ba
|
||||||
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|
||||||
|
theta = omega * dt
|
||||||
|
jr = _right_jacobian(theta)
|
||||||
|
r_dt = so3_exp(theta)
|
||||||
|
r_mid = delta_r # rotate body accel into i0 frame before update
|
||||||
|
|
||||||
|
# Bias Jacobians (Forster-style first-order recursion).
|
||||||
|
j_r_bg = j_bg[0:3]
|
||||||
|
j_v_bg = j_bg[3:6]
|
||||||
|
j_p_bg = j_bg[6:9]
|
||||||
|
j_r_ba = j_ba[0:3]
|
||||||
|
j_v_ba = j_ba[3:6]
|
||||||
|
j_p_ba = j_ba[6:9]
|
||||||
|
|
||||||
|
acc_skew = skew(acc)
|
||||||
|
j_p_bg_new = j_p_bg + j_v_bg * dt - 0.5 * r_mid @ acc_skew @ j_r_bg * (dt**2)
|
||||||
|
j_v_bg_new = j_v_bg - r_mid @ acc_skew @ j_r_bg * dt
|
||||||
|
j_r_bg_new = r_dt.T @ j_r_bg - jr * dt
|
||||||
|
|
||||||
|
j_p_ba_new = j_p_ba + j_v_ba * dt - 0.5 * r_mid * (dt**2)
|
||||||
|
j_v_ba_new = j_v_ba - r_mid * dt
|
||||||
|
j_r_ba_new = r_dt.T @ j_r_ba
|
||||||
|
|
||||||
|
j_bg = np.vstack([j_r_bg_new, j_v_bg_new, j_p_bg_new])
|
||||||
|
j_ba = np.vstack([j_r_ba_new, j_v_ba_new, j_p_ba_new])
|
||||||
|
|
||||||
|
# Nominal state update (use pre-update Δv in position).
|
||||||
|
delta_p = delta_p + delta_v * dt + 0.5 * r_mid @ acc * (dt**2)
|
||||||
|
delta_v = delta_v + r_mid @ acc * dt
|
||||||
|
delta_r = orthonormalize_rotation(delta_r @ r_dt)
|
||||||
|
|
||||||
|
# Linearized error-state transition (15×15).
|
||||||
|
f = np.eye(15)
|
||||||
|
a_mat = so3_exp(-theta)
|
||||||
|
f[0:3, 0:3] = a_mat
|
||||||
|
f[0:3, 9:12] = -jr * dt
|
||||||
|
f[3:6, 0:3] = -r_mid @ acc_skew * dt
|
||||||
|
f[3:6, 12:15] = -r_mid * dt
|
||||||
|
f[6:9, 0:3] = -0.5 * r_mid @ acc_skew * (dt**2)
|
||||||
|
f[6:9, 3:6] = np.eye(3) * dt
|
||||||
|
f[6:9, 12:15] = -0.5 * r_mid * (dt**2)
|
||||||
|
|
||||||
|
# Noise: continuous densities σ²; Var(∫n dt)=σ² dt. Columns: n_g, n_a, n_bg, n_ba.
|
||||||
|
g_mat = np.zeros((15, 12))
|
||||||
|
g_mat[0:3, 0:3] = jr
|
||||||
|
g_mat[3:6, 3:6] = r_mid
|
||||||
|
g_mat[6:9, 3:6] = 0.5 * r_mid * dt
|
||||||
|
g_mat[9:12, 6:9] = np.eye(3)
|
||||||
|
g_mat[12:15, 9:12] = np.eye(3)
|
||||||
|
q = np.zeros((12, 12))
|
||||||
|
q[0:3, 0:3] = sg2 * dt * np.eye(3)
|
||||||
|
q[3:6, 3:6] = sa2 * dt * np.eye(3)
|
||||||
|
q[6:9, 6:9] = sbg2 * dt * np.eye(3)
|
||||||
|
q[9:12, 9:12] = sba2 * dt * np.eye(3)
|
||||||
|
cov15 = f @ cov15 @ f.T + g_mat @ q @ g_mat.T
|
||||||
|
|
||||||
|
delta_r = orthonormalize_rotation(delta_r)
|
||||||
|
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||||
|
cov9 = cov15[0:9, 0:9]
|
||||||
|
cov9 = 0.5 * (cov9 + cov9.T) + np.eye(9) * 1e-12
|
||||||
|
if mean_gyro_norm < 0.02:
|
||||||
|
cov9 = cov9.copy()
|
||||||
|
cov9[0:3, 0:3] = cov9[0:3, 0:3] * 4.0
|
||||||
|
cov_trace = float(np.trace(cov9[0:3, 0:3]))
|
||||||
|
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||||
|
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||||
|
return ImuPreintegration(
|
||||||
|
delta_R=delta_r,
|
||||||
|
delta_v=np.asarray(delta_v, dtype=float),
|
||||||
|
delta_p=np.asarray(delta_p, dtype=float),
|
||||||
|
duration_s=duration,
|
||||||
|
mean_gyro_norm=mean_gyro_norm,
|
||||||
|
sigma_rad=sigma_rad,
|
||||||
|
weight=weight,
|
||||||
|
gyro_bias_rad_s=bg.copy(),
|
||||||
|
acc_bias_m_s2=ba.copy(),
|
||||||
|
cov=np.asarray(cov9, dtype=float),
|
||||||
|
J_bg=np.asarray(j_bg, dtype=float),
|
||||||
|
J_ba=np.asarray(j_ba, dtype=float),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def apply_bias_correction_imu(
|
||||||
|
preint: ImuPreintegration,
|
||||||
|
delta_gyro_bias: np.ndarray | None = None,
|
||||||
|
delta_acc_bias: np.ndarray | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""First-order bias correction of ``ΔR/Δv/Δp``.
|
||||||
|
|
||||||
|
Returns ``(delta_R, delta_v, delta_p)``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
dbg = np.zeros(3) if delta_gyro_bias is None else np.asarray(delta_gyro_bias, dtype=float).reshape(3)
|
||||||
|
dba = np.zeros(3) if delta_acc_bias is None else np.asarray(delta_acc_bias, dtype=float).reshape(3)
|
||||||
|
j_bg = np.asarray(preint.J_bg, dtype=float).reshape(9, 3)
|
||||||
|
j_ba = np.asarray(preint.J_ba, dtype=float).reshape(9, 3)
|
||||||
|
delta_r = orthonormalize_rotation(preint.delta_R @ so3_exp(j_bg[0:3] @ dbg))
|
||||||
|
delta_v = preint.delta_v + j_bg[3:6] @ dbg + j_ba[3:6] @ dba
|
||||||
|
delta_p = preint.delta_p + j_bg[6:9] @ dbg + j_ba[6:9] @ dba
|
||||||
|
return delta_r, np.asarray(delta_v, dtype=float), np.asarray(delta_p, dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def relative_rotation_from_lidar(R_X: np.ndarray, R_B: np.ndarray) -> np.ndarray:
|
||||||
|
"""Map LiDAR relative rotation into IMU frame: ``R_X R_B R_X^T``."""
|
||||||
|
|
||||||
|
r_x = orthonormalize_rotation(R_X)
|
||||||
|
r_b = orthonormalize_rotation(R_B)
|
||||||
|
return orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegration_rotation_residual(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
R_X: np.ndarray,
|
||||||
|
R_B: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""``log( delta_R^T * R_X R_B R_X^T )`` in so(3)."""
|
||||||
|
|
||||||
|
predicted = relative_rotation_from_lidar(R_X, R_B)
|
||||||
|
return so3_log(delta_R.T @ predicted)
|
||||||
|
|
||||||
|
|
||||||
|
def apply_bias_jacobian_correction(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
J_bg: np.ndarray,
|
||||||
|
delta_bias_rad_s: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""First-order update ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``."""
|
||||||
|
|
||||||
|
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
j_bg = np.asarray(J_bg, dtype=float).reshape(3, 3)
|
||||||
|
return orthonormalize_rotation(delta_R @ so3_exp(j_bg @ db))
|
||||||
|
|
||||||
|
|
||||||
|
def apply_constant_bias_correction(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
duration_s: float,
|
||||||
|
delta_bias_rad_s: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Legacy first-order correction when ``J_bg`` is unavailable.
|
||||||
|
|
||||||
|
``ΔR(b+δb) ≈ ΔR Exp(-δb Δt)`` (identity Jacobian approximation).
|
||||||
|
"""
|
||||||
|
|
||||||
|
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
return orthonormalize_rotation(delta_R @ so3_exp(-db * float(duration_s)))
|
||||||
|
|
||||||
|
|
||||||
|
def residual_whiten_matrix(cov: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return ``W`` such that ``W @ e`` is approximately information-whitened.
|
||||||
|
|
||||||
|
Accepts square ``n×n`` covariances (3×3 rotation or 9×9 full IMU).
|
||||||
|
"""
|
||||||
|
|
||||||
|
matrix = np.asarray(cov, dtype=float)
|
||||||
|
if matrix.ndim != 2 or matrix.shape[0] != matrix.shape[1]:
|
||||||
|
raise ValueError("cov must be square")
|
||||||
|
n = matrix.shape[0]
|
||||||
|
matrix = 0.5 * (matrix + matrix.T) + np.eye(n) * 1e-10
|
||||||
|
try:
|
||||||
|
info = np.linalg.inv(matrix)
|
||||||
|
return np.linalg.cholesky(info).T
|
||||||
|
except np.linalg.LinAlgError:
|
||||||
|
scale = 1.0 / max(float(np.sqrt(np.trace(matrix) / n)), 1e-6)
|
||||||
|
return np.eye(n) * scale
|
||||||
@@ -0,0 +1,977 @@
|
|||||||
|
"""Joint extrinsic refinement: Phase-A rotation factors + Phase-C SE(3) IMU factors."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Callable, Mapping
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, MotionPair
|
||||||
|
from .geometry import make_transform, orthonormalize_rotation, so3_exp, so3_log
|
||||||
|
from .imu_preintegration import (
|
||||||
|
apply_bias_jacobian_correction,
|
||||||
|
apply_constant_bias_correction,
|
||||||
|
preintegrate_gyro,
|
||||||
|
preintegration_rotation_residual,
|
||||||
|
residual_whiten_matrix,
|
||||||
|
)
|
||||||
|
from .observability import ObservabilityReport, analyze_observability
|
||||||
|
from .phase_a import phase_a_comparison_to_dict, solve_phase_a_comparison
|
||||||
|
from .rotation_handeye import select_strong_rotation_pairs
|
||||||
|
|
||||||
|
G_NORM = 9.80665
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class PhaseASessionResult:
|
||||||
|
session_id: str
|
||||||
|
pair_count: int
|
||||||
|
gyro_bias0_rad_s: np.ndarray
|
||||||
|
gyro_bias_rad_s: np.ndarray
|
||||||
|
residual_rms_deg: float
|
||||||
|
residual_median_deg: float
|
||||||
|
residual_p95_deg: float
|
||||||
|
outlier_fraction_gt_5deg: float
|
||||||
|
accepted: bool
|
||||||
|
included_in_final: bool
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointExtrinsicResult:
|
||||||
|
T_IMU_lidar: np.ndarray
|
||||||
|
translation_accepted: bool
|
||||||
|
residual_rms_rot_deg: float
|
||||||
|
residual_rms_trans_m: float
|
||||||
|
observability: ObservabilityReport
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None
|
||||||
|
accel_bias_m_s2: np.ndarray | None = None
|
||||||
|
gravity_m_s2: np.ndarray | None = None
|
||||||
|
gyro_bias_rad_s_per_session: dict[str, np.ndarray] = field(default_factory=dict)
|
||||||
|
phase_a_sessions: tuple[PhaseASessionResult, ...] = ()
|
||||||
|
phase_a_accepted: bool = False
|
||||||
|
phase_a_comparison: dict[str, Any] = field(default_factory=dict)
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_weight(pair: MotionPair) -> float:
|
||||||
|
weight = float(pair.metadata.get("weight", 1.0))
|
||||||
|
if not np.isfinite(weight) or weight <= 0:
|
||||||
|
return 1.0
|
||||||
|
return weight
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_j_bg(pair: MotionPair) -> np.ndarray | None:
|
||||||
|
raw = pair.metadata.get("J_bg")
|
||||||
|
if raw is None:
|
||||||
|
return None
|
||||||
|
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_cov(pair: MotionPair) -> np.ndarray:
|
||||||
|
raw = pair.metadata.get("cov")
|
||||||
|
if raw is None:
|
||||||
|
sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2))
|
||||||
|
return np.eye(3) * max(sigma, 1e-4) ** 2
|
||||||
|
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||||||
|
|
||||||
|
|
||||||
|
def _corrected_delta_r(
|
||||||
|
pair: MotionPair,
|
||||||
|
delta_bias: np.ndarray,
|
||||||
|
*,
|
||||||
|
imu: ImuSeries | None,
|
||||||
|
bias0: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
j_bg = _pair_j_bg(pair)
|
||||||
|
if j_bg is not None:
|
||||||
|
return apply_bias_jacobian_correction(pair.R_A, j_bg, delta_bias)
|
||||||
|
if imu is not None and "t_i_imu_s" in pair.metadata and "t_j_imu_s" in pair.metadata:
|
||||||
|
preint = preintegrate_gyro(
|
||||||
|
imu.t_s,
|
||||||
|
imu.gyro_rad_s,
|
||||||
|
float(pair.metadata["t_i_imu_s"]),
|
||||||
|
float(pair.metadata["t_j_imu_s"]),
|
||||||
|
bias0 + delta_bias,
|
||||||
|
)
|
||||||
|
return preint.delta_R
|
||||||
|
duration = float(pair.metadata.get("duration_s", max(pair.t_j_s - pair.t_i_s, 1e-3)))
|
||||||
|
return apply_constant_bias_correction(pair.R_A, duration, delta_bias)
|
||||||
|
|
||||||
|
|
||||||
|
def _gravity_basis(g0: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return 3×2 orthonormal basis spanning the plane orthogonal to ``g0``."""
|
||||||
|
|
||||||
|
g = np.asarray(g0, dtype=float).reshape(3)
|
||||||
|
n = np.linalg.norm(g)
|
||||||
|
if n < 1e-9:
|
||||||
|
g = np.array([0.0, 0.0, -G_NORM])
|
||||||
|
n = G_NORM
|
||||||
|
g = g / n
|
||||||
|
axis = np.array([1.0, 0.0, 0.0]) if abs(g[0]) < 0.9 else np.array([0.0, 1.0, 0.0])
|
||||||
|
e1 = np.cross(g, axis)
|
||||||
|
e1 /= max(np.linalg.norm(e1), 1e-12)
|
||||||
|
e2 = np.cross(g, e1)
|
||||||
|
return np.column_stack([e1, e2])
|
||||||
|
|
||||||
|
|
||||||
|
def _gravity_from_params(xy: np.ndarray, g0: np.ndarray, basis: np.ndarray) -> np.ndarray:
|
||||||
|
raw = np.asarray(g0, dtype=float).reshape(3) + basis @ np.asarray(xy, dtype=float).reshape(2)
|
||||||
|
n = float(np.linalg.norm(raw))
|
||||||
|
if n < 1e-9:
|
||||||
|
return np.asarray(g0, dtype=float).reshape(3)
|
||||||
|
return raw * (G_NORM / n)
|
||||||
|
|
||||||
|
|
||||||
|
def _lidar_to_imu_relative(r_x: np.ndarray, t_x: np.ndarray, r_b: np.ndarray, t_b: np.ndarray):
|
||||||
|
"""Map LiDAR relative pose to IMU: ``T_A = T_X T_B T_X^{-1}``."""
|
||||||
|
|
||||||
|
r_a = orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||||||
|
t_a = (np.eye(3) - r_a) @ t_x + r_x @ t_b
|
||||||
|
return r_a, t_a
|
||||||
|
|
||||||
|
|
||||||
|
def _corrected_preint_quantities(
|
||||||
|
pair: MotionPair,
|
||||||
|
bg_i: np.ndarray,
|
||||||
|
ba_i: np.ndarray,
|
||||||
|
bg0: np.ndarray,
|
||||||
|
ba0: np.ndarray,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""First-order correct ΔR/Δv/Δp for keyframe biases vs preintegration biases."""
|
||||||
|
|
||||||
|
dbg = np.asarray(bg_i, dtype=float).reshape(3) - np.asarray(bg0, dtype=float).reshape(3)
|
||||||
|
dba = np.asarray(ba_i, dtype=float).reshape(3) - np.asarray(ba0, dtype=float).reshape(3)
|
||||||
|
j_bg = pair.metadata.get("J_bg9")
|
||||||
|
j_ba = pair.metadata.get("J_ba")
|
||||||
|
delta_v0 = np.asarray(pair.metadata.get("delta_v", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||||||
|
delta_p0 = (
|
||||||
|
np.asarray(pair.t_A_m, dtype=float).reshape(3)
|
||||||
|
if pair.t_A_m is not None
|
||||||
|
else np.asarray(pair.metadata.get("delta_p", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||||||
|
)
|
||||||
|
if j_bg is None or j_ba is None:
|
||||||
|
delta_r = apply_bias_jacobian_correction(
|
||||||
|
pair.R_A,
|
||||||
|
_pair_j_bg(pair) if _pair_j_bg(pair) is not None else np.zeros((3, 3)),
|
||||||
|
dbg,
|
||||||
|
)
|
||||||
|
return delta_r, delta_v0, delta_p0
|
||||||
|
j_bg_m = np.asarray(j_bg, dtype=float).reshape(9, 3)
|
||||||
|
j_ba_m = np.asarray(j_ba, dtype=float).reshape(9, 3)
|
||||||
|
delta_r = orthonormalize_rotation(pair.R_A @ so3_exp(j_bg_m[0:3] @ dbg))
|
||||||
|
delta_v = delta_v0 + j_bg_m[3:6] @ dbg + j_ba_m[3:6] @ dba
|
||||||
|
delta_p = delta_p0 + j_bg_m[6:9] @ dbg + j_ba_m[6:9] @ dba
|
||||||
|
return delta_r, delta_v, delta_p
|
||||||
|
|
||||||
|
|
||||||
|
def _build_nav_rotations(
|
||||||
|
keyframe_ids: list[int],
|
||||||
|
id_to_idx: dict[int, int],
|
||||||
|
consecutive_pairs: dict[tuple[int, int], MotionPair],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
t_x: np.ndarray,
|
||||||
|
) -> list[np.ndarray]:
|
||||||
|
"""Chain IMU orientations; restart at session/gap boundaries (no cross-link)."""
|
||||||
|
|
||||||
|
del id_to_idx
|
||||||
|
rotations = [np.eye(3) for _ in keyframe_ids]
|
||||||
|
for k in range(len(keyframe_ids) - 1):
|
||||||
|
a = keyframe_ids[k]
|
||||||
|
b = keyframe_ids[k + 1]
|
||||||
|
pair = consecutive_pairs.get((a, b))
|
||||||
|
if pair is None:
|
||||||
|
# Missing link or new session: start a fresh nav chain.
|
||||||
|
rotations[k + 1] = np.eye(3)
|
||||||
|
continue
|
||||||
|
t_b = np.zeros(3) if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float)
|
||||||
|
r_meas, _ = _lidar_to_imu_relative(r_x, t_x, pair.R_B, t_b)
|
||||||
|
rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas)
|
||||||
|
return rotations
|
||||||
|
|
||||||
|
|
||||||
|
def _solve_phase_c_se3(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
gyro_bias_linearization: np.ndarray,
|
||||||
|
gyro_bias_init: np.ndarray,
|
||||||
|
gravity_init: np.ndarray,
|
||||||
|
sigma_bg_rw: float = 1.0e-5,
|
||||||
|
sigma_ba_rw: float = 1.0e-3,
|
||||||
|
t_init: np.ndarray | None = None,
|
||||||
|
t_prior: np.ndarray | None = None,
|
||||||
|
t_prior_sigma_m: np.ndarray | float | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]:
|
||||||
|
"""Keyframe IMU factor optimization for full SE(3)."""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
usable = [pair for pair in pairs if pair.t_B_m is not None and "delta_v" in pair.metadata]
|
||||||
|
if len(usable) < 3:
|
||||||
|
notes.append("phase-C skipped: need pairs with full preintegration metadata")
|
||||||
|
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
|
||||||
|
return r_x, t0, gravity_init, gyro_bias_init, np.zeros(3), 1e9, 1e9, notes
|
||||||
|
|
||||||
|
# Keyframes: group by session, sort each session by IMU time (no cross-session chain).
|
||||||
|
stamp: dict[int, float] = {}
|
||||||
|
kf_session: dict[int, str] = {}
|
||||||
|
for pair in usable:
|
||||||
|
stamp[pair.i] = float(pair.metadata.get("t_i_imu_s", pair.t_i_s))
|
||||||
|
stamp[pair.j] = float(pair.metadata.get("t_j_imu_s", pair.t_j_s))
|
||||||
|
kf_session[pair.i] = pair.session_id
|
||||||
|
kf_session[pair.j] = pair.session_id
|
||||||
|
session_ids = sorted(set(kf_session.values()))
|
||||||
|
keyframe_ids: list[int] = []
|
||||||
|
for sid in session_ids:
|
||||||
|
local = [kid for kid, sess in kf_session.items() if sess == sid]
|
||||||
|
local.sort(key=lambda kid: stamp[kid])
|
||||||
|
keyframe_ids.extend(local)
|
||||||
|
k_count = len(keyframe_ids)
|
||||||
|
id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)}
|
||||||
|
|
||||||
|
consecutive_pairs: dict[tuple[int, int], MotionPair] = {}
|
||||||
|
for pair in usable:
|
||||||
|
if kf_session.get(pair.i) != kf_session.get(pair.j):
|
||||||
|
continue
|
||||||
|
if id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
|
||||||
|
consecutive_pairs[(pair.i, pair.j)] = pair
|
||||||
|
notes.append(
|
||||||
|
f"phase-C multi-session graph: sessions={len(session_ids)}, "
|
||||||
|
f"keyframes={k_count}, consecutive_links={len(consecutive_pairs)}"
|
||||||
|
)
|
||||||
|
|
||||||
|
g0 = np.asarray(gravity_init, dtype=float).reshape(3)
|
||||||
|
if np.linalg.norm(g0) < 1e-6:
|
||||||
|
g0 = np.array([0.0, 0.0, -G_NORM])
|
||||||
|
g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9))
|
||||||
|
basis = _gravity_basis(g0)
|
||||||
|
ba0 = np.zeros(3)
|
||||||
|
bg0 = np.asarray(gyro_bias_linearization, dtype=float).reshape(3)
|
||||||
|
bg_init = np.asarray(gyro_bias_init, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
# State: dθ(3), t(3), g_xy(2), v(3K), bg(3K), ba(3K)
|
||||||
|
n_v = 3 * k_count
|
||||||
|
n_b = 3 * k_count
|
||||||
|
dim = 3 + 3 + 2 + n_v + n_b + n_b
|
||||||
|
x0 = np.zeros(dim)
|
||||||
|
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
|
||||||
|
x0[3:6] = t0
|
||||||
|
t_prior_vec = None if t_prior is None else np.asarray(t_prior, dtype=float).reshape(3)
|
||||||
|
if t_prior_sigma_m is None:
|
||||||
|
t_sigma = np.array([0.05, 0.05, 0.05], dtype=float)
|
||||||
|
else:
|
||||||
|
t_sigma = np.asarray(t_prior_sigma_m, dtype=float).reshape(-1)
|
||||||
|
if t_sigma.size == 1:
|
||||||
|
t_sigma = np.full(3, float(t_sigma[0]), dtype=float)
|
||||||
|
# velocities start at 0; biases at prior
|
||||||
|
for idx in range(k_count):
|
||||||
|
x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg_init
|
||||||
|
|
||||||
|
whitened = []
|
||||||
|
for pair in usable:
|
||||||
|
cov9 = pair.metadata.get("cov9")
|
||||||
|
if cov9 is None:
|
||||||
|
cov = _pair_cov(pair)
|
||||||
|
cov9_m = np.eye(9)
|
||||||
|
cov9_m[0:3, 0:3] = cov
|
||||||
|
cov9_m[3:6, 3:6] = np.eye(3) * 0.25
|
||||||
|
cov9_m[6:9, 6:9] = np.eye(3) * 1.0
|
||||||
|
else:
|
||||||
|
cov9_m = np.asarray(cov9, dtype=float).reshape(9, 9)
|
||||||
|
whitened.append(residual_whiten_matrix(cov9_m))
|
||||||
|
|
||||||
|
def unpack(vec: np.ndarray):
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[0:3]) @ r_x)
|
||||||
|
t_opt = vec[3:6]
|
||||||
|
g_opt = _gravity_from_params(vec[6:8], g0, basis)
|
||||||
|
base = 8
|
||||||
|
vels = vec[base : base + n_v].reshape(k_count, 3)
|
||||||
|
base += n_v
|
||||||
|
bgs = vec[base : base + n_b].reshape(k_count, 3)
|
||||||
|
base += n_b
|
||||||
|
bas = vec[base : base + n_b].reshape(k_count, 3)
|
||||||
|
return r_opt, t_opt, g_opt, vels, bgs, bas
|
||||||
|
|
||||||
|
def residuals(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt, t_opt, g_opt, vels, bgs, bas = unpack(vec)
|
||||||
|
nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
|
||||||
|
out: list[np.ndarray] = []
|
||||||
|
|
||||||
|
for pair, whiten in zip(usable, whitened):
|
||||||
|
i_idx = id_to_idx[pair.i]
|
||||||
|
j_idx = id_to_idx[pair.j]
|
||||||
|
dt = float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s))
|
||||||
|
dt = max(dt, 1e-3)
|
||||||
|
delta_r, delta_v, delta_p = _corrected_preint_quantities(
|
||||||
|
pair, bgs[i_idx], bas[i_idx], bg0, ba0
|
||||||
|
)
|
||||||
|
t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
|
||||||
|
r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
|
||||||
|
r_i = nav_r[i_idx]
|
||||||
|
v_i = vels[i_idx]
|
||||||
|
v_j = vels[j_idx]
|
||||||
|
|
||||||
|
err_r = so3_log(delta_r.T @ r_meas)
|
||||||
|
err_v = v_j - v_i - g_opt * dt - r_i @ delta_v
|
||||||
|
err_p = r_i @ (t_meas - delta_p) - v_i * dt - 0.5 * g_opt * (dt**2)
|
||||||
|
err = np.concatenate([err_r, err_v, err_p])
|
||||||
|
w = np.sqrt(_pair_weight(pair))
|
||||||
|
out.append(w * (whiten @ err))
|
||||||
|
|
||||||
|
# Bias random-walk between consecutive keyframes (same session only).
|
||||||
|
for k in range(k_count - 1):
|
||||||
|
a = keyframe_ids[k]
|
||||||
|
b = keyframe_ids[k + 1]
|
||||||
|
if kf_session.get(a) != kf_session.get(b):
|
||||||
|
continue
|
||||||
|
dt = max(stamp[b] - stamp[a], 1e-3)
|
||||||
|
scale_g = 1.0 / (max(sigma_bg_rw, 1e-8) * np.sqrt(dt))
|
||||||
|
scale_a = 1.0 / (max(sigma_ba_rw, 1e-8) * np.sqrt(dt))
|
||||||
|
out.append(scale_g * (bgs[k + 1] - bgs[k]))
|
||||||
|
out.append(scale_a * (bas[k + 1] - bas[k]))
|
||||||
|
|
||||||
|
# Weak priors: first keyframe of each session + CAD/installation translation.
|
||||||
|
for sid in session_ids:
|
||||||
|
first = next(kid for kid in keyframe_ids if kf_session[kid] == sid)
|
||||||
|
idx0 = id_to_idx[first]
|
||||||
|
out.append(50.0 * (bgs[idx0] - bg_init))
|
||||||
|
out.append(20.0 * bas[idx0])
|
||||||
|
if t_prior_vec is not None:
|
||||||
|
out.append((t_opt - t_prior_vec) / np.maximum(t_sigma, 1e-3))
|
||||||
|
else:
|
||||||
|
out.append(0.2 * t_opt) # soft |t|~0 prior when no CAD prior
|
||||||
|
return np.concatenate(out)
|
||||||
|
|
||||||
|
# Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime.
|
||||||
|
opt = least_squares(residuals, x0, loss="huber", f_scale=0.05, max_nfev=80)
|
||||||
|
r_opt, t_opt, g_opt, vels, bgs, bas = unpack(opt.x)
|
||||||
|
|
||||||
|
rot_errs = []
|
||||||
|
trans_errs = []
|
||||||
|
nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
|
||||||
|
for pair in usable:
|
||||||
|
i_idx = id_to_idx[pair.i]
|
||||||
|
j_idx = id_to_idx[pair.j]
|
||||||
|
dt = max(float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s)), 1e-3)
|
||||||
|
delta_r, delta_v, delta_p = _corrected_preint_quantities(
|
||||||
|
pair, bgs[i_idx], bas[i_idx], bg0, ba0
|
||||||
|
)
|
||||||
|
t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
|
||||||
|
r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
|
||||||
|
r_i = nav_r[i_idx]
|
||||||
|
err_r = so3_log(delta_r.T @ r_meas)
|
||||||
|
err_p = r_i @ (t_meas - delta_p) - vels[i_idx] * dt - 0.5 * g_opt * (dt**2)
|
||||||
|
rot_errs.append(np.degrees(np.linalg.norm(err_r)))
|
||||||
|
trans_errs.append(float(np.linalg.norm(err_p)))
|
||||||
|
del delta_v, j_idx
|
||||||
|
|
||||||
|
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
|
||||||
|
trans_rms = float(np.sqrt(np.mean(np.square(trans_errs)))) if trans_errs else 1e9
|
||||||
|
bg_mean = np.mean(bgs, axis=0)
|
||||||
|
ba_mean = np.mean(bas, axis=0)
|
||||||
|
notes.append(
|
||||||
|
"phase-C SE3 (Δv/Δp + g + keyframe v/bias RW): "
|
||||||
|
f"keyframes={k_count}, pairs={len(usable)}, "
|
||||||
|
f"|t|={float(np.linalg.norm(t_opt)):.3f} m, "
|
||||||
|
f"|g|={float(np.linalg.norm(g_opt)):.3f}, "
|
||||||
|
f"trans_rms={trans_rms:.3f} m"
|
||||||
|
)
|
||||||
|
return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_gyro_bias0(pair: MotionPair, fallback: np.ndarray) -> np.ndarray:
|
||||||
|
raw = pair.metadata.get("gyro_bias0_rad_s")
|
||||||
|
if raw is None:
|
||||||
|
return np.asarray(fallback, dtype=float).reshape(3)
|
||||||
|
return np.asarray(raw, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
|
||||||
|
def _phase_a_bias_bases(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
*,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None,
|
||||||
|
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None,
|
||||||
|
) -> dict[str, np.ndarray]:
|
||||||
|
session_ids = sorted({pair.session_id for pair in pairs})
|
||||||
|
scalar = None
|
||||||
|
if gyro_bias_rad_s is not None:
|
||||||
|
scalar = np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
supplied = {} if gyro_bias_rad_s_by_session is None else gyro_bias_rad_s_by_session
|
||||||
|
bases: dict[str, np.ndarray] = {}
|
||||||
|
for sid in session_ids:
|
||||||
|
if sid in supplied:
|
||||||
|
bases[sid] = np.asarray(supplied[sid], dtype=float).reshape(3)
|
||||||
|
continue
|
||||||
|
pair = next(
|
||||||
|
(
|
||||||
|
item
|
||||||
|
for item in pairs
|
||||||
|
if item.session_id == sid and "gyro_bias0_rad_s" in item.metadata
|
||||||
|
),
|
||||||
|
None,
|
||||||
|
)
|
||||||
|
if pair is not None:
|
||||||
|
bases[sid] = np.asarray(pair.metadata["gyro_bias0_rad_s"], dtype=float).reshape(3)
|
||||||
|
elif scalar is not None:
|
||||||
|
bases[sid] = scalar.copy()
|
||||||
|
else:
|
||||||
|
bases[sid] = np.zeros(3)
|
||||||
|
return bases
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_distribution(errs_deg: list[float]) -> tuple[float, float, float, float, bool]:
|
||||||
|
if not errs_deg:
|
||||||
|
return 1e9, 1e9, 1e9, 1.0, False
|
||||||
|
errs = np.asarray(errs_deg, dtype=float)
|
||||||
|
rms = float(np.sqrt(np.mean(errs**2)))
|
||||||
|
median = float(np.median(errs))
|
||||||
|
p95 = float(np.percentile(errs, 95.0))
|
||||||
|
outlier_fraction = float(np.mean(errs > 5.0))
|
||||||
|
accepted = (
|
||||||
|
len(errs) >= 3
|
||||||
|
and rms < 1.5
|
||||||
|
and median < 0.5
|
||||||
|
and p95 < 1.5
|
||||||
|
and outlier_fraction <= 0.005
|
||||||
|
)
|
||||||
|
return rms, median, p95, outlier_fraction, accepted
|
||||||
|
|
||||||
|
|
||||||
|
def _solve_phase_a_rotation(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
r_seed: np.ndarray,
|
||||||
|
*,
|
||||||
|
bias_bases: Mapping[str, np.ndarray],
|
||||||
|
imu: ImuSeries | None,
|
||||||
|
bias_prior_sigma_rad_s: float,
|
||||||
|
preexcluded_session_ids: set[str] | None = None,
|
||||||
|
) -> tuple[
|
||||||
|
np.ndarray,
|
||||||
|
dict[str, np.ndarray],
|
||||||
|
tuple[PhaseASessionResult, ...],
|
||||||
|
list[MotionPair],
|
||||||
|
float,
|
||||||
|
bool,
|
||||||
|
list[str],
|
||||||
|
]:
|
||||||
|
notes: list[str] = []
|
||||||
|
all_session_ids = sorted({pair.session_id for pair in pairs})
|
||||||
|
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
|
||||||
|
|
||||||
|
def optimize(
|
||||||
|
active_pairs: list[MotionPair],
|
||||||
|
r0: np.ndarray,
|
||||||
|
bias_seed: Mapping[str, np.ndarray],
|
||||||
|
) -> tuple[np.ndarray, dict[str, np.ndarray]]:
|
||||||
|
session_ids = sorted({pair.session_id for pair in active_pairs})
|
||||||
|
session_index = {sid: index for index, sid in enumerate(session_ids)}
|
||||||
|
whiten = [residual_whiten_matrix(_pair_cov(pair)) for pair in active_pairs]
|
||||||
|
x0 = np.zeros(3 + 3 * len(session_ids))
|
||||||
|
for sid, index in session_index.items():
|
||||||
|
x0[3 + 3 * index : 6 + 3 * index] = np.asarray(bias_seed[sid], dtype=float)
|
||||||
|
|
||||||
|
def residual(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r0)
|
||||||
|
out: list[np.ndarray] = []
|
||||||
|
for pair, sqrt_info in zip(active_pairs, whiten):
|
||||||
|
index = session_index[pair.session_id]
|
||||||
|
bias = vec[3 + 3 * index : 6 + 3 * index]
|
||||||
|
base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
|
||||||
|
delta_r = _corrected_delta_r(
|
||||||
|
pair, bias - base, imu=imu, bias0=base
|
||||||
|
)
|
||||||
|
out.append(
|
||||||
|
sqrt_info
|
||||||
|
@ preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||||||
|
)
|
||||||
|
for sid, index in session_index.items():
|
||||||
|
bias = vec[3 + 3 * index : 6 + 3 * index]
|
||||||
|
out.append(prior_w * (bias - bias_bases[sid]))
|
||||||
|
return np.concatenate(out)
|
||||||
|
|
||||||
|
opt = least_squares(residual, x0, loss="huber", f_scale=1.0, max_nfev=200)
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r0)
|
||||||
|
biases = {
|
||||||
|
sid: opt.x[3 + 3 * index : 6 + 3 * index].copy()
|
||||||
|
for sid, index in session_index.items()
|
||||||
|
}
|
||||||
|
return r_opt, biases
|
||||||
|
|
||||||
|
def summarize(
|
||||||
|
r_opt: np.ndarray,
|
||||||
|
biases: Mapping[str, np.ndarray],
|
||||||
|
included: set[str],
|
||||||
|
) -> tuple[PhaseASessionResult, ...]:
|
||||||
|
results: list[PhaseASessionResult] = []
|
||||||
|
for sid in all_session_ids:
|
||||||
|
local_pairs = [pair for pair in pairs if pair.session_id == sid]
|
||||||
|
bias = np.asarray(biases.get(sid, bias_bases[sid]), dtype=float).reshape(3)
|
||||||
|
errs: list[float] = []
|
||||||
|
for pair in local_pairs:
|
||||||
|
base = _pair_gyro_bias0(pair, bias_bases[sid])
|
||||||
|
delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||||||
|
errs.append(float(np.degrees(np.linalg.norm(err))))
|
||||||
|
rms, median, p95, outlier, accepted = _rotation_distribution(errs)
|
||||||
|
results.append(
|
||||||
|
PhaseASessionResult(
|
||||||
|
session_id=sid,
|
||||||
|
pair_count=len(local_pairs),
|
||||||
|
gyro_bias0_rad_s=np.asarray(bias_bases[sid], dtype=float),
|
||||||
|
gyro_bias_rad_s=bias,
|
||||||
|
residual_rms_deg=rms,
|
||||||
|
residual_median_deg=median,
|
||||||
|
residual_p95_deg=p95,
|
||||||
|
outlier_fraction_gt_5deg=outlier,
|
||||||
|
accepted=accepted,
|
||||||
|
included_in_final=sid in included,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return tuple(results)
|
||||||
|
|
||||||
|
if not pairs:
|
||||||
|
return r_seed, dict(bias_bases), (), [], 1e9, False, ["no pairs for phase-A"]
|
||||||
|
|
||||||
|
r_first, biases_first = optimize(pairs, r_seed, bias_bases)
|
||||||
|
first = summarize(r_first, biases_first, set(all_session_ids))
|
||||||
|
accepted_ids = {item.session_id for item in first if item.accepted}
|
||||||
|
preexcluded = set() if preexcluded_session_ids is None else set(preexcluded_session_ids)
|
||||||
|
accepted_ids -= preexcluded
|
||||||
|
active_ids = set(all_session_ids)
|
||||||
|
r_final = r_first
|
||||||
|
biases_final = dict(biases_first)
|
||||||
|
if preexcluded and not accepted_ids:
|
||||||
|
active_ids = set()
|
||||||
|
notes.append(f"phase-A pre-gate excluded all sessions: {sorted(preexcluded)}")
|
||||||
|
elif accepted_ids and accepted_ids != active_ids:
|
||||||
|
active_ids = accepted_ids
|
||||||
|
active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
|
||||||
|
r_final, active_biases = optimize(active_pairs, r_first, biases_first)
|
||||||
|
biases_final.update(active_biases)
|
||||||
|
excluded = sorted(set(all_session_ids) - active_ids)
|
||||||
|
notes.append(f"phase-A excluded sessions after local/pre residual gate: {excluded}")
|
||||||
|
active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
|
||||||
|
final = summarize(r_final, biases_final, active_ids)
|
||||||
|
active_results = [item for item in final if item.included_in_final]
|
||||||
|
global_errs: list[float] = []
|
||||||
|
for pair in active_pairs:
|
||||||
|
bias = biases_final[pair.session_id]
|
||||||
|
base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
|
||||||
|
delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r_final, pair.R_B)
|
||||||
|
global_errs.append(float(np.degrees(np.linalg.norm(err))))
|
||||||
|
rot_rms, _, _, _, global_ok = _rotation_distribution(global_errs)
|
||||||
|
accepted = bool(active_results and global_ok and all(item.accepted for item in active_results))
|
||||||
|
notes.append(
|
||||||
|
f"phase-A session-local bias refine: sessions={len(active_ids)}/{len(all_session_ids)}, "
|
||||||
|
f"pairs={len(active_pairs)}, rms={rot_rms:.3f} deg"
|
||||||
|
)
|
||||||
|
return r_final, biases_final, final, active_pairs, rot_rms, accepted, notes
|
||||||
|
|
||||||
|
|
||||||
|
def _solve_joint_extrinsic_legacy(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
force_rotation_only: bool = False,
|
||||||
|
imu: ImuSeries | None = None,
|
||||||
|
delta_t_s: float = 0.0,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
gravity_init_m_s2: np.ndarray | None = None,
|
||||||
|
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
|
||||||
|
time_offset_s_by_session: Mapping[str, float] | None = None,
|
||||||
|
bias_prior_sigma_rad_s: float = 0.02,
|
||||||
|
enable_phase_c: bool | None = None,
|
||||||
|
t_init_m: np.ndarray | None = None,
|
||||||
|
t_prior_m: np.ndarray | None = None,
|
||||||
|
t_prior_sigma_m: np.ndarray | float | None = None,
|
||||||
|
) -> JointExtrinsicResult:
|
||||||
|
"""Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3)."""
|
||||||
|
|
||||||
|
del delta_t_s # reserved for future SE(3) time coupling
|
||||||
|
if enable_phase_c is None:
|
||||||
|
enable_phase_c = not force_rotation_only
|
||||||
|
|
||||||
|
usable = [pair for pair in pairs if pair.t_B_m is not None]
|
||||||
|
observability = analyze_observability(usable, r_x)
|
||||||
|
notes = list(observability.notes)
|
||||||
|
|
||||||
|
r = orthonormalize_rotation(np.asarray(r_x, dtype=float))
|
||||||
|
bias0 = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
t_seed = None if t_init_m is None else np.asarray(t_init_m, dtype=float).reshape(3)
|
||||||
|
weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
|
||||||
|
whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable]
|
||||||
|
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
|
||||||
|
|
||||||
|
def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray:
|
||||||
|
residuals = []
|
||||||
|
for pair, whiten in zip(usable, whitens):
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||||||
|
residuals.append(whiten @ err)
|
||||||
|
residuals.append(prior_w * delta_bias)
|
||||||
|
return np.concatenate(residuals) if residuals else np.zeros(0)
|
||||||
|
|
||||||
|
def residual_rot_bias(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
|
||||||
|
return rotation_residuals(r_opt, vec[3:])
|
||||||
|
|
||||||
|
if usable:
|
||||||
|
opt = least_squares(
|
||||||
|
residual_rot_bias,
|
||||||
|
np.zeros(6),
|
||||||
|
loss="huber",
|
||||||
|
f_scale=1.0,
|
||||||
|
max_nfev=200,
|
||||||
|
)
|
||||||
|
r = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r)
|
||||||
|
delta_bias = opt.x[3:]
|
||||||
|
bias_out = bias0 + delta_bias
|
||||||
|
notes.append(
|
||||||
|
"phase-A joint refine (single Σ whitening + J_bg): "
|
||||||
|
f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
|
||||||
|
f"pairs={len(usable)}"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
bias_out = bias0
|
||||||
|
delta_bias = np.zeros(3)
|
||||||
|
notes.append("no pairs for joint refine")
|
||||||
|
|
||||||
|
rot_errs = []
|
||||||
|
for pair in usable:
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r, pair.R_B)
|
||||||
|
rot_errs.append(np.degrees(np.linalg.norm(err)))
|
||||||
|
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
|
||||||
|
|
||||||
|
t = np.zeros(3) if t_seed is None else t_seed.copy()
|
||||||
|
translation_accepted = False
|
||||||
|
trans_rms = 1e9
|
||||||
|
gravity_out: np.ndarray | None = None
|
||||||
|
accel_bias_out: np.ndarray | None = None
|
||||||
|
|
||||||
|
if gravity_init_m_s2 is None:
|
||||||
|
gravity_init = np.array([0.0, 0.0, -G_NORM])
|
||||||
|
else:
|
||||||
|
gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
if t_prior_m is not None:
|
||||||
|
notes.append(
|
||||||
|
"using CAD/installation translation prior "
|
||||||
|
f"t={np.asarray(t_prior_m, dtype=float).reshape(3).tolist()}"
|
||||||
|
)
|
||||||
|
|
||||||
|
if (
|
||||||
|
enable_phase_c
|
||||||
|
and not force_rotation_only
|
||||||
|
and observability.translation_observable
|
||||||
|
and observability.rotation_observable
|
||||||
|
and len(usable) >= 5
|
||||||
|
):
|
||||||
|
r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3(
|
||||||
|
usable,
|
||||||
|
r,
|
||||||
|
gyro_bias_linearization=bias0,
|
||||||
|
gyro_bias_init=bias_out,
|
||||||
|
gravity_init=gravity_init,
|
||||||
|
t_init=t_seed if t_seed is not None else t_prior_m,
|
||||||
|
t_prior=t_prior_m,
|
||||||
|
t_prior_sigma_m=t_prior_sigma_m,
|
||||||
|
)
|
||||||
|
notes.extend(c_notes)
|
||||||
|
translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
|
||||||
|
if not translation_accepted:
|
||||||
|
# Prefer CAD prior over silent zero when motion SE3 is rejected.
|
||||||
|
if t_prior_m is not None:
|
||||||
|
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
notes.append(
|
||||||
|
"phase-C translation residual/gate failed; CAD translation is reported "
|
||||||
|
"as a prior only and is not accepted as calibration"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("phase-C translation residual/gate failed; keeping translation at zero")
|
||||||
|
t = np.zeros(3)
|
||||||
|
elif (
|
||||||
|
not force_rotation_only
|
||||||
|
and observability.translation_observable
|
||||||
|
and observability.rotation_observable
|
||||||
|
and len(usable) >= 5
|
||||||
|
):
|
||||||
|
# Legacy hand-eye translation fallback when Phase-C metadata missing.
|
||||||
|
def residual_se3(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
|
||||||
|
t_opt = vec[3:]
|
||||||
|
residuals = []
|
||||||
|
for pair, weight, whiten in zip(usable, weights, whitens):
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
residuals.append(
|
||||||
|
np.sqrt(weight) * (whiten @ preintegration_rotation_residual(delta_r, r_opt, pair.R_B))
|
||||||
|
)
|
||||||
|
pred = (pair.R_A - np.eye(3)) @ t_opt
|
||||||
|
meas = r_opt @ np.asarray(pair.t_B_m, dtype=float)
|
||||||
|
residuals.append(np.sqrt(weight) * (pred - meas))
|
||||||
|
if t_prior_m is not None:
|
||||||
|
sigma = np.asarray(t_prior_sigma_m if t_prior_sigma_m is not None else 0.05, dtype=float)
|
||||||
|
if sigma.size == 1:
|
||||||
|
sigma = np.full(3, float(sigma), dtype=float)
|
||||||
|
residuals.append((t_opt - np.asarray(t_prior_m, dtype=float).reshape(3)) / np.maximum(sigma, 1e-3))
|
||||||
|
return np.concatenate(residuals)
|
||||||
|
|
||||||
|
x_se3 = np.zeros(6)
|
||||||
|
if t_seed is not None:
|
||||||
|
x_se3[3:] = t_seed
|
||||||
|
elif t_prior_m is not None:
|
||||||
|
x_se3[3:] = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
opt_t = least_squares(residual_se3, x_se3, loss="huber", f_scale=0.05, max_nfev=200)
|
||||||
|
r = orthonormalize_rotation(so3_exp(opt_t.x[:3]) @ r)
|
||||||
|
t = opt_t.x[3:]
|
||||||
|
rot_errs = []
|
||||||
|
trans_errs = []
|
||||||
|
for pair in usable:
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
rot_errs.append(np.degrees(np.linalg.norm(preintegration_rotation_residual(delta_r, r, pair.R_B))))
|
||||||
|
pred = (pair.R_A - np.eye(3)) @ t
|
||||||
|
meas = r @ np.asarray(pair.t_B_m, dtype=float)
|
||||||
|
trans_errs.append(np.linalg.norm(pred - meas))
|
||||||
|
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs))))
|
||||||
|
trans_rms = float(np.sqrt(np.mean(np.square(trans_errs))))
|
||||||
|
translation_accepted = trans_rms < 0.5
|
||||||
|
notes.append(f"legacy translation refine rms={trans_rms:.3f} m")
|
||||||
|
if not translation_accepted:
|
||||||
|
notes.append("translation residual too large; keeping translation at zero")
|
||||||
|
t = np.zeros(3)
|
||||||
|
elif not force_rotation_only and t_prior_m is not None:
|
||||||
|
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
translation_accepted = False
|
||||||
|
notes.append(
|
||||||
|
"SE3 motion solve gated off; CAD translation is reported as a prior only "
|
||||||
|
"and is not accepted as calibration"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")
|
||||||
|
|
||||||
|
return JointExtrinsicResult(
|
||||||
|
T_IMU_lidar=make_transform(t, r),
|
||||||
|
translation_accepted=bool(translation_accepted and np.linalg.norm(t) > 0),
|
||||||
|
residual_rms_rot_deg=rot_rms,
|
||||||
|
residual_rms_trans_m=trans_rms,
|
||||||
|
observability=observability,
|
||||||
|
gyro_bias_rad_s=np.asarray(bias_out, dtype=float),
|
||||||
|
accel_bias_m_s2=None if accel_bias_out is None else np.asarray(accel_bias_out, dtype=float),
|
||||||
|
gravity_m_s2=None if gravity_out is None else np.asarray(gravity_out, dtype=float),
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def solve_joint_extrinsic(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
force_rotation_only: bool = False,
|
||||||
|
imu: ImuSeries | None = None,
|
||||||
|
delta_t_s: float = 0.0,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
|
||||||
|
time_offset_s_by_session: Mapping[str, float] | None = None,
|
||||||
|
preexcluded_session_ids: set[str] | None = None,
|
||||||
|
gravity_init_m_s2: np.ndarray | None = None,
|
||||||
|
bias_prior_sigma_rad_s: float = 0.002,
|
||||||
|
rotation_prior: np.ndarray | None = None,
|
||||||
|
rotation_prior_sigma_deg: float = 15.0,
|
||||||
|
phase_a_yaw_std_max_deg: float = 0.5,
|
||||||
|
phase_a_loo_yaw_range_max_deg: float = 1.0,
|
||||||
|
phase_a_data_prior_difference_max_deg: float = 1.0,
|
||||||
|
run_phase_a_leave_one_out: bool = True,
|
||||||
|
phase_a_progress_callback: (
|
||||||
|
Callable[[str, dict[str, Any]], None] | None
|
||||||
|
) = None,
|
||||||
|
enable_phase_c: bool | None = None,
|
||||||
|
t_init_m: np.ndarray | None = None,
|
||||||
|
t_prior_m: np.ndarray | None = None,
|
||||||
|
t_prior_sigma_m: np.ndarray | float | None = None,
|
||||||
|
) -> JointExtrinsicResult:
|
||||||
|
"""Run the corrected session-aware Phase-A and gate unfinished SE(3) stages."""
|
||||||
|
|
||||||
|
del gravity_init_m_s2, t_init_m, t_prior_sigma_m, imu, r_x
|
||||||
|
usable_input = [pair for pair in pairs if pair.t_B_m is not None]
|
||||||
|
bias_bases = _phase_a_bias_bases(
|
||||||
|
usable_input,
|
||||||
|
gyro_bias_rad_s=gyro_bias_rad_s,
|
||||||
|
gyro_bias_rad_s_by_session=gyro_bias_rad_s_by_session,
|
||||||
|
)
|
||||||
|
comparison = solve_phase_a_comparison(
|
||||||
|
usable_input,
|
||||||
|
gyro_bias_rad_s_by_session=bias_bases,
|
||||||
|
rotation_prior=rotation_prior,
|
||||||
|
rotation_prior_sigma_deg=rotation_prior_sigma_deg,
|
||||||
|
preexcluded_session_ids=preexcluded_session_ids,
|
||||||
|
bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
|
||||||
|
yaw_std_max_deg=phase_a_yaw_std_max_deg,
|
||||||
|
leave_one_out_yaw_range_max_deg=(
|
||||||
|
phase_a_loo_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
data_prior_difference_max_deg=(
|
||||||
|
phase_a_data_prior_difference_max_deg
|
||||||
|
),
|
||||||
|
run_leave_one_out=run_phase_a_leave_one_out,
|
||||||
|
progress_callback=phase_a_progress_callback,
|
||||||
|
)
|
||||||
|
primary = comparison.session_bg_data_only
|
||||||
|
r = primary.R_IMU_lidar
|
||||||
|
biases = primary.gyro_bias_rad_s_per_session
|
||||||
|
rot_rms = primary.residual_rms_deg
|
||||||
|
phase_a_accepted = comparison.accepted
|
||||||
|
notes = list(comparison.notes)
|
||||||
|
notes.append(
|
||||||
|
"phase-A primary=A1_session_bg_data_only; "
|
||||||
|
f"A0 RPY={comparison.fixed_bg_data_only.rpy_deg_xyz.tolist()}, "
|
||||||
|
f"A1 RPY={primary.rpy_deg_xyz.tolist()}, "
|
||||||
|
"A2 RPY="
|
||||||
|
f"{comparison.session_bg_with_rotation_prior.rpy_deg_xyz.tolist()}"
|
||||||
|
)
|
||||||
|
notes.append(
|
||||||
|
f"phase-A marginal yaw_std={comparison.marginal_observability.yaw_std_deg:.3f} deg, "
|
||||||
|
f"LOO yaw range={comparison.leave_one_out_yaw_range_deg:.3f} deg"
|
||||||
|
)
|
||||||
|
|
||||||
|
session_results_list: list[PhaseASessionResult] = [
|
||||||
|
PhaseASessionResult(
|
||||||
|
session_id=item.session_id,
|
||||||
|
pair_count=item.pair_count,
|
||||||
|
gyro_bias0_rad_s=item.gyro_bias0_rad_s,
|
||||||
|
gyro_bias_rad_s=item.gyro_bias_rad_s,
|
||||||
|
residual_rms_deg=item.residual_rms_deg,
|
||||||
|
residual_median_deg=item.residual_median_deg,
|
||||||
|
residual_p95_deg=item.residual_p95_deg,
|
||||||
|
outlier_fraction_gt_5deg=item.outlier_fraction_gt_5deg,
|
||||||
|
accepted=item.accepted,
|
||||||
|
included_in_final=True,
|
||||||
|
)
|
||||||
|
for item in primary.sessions
|
||||||
|
]
|
||||||
|
preexcluded = (
|
||||||
|
set()
|
||||||
|
if preexcluded_session_ids is None
|
||||||
|
else set(preexcluded_session_ids)
|
||||||
|
)
|
||||||
|
strong_all = select_strong_rotation_pairs(usable_input)
|
||||||
|
for session_id in sorted(preexcluded):
|
||||||
|
local_pairs = [
|
||||||
|
pair for pair in strong_all if pair.session_id == session_id
|
||||||
|
]
|
||||||
|
errors = [
|
||||||
|
float(
|
||||||
|
np.degrees(
|
||||||
|
np.linalg.norm(
|
||||||
|
preintegration_rotation_residual(
|
||||||
|
pair.R_A, r, pair.R_B
|
||||||
|
)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
for pair in local_pairs
|
||||||
|
]
|
||||||
|
rms, median, p95, outlier, accepted = _rotation_distribution(
|
||||||
|
errors
|
||||||
|
)
|
||||||
|
base = np.asarray(
|
||||||
|
bias_bases.get(session_id, np.zeros(3)), dtype=float
|
||||||
|
).reshape(3)
|
||||||
|
session_results_list.append(
|
||||||
|
PhaseASessionResult(
|
||||||
|
session_id=session_id,
|
||||||
|
pair_count=len(local_pairs),
|
||||||
|
gyro_bias0_rad_s=base,
|
||||||
|
gyro_bias_rad_s=base,
|
||||||
|
residual_rms_deg=rms,
|
||||||
|
residual_median_deg=median,
|
||||||
|
residual_p95_deg=p95,
|
||||||
|
outlier_fraction_gt_5deg=outlier,
|
||||||
|
accepted=accepted,
|
||||||
|
included_in_final=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
session_results = tuple(
|
||||||
|
sorted(session_results_list, key=lambda item: item.session_id)
|
||||||
|
)
|
||||||
|
usable = [
|
||||||
|
pair
|
||||||
|
for pair in strong_all
|
||||||
|
if pair.session_id not in preexcluded
|
||||||
|
]
|
||||||
|
base_observability = analyze_observability(usable, r)
|
||||||
|
marginal = comparison.marginal_observability
|
||||||
|
observability = ObservabilityReport(
|
||||||
|
rotation_observable=bool(
|
||||||
|
marginal.rank == 3
|
||||||
|
and marginal.yaw_std_deg <= phase_a_yaw_std_max_deg
|
||||||
|
),
|
||||||
|
translation_observable=base_observability.translation_observable,
|
||||||
|
condition_rotation=marginal.condition,
|
||||||
|
condition_translation=base_observability.condition_translation,
|
||||||
|
notes=tuple(
|
||||||
|
list(marginal.notes)
|
||||||
|
+ list(base_observability.notes)
|
||||||
|
),
|
||||||
|
)
|
||||||
|
notes.extend(observability.notes)
|
||||||
|
if time_offset_s_by_session is None:
|
||||||
|
notes.append(
|
||||||
|
f"legacy scalar time offset fixed during pair construction: {float(delta_t_s):.6f}s"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
fixed_offsets = {
|
||||||
|
str(sid): float(value) for sid, value in time_offset_s_by_session.items()
|
||||||
|
}
|
||||||
|
notes.append(
|
||||||
|
f"time offsets fixed during pair construction (not optimized): {fixed_offsets}"
|
||||||
|
)
|
||||||
|
|
||||||
|
for item in session_results:
|
||||||
|
notes.append(
|
||||||
|
f"phase-A session {item.session_id}: included={item.included_in_final}, "
|
||||||
|
f"pairs={item.pair_count}, rms={item.residual_rms_deg:.3f} deg, "
|
||||||
|
f"p95={item.residual_p95_deg:.3f} deg, "
|
||||||
|
f"|bias-bias0|={float(np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)):.3e}"
|
||||||
|
)
|
||||||
|
|
||||||
|
phase_c_requested = (not force_rotation_only) if enable_phase_c is None else bool(enable_phase_c)
|
||||||
|
t = np.zeros(3)
|
||||||
|
if not force_rotation_only:
|
||||||
|
if phase_c_requested:
|
||||||
|
notes.append(
|
||||||
|
"phase-B/C gated off: session-aware translation/gravity/navigation "
|
||||||
|
"states are not implemented yet"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("phase-C disabled; translation is not accepted")
|
||||||
|
if t_prior_m is not None:
|
||||||
|
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
notes.append(
|
||||||
|
"CAD translation is reported as a prior only and is not accepted as calibration"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("rotation-only extrinsic returned after corrected phase-A")
|
||||||
|
|
||||||
|
single_bias = None
|
||||||
|
if len(biases) == 1:
|
||||||
|
single_bias = np.asarray(next(iter(biases.values())), dtype=float)
|
||||||
|
return JointExtrinsicResult(
|
||||||
|
T_IMU_lidar=make_transform(t, r),
|
||||||
|
translation_accepted=False,
|
||||||
|
residual_rms_rot_deg=rot_rms,
|
||||||
|
residual_rms_trans_m=1e9,
|
||||||
|
observability=observability,
|
||||||
|
gyro_bias_rad_s=single_bias,
|
||||||
|
accel_bias_m_s2=None,
|
||||||
|
gravity_m_s2=None,
|
||||||
|
gyro_bias_rad_s_per_session={
|
||||||
|
sid: np.asarray(value, dtype=float) for sid, value in biases.items()
|
||||||
|
},
|
||||||
|
phase_a_sessions=session_results,
|
||||||
|
phase_a_accepted=phase_a_accepted,
|
||||||
|
phase_a_comparison=phase_a_comparison_to_dict(comparison),
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
"""LiDAR keyframe selection."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import LidarFrame
|
||||||
|
from .registration import register_lidar_pair
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class KeyframeSet:
|
||||||
|
indices: tuple[int, ...]
|
||||||
|
frames: tuple[LidarFrame, ...]
|
||||||
|
|
||||||
|
|
||||||
|
def build_keyframes(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
min_translation_m: float = 0.3,
|
||||||
|
min_rotation_deg: float = 3.0,
|
||||||
|
min_registration_fitness: float = 0.5,
|
||||||
|
max_frame_gap: int = 8,
|
||||||
|
) -> KeyframeSet:
|
||||||
|
"""Select keyframes with enough relative motion for hand-eye pairs."""
|
||||||
|
|
||||||
|
if not frames:
|
||||||
|
return KeyframeSet((), ())
|
||||||
|
|
||||||
|
selected = [0]
|
||||||
|
last = 0
|
||||||
|
for index in range(1, len(frames)):
|
||||||
|
if index - last > max_frame_gap:
|
||||||
|
selected.append(index)
|
||||||
|
last = index
|
||||||
|
continue
|
||||||
|
result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
|
||||||
|
if not result.ok or result.fitness < min_registration_fitness:
|
||||||
|
continue
|
||||||
|
if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
|
||||||
|
selected.append(index)
|
||||||
|
last = index
|
||||||
|
|
||||||
|
if selected[-1] != len(frames) - 1 and len(frames) > 1:
|
||||||
|
selected.append(len(frames) - 1)
|
||||||
|
|
||||||
|
unique = tuple(dict.fromkeys(selected))
|
||||||
|
return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))
|
||||||
@@ -0,0 +1,59 @@
|
|||||||
|
"""Coarse LiDAR deskew using a constant body rate over the sweep."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
from .geometry import so3_exp
|
||||||
|
from .time_offset import lidar_time_to_imu_time
|
||||||
|
|
||||||
|
|
||||||
|
def deskew_lidar_frames(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
imu: ImuSeries,
|
||||||
|
*,
|
||||||
|
delta_t_s: float,
|
||||||
|
R_IMU_lidar: np.ndarray | None = None,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
) -> list[LidarFrame]:
|
||||||
|
"""Return deskewed copies when extrinsic is known; otherwise return originals."""
|
||||||
|
|
||||||
|
if R_IMU_lidar is None:
|
||||||
|
return frames
|
||||||
|
|
||||||
|
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
r_li = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3).T
|
||||||
|
output: list[LidarFrame] = []
|
||||||
|
|
||||||
|
for frame in frames:
|
||||||
|
n = frame.points_xyz.shape[0]
|
||||||
|
if n < 10:
|
||||||
|
output.append(frame)
|
||||||
|
continue
|
||||||
|
t_mid_imu = lidar_time_to_imu_time(frame.t_mid_s, delta_t_s)
|
||||||
|
index = int(np.clip(np.searchsorted(imu.t_s, t_mid_imu), 1, imu.t_s.size - 1))
|
||||||
|
omega_lidar = r_li @ (imu.gyro_rad_s[index] - bias)
|
||||||
|
duration = max(frame.t_end_s - frame.t_start_s, 1e-3)
|
||||||
|
rel = np.linspace(-0.5, 0.5, n) * duration
|
||||||
|
deskewed = np.empty_like(frame.points_xyz)
|
||||||
|
# Piecewise-constant rotation over a few time bins.
|
||||||
|
bins = 12
|
||||||
|
edges = np.linspace(-0.5 * duration, 0.5 * duration, bins + 1)
|
||||||
|
for b in range(bins):
|
||||||
|
mask = (rel >= edges[b]) & (rel <= edges[b + 1] if b == bins - 1 else rel < edges[b + 1])
|
||||||
|
if not np.any(mask):
|
||||||
|
continue
|
||||||
|
tau = 0.5 * (edges[b] + edges[b + 1])
|
||||||
|
rot = so3_exp(omega_lidar * float(tau))
|
||||||
|
deskewed[mask] = frame.points_xyz[mask] @ rot.T
|
||||||
|
output.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=frame.frame_id,
|
||||||
|
t_start_s=frame.t_start_s,
|
||||||
|
t_end_s=frame.t_end_s,
|
||||||
|
points_xyz=deskewed,
|
||||||
|
path=frame.path,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return output
|
||||||
@@ -0,0 +1,127 @@
|
|||||||
|
"""LiDAR adapters for the V1 standard intermediate format.
|
||||||
|
|
||||||
|
Accepted input: a directory containing ``frames_index.csv`` and per-frame NPZ files.
|
||||||
|
|
||||||
|
frames_index.csv
|
||||||
|
----------------
|
||||||
|
frame_id,file,t_start,t_end
|
||||||
|
|
||||||
|
Each NPZ referenced by ``file`` must contain:
|
||||||
|
- points: float array shaped (N, 3) in LiDAR Cartesian coordinates (metres)
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import LidarFrame
|
||||||
|
|
||||||
|
|
||||||
|
def _read_frames_index(root: Path) -> tuple[np.ndarray, str]:
|
||||||
|
index_path = root / "frames_index.csv"
|
||||||
|
if not index_path.exists():
|
||||||
|
raise FileNotFoundError(f"missing frames_index.csv under {root}")
|
||||||
|
|
||||||
|
rows = np.genfromtxt(index_path, delimiter=",", names=True, dtype=None, encoding="utf-8")
|
||||||
|
if rows.ndim == 0:
|
||||||
|
rows = np.array([rows])
|
||||||
|
names = set(rows.dtype.names or ())
|
||||||
|
# NumPy may rename reserved name ``file`` to ``file_``.
|
||||||
|
file_key = "filename" if "filename" in names else ("file_" if "file_" in names else "file")
|
||||||
|
required = {"frame_id", "t_start", "t_end"}
|
||||||
|
if not required.issubset(names) or file_key not in names:
|
||||||
|
raise ValueError(
|
||||||
|
f"frames_index.csv must contain frame_id,{file_key}/filename,t_start,t_end; got {sorted(names)}"
|
||||||
|
)
|
||||||
|
return rows, file_key
|
||||||
|
|
||||||
|
|
||||||
|
def list_lidar_frame_entries(path: Path | str) -> list[tuple[str, float, float, Path]]:
|
||||||
|
"""Return ``(frame_id, t_start, t_end, npz_path)`` sorted by mid time (same as ``load_lidar_frames``)."""
|
||||||
|
|
||||||
|
root = Path(path)
|
||||||
|
rows, file_key = _read_frames_index(root)
|
||||||
|
entries: list[tuple[str, float, float, Path]] = []
|
||||||
|
for row in rows:
|
||||||
|
t0 = float(row["t_start"])
|
||||||
|
t1 = float(row["t_end"])
|
||||||
|
entries.append((str(row["frame_id"]), t0, t1, root / str(row[file_key])))
|
||||||
|
entries.sort(key=lambda item: 0.5 * (item[1] + item[2]))
|
||||||
|
return entries
|
||||||
|
|
||||||
|
|
||||||
|
def load_lidar_frame_at(root: Path | str, index: int) -> LidarFrame:
|
||||||
|
"""Load one frame by index in mid-time-sorted order (matches motion-pair ``i``/``j``)."""
|
||||||
|
|
||||||
|
entries = list_lidar_frame_entries(root)
|
||||||
|
if index < 0 or index >= len(entries):
|
||||||
|
raise IndexError(f"frame index {index} outside [0, {len(entries) - 1}] for {root}")
|
||||||
|
frame_id, t0, t1, npz_path = entries[index]
|
||||||
|
with np.load(npz_path) as payload:
|
||||||
|
if "points" not in payload.files:
|
||||||
|
raise ValueError(f"{npz_path} must contain array 'points'")
|
||||||
|
points = np.asarray(payload["points"], dtype=float)
|
||||||
|
if points.ndim != 2 or points.shape[1] < 3:
|
||||||
|
raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
|
||||||
|
return LidarFrame(
|
||||||
|
frame_id=frame_id,
|
||||||
|
t_start_s=t0,
|
||||||
|
t_end_s=t1,
|
||||||
|
points_xyz=points[:, :3],
|
||||||
|
path=npz_path,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def lidar_frame_count(path: Path | str) -> int:
|
||||||
|
return len(list_lidar_frame_entries(path))
|
||||||
|
|
||||||
|
|
||||||
|
def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
|
||||||
|
"""Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
|
||||||
|
|
||||||
|
root = Path(path)
|
||||||
|
rows, file_key = _read_frames_index(root)
|
||||||
|
|
||||||
|
frames: list[LidarFrame] = []
|
||||||
|
for row in rows:
|
||||||
|
frame_id = str(row["frame_id"])
|
||||||
|
rel = str(row[file_key])
|
||||||
|
npz_path = root / rel
|
||||||
|
with np.load(npz_path) as payload:
|
||||||
|
if "points" not in payload.files:
|
||||||
|
raise ValueError(f"{npz_path} must contain array 'points'")
|
||||||
|
points = np.asarray(payload["points"], dtype=float)
|
||||||
|
if points.ndim != 2 or points.shape[1] < 3:
|
||||||
|
raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
|
||||||
|
frames.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=frame_id,
|
||||||
|
t_start_s=float(row["t_start"]),
|
||||||
|
t_end_s=float(row["t_end"]),
|
||||||
|
points_xyz=points[:, :3],
|
||||||
|
path=npz_path,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
frames.sort(key=lambda frame: frame.t_mid_s)
|
||||||
|
return frames
|
||||||
|
|
||||||
|
|
||||||
|
def save_lidar_session(
|
||||||
|
root: Path | str,
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
points_dirname: str = "frames",
|
||||||
|
) -> None:
|
||||||
|
"""Write a LiDAR session directory in the standard intermediate format."""
|
||||||
|
|
||||||
|
destination = Path(root)
|
||||||
|
frames_dir = destination / points_dirname
|
||||||
|
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||||
|
index_rows: list[str] = ["frame_id,filename,t_start,t_end"]
|
||||||
|
for index, frame in enumerate(frames):
|
||||||
|
relative = f"{points_dirname}/frame_{index:05d}.npz"
|
||||||
|
np.savez_compressed(destination / relative, points=np.asarray(frame.points_xyz, dtype=float))
|
||||||
|
index_rows.append(f"{frame.frame_id},{relative},{frame.t_start_s:.9f},{frame.t_end_s:.9f}")
|
||||||
|
(destination / "frames_index.csv").write_text("\n".join(index_rows) + "\n", encoding="utf-8")
|
||||||
@@ -0,0 +1,220 @@
|
|||||||
|
"""Build IMU/LiDAR relative-motion pairs for hand-eye calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Callable
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from time import perf_counter
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame, MotionPair
|
||||||
|
from .geometry import make_transform, rotation_angle_deg
|
||||||
|
from .imu_preintegration import preintegrate_imu
|
||||||
|
from .registration import register_lidar_pair
|
||||||
|
from .time_offset import lidar_time_to_imu_time
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MotionPairSet:
|
||||||
|
pairs: tuple[MotionPair, ...]
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def build_motion_pairs(
|
||||||
|
*,
|
||||||
|
session_id: str,
|
||||||
|
keyframes: list[LidarFrame],
|
||||||
|
keyframe_indices: list[int] | tuple[int, ...],
|
||||||
|
imu: ImuSeries,
|
||||||
|
delta_t_s: float,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
acc_bias_m_s2: np.ndarray | None = None,
|
||||||
|
min_rotation_deg: float = 3.0,
|
||||||
|
min_translation_m: float = 0.3,
|
||||||
|
min_registration_fitness: float = 0.5,
|
||||||
|
max_imu_gap_s: float = 0.05,
|
||||||
|
max_lidar_gap_s: float = 1.0,
|
||||||
|
all_frame_times_s: np.ndarray | None = None,
|
||||||
|
max_index_span: int = 4,
|
||||||
|
progress_callback: Callable[[dict[str, Any]], None] | None = None,
|
||||||
|
) -> MotionPairSet:
|
||||||
|
"""Create A/B motion pairs between nearby keyframes.
|
||||||
|
|
||||||
|
IMU side uses full Phase-C preintegration (``ΔR/Δv/Δp``, ``Σ9``, ``J_bg/J_ba``).
|
||||||
|
Rotation hand-eye still consumes ``R_A = ΔR`` only.
|
||||||
|
"""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
pairs: list[MotionPair] = []
|
||||||
|
rejected_fitness = 0
|
||||||
|
rejected_imu_gap = 0
|
||||||
|
rejected_lidar_gap = 0
|
||||||
|
frame_times = (
|
||||||
|
None
|
||||||
|
if all_frame_times_s is None
|
||||||
|
else np.asarray(all_frame_times_s, dtype=float).reshape(-1)
|
||||||
|
)
|
||||||
|
bias_g = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
bias_a = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float)
|
||||||
|
n = len(keyframes)
|
||||||
|
if n < 2:
|
||||||
|
return MotionPairSet((), ("need at least two keyframes",))
|
||||||
|
|
||||||
|
total_candidates = sum(max(n - span, 0) for span in range(1, max_index_span + 1))
|
||||||
|
processed_candidates = 0
|
||||||
|
started_at = perf_counter()
|
||||||
|
last_progress_at = started_at
|
||||||
|
|
||||||
|
def report_progress(*, event: str, span: int, force: bool = False) -> None:
|
||||||
|
nonlocal last_progress_at
|
||||||
|
if progress_callback is None:
|
||||||
|
return
|
||||||
|
now = perf_counter()
|
||||||
|
if not force and processed_candidates > 1 and now - last_progress_at < 10.0:
|
||||||
|
return
|
||||||
|
last_progress_at = now
|
||||||
|
progress_callback(
|
||||||
|
{
|
||||||
|
"event": event,
|
||||||
|
"processed_candidates": processed_candidates,
|
||||||
|
"total_candidates": total_candidates,
|
||||||
|
"progress_pct": 100.0 * processed_candidates / max(total_candidates, 1),
|
||||||
|
"current_span": span,
|
||||||
|
"max_span": max_index_span,
|
||||||
|
"accepted_pairs": len(pairs),
|
||||||
|
"rejected_fitness": rejected_fitness,
|
||||||
|
"rejected_imu_gap": rejected_imu_gap,
|
||||||
|
"rejected_lidar_gap": rejected_lidar_gap,
|
||||||
|
"elapsed_s": now - started_at,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
report_progress(event="start", span=1, force=True)
|
||||||
|
|
||||||
|
for span in range(1, max_index_span + 1):
|
||||||
|
for start in range(0, n - span):
|
||||||
|
processed_candidates += 1
|
||||||
|
report_progress(event="running", span=span)
|
||||||
|
i = start
|
||||||
|
j = start + span
|
||||||
|
frame_i = keyframes[i]
|
||||||
|
frame_j = keyframes[j]
|
||||||
|
source_i = int(keyframe_indices[i])
|
||||||
|
source_j = int(keyframe_indices[j])
|
||||||
|
if frame_times is not None:
|
||||||
|
lo = min(source_i, source_j)
|
||||||
|
hi = max(source_i, source_j)
|
||||||
|
local_times = frame_times[lo : hi + 1]
|
||||||
|
if local_times.size >= 2 and np.any(np.diff(local_times) > max_lidar_gap_s):
|
||||||
|
rejected_lidar_gap += 1
|
||||||
|
continue
|
||||||
|
reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
|
||||||
|
if not reg.ok:
|
||||||
|
continue
|
||||||
|
if reg.fitness < min_registration_fitness:
|
||||||
|
rejected_fitness += 1
|
||||||
|
continue
|
||||||
|
if reg.rotation_deg < min_rotation_deg and reg.translation_m < min_translation_m:
|
||||||
|
continue
|
||||||
|
|
||||||
|
t_i_imu = lidar_time_to_imu_time(frame_i.t_mid_s, delta_t_s)
|
||||||
|
t_j_imu = lidar_time_to_imu_time(frame_j.t_mid_s, delta_t_s)
|
||||||
|
if t_j_imu <= t_i_imu:
|
||||||
|
continue
|
||||||
|
if t_i_imu < imu.t_s[0] or t_j_imu > imu.t_s[-1]:
|
||||||
|
continue
|
||||||
|
imu_lo = max(int(np.searchsorted(imu.t_s, t_i_imu, side="right")) - 1, 0)
|
||||||
|
imu_hi = min(
|
||||||
|
int(np.searchsorted(imu.t_s, t_j_imu, side="left")) + 1,
|
||||||
|
imu.t_s.size,
|
||||||
|
)
|
||||||
|
if imu_hi - imu_lo >= 2 and np.any(
|
||||||
|
np.diff(imu.t_s[imu_lo:imu_hi]) > max_imu_gap_s
|
||||||
|
):
|
||||||
|
rejected_imu_gap += 1
|
||||||
|
continue
|
||||||
|
|
||||||
|
preint = preintegrate_imu(
|
||||||
|
imu.t_s,
|
||||||
|
imu.gyro_rad_s,
|
||||||
|
imu.acc_m_s2,
|
||||||
|
t_i_imu,
|
||||||
|
t_j_imu,
|
||||||
|
bias_g,
|
||||||
|
bias_a,
|
||||||
|
)
|
||||||
|
r_a = preint.delta_R
|
||||||
|
r_b = reg.transform[:3, :3]
|
||||||
|
t_b = reg.transform[:3, 3]
|
||||||
|
rot_a = rotation_angle_deg(r_a)
|
||||||
|
if abs(rot_a - reg.rotation_deg) > max(15.0, 1.0 * max(rot_a, reg.rotation_deg)):
|
||||||
|
continue
|
||||||
|
|
||||||
|
pairs.append(
|
||||||
|
MotionPair(
|
||||||
|
session_id=session_id,
|
||||||
|
i=int(keyframe_indices[i]),
|
||||||
|
j=int(keyframe_indices[j]),
|
||||||
|
t_i_s=frame_i.t_mid_s,
|
||||||
|
t_j_s=frame_j.t_mid_s,
|
||||||
|
R_A=r_a,
|
||||||
|
R_B=r_b,
|
||||||
|
t_A_m=np.asarray(preint.delta_p, dtype=float),
|
||||||
|
t_B_m=np.asarray(t_b, dtype=float),
|
||||||
|
fitness=reg.fitness,
|
||||||
|
metadata={
|
||||||
|
"backend": reg.backend,
|
||||||
|
"rotation_deg_B": reg.rotation_deg,
|
||||||
|
"translation_m_B": reg.translation_m,
|
||||||
|
"rotation_deg_A": rot_a,
|
||||||
|
"weight": preint.weight,
|
||||||
|
"duration_s": preint.duration_s,
|
||||||
|
"mean_gyro_norm": preint.mean_gyro_norm,
|
||||||
|
"preint_sigma_rad": preint.sigma_rad,
|
||||||
|
"cov": preint.cov[0:3, 0:3].tolist(),
|
||||||
|
"cov9": preint.cov.tolist(),
|
||||||
|
"J_bg": preint.J_bg[0:3, 0:3].tolist(),
|
||||||
|
"J_bg9": preint.J_bg.tolist(),
|
||||||
|
"J_ba": preint.J_ba.tolist(),
|
||||||
|
"delta_v": preint.delta_v.tolist(),
|
||||||
|
"delta_p": preint.delta_p.tolist(),
|
||||||
|
"t_i_imu_s": t_i_imu,
|
||||||
|
"t_j_imu_s": t_j_imu,
|
||||||
|
"gyro_bias0_rad_s": bias_g.tolist(),
|
||||||
|
"accel_bias0_m_s2": bias_a.tolist(),
|
||||||
|
"time_offset_s": float(delta_t_s),
|
||||||
|
"keyframe_span": int(span),
|
||||||
|
"is_consecutive": bool(span == 1),
|
||||||
|
"modeling": "imu_preintegration_factor_phase_c",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
report_progress(event="complete", span=max_index_span, force=True)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)"
|
||||||
|
)
|
||||||
|
notes.append(
|
||||||
|
"quality rejects: "
|
||||||
|
f"fitness<{min_registration_fitness:.2f}: {rejected_fitness}, "
|
||||||
|
f"IMU gap>{max_imu_gap_s:.3f}s: {rejected_imu_gap}, "
|
||||||
|
f"LiDAR gap>{max_lidar_gap_s:.3f}s: {rejected_lidar_gap}"
|
||||||
|
)
|
||||||
|
return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes))
|
||||||
|
|
||||||
|
|
||||||
|
def pairs_to_transforms(pairs: tuple[MotionPair, ...]) -> tuple[list[np.ndarray], list[np.ndarray]]:
|
||||||
|
"""Helper returning SE(3) lists when translations are present."""
|
||||||
|
|
||||||
|
a_list: list[np.ndarray] = []
|
||||||
|
b_list: list[np.ndarray] = []
|
||||||
|
for pair in pairs:
|
||||||
|
if pair.t_B_m is None:
|
||||||
|
continue
|
||||||
|
t_a = np.zeros(3) if pair.t_A_m is None else pair.t_A_m
|
||||||
|
a_list.append(make_transform(t_a, pair.R_A))
|
||||||
|
b_list.append(make_transform(pair.t_B_m, pair.R_B))
|
||||||
|
return a_list, b_list
|
||||||
@@ -0,0 +1,154 @@
|
|||||||
|
"""Serialize / deserialize motion pairs for fast visualization."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import MotionPair
|
||||||
|
|
||||||
|
SCHEMA_VERSION = 2
|
||||||
|
|
||||||
|
# Keep visualization fields plus the compact 3x3 rotation metadata needed to
|
||||||
|
# rerun Phase-A without repeating LiDAR registration. Full 9x9 Phase-C matrices
|
||||||
|
# remain excluded from this cache.
|
||||||
|
_METADATA_KEEP = frozenset(
|
||||||
|
{
|
||||||
|
"backend",
|
||||||
|
"rotation_deg_A",
|
||||||
|
"rotation_deg_B",
|
||||||
|
"translation_m_B",
|
||||||
|
"weight",
|
||||||
|
"duration_s",
|
||||||
|
"mean_gyro_norm",
|
||||||
|
"preint_sigma_rad",
|
||||||
|
"cov",
|
||||||
|
"J_bg",
|
||||||
|
"phase_a_metadata_rehydrated",
|
||||||
|
"rehydrated_R_A_error_deg",
|
||||||
|
"t_i_imu_s",
|
||||||
|
"t_j_imu_s",
|
||||||
|
"gyro_bias0_rad_s",
|
||||||
|
"accel_bias0_m_s2",
|
||||||
|
"time_offset_s",
|
||||||
|
"keyframe_span",
|
||||||
|
"is_consecutive",
|
||||||
|
"modeling",
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _to_list(value: Any) -> Any:
|
||||||
|
if isinstance(value, np.ndarray):
|
||||||
|
return value.tolist()
|
||||||
|
if isinstance(value, (np.floating, np.integer, np.bool_)):
|
||||||
|
return value.item()
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def pair_to_dict(pair: MotionPair) -> dict[str, Any]:
|
||||||
|
meta = {
|
||||||
|
str(k): _to_list(v)
|
||||||
|
for k, v in (pair.metadata or {}).items()
|
||||||
|
if str(k) in _METADATA_KEEP
|
||||||
|
}
|
||||||
|
return {
|
||||||
|
"session_id": pair.session_id,
|
||||||
|
"i": int(pair.i),
|
||||||
|
"j": int(pair.j),
|
||||||
|
"t_i_s": float(pair.t_i_s),
|
||||||
|
"t_j_s": float(pair.t_j_s),
|
||||||
|
"R_A": np.asarray(pair.R_A, dtype=float).reshape(3, 3).tolist(),
|
||||||
|
"R_B": np.asarray(pair.R_B, dtype=float).reshape(3, 3).tolist(),
|
||||||
|
"t_A_m": None if pair.t_A_m is None else np.asarray(pair.t_A_m, dtype=float).reshape(3).tolist(),
|
||||||
|
"t_B_m": None if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float).reshape(3).tolist(),
|
||||||
|
"fitness": float(pair.fitness),
|
||||||
|
"metadata": meta,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def pair_from_dict(payload: dict[str, Any]) -> MotionPair:
|
||||||
|
t_a = payload.get("t_A_m")
|
||||||
|
t_b = payload.get("t_B_m")
|
||||||
|
return MotionPair(
|
||||||
|
session_id=str(payload.get("session_id", "")),
|
||||||
|
i=int(payload["i"]),
|
||||||
|
j=int(payload["j"]),
|
||||||
|
t_i_s=float(payload["t_i_s"]),
|
||||||
|
t_j_s=float(payload["t_j_s"]),
|
||||||
|
R_A=np.asarray(payload["R_A"], dtype=float).reshape(3, 3),
|
||||||
|
R_B=np.asarray(payload["R_B"], dtype=float).reshape(3, 3),
|
||||||
|
t_A_m=None if t_a is None else np.asarray(t_a, dtype=float).reshape(3),
|
||||||
|
t_B_m=None if t_b is None else np.asarray(t_b, dtype=float).reshape(3),
|
||||||
|
fitness=float(payload.get("fitness", 0.0)),
|
||||||
|
metadata=dict(payload.get("metadata") or {}),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def build_motion_pairs_payload(
|
||||||
|
*,
|
||||||
|
prepared_sessions: list[dict[str, Any]],
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Build a JSON-serializable cache from pipeline ``prepared`` session dicts."""
|
||||||
|
|
||||||
|
sessions_out: list[dict[str, Any]] = []
|
||||||
|
for prep in prepared_sessions:
|
||||||
|
pairs = prep.get("pairs") or ()
|
||||||
|
sessions_out.append(
|
||||||
|
{
|
||||||
|
"session_id": prep.get("session_id"),
|
||||||
|
"delta_t_s": float(prep.get("time_offset_s", 0.0)),
|
||||||
|
"gyro_bias_rad_s": np.asarray(prep.get("gyro_bias_rad_s", np.zeros(3)), dtype=float)
|
||||||
|
.reshape(3)
|
||||||
|
.tolist(),
|
||||||
|
"pair_count": len(pairs),
|
||||||
|
"pairs": [pair_to_dict(pair) for pair in pairs],
|
||||||
|
}
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"schema_version": SCHEMA_VERSION,
|
||||||
|
"sessions": sessions_out,
|
||||||
|
"note": "Cached motion pairs for visualization; A=IMU preintegration, B=LiDAR registration",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def save_motion_pairs(path: Path | str, payload: dict[str, Any]) -> Path:
|
||||||
|
destination = Path(path)
|
||||||
|
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
destination.write_text(json.dumps(payload, indent=2), encoding="utf-8")
|
||||||
|
return destination
|
||||||
|
|
||||||
|
|
||||||
|
def load_motion_pairs(path: Path | str) -> dict[str, Any]:
|
||||||
|
payload = json.loads(Path(path).read_text(encoding="utf-8"))
|
||||||
|
version = int(payload.get("schema_version", 0))
|
||||||
|
if version not in {1, SCHEMA_VERSION}:
|
||||||
|
raise ValueError(
|
||||||
|
f"unsupported motion_pairs schema_version={version}; "
|
||||||
|
f"expected 1 or {SCHEMA_VERSION}"
|
||||||
|
)
|
||||||
|
return payload
|
||||||
|
|
||||||
|
|
||||||
|
def pairs_for_session(payload: dict[str, Any], session_id: str | None = None) -> list[MotionPair]:
|
||||||
|
sessions = payload.get("sessions") or []
|
||||||
|
if not sessions:
|
||||||
|
return []
|
||||||
|
if session_id is None:
|
||||||
|
chosen = sessions[0]
|
||||||
|
else:
|
||||||
|
chosen = next((s for s in sessions if s.get("session_id") == session_id), None)
|
||||||
|
if chosen is None:
|
||||||
|
raise KeyError(f"session_id {session_id!r} not found in motion_pairs cache")
|
||||||
|
return [pair_from_dict(item) for item in chosen.get("pairs") or []]
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_motion_pairs_path(summary_path: Path | str) -> Path | None:
|
||||||
|
"""Return ``motion_pairs.json`` next to a summary if it exists."""
|
||||||
|
|
||||||
|
summary = Path(summary_path)
|
||||||
|
candidate = summary.parent / "motion_pairs.json"
|
||||||
|
return candidate if candidate.is_file() else None
|
||||||
@@ -0,0 +1,114 @@
|
|||||||
|
"""Normalized-Jacobian observability analysis for rotation / SE(3) gates."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import MotionPair
|
||||||
|
from .geometry import skew, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ObservabilityReport:
|
||||||
|
rotation_observable: bool
|
||||||
|
translation_observable: bool
|
||||||
|
condition_rotation: float
|
||||||
|
condition_translation: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_jacobian(pairs: list[MotionPair], r_x: np.ndarray) -> np.ndarray:
|
||||||
|
rows = []
|
||||||
|
for pair in pairs:
|
||||||
|
# Residual r = log(R_x^T R_A R_x R_B^T); approximate J w.r.t. left perturbation of R_x.
|
||||||
|
# Use finite-difference columns for robustness in V1.
|
||||||
|
base = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||||
|
cols = []
|
||||||
|
eps = 1e-5
|
||||||
|
for axis in range(3):
|
||||||
|
delta = np.zeros(3)
|
||||||
|
delta[axis] = eps
|
||||||
|
r_pert = r_x @ (np.eye(3) + skew(delta))
|
||||||
|
# Orthonormalize lightly
|
||||||
|
u, _, vt = np.linalg.svd(r_pert)
|
||||||
|
r_pert = u @ vt
|
||||||
|
pert = so3_log(r_pert.T @ pair.R_A @ r_pert @ pair.R_B.T)
|
||||||
|
cols.append((pert - base) / eps)
|
||||||
|
rows.append(np.column_stack(cols))
|
||||||
|
return np.vstack(rows) if rows else np.zeros((0, 3))
|
||||||
|
|
||||||
|
|
||||||
|
def analyze_observability(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
condition_threshold: float = 100.0,
|
||||||
|
) -> ObservabilityReport:
|
||||||
|
"""Gate whether rotation-only or full SE(3) should be accepted."""
|
||||||
|
|
||||||
|
usable = list(pairs)
|
||||||
|
notes: list[str] = []
|
||||||
|
if len(usable) < 3:
|
||||||
|
return ObservabilityReport(False, False, 1e9, 1e9, ("insufficient pairs",))
|
||||||
|
|
||||||
|
j_r = _rotation_jacobian(usable, np.asarray(r_x, dtype=float))
|
||||||
|
if j_r.size == 0:
|
||||||
|
return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
|
||||||
|
|
||||||
|
singular = np.linalg.svd(j_r, compute_uv=False)
|
||||||
|
cond_r = float(singular[0] / max(singular[-1], 1e-12))
|
||||||
|
rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1)))
|
||||||
|
rotation_ok = (
|
||||||
|
cond_r < condition_threshold
|
||||||
|
and rotation_information > 1e-3
|
||||||
|
and singular[-1] > 1e-6
|
||||||
|
)
|
||||||
|
|
||||||
|
# Translation lever arm is observable through stacked (R_A - I). Pure
|
||||||
|
# planar yaw leaves its vertical column in the nullspace and must fail.
|
||||||
|
translation_rows = [
|
||||||
|
np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
|
||||||
|
for pair in usable
|
||||||
|
if pair.t_B_m is not None
|
||||||
|
]
|
||||||
|
if translation_rows:
|
||||||
|
j_t = np.vstack(translation_rows)
|
||||||
|
singular_t = np.linalg.svd(j_t, compute_uv=False)
|
||||||
|
cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
|
||||||
|
translation_information = float(
|
||||||
|
singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
cond_t = 1e9
|
||||||
|
translation_information = 0.0
|
||||||
|
translation_ok = (
|
||||||
|
len(translation_rows) >= 5
|
||||||
|
and cond_t < condition_threshold
|
||||||
|
and translation_information > 0.02
|
||||||
|
)
|
||||||
|
|
||||||
|
if not rotation_ok:
|
||||||
|
notes.append(
|
||||||
|
f"rotation not observable: condition={cond_r:.1f}, "
|
||||||
|
f"min_information={rotation_information:.3e}"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append(
|
||||||
|
f"rotation observable: condition={cond_r:.1f}, "
|
||||||
|
f"min_information={rotation_information:.3e}"
|
||||||
|
)
|
||||||
|
if not translation_ok:
|
||||||
|
notes.append(
|
||||||
|
f"translation not observable: condition={cond_t:.1f}, "
|
||||||
|
f"min_information={translation_information:.3e}; "
|
||||||
|
"full SE3 will be rejected"
|
||||||
|
)
|
||||||
|
return ObservabilityReport(
|
||||||
|
rotation_observable=rotation_ok,
|
||||||
|
translation_observable=translation_ok,
|
||||||
|
condition_rotation=cond_r,
|
||||||
|
condition_translation=cond_t,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,290 @@
|
|||||||
|
"""Cached Phase-A replay: rehydrate gyro factors, compare variants, write reports."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
from collections import defaultdict
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .imu_io import load_imu_samples
|
||||||
|
from .motion_pairs_io import (
|
||||||
|
build_motion_pairs_payload,
|
||||||
|
load_motion_pairs,
|
||||||
|
pair_from_dict,
|
||||||
|
save_motion_pairs,
|
||||||
|
)
|
||||||
|
from .phase_a import (
|
||||||
|
ProgressCallback,
|
||||||
|
phase_a_comparison_to_dict,
|
||||||
|
phase_a_metadata_complete,
|
||||||
|
rehydrate_phase_a_pairs,
|
||||||
|
solve_phase_a_comparison,
|
||||||
|
)
|
||||||
|
from .vehicle_config import load_vehicle_config, prior_enabled
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_prior(
|
||||||
|
vehicle_config_path: Path,
|
||||||
|
) -> tuple[np.ndarray | None, float]:
|
||||||
|
config = load_vehicle_config(vehicle_config_path)
|
||||||
|
if not prior_enabled(config, "rotation_prior"):
|
||||||
|
return None, 15.0
|
||||||
|
prior = (config.get("initialization") or {}).get("rotation_prior") or {}
|
||||||
|
matrix = prior.get("R_IMU_lidar")
|
||||||
|
if matrix is None:
|
||||||
|
return None, float(prior.get("sigma_deg", 15.0))
|
||||||
|
return (
|
||||||
|
np.asarray(matrix, dtype=float).reshape(3, 3),
|
||||||
|
float(prior.get("sigma_deg", 15.0)),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _sanitize_json(value: Any) -> Any:
|
||||||
|
if isinstance(value, dict):
|
||||||
|
return {str(key): _sanitize_json(item) for key, item in value.items()}
|
||||||
|
if isinstance(value, (list, tuple)):
|
||||||
|
return [_sanitize_json(item) for item in value]
|
||||||
|
if isinstance(value, np.ndarray):
|
||||||
|
return _sanitize_json(value.tolist())
|
||||||
|
if isinstance(value, (np.floating, float)):
|
||||||
|
number = float(value)
|
||||||
|
return number if np.isfinite(number) else None
|
||||||
|
if isinstance(value, (np.integer, np.bool_)):
|
||||||
|
return value.item()
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def _write_json(path: Path, payload: Any) -> None:
|
||||||
|
path.write_text(
|
||||||
|
json.dumps(_sanitize_json(payload), indent=2, ensure_ascii=False) + "\n",
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _load_cached_sessions(
|
||||||
|
motion_pairs_path: Path,
|
||||||
|
) -> tuple[
|
||||||
|
dict[str, Any],
|
||||||
|
list,
|
||||||
|
dict[str, np.ndarray],
|
||||||
|
dict[str, float],
|
||||||
|
]:
|
||||||
|
payload = load_motion_pairs(motion_pairs_path)
|
||||||
|
pairs = []
|
||||||
|
biases: dict[str, np.ndarray] = {}
|
||||||
|
offsets: dict[str, float] = {}
|
||||||
|
for session in payload.get("sessions") or []:
|
||||||
|
session_id = str(session["session_id"])
|
||||||
|
biases[session_id] = np.asarray(
|
||||||
|
session.get("gyro_bias_rad_s", np.zeros(3)),
|
||||||
|
dtype=float,
|
||||||
|
).reshape(3)
|
||||||
|
offsets[session_id] = float(session.get("delta_t_s", 0.0))
|
||||||
|
pairs.extend(
|
||||||
|
pair_from_dict(item)
|
||||||
|
for item in session.get("pairs") or []
|
||||||
|
)
|
||||||
|
if not pairs:
|
||||||
|
raise ValueError(f"motion-pair cache is empty: {motion_pairs_path}")
|
||||||
|
return payload, pairs, biases, offsets
|
||||||
|
|
||||||
|
|
||||||
|
def run_phase_a_replay(
|
||||||
|
*,
|
||||||
|
motion_pairs_path: Path,
|
||||||
|
vehicle_config_path: Path,
|
||||||
|
output_directory: Path,
|
||||||
|
imu_paths_by_session: dict[str, Path] | None = None,
|
||||||
|
excluded_sessions: set[str] | None = None,
|
||||||
|
strong_rotation_min_deg: float = 1.0,
|
||||||
|
decorrelation_block_s: float = 3.0,
|
||||||
|
max_pairs_per_block: int = 1,
|
||||||
|
bias_prior_sigma_rad_s: float = 0.002,
|
||||||
|
yaw_std_max_deg: float = 0.5,
|
||||||
|
leave_one_out_yaw_range_max_deg: float = 1.0,
|
||||||
|
data_prior_difference_max_deg: float = 1.0,
|
||||||
|
max_nfev: int = 200,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Run Phase-A only. Existing LiDAR relative motions are never recomputed."""
|
||||||
|
|
||||||
|
output_directory.mkdir(parents=True, exist_ok=True)
|
||||||
|
source_payload, pairs, bias0, offsets = _load_cached_sessions(
|
||||||
|
motion_pairs_path
|
||||||
|
)
|
||||||
|
session_ids = sorted(bias0)
|
||||||
|
if progress_callback is not None:
|
||||||
|
progress_callback(
|
||||||
|
"cache_loaded",
|
||||||
|
{
|
||||||
|
"schema_version": source_payload.get("schema_version"),
|
||||||
|
"sessions": len(session_ids),
|
||||||
|
"pairs": len(pairs),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
rehydration_report: dict[str, Any] = {
|
||||||
|
"required": not phase_a_metadata_complete(pairs),
|
||||||
|
"pair_count": len(pairs),
|
||||||
|
}
|
||||||
|
if not phase_a_metadata_complete(pairs):
|
||||||
|
supplied_paths = {} if imu_paths_by_session is None else imu_paths_by_session
|
||||||
|
missing = [sid for sid in session_ids if sid not in supplied_paths]
|
||||||
|
if missing:
|
||||||
|
raise ValueError(
|
||||||
|
"v1 cache lacks J_bg/cov; provide --session-imu for: "
|
||||||
|
+ ", ".join(missing)
|
||||||
|
)
|
||||||
|
imu_by_session = {
|
||||||
|
sid: load_imu_samples(supplied_paths[sid])
|
||||||
|
for sid in session_ids
|
||||||
|
}
|
||||||
|
pairs, details = rehydrate_phase_a_pairs(
|
||||||
|
pairs,
|
||||||
|
imu_by_session=imu_by_session,
|
||||||
|
bias0_by_session=bias0,
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
)
|
||||||
|
rehydration_report.update(details)
|
||||||
|
if float(details["max_R_A_error_deg"]) > 0.05:
|
||||||
|
raise ValueError(
|
||||||
|
"rehydrated IMU rotations do not match cached R_A: "
|
||||||
|
f"max error={details['max_R_A_error_deg']:.6f} deg; "
|
||||||
|
"check session-to-IMU path mapping"
|
||||||
|
)
|
||||||
|
|
||||||
|
grouped: dict[str, list] = defaultdict(list)
|
||||||
|
for pair in pairs:
|
||||||
|
grouped[pair.session_id].append(pair)
|
||||||
|
enriched_payload = build_motion_pairs_payload(
|
||||||
|
prepared_sessions=[
|
||||||
|
{
|
||||||
|
"session_id": sid,
|
||||||
|
"time_offset_s": offsets[sid],
|
||||||
|
"gyro_bias_rad_s": bias0[sid],
|
||||||
|
"pairs": tuple(grouped[sid]),
|
||||||
|
}
|
||||||
|
for sid in session_ids
|
||||||
|
]
|
||||||
|
)
|
||||||
|
enriched_cache_path = save_motion_pairs(
|
||||||
|
output_directory / "motion_pairs_phase_a_v2.json",
|
||||||
|
enriched_payload,
|
||||||
|
)
|
||||||
|
|
||||||
|
rotation_prior, rotation_prior_sigma_deg = _rotation_prior(
|
||||||
|
vehicle_config_path
|
||||||
|
)
|
||||||
|
comparison = solve_phase_a_comparison(
|
||||||
|
pairs,
|
||||||
|
gyro_bias_rad_s_by_session=bias0,
|
||||||
|
rotation_prior=rotation_prior,
|
||||||
|
rotation_prior_sigma_deg=rotation_prior_sigma_deg,
|
||||||
|
preexcluded_session_ids=excluded_sessions,
|
||||||
|
strong_rotation_min_deg=strong_rotation_min_deg,
|
||||||
|
decorrelation_block_s=decorrelation_block_s,
|
||||||
|
max_pairs_per_block=max_pairs_per_block,
|
||||||
|
bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
|
||||||
|
yaw_std_max_deg=yaw_std_max_deg,
|
||||||
|
leave_one_out_yaw_range_max_deg=(
|
||||||
|
leave_one_out_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
data_prior_difference_max_deg=data_prior_difference_max_deg,
|
||||||
|
run_leave_one_out=True,
|
||||||
|
max_nfev=max_nfev,
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
)
|
||||||
|
full = phase_a_comparison_to_dict(comparison)
|
||||||
|
full["input"] = {
|
||||||
|
"motion_pairs": str(motion_pairs_path),
|
||||||
|
"source_schema_version": source_payload.get("schema_version"),
|
||||||
|
"vehicle_config": str(vehicle_config_path),
|
||||||
|
"session_imu_paths": {
|
||||||
|
sid: str(path)
|
||||||
|
for sid, path in (imu_paths_by_session or {}).items()
|
||||||
|
},
|
||||||
|
"excluded_sessions": sorted(excluded_sessions or set()),
|
||||||
|
}
|
||||||
|
full["rehydration"] = rehydration_report
|
||||||
|
full["enriched_cache"] = str(enriched_cache_path)
|
||||||
|
full["parameters"] = {
|
||||||
|
"strong_rotation_min_deg": strong_rotation_min_deg,
|
||||||
|
"decorrelation_block_s": decorrelation_block_s,
|
||||||
|
"max_pairs_per_block": max_pairs_per_block,
|
||||||
|
"bias_prior_sigma_rad_s": bias_prior_sigma_rad_s,
|
||||||
|
"rotation_prior_sigma_deg": rotation_prior_sigma_deg,
|
||||||
|
"yaw_std_max_deg": yaw_std_max_deg,
|
||||||
|
"leave_one_out_yaw_range_max_deg": (
|
||||||
|
leave_one_out_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
"data_prior_difference_max_deg": (
|
||||||
|
data_prior_difference_max_deg
|
||||||
|
),
|
||||||
|
"max_nfev": max_nfev,
|
||||||
|
}
|
||||||
|
|
||||||
|
variants = full["variants"]
|
||||||
|
summary = {
|
||||||
|
"status": comparison.solution_status,
|
||||||
|
"accepted": comparison.accepted,
|
||||||
|
"partial_accepted": comparison.partial_accepted,
|
||||||
|
"acceptance_checks": comparison.acceptance_checks,
|
||||||
|
"primary_result": comparison.recommended_result,
|
||||||
|
"variants": {
|
||||||
|
name: {
|
||||||
|
"rpy_deg_xyz": item["rpy_deg_xyz"],
|
||||||
|
"R_IMU_lidar": item["R_IMU_lidar"],
|
||||||
|
"residual_rms_deg": item["residual_rms_deg"],
|
||||||
|
"residual_p95_deg": item["residual_p95_deg"],
|
||||||
|
"accepted": item["accepted"],
|
||||||
|
"gyro_bias_rad_s_per_session": item[
|
||||||
|
"gyro_bias_rad_s_per_session"
|
||||||
|
],
|
||||||
|
}
|
||||||
|
for name, item in variants.items()
|
||||||
|
if item is not None
|
||||||
|
},
|
||||||
|
"marginal_observability_A1": full[
|
||||||
|
"marginal_observability_A1"
|
||||||
|
],
|
||||||
|
"data_vs_prior_yaw_diff_deg": (
|
||||||
|
comparison.data_vs_prior_yaw_diff_deg
|
||||||
|
),
|
||||||
|
"data_vs_prior_geodesic_deg": (
|
||||||
|
comparison.data_vs_prior_geodesic_deg
|
||||||
|
),
|
||||||
|
"leave_one_out_yaw_range_deg": (
|
||||||
|
comparison.leave_one_out_yaw_range_deg
|
||||||
|
),
|
||||||
|
"leave_one_out_observable_max_deg": (
|
||||||
|
comparison.leave_one_out_observable_max_deg
|
||||||
|
),
|
||||||
|
"strong_pair_candidate_count": (
|
||||||
|
comparison.strong_pair_candidate_count
|
||||||
|
),
|
||||||
|
"decorrelated_pair_count": comparison.decorrelated_pair_count,
|
||||||
|
"strong_pair_counts_per_session": (
|
||||||
|
comparison.strong_pair_counts_per_session
|
||||||
|
),
|
||||||
|
"excluded_sessions": list(comparison.excluded_sessions),
|
||||||
|
"rehydration": rehydration_report,
|
||||||
|
"comparison_file": "phase_a_comparison.json",
|
||||||
|
"observability_file": "phase_a_observability.json",
|
||||||
|
"leave_one_out_file": "phase_a_leave_one_out.json",
|
||||||
|
"enriched_cache_file": enriched_cache_path.name,
|
||||||
|
}
|
||||||
|
|
||||||
|
_write_json(output_directory / "phase_a_comparison.json", full)
|
||||||
|
_write_json(
|
||||||
|
output_directory / "phase_a_observability.json",
|
||||||
|
full["marginal_observability_A1"],
|
||||||
|
)
|
||||||
|
_write_json(
|
||||||
|
output_directory / "phase_a_leave_one_out.json",
|
||||||
|
full["leave_one_out"],
|
||||||
|
)
|
||||||
|
_write_json(output_directory / "phase_a_summary.json", summary)
|
||||||
|
return summary
|
||||||
@@ -0,0 +1,941 @@
|
|||||||
|
"""Executable LiDAR–IMU calibration pipeline (V1)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Callable
|
||||||
|
from dataclasses import asdict, dataclass, replace
|
||||||
|
from pathlib import Path
|
||||||
|
from time import perf_counter
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import (
|
||||||
|
CalibrationMode,
|
||||||
|
CalibrationRequest,
|
||||||
|
CalibrationResult,
|
||||||
|
CalibrationStatus,
|
||||||
|
MotionPair,
|
||||||
|
SessionInput,
|
||||||
|
)
|
||||||
|
from .finalize import finalize_result
|
||||||
|
from .imu_audit import audit_imu
|
||||||
|
from .imu_io import load_imu_samples
|
||||||
|
from .joint_optimizer import solve_joint_extrinsic
|
||||||
|
from .keyframes import build_keyframes
|
||||||
|
from .lidar_deskew import deskew_lidar_frames
|
||||||
|
from .lidar_io import load_lidar_frames
|
||||||
|
from .motion_pairs import build_motion_pairs
|
||||||
|
from .motion_pairs_io import build_motion_pairs_payload
|
||||||
|
from .rotation_handeye import solve_rotation_handeye
|
||||||
|
from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
|
||||||
|
from .timestamp_audit import audit_timestamps
|
||||||
|
from .vehicle_config import load_vehicle_config, prior_enabled
|
||||||
|
|
||||||
|
# Remap keyframe indices so multi-session Phase-C graphs do not collide.
|
||||||
|
_SESSION_INDEX_OFFSET = 1_000_000
|
||||||
|
|
||||||
|
|
||||||
|
def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult:
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=refined.delta_t_s,
|
||||||
|
correlation_peak=refined.correlation_peak,
|
||||||
|
search_s=previous.search_s,
|
||||||
|
notes=tuple(list(previous.notes) + list(refined.notes)),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class PipelineStage:
|
||||||
|
name: str
|
||||||
|
responsibility: str
|
||||||
|
|
||||||
|
|
||||||
|
STAGES = (
|
||||||
|
PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"),
|
||||||
|
PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"),
|
||||||
|
PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"),
|
||||||
|
PipelineStage("time_offset", "各会话独立粗估/精修 δt"),
|
||||||
|
PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"),
|
||||||
|
PipelineStage("motion_pairs", "各会话构造运动对，再合并"),
|
||||||
|
PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"),
|
||||||
|
PipelineStage("joint_optimizer", "Phase-A 会话级零偏联合精修；Phase-B/C 暂时门控"),
|
||||||
|
PipelineStage("finalize", "写出结果与质量报告"),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
ProgressCallback = Callable[[dict[str, Any]], None]
|
||||||
|
|
||||||
|
|
||||||
|
def _emit_progress(
|
||||||
|
callback: ProgressCallback | None,
|
||||||
|
stage_index: int,
|
||||||
|
event: str,
|
||||||
|
**fields: Any,
|
||||||
|
) -> None:
|
||||||
|
if callback is None:
|
||||||
|
return
|
||||||
|
callback(
|
||||||
|
{
|
||||||
|
"stage_index": stage_index,
|
||||||
|
"stage_total": len(STAGES),
|
||||||
|
"stage": STAGES[stage_index - 1].name,
|
||||||
|
"event": event,
|
||||||
|
**fields,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def describe_pipeline(_: CalibrationRequest) -> tuple[PipelineStage, ...]:
|
||||||
|
"""Return the planned stages."""
|
||||||
|
|
||||||
|
return STAGES
|
||||||
|
|
||||||
|
|
||||||
|
def _build_pairs_and_handeye(
|
||||||
|
*,
|
||||||
|
session_id: str,
|
||||||
|
working_frames,
|
||||||
|
imu,
|
||||||
|
delta_t_s: float,
|
||||||
|
gyro_bias_rad_s: np.ndarray,
|
||||||
|
request: CalibrationRequest,
|
||||||
|
R_prior: np.ndarray | None = None,
|
||||||
|
prior_sigma_deg: float | None = None,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
):
|
||||||
|
keyframes = build_keyframes(
|
||||||
|
working_frames,
|
||||||
|
min_translation_m=request.min_pair_translation_m,
|
||||||
|
min_rotation_deg=request.min_pair_rotation_deg,
|
||||||
|
min_registration_fitness=request.min_registration_fitness,
|
||||||
|
)
|
||||||
|
if progress_callback is not None:
|
||||||
|
progress_callback(
|
||||||
|
{
|
||||||
|
"event": "keyframes_ready",
|
||||||
|
"keyframe_count": len(keyframes.indices),
|
||||||
|
"lidar_frame_count": len(working_frames),
|
||||||
|
}
|
||||||
|
)
|
||||||
|
pair_set = build_motion_pairs(
|
||||||
|
session_id=session_id,
|
||||||
|
keyframes=list(keyframes.frames),
|
||||||
|
keyframe_indices=keyframes.indices,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=delta_t_s,
|
||||||
|
gyro_bias_rad_s=gyro_bias_rad_s,
|
||||||
|
min_rotation_deg=request.min_pair_rotation_deg,
|
||||||
|
min_translation_m=request.min_pair_translation_m,
|
||||||
|
min_registration_fitness=request.min_registration_fitness,
|
||||||
|
max_imu_gap_s=request.max_imu_gap_s,
|
||||||
|
max_lidar_gap_s=request.max_lidar_gap_s,
|
||||||
|
all_frame_times_s=np.asarray([frame.t_mid_s for frame in working_frames], dtype=float),
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
)
|
||||||
|
handeye = solve_rotation_handeye(
|
||||||
|
pair_set.pairs,
|
||||||
|
R_prior=R_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
)
|
||||||
|
return keyframes, pair_set, handeye
|
||||||
|
|
||||||
|
|
||||||
|
def _translation_prior_from_config(
|
||||||
|
vehicle_config: dict[str, Any] | None,
|
||||||
|
) -> tuple[np.ndarray | None, np.ndarray | float | None]:
|
||||||
|
if vehicle_config is None or not prior_enabled(vehicle_config, "translation_prior"):
|
||||||
|
return None, None
|
||||||
|
init_cfg = vehicle_config.get("initialization") or {}
|
||||||
|
tp = init_cfg.get("translation_prior") or {}
|
||||||
|
if tp.get("t_IMU_lidar_m") is None:
|
||||||
|
return None, None
|
||||||
|
return np.asarray(tp["t_IMU_lidar_m"], dtype=float).reshape(3), tp.get("sigma_m", [0.05, 0.05, 0.05])
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_prior_from_config(
|
||||||
|
vehicle_config: dict[str, Any] | None,
|
||||||
|
) -> tuple[np.ndarray | None, float | None]:
|
||||||
|
if vehicle_config is None or not prior_enabled(vehicle_config, "rotation_prior"):
|
||||||
|
return None, None
|
||||||
|
init_cfg = vehicle_config.get("initialization") or {}
|
||||||
|
rp = init_cfg.get("rotation_prior") or {}
|
||||||
|
if rp.get("R_IMU_lidar") is None:
|
||||||
|
return None, None
|
||||||
|
return np.asarray(rp["R_IMU_lidar"], dtype=float).reshape(3, 3), float(rp.get("sigma_deg", 15.0))
|
||||||
|
|
||||||
|
|
||||||
|
def _prepare_session_pairs(
|
||||||
|
session: SessionInput,
|
||||||
|
request: CalibrationRequest,
|
||||||
|
*,
|
||||||
|
R_prior: np.ndarray | None = None,
|
||||||
|
prior_sigma_deg: float | None = None,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
session_index: int = 1,
|
||||||
|
session_total: int = 1,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet."""
|
||||||
|
|
||||||
|
started_at = perf_counter()
|
||||||
|
|
||||||
|
def emit(stage_index: int, event: str, **fields: Any) -> None:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
stage_index,
|
||||||
|
event,
|
||||||
|
session=session.session_id,
|
||||||
|
session_index=session_index,
|
||||||
|
session_total=session_total,
|
||||||
|
**fields,
|
||||||
|
)
|
||||||
|
|
||||||
|
emit(
|
||||||
|
2,
|
||||||
|
"session_start",
|
||||||
|
imu_source=str(session.imu_source),
|
||||||
|
lidar_source=str(session.lidar_source),
|
||||||
|
)
|
||||||
|
imu = load_imu_samples(session.imu_source)
|
||||||
|
frames = load_lidar_frames(session.lidar_source)
|
||||||
|
emit(
|
||||||
|
2,
|
||||||
|
"data_loaded",
|
||||||
|
imu_samples=int(imu.t_s.size),
|
||||||
|
lidar_frames=len(frames),
|
||||||
|
imu_span_s=float(imu.t_s[-1] - imu.t_s[0]) if imu.t_s.size >= 2 else 0.0,
|
||||||
|
lidar_span_s=(
|
||||||
|
float(frames[-1].t_mid_s - frames[0].t_mid_s) if len(frames) >= 2 else 0.0
|
||||||
|
),
|
||||||
|
elapsed_s=perf_counter() - started_at,
|
||||||
|
)
|
||||||
|
|
||||||
|
ts = audit_timestamps(imu, frames)
|
||||||
|
emit(2, "audit_complete", ok=ts.ok)
|
||||||
|
if not ts.ok:
|
||||||
|
emit(2, "blocked", reason="timestamp_audit")
|
||||||
|
return {"ok": False, "stage": "timestamp_audit", "session_id": session.session_id, "report": asdict(ts)}
|
||||||
|
|
||||||
|
imu_report = audit_imu(imu)
|
||||||
|
emit(
|
||||||
|
3,
|
||||||
|
"audit_complete",
|
||||||
|
ok=imu_report.ok,
|
||||||
|
gyro_bias_norm_rad_s=float(np.linalg.norm(imu_report.gyro_bias_rad_s)),
|
||||||
|
)
|
||||||
|
if not imu_report.ok:
|
||||||
|
emit(3, "blocked", reason="imu_audit")
|
||||||
|
return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)}
|
||||||
|
|
||||||
|
fixed_time_offset_s = (
|
||||||
|
session.fixed_time_offset_s
|
||||||
|
if session.fixed_time_offset_s is not None
|
||||||
|
else request.fixed_time_offset_s
|
||||||
|
)
|
||||||
|
if fixed_time_offset_s is not None:
|
||||||
|
offset_source = "fixed"
|
||||||
|
offset = TimeOffsetResult(
|
||||||
|
delta_t_s=float(fixed_time_offset_s),
|
||||||
|
correlation_peak=1.0,
|
||||||
|
search_s=0.0,
|
||||||
|
notes=(
|
||||||
|
f"fixed_time_offset_s={float(fixed_time_offset_s):.6f} "
|
||||||
|
"(skip |ω| search; intended for host-UTC-bridged sessions)",
|
||||||
|
),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
offset_source = "estimated"
|
||||||
|
offset = estimate_time_offset(
|
||||||
|
imu,
|
||||||
|
frames,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
search_s=request.time_offset_search_s,
|
||||||
|
)
|
||||||
|
if not offset.ok:
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"blocked",
|
||||||
|
reason="time_offset",
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
correlation_peak=float(offset.correlation_peak),
|
||||||
|
)
|
||||||
|
return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}
|
||||||
|
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"offset_ready",
|
||||||
|
source=offset_source,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
correlation_peak=float(offset.correlation_peak),
|
||||||
|
)
|
||||||
|
coarse_delta_t = float(offset.delta_t_s)
|
||||||
|
working_frames = frames
|
||||||
|
r_x = np.eye(3) if R_prior is None else np.asarray(R_prior, dtype=float).reshape(3, 3)
|
||||||
|
handeye = None
|
||||||
|
pair_set = None
|
||||||
|
keyframes = None
|
||||||
|
pairs_notes: list[str] = []
|
||||||
|
pair_count = 0
|
||||||
|
|
||||||
|
iterations_total = max(1, request.max_iterations)
|
||||||
|
build_pass = "outer"
|
||||||
|
|
||||||
|
def on_build_progress(payload: dict[str, Any]) -> None:
|
||||||
|
event = str(payload.get("event", "running"))
|
||||||
|
stage_index = 5 if event == "keyframes_ready" else 6
|
||||||
|
fields = {key: value for key, value in payload.items() if key != "event"}
|
||||||
|
emit(
|
||||||
|
stage_index,
|
||||||
|
event,
|
||||||
|
iteration=iteration + 1,
|
||||||
|
iterations_total=iterations_total,
|
||||||
|
build_pass=build_pass,
|
||||||
|
**fields,
|
||||||
|
)
|
||||||
|
|
||||||
|
for iteration in range(iterations_total):
|
||||||
|
build_pass = "outer"
|
||||||
|
emit(
|
||||||
|
5,
|
||||||
|
"iteration_start",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
iterations_total=iterations_total,
|
||||||
|
deskew=iteration > 0,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
)
|
||||||
|
if iteration > 0:
|
||||||
|
deskew_started_at = perf_counter()
|
||||||
|
emit(5, "deskew_start", iteration=iteration + 1)
|
||||||
|
working_frames = deskew_lidar_frames(
|
||||||
|
frames,
|
||||||
|
imu,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
R_IMU_lidar=r_x,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
)
|
||||||
|
emit(
|
||||||
|
5,
|
||||||
|
"deskew_complete",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
lidar_frames=len(working_frames),
|
||||||
|
elapsed_s=perf_counter() - deskew_started_at,
|
||||||
|
)
|
||||||
|
keyframes, pair_set, handeye = _build_pairs_and_handeye(
|
||||||
|
session_id=session.session_id,
|
||||||
|
working_frames=working_frames,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
request=request,
|
||||||
|
R_prior=R_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
progress_callback=on_build_progress,
|
||||||
|
)
|
||||||
|
pairs_notes = list(pair_set.notes)
|
||||||
|
pair_count = len(pair_set.pairs)
|
||||||
|
emit(
|
||||||
|
7,
|
||||||
|
"local_handeye",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
build_pass=build_pass,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
rms_deg=float(handeye.residual_rms_deg),
|
||||||
|
p95_deg=float(handeye.residual_p95_deg),
|
||||||
|
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
|
||||||
|
ok=handeye.ok,
|
||||||
|
)
|
||||||
|
if pair_count < 3:
|
||||||
|
emit(
|
||||||
|
6,
|
||||||
|
"blocked",
|
||||||
|
reason="insufficient_motion_pairs",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"ok": False,
|
||||||
|
"stage": "motion_pairs",
|
||||||
|
"session_id": session.session_id,
|
||||||
|
"iteration": iteration,
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"imu_audit": asdict(imu_report),
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"keyframes": 0 if keyframes is None else len(keyframes.indices),
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": asdict(handeye),
|
||||||
|
}
|
||||||
|
r_x = handeye.R_IMU_lidar
|
||||||
|
|
||||||
|
if not request.enable_signed_time_refine:
|
||||||
|
continue
|
||||||
|
|
||||||
|
for refine_step in range(1, 3):
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"signed_refine_start",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
refine_step=refine_step,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
)
|
||||||
|
refined = refine_time_offset_signed(
|
||||||
|
imu,
|
||||||
|
frames,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
R_IMU_lidar=r_x,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
|
||||||
|
max_shift_s=request.max_signed_refine_shift_s,
|
||||||
|
)
|
||||||
|
# Also bound total walk away from the original coarse estimate.
|
||||||
|
if abs(refined.delta_t_s - coarse_delta_t) > request.max_signed_refine_shift_s:
|
||||||
|
refined = TimeOffsetResult(
|
||||||
|
delta_t_s=float(offset.delta_t_s),
|
||||||
|
correlation_peak=refined.correlation_peak,
|
||||||
|
search_s=refined.search_s,
|
||||||
|
notes=tuple(
|
||||||
|
list(refined.notes)
|
||||||
|
+ [
|
||||||
|
f"signed refine clamped: |δt-coarse| would exceed "
|
||||||
|
f"{request.max_signed_refine_shift_s:.3f}s"
|
||||||
|
]
|
||||||
|
),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
|
||||||
|
offset = _merge_time_offset(offset, refined)
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"signed_refine_complete",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
refine_step=refine_step,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
shift_s=float(delta_shift),
|
||||||
|
correlation_peak=float(refined.correlation_peak),
|
||||||
|
)
|
||||||
|
if delta_shift < 1e-3:
|
||||||
|
break
|
||||||
|
build_pass = f"signed_refine_{refine_step}"
|
||||||
|
keyframes, pair_set, handeye = _build_pairs_and_handeye(
|
||||||
|
session_id=session.session_id,
|
||||||
|
working_frames=working_frames,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
request=request,
|
||||||
|
R_prior=R_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
progress_callback=on_build_progress,
|
||||||
|
)
|
||||||
|
pairs_notes = list(pair_set.notes)
|
||||||
|
pair_count = len(pair_set.pairs)
|
||||||
|
emit(
|
||||||
|
7,
|
||||||
|
"local_handeye",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
build_pass=build_pass,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
rms_deg=float(handeye.residual_rms_deg),
|
||||||
|
p95_deg=float(handeye.residual_p95_deg),
|
||||||
|
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
|
||||||
|
ok=handeye.ok,
|
||||||
|
)
|
||||||
|
if pair_count < 3:
|
||||||
|
emit(
|
||||||
|
6,
|
||||||
|
"blocked",
|
||||||
|
reason="insufficient_motion_pairs_after_signed_refine",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"ok": False,
|
||||||
|
"stage": "motion_pairs",
|
||||||
|
"session_id": session.session_id,
|
||||||
|
"iteration": iteration,
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"imu_audit": asdict(imu_report),
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"keyframes": 0 if keyframes is None else len(keyframes.indices),
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": asdict(handeye),
|
||||||
|
}
|
||||||
|
r_x = handeye.R_IMU_lidar
|
||||||
|
assert handeye is not None and pair_set is not None and keyframes is not None
|
||||||
|
acc_mean = np.asarray(imu_report.static_acc_mean_m_s2, dtype=float).reshape(3)
|
||||||
|
acc_n = float(np.linalg.norm(acc_mean))
|
||||||
|
if acc_n > 1e-6:
|
||||||
|
gravity_init = -acc_mean * (9.80665 / acc_n)
|
||||||
|
else:
|
||||||
|
gravity_init = np.array([0.0, 0.0, -9.80665])
|
||||||
|
|
||||||
|
emit(
|
||||||
|
7,
|
||||||
|
"session_complete",
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
local_handeye_ok=handeye.ok,
|
||||||
|
elapsed_s=perf_counter() - started_at,
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"ok": True,
|
||||||
|
"session_id": session.session_id,
|
||||||
|
"pairs": tuple(pair_set.pairs),
|
||||||
|
"gyro_bias_rad_s": np.asarray(imu_report.gyro_bias_rad_s, dtype=float).reshape(3),
|
||||||
|
"gravity_init_m_s2": gravity_init,
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"imu_audit": {
|
||||||
|
**asdict(imu_report),
|
||||||
|
"gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(),
|
||||||
|
"static_acc_mean_m_s2": imu_report.static_acc_mean_m_s2.tolist(),
|
||||||
|
},
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"time_offset_s": float(offset.delta_t_s),
|
||||||
|
"keyframes": len(keyframes.indices),
|
||||||
|
"pair_count": pair_count,
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye_local": {
|
||||||
|
"residual_rms_deg": handeye.residual_rms_deg,
|
||||||
|
"residual_median_deg": handeye.residual_median_deg,
|
||||||
|
"residual_p95_deg": handeye.residual_p95_deg,
|
||||||
|
"outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
|
||||||
|
"pair_count": handeye.pair_count,
|
||||||
|
"ok": handeye.ok,
|
||||||
|
"notes": handeye.notes,
|
||||||
|
"R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]:
|
||||||
|
merged: list[MotionPair] = []
|
||||||
|
for index, prep in enumerate(prepared):
|
||||||
|
id_offset = (index + 1) * _SESSION_INDEX_OFFSET
|
||||||
|
for pair in prep["pairs"]:
|
||||||
|
merged.append(
|
||||||
|
replace(
|
||||||
|
pair,
|
||||||
|
i=int(pair.i) + id_offset,
|
||||||
|
j=int(pair.j) + id_offset,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return merged
|
||||||
|
|
||||||
|
|
||||||
|
def run_calibration(
|
||||||
|
request: CalibrationRequest,
|
||||||
|
*,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
) -> CalibrationResult:
|
||||||
|
"""Run the V1 calibration pipeline for one or more sessions.
|
||||||
|
|
||||||
|
Multi-session: each session estimates its own δt and builds motion pairs;
|
||||||
|
rotation hand-eye and joint SE3 are solved once on the merged pair set.
|
||||||
|
"""
|
||||||
|
|
||||||
|
overall_started_at = perf_counter()
|
||||||
|
|
||||||
|
def finish(
|
||||||
|
*,
|
||||||
|
status: CalibrationStatus,
|
||||||
|
message: str,
|
||||||
|
details: dict[str, Any],
|
||||||
|
T_IMU_lidar: np.ndarray | None = None,
|
||||||
|
time_offset_s: float | None = None,
|
||||||
|
motion_pairs_payload: dict[str, Any] | None = None,
|
||||||
|
) -> CalibrationResult:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
9,
|
||||||
|
"writing_result",
|
||||||
|
status=status.value,
|
||||||
|
output_directory=str(request.output_directory),
|
||||||
|
)
|
||||||
|
result = finalize_result(
|
||||||
|
status=status,
|
||||||
|
message=message,
|
||||||
|
details=details,
|
||||||
|
T_IMU_lidar=T_IMU_lidar,
|
||||||
|
time_offset_s=time_offset_s,
|
||||||
|
output_directory=request.output_directory,
|
||||||
|
motion_pairs_payload=motion_pairs_payload,
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
9,
|
||||||
|
"complete",
|
||||||
|
status=result.status.value,
|
||||||
|
elapsed_s=perf_counter() - overall_started_at,
|
||||||
|
)
|
||||||
|
return result
|
||||||
|
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"pipeline_start",
|
||||||
|
mode=request.requested_mode.value,
|
||||||
|
session_count=len(request.sessions),
|
||||||
|
max_iterations=max(1, request.max_iterations),
|
||||||
|
output_directory=str(request.output_directory),
|
||||||
|
)
|
||||||
|
if not request.sessions:
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message="no sessions provided",
|
||||||
|
details={},
|
||||||
|
)
|
||||||
|
|
||||||
|
vehicle_config = None
|
||||||
|
if request.vehicle_config is not None:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"loading_vehicle_config",
|
||||||
|
path=str(request.vehicle_config),
|
||||||
|
)
|
||||||
|
try:
|
||||||
|
vehicle_config = load_vehicle_config(request.vehicle_config)
|
||||||
|
except Exception as exc: # noqa: BLE001 - surface config problems as blocked
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"blocked",
|
||||||
|
reason="vehicle_config",
|
||||||
|
error=str(exc),
|
||||||
|
)
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=f"vehicle config failed: {exc}",
|
||||||
|
details={},
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"vehicle_config_ready",
|
||||||
|
loaded=vehicle_config is not None,
|
||||||
|
)
|
||||||
|
|
||||||
|
r_prior, prior_sigma_deg = _rotation_prior_from_config(vehicle_config)
|
||||||
|
|
||||||
|
prepared: list[dict[str, Any]] = []
|
||||||
|
session_total = len(request.sessions)
|
||||||
|
for session_index, session in enumerate(request.sessions, start=1):
|
||||||
|
prep = _prepare_session_pairs(
|
||||||
|
session,
|
||||||
|
request,
|
||||||
|
R_prior=r_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
session_index=session_index,
|
||||||
|
session_total=session_total,
|
||||||
|
)
|
||||||
|
if not prep.get("ok"):
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})",
|
||||||
|
details={"sessions": [prep]},
|
||||||
|
)
|
||||||
|
prepared.append(prep)
|
||||||
|
|
||||||
|
all_pairs = _remap_pairs_for_joint(prepared)
|
||||||
|
pair_counts_per_session = {
|
||||||
|
p["session_id"]: int(p["pair_count"]) for p in prepared
|
||||||
|
}
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
7,
|
||||||
|
"joint_handeye_start",
|
||||||
|
session_count=len(prepared),
|
||||||
|
merged_pair_count=len(all_pairs),
|
||||||
|
pair_counts_per_session=pair_counts_per_session,
|
||||||
|
)
|
||||||
|
handeye_started_at = perf_counter()
|
||||||
|
handeye = solve_rotation_handeye(
|
||||||
|
all_pairs,
|
||||||
|
R_prior=r_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
7,
|
||||||
|
"joint_handeye_complete",
|
||||||
|
pair_count=handeye.pair_count,
|
||||||
|
rms_deg=float(handeye.residual_rms_deg),
|
||||||
|
p95_deg=float(handeye.residual_p95_deg),
|
||||||
|
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
|
||||||
|
ok=handeye.ok,
|
||||||
|
elapsed_s=perf_counter() - handeye_started_at,
|
||||||
|
)
|
||||||
|
if handeye.pair_count < 3:
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message="blocked at stage rotation_handeye (joint)",
|
||||||
|
details={
|
||||||
|
"sessions": [_public_session(p) for p in prepared],
|
||||||
|
"joint_handeye": asdict(handeye),
|
||||||
|
"merged_pair_count": len(all_pairs),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
|
||||||
|
t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
|
||||||
|
gyro_bias_by_session = {
|
||||||
|
p["session_id"]: np.asarray(p["gyro_bias_rad_s"], dtype=float) for p in prepared
|
||||||
|
}
|
||||||
|
time_offset_by_session = {
|
||||||
|
p["session_id"]: float(p["time_offset_s"]) for p in prepared
|
||||||
|
}
|
||||||
|
|
||||||
|
preexcluded_session_ids = {
|
||||||
|
p["session_id"] for p in prepared if not p["handeye_local"]["ok"]
|
||||||
|
}
|
||||||
|
if len(preexcluded_session_ids) == len(prepared):
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_complete",
|
||||||
|
accepted=False,
|
||||||
|
reason="all_sessions_failed_local_handeye_gate",
|
||||||
|
excluded_sessions=sorted(preexcluded_session_ids),
|
||||||
|
)
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=(
|
||||||
|
"Phase-A blocked: all sessions failed the local "
|
||||||
|
"rotation residual gate"
|
||||||
|
),
|
||||||
|
details={
|
||||||
|
"sessions": [_public_session(p) for p in prepared],
|
||||||
|
"joint_handeye": asdict(handeye),
|
||||||
|
"merged_pair_count": len(all_pairs),
|
||||||
|
"excluded_sessions": sorted(
|
||||||
|
preexcluded_session_ids
|
||||||
|
),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_start",
|
||||||
|
session_count=len(prepared),
|
||||||
|
merged_pair_count=len(all_pairs),
|
||||||
|
preexcluded_sessions=sorted(preexcluded_session_ids),
|
||||||
|
)
|
||||||
|
phase_a_started_at = perf_counter()
|
||||||
|
|
||||||
|
def on_phase_a_progress(
|
||||||
|
event: str,
|
||||||
|
fields: dict[str, Any],
|
||||||
|
) -> None:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
event,
|
||||||
|
**fields,
|
||||||
|
)
|
||||||
|
|
||||||
|
joint = solve_joint_extrinsic(
|
||||||
|
all_pairs,
|
||||||
|
handeye.R_IMU_lidar,
|
||||||
|
force_rotation_only=force_rotation_only,
|
||||||
|
imu=None,
|
||||||
|
gyro_bias_rad_s_by_session=gyro_bias_by_session,
|
||||||
|
time_offset_s_by_session=time_offset_by_session,
|
||||||
|
preexcluded_session_ids=preexcluded_session_ids,
|
||||||
|
rotation_prior=r_prior,
|
||||||
|
rotation_prior_sigma_deg=(
|
||||||
|
15.0 if prior_sigma_deg is None else prior_sigma_deg
|
||||||
|
),
|
||||||
|
phase_a_progress_callback=on_phase_a_progress,
|
||||||
|
enable_phase_c=not force_rotation_only,
|
||||||
|
t_init_m=t_prior,
|
||||||
|
t_prior_m=t_prior,
|
||||||
|
t_prior_sigma_m=t_prior_sigma,
|
||||||
|
)
|
||||||
|
included_sessions = [
|
||||||
|
item.session_id for item in joint.phase_a_sessions if item.included_in_final
|
||||||
|
]
|
||||||
|
excluded_sessions = [
|
||||||
|
item.session_id for item in joint.phase_a_sessions if not item.included_in_final
|
||||||
|
]
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_complete",
|
||||||
|
accepted=joint.phase_a_accepted,
|
||||||
|
joint_rms_deg=float(joint.residual_rms_rot_deg),
|
||||||
|
rotation_observable=joint.observability.rotation_observable,
|
||||||
|
included_sessions=included_sessions,
|
||||||
|
excluded_sessions=excluded_sessions,
|
||||||
|
elapsed_s=perf_counter() - phase_a_started_at,
|
||||||
|
)
|
||||||
|
for item in joint.phase_a_sessions:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_session",
|
||||||
|
session=item.session_id,
|
||||||
|
included=item.included_in_final,
|
||||||
|
accepted=item.accepted,
|
||||||
|
pair_count=item.pair_count,
|
||||||
|
rms_deg=float(item.residual_rms_deg),
|
||||||
|
p95_deg=float(item.residual_p95_deg),
|
||||||
|
bias_delta_norm_rad_s=float(
|
||||||
|
np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)
|
||||||
|
),
|
||||||
|
gyro_bias_rad_s=np.asarray(item.gyro_bias_rad_s, dtype=float).round(8).tolist(),
|
||||||
|
)
|
||||||
|
|
||||||
|
phase_a_by_session = {
|
||||||
|
item.session_id: item for item in joint.phase_a_sessions
|
||||||
|
}
|
||||||
|
session_results = []
|
||||||
|
for prep in prepared:
|
||||||
|
phase_a = phase_a_by_session.get(prep["session_id"])
|
||||||
|
session_bias = joint.gyro_bias_rad_s_per_session.get(prep["session_id"])
|
||||||
|
session_results.append(
|
||||||
|
{
|
||||||
|
**_public_session(prep),
|
||||||
|
"vehicle_config_loaded": vehicle_config is not None,
|
||||||
|
"handeye": {
|
||||||
|
"residual_rms_deg": handeye.residual_rms_deg,
|
||||||
|
"residual_median_deg": handeye.residual_median_deg,
|
||||||
|
"residual_p95_deg": handeye.residual_p95_deg,
|
||||||
|
"outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
|
||||||
|
"pair_count": handeye.pair_count,
|
||||||
|
"ok": handeye.ok,
|
||||||
|
"notes": tuple(list(handeye.notes) + [f"joint over {len(request.sessions)} sessions"]),
|
||||||
|
"R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
|
||||||
|
},
|
||||||
|
"joint": {
|
||||||
|
"translation_accepted": joint.translation_accepted,
|
||||||
|
"residual_rms_rot_deg": joint.residual_rms_rot_deg,
|
||||||
|
"residual_rms_trans_m": joint.residual_rms_trans_m,
|
||||||
|
"observability": asdict(joint.observability),
|
||||||
|
"notes": joint.notes,
|
||||||
|
"T_IMU_lidar": joint.T_IMU_lidar.tolist(),
|
||||||
|
"phase_a": None if phase_a is None else asdict(phase_a),
|
||||||
|
"gyro_bias_rad_s": None
|
||||||
|
if session_bias is None
|
||||||
|
else np.asarray(session_bias, dtype=float).tolist(),
|
||||||
|
"accel_bias_m_s2": None
|
||||||
|
if joint.accel_bias_m_s2 is None
|
||||||
|
else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(),
|
||||||
|
"gravity_m_s2": None
|
||||||
|
if joint.gravity_m_s2 is None
|
||||||
|
else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
|
||||||
|
},
|
||||||
|
"translation_accepted": joint.translation_accepted,
|
||||||
|
"rotation_ok": (
|
||||||
|
phase_a is not None
|
||||||
|
and phase_a.included_in_final
|
||||||
|
and phase_a.accepted
|
||||||
|
and joint.phase_a_accepted
|
||||||
|
and joint.observability.rotation_observable
|
||||||
|
),
|
||||||
|
"rotation_prior_constrained": (
|
||||||
|
phase_a is not None
|
||||||
|
and phase_a.included_in_final
|
||||||
|
and phase_a.accepted
|
||||||
|
and joint.phase_a_accepted
|
||||||
|
and not joint.observability.rotation_observable
|
||||||
|
and r_prior is not None
|
||||||
|
),
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
T = np.asarray(joint.T_IMU_lidar, dtype=float)
|
||||||
|
if request.requested_mode == CalibrationMode.ROTATION_ONLY:
|
||||||
|
# A rotation-only result must never expose a seed/prior translation,
|
||||||
|
# including when the rotation itself is rejected by a later gate.
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
# Multi-session offsets stay in details; the legacy scalar is single-session only.
|
||||||
|
delta_t = float(prepared[0]["time_offset_s"]) if len(prepared) == 1 else None
|
||||||
|
joint_rotation_ok = joint.phase_a_accepted
|
||||||
|
if not joint_rotation_ok:
|
||||||
|
status = CalibrationStatus.BLOCKED
|
||||||
|
message = (
|
||||||
|
f"joint rotation rejected: RMS={joint.residual_rms_rot_deg:.3f} deg "
|
||||||
|
"or a retained session failed the Phase-A residual gates"
|
||||||
|
)
|
||||||
|
elif request.requested_mode == CalibrationMode.FULL_SE3:
|
||||||
|
if joint.translation_accepted:
|
||||||
|
status = CalibrationStatus.FULL_SE3_ACCEPTED
|
||||||
|
message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
|
||||||
|
else:
|
||||||
|
status = CalibrationStatus.FULL_SE3_REJECTED
|
||||||
|
message = (
|
||||||
|
f"rotation accepted jointly ({len(prepared)} sessions); "
|
||||||
|
"translation deferred until Phase-B/C session-state redesign"
|
||||||
|
)
|
||||||
|
elif joint.observability.rotation_observable:
|
||||||
|
status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
|
||||||
|
message = (
|
||||||
|
f"rotation-only calibration accepted "
|
||||||
|
f"(joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
|
||||||
|
)
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
elif r_prior is not None:
|
||||||
|
status = CalibrationStatus.ROTATION_ONLY_PRIOR_CONSTRAINED
|
||||||
|
message = (
|
||||||
|
"rotation residuals passed, but motion does not independently observe all "
|
||||||
|
"rotation axes; result remains constrained by the installation prior"
|
||||||
|
)
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
else:
|
||||||
|
status = CalibrationStatus.BLOCKED
|
||||||
|
message = "rotation residuals passed but rotation observability failed without a prior"
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
|
||||||
|
return finish(
|
||||||
|
status=status,
|
||||||
|
message=message,
|
||||||
|
details={
|
||||||
|
"sessions": session_results,
|
||||||
|
"joint": {
|
||||||
|
"session_count": len(prepared),
|
||||||
|
"merged_pair_count": len(all_pairs),
|
||||||
|
"pair_counts_per_session": {p["session_id"]: p["pair_count"] for p in prepared},
|
||||||
|
"time_offset_s_per_session": {p["session_id"]: p["time_offset_s"] for p in prepared},
|
||||||
|
"handeye_rms_deg": handeye.residual_rms_deg,
|
||||||
|
"handeye_p95_deg": handeye.residual_p95_deg,
|
||||||
|
"handeye_outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
|
||||||
|
"phase_a_accepted": joint.phase_a_accepted,
|
||||||
|
"phase_a_comparison": joint.phase_a_comparison,
|
||||||
|
"phase_a_sessions": [asdict(item) for item in joint.phase_a_sessions],
|
||||||
|
"gyro_bias_rad_s_per_session": {
|
||||||
|
sid: np.asarray(value, dtype=float).tolist()
|
||||||
|
for sid, value in joint.gyro_bias_rad_s_per_session.items()
|
||||||
|
},
|
||||||
|
"excluded_sessions": [
|
||||||
|
item.session_id for item in joint.phase_a_sessions if not item.included_in_final
|
||||||
|
],
|
||||||
|
"joint_rotation_rms_deg": joint.residual_rms_rot_deg,
|
||||||
|
"rotation_observable": joint.observability.rotation_observable,
|
||||||
|
"translation_accepted": joint.translation_accepted,
|
||||||
|
},
|
||||||
|
"joint_handeye": asdict(handeye),
|
||||||
|
},
|
||||||
|
T_IMU_lidar=None if status == CalibrationStatus.BLOCKED else T,
|
||||||
|
time_offset_s=delta_t,
|
||||||
|
motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
|
||||||
|
payload = dict(session_result)
|
||||||
|
payload.pop("T_IMU_lidar", None)
|
||||||
|
payload.pop("pairs", None)
|
||||||
|
payload.pop("gyro_bias_rad_s", None)
|
||||||
|
payload.pop("gravity_init_m_s2", None)
|
||||||
|
return payload
|
||||||
@@ -0,0 +1,159 @@
|
|||||||
|
"""LiDAR relative-motion registration.
|
||||||
|
|
||||||
|
Uses Open3D Generalized ICP when available; otherwise a NumPy point-to-point ICP.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import LidarFrame
|
||||||
|
from .geometry import make_transform, orthonormalize_rotation, rotation_angle_deg, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RegistrationResult:
|
||||||
|
transform: np.ndarray
|
||||||
|
fitness: float
|
||||||
|
rotation_deg: float
|
||||||
|
translation_m: float
|
||||||
|
backend: str
|
||||||
|
ok: bool
|
||||||
|
|
||||||
|
|
||||||
|
def _voxel_downsample(points: np.ndarray, voxel: float) -> np.ndarray:
|
||||||
|
if points.shape[0] == 0:
|
||||||
|
return points
|
||||||
|
quantized = np.floor(points / voxel).astype(np.int64)
|
||||||
|
_, unique_indices = np.unique(quantized, axis=0, return_index=True)
|
||||||
|
return points[np.sort(unique_indices)]
|
||||||
|
|
||||||
|
|
||||||
|
def _numpy_icp(
|
||||||
|
source: np.ndarray,
|
||||||
|
target: np.ndarray,
|
||||||
|
*,
|
||||||
|
max_iterations: int = 30,
|
||||||
|
max_correspondence: float = 1.0,
|
||||||
|
) -> RegistrationResult:
|
||||||
|
src = _voxel_downsample(source, 0.2)
|
||||||
|
tgt = _voxel_downsample(target, 0.2)
|
||||||
|
if src.shape[0] < 50 or tgt.shape[0] < 50:
|
||||||
|
return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "numpy_icp", False)
|
||||||
|
|
||||||
|
# Subsample for speed.
|
||||||
|
rng = np.random.default_rng(0)
|
||||||
|
if src.shape[0] > 4000:
|
||||||
|
src = src[rng.choice(src.shape[0], 4000, replace=False)]
|
||||||
|
if tgt.shape[0] > 8000:
|
||||||
|
tgt = tgt[rng.choice(tgt.shape[0], 8000, replace=False)]
|
||||||
|
|
||||||
|
r = np.eye(3)
|
||||||
|
t = np.zeros(3)
|
||||||
|
last_error = 1e9
|
||||||
|
inlier_ratio = 0.0
|
||||||
|
for _ in range(max_iterations):
|
||||||
|
transformed = src @ r.T + t
|
||||||
|
# Nearest neighbour in target via brute force on chunks.
|
||||||
|
diff = transformed[:, None, :] - tgt[None, :, :]
|
||||||
|
dist2 = np.sum(diff * diff, axis=2)
|
||||||
|
nn = np.argmin(dist2, axis=1)
|
||||||
|
dist = np.sqrt(dist2[np.arange(src.shape[0]), nn])
|
||||||
|
mask = dist < max_correspondence
|
||||||
|
inlier_ratio = float(np.mean(mask))
|
||||||
|
if np.count_nonzero(mask) < 30:
|
||||||
|
break
|
||||||
|
p = transformed[mask]
|
||||||
|
q = tgt[nn[mask]]
|
||||||
|
mu_p = p.mean(axis=0)
|
||||||
|
mu_q = q.mean(axis=0)
|
||||||
|
h = (p - mu_p).T @ (q - mu_q)
|
||||||
|
u, _, vt = np.linalg.svd(h)
|
||||||
|
r_delta = vt.T @ u.T
|
||||||
|
if np.linalg.det(r_delta) < 0:
|
||||||
|
vt[-1, :] *= -1
|
||||||
|
r_delta = vt.T @ u.T
|
||||||
|
t_delta = mu_q - r_delta @ mu_p
|
||||||
|
# Update global transform: x' = r_delta (r x + t) + t_delta
|
||||||
|
r = orthonormalize_rotation(r_delta @ r)
|
||||||
|
t = r_delta @ t + t_delta
|
||||||
|
mean_err = float(np.mean(dist[mask]))
|
||||||
|
if abs(last_error - mean_err) < 1e-4:
|
||||||
|
break
|
||||||
|
last_error = mean_err
|
||||||
|
|
||||||
|
transform = make_transform(t, r)
|
||||||
|
return RegistrationResult(
|
||||||
|
transform=transform,
|
||||||
|
fitness=inlier_ratio,
|
||||||
|
rotation_deg=rotation_angle_deg(r),
|
||||||
|
translation_m=float(np.linalg.norm(t)),
|
||||||
|
backend="numpy_icp",
|
||||||
|
ok=inlier_ratio > 0.15,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _open3d_gicp(source: np.ndarray, target: np.ndarray) -> RegistrationResult | None:
|
||||||
|
try:
|
||||||
|
import open3d as o3d
|
||||||
|
except ImportError:
|
||||||
|
return None
|
||||||
|
|
||||||
|
src = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(source))
|
||||||
|
tgt = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(target))
|
||||||
|
src = src.voxel_down_sample(0.2)
|
||||||
|
tgt = tgt.voxel_down_sample(0.2)
|
||||||
|
if len(src.points) < 50 or len(tgt.points) < 50:
|
||||||
|
return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "open3d_gicp", False)
|
||||||
|
src.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
|
||||||
|
tgt.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
|
||||||
|
result = o3d.pipelines.registration.registration_generalized_icp(
|
||||||
|
src,
|
||||||
|
tgt,
|
||||||
|
1.0,
|
||||||
|
np.eye(4),
|
||||||
|
o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),
|
||||||
|
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50),
|
||||||
|
)
|
||||||
|
transform = np.asarray(result.transformation, dtype=float)
|
||||||
|
return RegistrationResult(
|
||||||
|
transform=transform,
|
||||||
|
fitness=float(result.fitness),
|
||||||
|
rotation_deg=rotation_angle_deg(transform[:3, :3]),
|
||||||
|
translation_m=float(np.linalg.norm(transform[:3, 3])),
|
||||||
|
backend="open3d_gicp",
|
||||||
|
ok=float(result.fitness) > 0.15,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def register_lidar_pair(source_points: np.ndarray, target_points: np.ndarray) -> RegistrationResult:
|
||||||
|
"""Register source -> target and return ``T_target_source``."""
|
||||||
|
|
||||||
|
source = np.asarray(source_points, dtype=float).reshape(-1, 3)
|
||||||
|
target = np.asarray(target_points, dtype=float).reshape(-1, 3)
|
||||||
|
open3d_result = _open3d_gicp(source, target)
|
||||||
|
if open3d_result is not None:
|
||||||
|
return open3d_result
|
||||||
|
return _numpy_icp(source, target)
|
||||||
|
|
||||||
|
|
||||||
|
def estimate_frame_rotations(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
stride: int = 1,
|
||||||
|
) -> tuple[list[np.ndarray], list[tuple[float, float]]]:
|
||||||
|
"""Estimate consecutive (or strided) LiDAR relative rotations for time sync."""
|
||||||
|
|
||||||
|
rotations: list[np.ndarray] = []
|
||||||
|
pair_times: list[tuple[float, float]] = []
|
||||||
|
for index in range(0, len(frames) - stride, max(stride, 1)):
|
||||||
|
a = frames[index]
|
||||||
|
b = frames[index + stride]
|
||||||
|
result = register_lidar_pair(b.points_xyz, a.points_xyz)
|
||||||
|
if not result.ok:
|
||||||
|
continue
|
||||||
|
rotations.append(result.transform[:3, :3])
|
||||||
|
pair_times.append((a.t_mid_s, b.t_mid_s))
|
||||||
|
return rotations, pair_times
|
||||||
@@ -0,0 +1,217 @@
|
|||||||
|
"""SO(3) rotation hand-eye solver for ``R_A R_X = R_X R_B``."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
|
||||||
|
from .contracts import MotionPair
|
||||||
|
from .geometry import orthonormalize_rotation, rotation_angle_deg, skew, so3_exp, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RotationHandeyeResult:
|
||||||
|
R_IMU_lidar: np.ndarray
|
||||||
|
residual_rms_deg: float
|
||||||
|
residual_median_deg: float
|
||||||
|
residual_p95_deg: float
|
||||||
|
outlier_fraction_gt_5deg: float
|
||||||
|
pair_count: int
|
||||||
|
ok: bool
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_weight(pair: MotionPair) -> float:
|
||||||
|
weight = float(pair.metadata.get("weight", 1.0))
|
||||||
|
if not np.isfinite(weight) or weight <= 0:
|
||||||
|
return 1.0
|
||||||
|
return weight
|
||||||
|
|
||||||
|
|
||||||
|
def _tsai_rotation_initial(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
pair_weights: np.ndarray | None = None,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Closed-form rotation hand-eye initial guess (Tsai-style linear solve)."""
|
||||||
|
|
||||||
|
rows: list[np.ndarray] = []
|
||||||
|
rhs: list[np.ndarray] = []
|
||||||
|
weights = np.ones(len(pairs)) if pair_weights is None else np.asarray(pair_weights, dtype=float)
|
||||||
|
for pair, pair_weight in zip(pairs, weights):
|
||||||
|
alpha = so3_log(pair.R_A)
|
||||||
|
beta = so3_log(pair.R_B)
|
||||||
|
if np.linalg.norm(alpha) < 1e-6 or np.linalg.norm(beta) < 1e-6:
|
||||||
|
continue
|
||||||
|
w = np.sqrt(float(pair_weight))
|
||||||
|
rows.append(w * skew(alpha + beta))
|
||||||
|
rhs.append(w * (beta - alpha))
|
||||||
|
if len(rows) < 2:
|
||||||
|
return np.eye(3)
|
||||||
|
a = np.vstack(rows)
|
||||||
|
b = np.concatenate(rhs)
|
||||||
|
try:
|
||||||
|
rotvec, *_ = np.linalg.lstsq(a, b, rcond=None)
|
||||||
|
except np.linalg.LinAlgError:
|
||||||
|
return np.eye(3)
|
||||||
|
return orthonormalize_rotation(so3_exp(rotvec))
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
|
||||||
|
err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||||
|
return float(np.degrees(np.linalg.norm(err)))
|
||||||
|
|
||||||
|
|
||||||
|
def _rms_deg(r_x: np.ndarray, pairs: list[MotionPair]) -> float:
|
||||||
|
if not pairs:
|
||||||
|
return 1e9
|
||||||
|
errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in pairs], dtype=float)
|
||||||
|
return float(np.sqrt(np.mean(errs**2)))
|
||||||
|
|
||||||
|
|
||||||
|
def select_strong_rotation_pairs(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
*,
|
||||||
|
min_rotation_deg: float = 1.0,
|
||||||
|
) -> list[MotionPair]:
|
||||||
|
"""Return pairs that independently excite rotation on both sensor sides."""
|
||||||
|
|
||||||
|
threshold = float(min_rotation_deg)
|
||||||
|
return [
|
||||||
|
pair
|
||||||
|
for pair in pairs
|
||||||
|
if rotation_angle_deg(pair.R_A) > threshold
|
||||||
|
and rotation_angle_deg(pair.R_B) > threshold
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def estimate_rotation_handeye_initial(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
*,
|
||||||
|
min_rotation_deg: float = 1.0,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Return the fast data-only Tsai initialization without nonlinear refine."""
|
||||||
|
|
||||||
|
usable = select_strong_rotation_pairs(
|
||||||
|
pairs,
|
||||||
|
min_rotation_deg=min_rotation_deg,
|
||||||
|
)
|
||||||
|
if not usable:
|
||||||
|
return np.eye(3)
|
||||||
|
raw_weights = np.asarray(
|
||||||
|
[_pair_weight(pair) for pair in usable],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
median = max(float(np.median(raw_weights)), 1e-12)
|
||||||
|
weights = np.clip(raw_weights / median, 0.1, 10.0)
|
||||||
|
return _tsai_rotation_initial(usable, weights)
|
||||||
|
|
||||||
|
|
||||||
|
def solve_rotation_handeye(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
*,
|
||||||
|
R_prior: np.ndarray | None = None,
|
||||||
|
prior_sigma_deg: float | None = None,
|
||||||
|
) -> RotationHandeyeResult:
|
||||||
|
"""Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement.
|
||||||
|
|
||||||
|
Optional CAD / installation ``R_prior`` soft-constrains the extrinsic yaw that
|
||||||
|
is weakly observable under near-planar motion.
|
||||||
|
"""
|
||||||
|
|
||||||
|
usable = select_strong_rotation_pairs(pairs)
|
||||||
|
notes: list[str] = []
|
||||||
|
if len(usable) < 3:
|
||||||
|
return RotationHandeyeResult(
|
||||||
|
R_IMU_lidar=np.eye(3),
|
||||||
|
residual_rms_deg=1e9,
|
||||||
|
residual_median_deg=1e9,
|
||||||
|
residual_p95_deg=1e9,
|
||||||
|
outlier_fraction_gt_5deg=1.0,
|
||||||
|
pair_count=len(usable),
|
||||||
|
ok=False,
|
||||||
|
notes=("need at least 3 motion pairs with meaningful rotation",),
|
||||||
|
)
|
||||||
|
|
||||||
|
raw_weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
|
||||||
|
median_raw_weight = max(float(np.median(raw_weights)), 1e-12)
|
||||||
|
weights = np.clip(raw_weights / median_raw_weight, 0.1, 10.0)
|
||||||
|
r0 = _tsai_rotation_initial(usable, weights)
|
||||||
|
r_prior = None
|
||||||
|
if R_prior is not None:
|
||||||
|
r_prior = orthonormalize_rotation(np.asarray(R_prior, dtype=float).reshape(3, 3))
|
||||||
|
rms_tsai = _rms_deg(r0, usable)
|
||||||
|
rms_prior = _rms_deg(r_prior, usable)
|
||||||
|
if rms_prior <= rms_tsai * 1.25:
|
||||||
|
r0 = r_prior
|
||||||
|
notes.append(
|
||||||
|
f"init from rotation prior (rms={rms_prior:.3f} deg vs Tsai {rms_tsai:.3f} deg)"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append(
|
||||||
|
f"init from Tsai (rms={rms_tsai:.3f} deg; prior {rms_prior:.3f} deg kept as soft constraint)"
|
||||||
|
)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
"weighted hand-eye: normalized/clipped IMU confidence "
|
||||||
|
f"raw_median={median_raw_weight:.3g}, "
|
||||||
|
f"normalized_min={float(np.min(weights)):.3g}, "
|
||||||
|
f"normalized_max={float(np.max(weights)):.3g}"
|
||||||
|
)
|
||||||
|
|
||||||
|
def pack(r: np.ndarray) -> np.ndarray:
|
||||||
|
return so3_log(r)
|
||||||
|
|
||||||
|
def unpack(vec: np.ndarray) -> np.ndarray:
|
||||||
|
return orthonormalize_rotation(so3_exp(vec))
|
||||||
|
|
||||||
|
sigma = 15.0 if prior_sigma_deg is None else float(prior_sigma_deg)
|
||||||
|
prior_w = 0.0
|
||||||
|
if r_prior is not None and sigma > 1e-6:
|
||||||
|
# Scale prior to a few strong pairs so it regularizes yaw without dominating.
|
||||||
|
prior_w = float(np.sqrt(np.median(weights)) / np.deg2rad(sigma))
|
||||||
|
notes.append(f"rotation prior soft constraint sigma={sigma:.1f} deg, weight={prior_w:.3g}")
|
||||||
|
|
||||||
|
def residual(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_x = unpack(vec)
|
||||||
|
residuals = []
|
||||||
|
for pair, weight in zip(usable, weights):
|
||||||
|
err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||||
|
residuals.append(np.sqrt(weight) * err)
|
||||||
|
if r_prior is not None and prior_w > 0:
|
||||||
|
residuals.append(prior_w * so3_log(r_prior.T @ r_x))
|
||||||
|
return np.concatenate(residuals)
|
||||||
|
|
||||||
|
opt = least_squares(residual, pack(r0), loss="huber", f_scale=np.deg2rad(1.0), max_nfev=200)
|
||||||
|
r_x = unpack(opt.x)
|
||||||
|
errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in usable], dtype=float)
|
||||||
|
# Report unweighted RMS/median for interpretability.
|
||||||
|
rms = float(np.sqrt(np.mean(errs**2)))
|
||||||
|
med = float(np.median(errs))
|
||||||
|
p95 = float(np.percentile(errs, 95.0))
|
||||||
|
outlier_fraction = float(np.mean(errs > 5.0))
|
||||||
|
notes.append(f"optimized over {len(usable)} pairs")
|
||||||
|
notes.append(
|
||||||
|
f"rotation residual quality: rms={rms:.3f} deg, median={med:.3f} deg, "
|
||||||
|
f"p95={p95:.3f} deg, >5deg={100.0 * outlier_fraction:.2f}%"
|
||||||
|
)
|
||||||
|
ok = (
|
||||||
|
len(usable) >= 3
|
||||||
|
and rms < 1.5
|
||||||
|
and med < 0.5
|
||||||
|
and p95 < 1.5
|
||||||
|
and outlier_fraction <= 0.005
|
||||||
|
)
|
||||||
|
if not ok:
|
||||||
|
notes.append("rotation residual distribution failed acceptance gates")
|
||||||
|
return RotationHandeyeResult(
|
||||||
|
R_IMU_lidar=r_x,
|
||||||
|
residual_rms_deg=rms,
|
||||||
|
residual_median_deg=med,
|
||||||
|
residual_p95_deg=p95,
|
||||||
|
outlier_fraction_gt_5deg=outlier_fraction,
|
||||||
|
pair_count=len(usable),
|
||||||
|
ok=ok,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,332 @@
|
|||||||
|
"""Constant IMU-to-LiDAR clock-offset estimation via angular-rate correlation."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy import signal
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
from .geometry import rotation_angle_deg, so3_log
|
||||||
|
from .registration import estimate_frame_rotations
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class TimeOffsetResult:
|
||||||
|
delta_t_s: float
|
||||||
|
correlation_peak: float
|
||||||
|
search_s: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
ok: bool = True
|
||||||
|
|
||||||
|
|
||||||
|
def _magnitude_series(times: np.ndarray, values: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||||
|
mag = np.linalg.norm(values, axis=1) if values.ndim == 2 else np.asarray(values, dtype=float)
|
||||||
|
return np.asarray(times, dtype=float), np.asarray(mag, dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def _correlate_offset(
|
||||||
|
imu_t: np.ndarray,
|
||||||
|
imu_mag: np.ndarray,
|
||||||
|
lidar_t: np.ndarray,
|
||||||
|
lidar_mag: np.ndarray,
|
||||||
|
*,
|
||||||
|
search_s: float,
|
||||||
|
sample_hz: float,
|
||||||
|
) -> tuple[float, float]:
|
||||||
|
"""Return ``(delta_t, peak)`` for ``t_imu = t_lidar + delta_t``.
|
||||||
|
|
||||||
|
Implementation: resample both on LiDAR-relative grid, shift IMU by candidate
|
||||||
|
offsets, maximize normalized correlation. This avoids ambiguous lag signs.
|
||||||
|
"""
|
||||||
|
|
||||||
|
t_start = float(lidar_t[0])
|
||||||
|
t_end = float(lidar_t[-1])
|
||||||
|
if t_end - t_start < 0.5:
|
||||||
|
return 0.0, 0.0
|
||||||
|
dt = 1.0 / sample_hz
|
||||||
|
grid = np.arange(t_start, t_end, dt)
|
||||||
|
lidar_sig = np.interp(grid, lidar_t, lidar_mag, left=0.0, right=0.0)
|
||||||
|
lidar_sig = lidar_sig - np.mean(lidar_sig)
|
||||||
|
lidar_norm = float(np.linalg.norm(lidar_sig)) + 1e-12
|
||||||
|
|
||||||
|
best_delta = 0.0
|
||||||
|
best_peak = -1.0
|
||||||
|
for delta in np.arange(-search_s, search_s + 1e-12, dt):
|
||||||
|
imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
|
||||||
|
imu_sig = imu_sig - np.mean(imu_sig)
|
||||||
|
denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
|
||||||
|
peak = float(np.dot(imu_sig, lidar_sig) / denom)
|
||||||
|
if peak > best_peak:
|
||||||
|
best_peak = peak
|
||||||
|
best_delta = float(delta)
|
||||||
|
|
||||||
|
# Local parabolic refinement.
|
||||||
|
deltas = np.array([best_delta - dt, best_delta, best_delta + dt], dtype=float)
|
||||||
|
peaks = []
|
||||||
|
for delta in deltas:
|
||||||
|
imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
|
||||||
|
imu_sig = imu_sig - np.mean(imu_sig)
|
||||||
|
denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
|
||||||
|
peaks.append(float(np.dot(imu_sig, lidar_sig) / denom))
|
||||||
|
y0, y1, y2 = peaks
|
||||||
|
denom = y0 - 2 * y1 + y2
|
||||||
|
if abs(denom) > 1e-12:
|
||||||
|
refined = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
|
||||||
|
# Parabola can jump outside the searched window; keep it clamped.
|
||||||
|
if abs(refined) <= search_s + dt:
|
||||||
|
best_delta = refined
|
||||||
|
best_peak = float(y1)
|
||||||
|
return best_delta, best_peak
|
||||||
|
|
||||||
|
|
||||||
|
def estimate_time_offset(
|
||||||
|
imu: ImuSeries,
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
search_s: float = 1.0,
|
||||||
|
sample_hz: float = 50.0,
|
||||||
|
) -> TimeOffsetResult:
|
||||||
|
"""Estimate ``t_imu = t_lidar + delta_t``.
|
||||||
|
|
||||||
|
Positive ``delta_t`` means the IMU clock reading is ahead of the LiDAR clock
|
||||||
|
for the same physical instant (IMU timestamps are larger).
|
||||||
|
"""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
if len(frames) < 5:
|
||||||
|
return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR frames",), False)
|
||||||
|
|
||||||
|
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
gyro = imu.gyro_rad_s - bias
|
||||||
|
|
||||||
|
# Use short consecutive (or near-consecutive) pairs. A large stride (e.g.
|
||||||
|
# len//20) averages over many seconds and destroys |ω| correlation even when
|
||||||
|
# host/device clocks are already aligned.
|
||||||
|
stride = 1 if len(frames) < 80 else 2
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
|
||||||
|
if len(rotations) < 8:
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=1)
|
||||||
|
if len(rotations) < 4:
|
||||||
|
return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR relative rotations",), False)
|
||||||
|
|
||||||
|
lidar_t = []
|
||||||
|
lidar_w = []
|
||||||
|
for (t_a, t_b), rotation in zip(pair_times, rotations):
|
||||||
|
dt_pair = max(t_b - t_a, 1e-3)
|
||||||
|
omega = so3_log(rotation) / dt_pair
|
||||||
|
lidar_t.append(0.5 * (t_a + t_b))
|
||||||
|
lidar_w.append(omega)
|
||||||
|
lidar_t_arr = np.asarray(lidar_t, dtype=float)
|
||||||
|
lidar_w_arr = np.asarray(lidar_w, dtype=float)
|
||||||
|
|
||||||
|
imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
|
||||||
|
lidar_t_mag, lidar_mag = _magnitude_series(lidar_t_arr, lidar_w_arr)
|
||||||
|
delta, peak = _correlate_offset(
|
||||||
|
imu_t,
|
||||||
|
imu_mag,
|
||||||
|
lidar_t_mag,
|
||||||
|
lidar_mag,
|
||||||
|
search_s=search_s,
|
||||||
|
sample_hz=sample_hz,
|
||||||
|
)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
|
||||||
|
)
|
||||||
|
notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
|
||||||
|
# Host-UTC-bridged sessions are already on one timeline; |ω| peak can stay
|
||||||
|
# weak even at the correct lag (ICP rate vs gyro scale). Accept near-zero δt.
|
||||||
|
near_zero = abs(float(delta)) <= min(0.05, 0.25 * float(search_s))
|
||||||
|
ok = peak > 0.15 or near_zero
|
||||||
|
if peak <= 0.15 and near_zero:
|
||||||
|
notes.append(
|
||||||
|
f"correlation peak weak ({peak:.3f}) but |delta_t|={abs(delta):.4f}s ~0; "
|
||||||
|
"accepting as already-aligned (e.g. host UTC bridge)"
|
||||||
|
)
|
||||||
|
elif not ok:
|
||||||
|
notes.append("correlation peak is weak; check overlapping motion and axis units")
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=delta,
|
||||||
|
correlation_peak=peak,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=ok,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def lidar_time_to_imu_time(t_lidar_s: float, delta_t_s: float) -> float:
|
||||||
|
"""Convert a LiDAR timestamp to the IMU clock using ``t_imu = t_lidar + delta_t``."""
|
||||||
|
|
||||||
|
return float(t_lidar_s + delta_t_s)
|
||||||
|
|
||||||
|
|
||||||
|
def _lidar_omega_series(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
stride: int,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray]:
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
|
||||||
|
if len(rotations) < 4:
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=1)
|
||||||
|
lidar_t: list[float] = []
|
||||||
|
lidar_w: list[np.ndarray] = []
|
||||||
|
for (t_a, t_b), rotation in zip(pair_times, rotations):
|
||||||
|
dt_pair = max(t_b - t_a, 1e-3)
|
||||||
|
omega = so3_log(rotation) / dt_pair
|
||||||
|
lidar_t.append(0.5 * (t_a + t_b))
|
||||||
|
lidar_w.append(omega)
|
||||||
|
return np.asarray(lidar_t, dtype=float), np.asarray(lidar_w, dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def refine_time_offset_signed(
|
||||||
|
imu: ImuSeries,
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
delta_t_s: float,
|
||||||
|
R_IMU_lidar: np.ndarray,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
search_s: float = 0.08,
|
||||||
|
sample_hz: float = 50.0,
|
||||||
|
max_shift_s: float | None = 0.05,
|
||||||
|
) -> TimeOffsetResult:
|
||||||
|
"""Refine ``δt`` with signed 3-axis rates using a known ``R_IMU_lidar``.
|
||||||
|
|
||||||
|
Cost: mean squared error between ``gyro_imu(t_lidar+δt)`` and
|
||||||
|
``R_IMU_lidar @ omega_lidar(t_lidar)`` on a common grid around the coarse ``δt``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
notes: list[str] = [f"signed refine around coarse delta_t={delta_t_s:.6f}s"]
|
||||||
|
if len(frames) < 5:
|
||||||
|
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR frames",), False)
|
||||||
|
|
||||||
|
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
gyro = imu.gyro_rad_s - bias
|
||||||
|
r_x = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3)
|
||||||
|
|
||||||
|
stride = max(1, len(frames) // 20)
|
||||||
|
lidar_t, lidar_w = _lidar_omega_series(frames, stride=stride)
|
||||||
|
if lidar_t.size < 4:
|
||||||
|
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR omega samples",), False)
|
||||||
|
|
||||||
|
# Predicted IMU-frame angular rate from LiDAR relative rotations.
|
||||||
|
pred = (r_x @ lidar_w.T).T
|
||||||
|
t_start = float(lidar_t[0])
|
||||||
|
t_end = float(lidar_t[-1])
|
||||||
|
if t_end - t_start < 0.5:
|
||||||
|
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("LiDAR span too short for signed refine",), False)
|
||||||
|
|
||||||
|
dt = 1.0 / sample_hz
|
||||||
|
grid = np.arange(t_start, t_end, dt)
|
||||||
|
pred_grid = np.column_stack(
|
||||||
|
[np.interp(grid, lidar_t, pred[:, axis], left=np.nan, right=np.nan) for axis in range(3)]
|
||||||
|
)
|
||||||
|
|
||||||
|
def _cost_and_corr(delta: float) -> tuple[float, float]:
|
||||||
|
meas = np.column_stack(
|
||||||
|
[
|
||||||
|
np.interp(grid + delta, imu.t_s, gyro[:, axis], left=np.nan, right=np.nan)
|
||||||
|
for axis in range(3)
|
||||||
|
]
|
||||||
|
)
|
||||||
|
mask = np.isfinite(pred_grid).all(axis=1) & np.isfinite(meas).all(axis=1)
|
||||||
|
if int(np.count_nonzero(mask)) < 10:
|
||||||
|
return float("inf"), -1.0
|
||||||
|
err = meas[mask] - pred_grid[mask]
|
||||||
|
cost = float(np.mean(np.sum(err * err, axis=1)))
|
||||||
|
a = meas[mask].reshape(-1)
|
||||||
|
b = pred_grid[mask].reshape(-1)
|
||||||
|
a = a - np.mean(a)
|
||||||
|
b = b - np.mean(b)
|
||||||
|
corr = float(np.dot(a, b) / ((np.linalg.norm(a) + 1e-12) * (np.linalg.norm(b) + 1e-12)))
|
||||||
|
return cost, corr
|
||||||
|
|
||||||
|
coarse_cost, coarse_corr = _cost_and_corr(float(delta_t_s))
|
||||||
|
best_delta = float(delta_t_s)
|
||||||
|
best_cost = coarse_cost
|
||||||
|
best_corr = coarse_corr
|
||||||
|
half = abs(float(search_s))
|
||||||
|
for delta in np.arange(delta_t_s - half, delta_t_s + half + 1e-12, dt):
|
||||||
|
cost, corr = _cost_and_corr(float(delta))
|
||||||
|
if cost < best_cost:
|
||||||
|
best_cost = cost
|
||||||
|
best_delta = float(delta)
|
||||||
|
best_corr = corr
|
||||||
|
|
||||||
|
# Parabolic refine on cost around the best discrete delta.
|
||||||
|
samples = []
|
||||||
|
for delta in (best_delta - dt, best_delta, best_delta + dt):
|
||||||
|
cost, _ = _cost_and_corr(float(delta))
|
||||||
|
samples.append(cost if np.isfinite(cost) else best_cost)
|
||||||
|
y0, y1, y2 = samples
|
||||||
|
denom = y0 - 2 * y1 + y2
|
||||||
|
if abs(denom) > 1e-12 and y1 <= y0 and y1 <= y2:
|
||||||
|
candidate = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
|
||||||
|
cand_cost, cand_corr = _cost_and_corr(candidate)
|
||||||
|
if cand_cost < best_cost:
|
||||||
|
best_delta = candidate
|
||||||
|
best_cost = cand_cost
|
||||||
|
best_corr = cand_corr
|
||||||
|
|
||||||
|
# Guard with magnitude correlation so ICP-biased signed minima cannot wander.
|
||||||
|
imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
|
||||||
|
lidar_t_mag, lidar_mag = _magnitude_series(lidar_t, lidar_w)
|
||||||
|
|
||||||
|
def _mag_score(delta: float) -> float:
|
||||||
|
t_start_l = float(lidar_t_mag[0])
|
||||||
|
t_end_l = float(lidar_t_mag[-1])
|
||||||
|
grid_m = np.arange(t_start_l, t_end_l, dt)
|
||||||
|
lidar_sig = np.interp(grid_m, lidar_t_mag, lidar_mag, left=0.0, right=0.0)
|
||||||
|
lidar_sig = lidar_sig - np.mean(lidar_sig)
|
||||||
|
imu_sig = np.interp(grid_m + delta, imu_t, imu_mag, left=0.0, right=0.0)
|
||||||
|
imu_sig = imu_sig - np.mean(imu_sig)
|
||||||
|
denom = (float(np.linalg.norm(lidar_sig)) + 1e-12) * (float(np.linalg.norm(imu_sig)) + 1e-12)
|
||||||
|
return float(np.dot(imu_sig, lidar_sig) / denom)
|
||||||
|
|
||||||
|
mag_at_coarse = _mag_score(float(delta_t_s))
|
||||||
|
mag_at_best = _mag_score(best_delta)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"signed 3-axis refine: delta_t={best_delta:.6f}s, "
|
||||||
|
f"mse={best_cost:.4g} (coarse_mse={coarse_cost:.4g}), "
|
||||||
|
f"corr={best_corr:.3f}, mag_corr={mag_at_best:.3f} (coarse_mag={mag_at_coarse:.3f}), "
|
||||||
|
f"search=±{half:.3f}s"
|
||||||
|
)
|
||||||
|
shift = abs(best_delta - float(delta_t_s))
|
||||||
|
if max_shift_s is not None and shift > float(max_shift_s):
|
||||||
|
notes.append(
|
||||||
|
f"signed refine rejected: |Δδt|={shift:.4f}s exceeds max_shift={float(max_shift_s):.4f}s; "
|
||||||
|
"keeping previous delta_t"
|
||||||
|
)
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=float(delta_t_s),
|
||||||
|
correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
# Require a meaningful MSE drop so tiny downhill noise cannot walk δt across iterations.
|
||||||
|
improved = (
|
||||||
|
np.isfinite(best_cost)
|
||||||
|
and best_cost < coarse_cost * 0.98
|
||||||
|
# Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
|
||||||
|
and mag_at_best + 1e-4 >= mag_at_coarse
|
||||||
|
)
|
||||||
|
if not improved:
|
||||||
|
notes.append("signed refine rejected by MSE/mag-consistency; keeping previous delta_t")
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=float(delta_t_s),
|
||||||
|
correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=best_delta,
|
||||||
|
correlation_peak=mag_at_best,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
"""Timestamp audit for IMU and LiDAR streams."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class TimestampAuditReport:
|
||||||
|
monotonic: bool
|
||||||
|
epoch_count: int
|
||||||
|
imu_rate_hz: float
|
||||||
|
lidar_rate_hz: float
|
||||||
|
imu_duration_s: float
|
||||||
|
lidar_duration_s: float
|
||||||
|
max_imu_gap_s: float
|
||||||
|
max_lidar_gap_s: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
ok: bool = True
|
||||||
|
|
||||||
|
|
||||||
|
def _rate_and_gaps(times: np.ndarray) -> tuple[float, float]:
|
||||||
|
if times.size < 2:
|
||||||
|
return 0.0, 0.0
|
||||||
|
dt = np.diff(times)
|
||||||
|
positive = dt[dt > 0]
|
||||||
|
if positive.size == 0:
|
||||||
|
return 0.0, float("inf")
|
||||||
|
rate = float(1.0 / np.median(positive))
|
||||||
|
return rate, float(np.max(dt))
|
||||||
|
|
||||||
|
|
||||||
|
def audit_timestamps(imu: ImuSeries, frames: list[LidarFrame]) -> TimestampAuditReport:
|
||||||
|
"""Audit native timestamps without assuming the two clocks share an epoch."""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
imu_t = imu.t_s
|
||||||
|
lidar_t = np.asarray([frame.t_mid_s for frame in frames], dtype=float)
|
||||||
|
|
||||||
|
imu_mono = bool(np.all(np.diff(imu_t) >= 0)) if imu_t.size > 1 else False
|
||||||
|
lidar_mono = bool(np.all(np.diff(lidar_t) >= 0)) if lidar_t.size > 1 else False
|
||||||
|
if not imu_mono:
|
||||||
|
notes.append("IMU timestamps are not monotonic")
|
||||||
|
if not lidar_mono:
|
||||||
|
notes.append("LiDAR timestamps are not monotonic")
|
||||||
|
|
||||||
|
imu_rate, imu_gap = _rate_and_gaps(imu_t)
|
||||||
|
lidar_rate, lidar_gap = _rate_and_gaps(lidar_t)
|
||||||
|
if imu_t.size < 50:
|
||||||
|
notes.append(f"IMU sample count is low ({imu_t.size})")
|
||||||
|
if len(frames) < 5:
|
||||||
|
notes.append(f"LiDAR frame count is low ({len(frames)})")
|
||||||
|
if imu_gap > 0.05:
|
||||||
|
notes.append(f"large IMU gap detected: {imu_gap:.3f}s")
|
||||||
|
if lidar_gap > 1.0:
|
||||||
|
notes.append(f"large LiDAR gap detected: {lidar_gap:.3f}s")
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
"IMU and LiDAR clocks are treated as independent; constant offset is estimated later."
|
||||||
|
)
|
||||||
|
|
||||||
|
ok = imu_mono and lidar_mono and imu_t.size >= 50 and len(frames) >= 5
|
||||||
|
return TimestampAuditReport(
|
||||||
|
monotonic=imu_mono and lidar_mono,
|
||||||
|
epoch_count=2,
|
||||||
|
imu_rate_hz=imu_rate,
|
||||||
|
lidar_rate_hz=lidar_rate,
|
||||||
|
imu_duration_s=float(imu_t[-1] - imu_t[0]) if imu_t.size else 0.0,
|
||||||
|
lidar_duration_s=float(lidar_t[-1] - lidar_t[0]) if lidar_t.size else 0.0,
|
||||||
|
max_imu_gap_s=imu_gap,
|
||||||
|
max_lidar_gap_s=lidar_gap,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=ok,
|
||||||
|
)
|
||||||
@@ -0,0 +1,83 @@
|
|||||||
|
"""Vehicle-installation configuration loading and light validation."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Mapping
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
REQUIRED_TOP_LEVEL_KEYS = frozenset({"schema_version", "vehicle", "installation", "sensors", "time"})
|
||||||
|
|
||||||
|
|
||||||
|
def validate_config_shape(config: Mapping[str, object]) -> list[str]:
|
||||||
|
"""Return missing top-level keys without inventing default values."""
|
||||||
|
|
||||||
|
return sorted(REQUIRED_TOP_LEVEL_KEYS.difference(config))
|
||||||
|
|
||||||
|
|
||||||
|
def validate_config_semantics(config: Mapping[str, Any]) -> list[str]:
|
||||||
|
"""Return semantic issues that block calibration interpretation."""
|
||||||
|
|
||||||
|
issues: list[str] = []
|
||||||
|
sensors = config.get("sensors")
|
||||||
|
if not isinstance(sensors, Mapping):
|
||||||
|
return ["sensors must be a mapping"]
|
||||||
|
imu = sensors.get("imu")
|
||||||
|
lidar = sensors.get("lidar")
|
||||||
|
if not isinstance(imu, Mapping):
|
||||||
|
issues.append("sensors.imu missing")
|
||||||
|
else:
|
||||||
|
axes = ((imu.get("raw_frame") or {}) if isinstance(imu.get("raw_frame"), Mapping) else {}).get("axes")
|
||||||
|
if not axes:
|
||||||
|
issues.append("sensors.imu.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||||
|
if not isinstance(lidar, Mapping):
|
||||||
|
issues.append("sensors.lidar missing")
|
||||||
|
else:
|
||||||
|
axes = ((lidar.get("raw_frame") or {}) if isinstance(lidar.get("raw_frame"), Mapping) else {}).get("axes")
|
||||||
|
if not axes:
|
||||||
|
issues.append("sensors.lidar.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||||
|
|
||||||
|
time_cfg = config.get("time")
|
||||||
|
if not isinstance(time_cfg, Mapping):
|
||||||
|
issues.append("time missing")
|
||||||
|
else:
|
||||||
|
for key in ("imu_timestamp_source", "lidar_timestamp_source", "lidar_frame_time_definition"):
|
||||||
|
if not time_cfg.get(key):
|
||||||
|
issues.append(f"time.{key} is empty")
|
||||||
|
return issues
|
||||||
|
|
||||||
|
|
||||||
|
def load_vehicle_config(path: str | Path) -> dict[str, Any]:
|
||||||
|
"""Load and lightly validate a YAML vehicle configuration."""
|
||||||
|
|
||||||
|
try:
|
||||||
|
import yaml
|
||||||
|
except ImportError as exc: # pragma: no cover
|
||||||
|
raise ImportError("PyYAML is required to load vehicle configuration files") from exc
|
||||||
|
|
||||||
|
config_path = Path(path)
|
||||||
|
with config_path.open("r", encoding="utf-8") as handle:
|
||||||
|
loaded = yaml.safe_load(handle)
|
||||||
|
if not isinstance(loaded, dict):
|
||||||
|
raise ValueError(f"vehicle config must be a mapping: {config_path}")
|
||||||
|
|
||||||
|
missing = validate_config_shape(loaded)
|
||||||
|
if missing:
|
||||||
|
raise ValueError(f"vehicle config missing keys {missing}: {config_path}")
|
||||||
|
|
||||||
|
semantic = validate_config_semantics(loaded)
|
||||||
|
if semantic:
|
||||||
|
raise ValueError("vehicle config semantic issues:\n- " + "\n- ".join(semantic))
|
||||||
|
return loaded
|
||||||
|
|
||||||
|
|
||||||
|
def prior_enabled(config: Mapping[str, Any], name: str) -> bool:
|
||||||
|
"""Return whether an optional prior is enabled."""
|
||||||
|
|
||||||
|
init = config.get("initialization")
|
||||||
|
if not isinstance(init, Mapping):
|
||||||
|
return False
|
||||||
|
prior = init.get(name)
|
||||||
|
if not isinstance(prior, Mapping):
|
||||||
|
return False
|
||||||
|
return bool(prior.get("enabled", False))
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
# `imu_lidar` 模块说明
|
||||||
|
|
||||||
|
**用途：** 改 `imu_lidar/` 源码时查阅。对外用法见根目录 [`README.md`](../README.md)。
|
||||||
|
|
||||||
|
本包实现 LiDAR–IMU 外参标定：在连续行驶数据上选取关键帧，用 IMU 预积分与雷达配准构造相对运动对，求解安装外参。
|
||||||
|
|
||||||
|
```text
|
||||||
|
A ≈ 关键帧间 IMU 相对运动（预积分：旋转 / 速度增量 / 位移增量）
|
||||||
|
B ≈ 关键帧间雷达配准
|
||||||
|
解 R_A R_X = R_X R_B → 旋转外参（手眼阶段只用旋转）
|
||||||
|
再精修旋转与陀螺零偏；在完整六自由度模式下，可观时再估计平移等
|
||||||
|
```
|
||||||
|
|
||||||
|
入口：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m imu_lidar.cli plan
|
||||||
|
python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output ...
|
||||||
|
```
|
||||||
|
|
||||||
|
整体流程由 `pipeline.py` 串联。
|
||||||
|
修改本目录代码时，请同步更新本说明，并在 [`CHANGELOG.md`](CHANGELOG.md) 追加「时间戳 + 原本 → 改成」。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 流水线顺序与文件
|
||||||
|
|
||||||
|
|
||||||
|
| 顺序 | 文件 | 作用 |
|
||||||
|
| --- | ----------------------- | ----------------------------- |
|
||||||
|
| 0 | `contracts.py` | 公共数据类型与状态枚举 |
|
||||||
|
| 0 | `geometry.py` | 刚体变换与旋转工具 |
|
||||||
|
| 0 | `vehicle_config.py` | 读取并校验车辆 YAML |
|
||||||
|
| 1 | `imu_io.py` | 读标准 IMU 中间格式 |
|
||||||
|
| 1 | `lidar_io.py` | 读标准雷达会话目录 |
|
||||||
|
| 2 | `timestamp_audit.py` | 时间单调 / 频率 / 空洞检查 |
|
||||||
|
| 3 | `imu_audit.py` | 静止零偏、加速度模长检查、建议竖直轴 |
|
||||||
|
| 4 | `time_offset.py` | 粗估时间偏置 δt，并用旋转外参精修 |
|
||||||
|
| 5 | `registration.py` | 帧间点云配准 |
|
||||||
|
| 5 | `keyframes.py` | 按运动量抽取关键帧 |
|
||||||
|
| 5 | `lidar_deskew.py` | 可选点云去畸变（低速可关） |
|
||||||
|
| 6 | `imu_preintegration.py` | IMU 预积分（旋转及速度/位移增量、协方差、零偏雅可比） |
|
||||||
|
| 6 | `motion_pairs.py` | 构造运动对；手眼使用其中的旋转 |
|
||||||
|
| 6 | `motion_pairs_io.py` | 运动对 JSON 缓存读写（供可视化直读） |
|
||||||
|
| 7 | `rotation_handeye.py` | 加权旋转手眼 |
|
||||||
|
| 8 | `observability.py` | 旋转 / 平移可观性检查 |
|
||||||
|
| 8 | `joint_optimizer.py` | 联合精修；完整模式下可估计平移、重力、速度与时变零偏 |
|
||||||
|
| 9 | `finalize.py` | 写出结果 JSON（含 `motion_pairs.json`） |
|
||||||
|
| — | `pipeline.py` | 编排全流程 |
|
||||||
|
| — | `cli.py` | 命令行入口 |
|
||||||
|
| — | `CHANGELOG.md` | 改动记录 |
|
||||||
|
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 运行模式要点
|
||||||
|
|
||||||
|
- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。
|
||||||
|
- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。
|
||||||
|
- `--mode full_se3`：当前完成 Phase-A 后明确拒绝平移；待 Phase-B/C 会话状态重构完成后再恢复完整 SE(3) 交付。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 输入格式
|
||||||
|
|
||||||
|
```text
|
||||||
|
imu.csv # t,gx,gy,gz,ax,ay,az（建议设备时间）
|
||||||
|
lidar_session/
|
||||||
|
frames_index.csv # frame_id,filename,t_start,t_end
|
||||||
|
frames/frame_XXXXX.npz # points: (N,3) 米
|
||||||
|
```
|
||||||
|
|
||||||
|
原始 N300 `.rscap` + H32 dlog（或旧 MSOP `.rscap`）用仓库工具导出：`python tools/export_rscap_to_v1.py ...`（见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md)）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 当前能力
|
||||||
|
|
||||||
|
- 本包是仓库**唯一**标定路径：质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →（可选）完整六自由度 → 报告
|
||||||
|
- 点云去畸变：可选
|
||||||
|
- 阶段与用法见根目录 [`README.md`](../README.md)
|
||||||
|
- 改动史：[`CHANGELOG.md`](CHANGELOG.md)
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
[project]
|
||||||
|
name = "lidar-imu-calibration"
|
||||||
|
version = "0.3.0"
|
||||||
|
description = "LiDAR–IMU extrinsic calibration from continuous-motion keyframes (imu_lidar)"
|
||||||
|
requires-python = ">=3.10"
|
||||||
|
dependencies = [
|
||||||
|
"numpy>=1.26",
|
||||||
|
"scipy>=1.11",
|
||||||
|
"pyyaml>=6.0",
|
||||||
|
]
|
||||||
|
|
||||||
|
[project.optional-dependencies]
|
||||||
|
open3d = ["open3d>=0.17"]
|
||||||
|
dev = ["pytest>=7.4"]
|
||||||
|
|
||||||
|
[project.scripts]
|
||||||
|
lidar-imu-calibration = "imu_lidar.cli:main"
|
||||||
|
|
||||||
|
[build-system]
|
||||||
|
requires = ["setuptools>=68", "wheel"]
|
||||||
|
build-backend = "setuptools.build_meta"
|
||||||
|
|
||||||
|
[tool.setuptools]
|
||||||
|
packages = ["imu_lidar", "tools"]
|
||||||
|
|
||||||
|
[tool.pytest.ini_options]
|
||||||
|
testpaths = ["tests"]
|
||||||
|
pythonpath = ["."]
|
||||||
@@ -1,4 +0,0 @@
|
|||||||
numpy>=1.26
|
|
||||||
scipy>=1.11
|
|
||||||
open3d>=0.18
|
|
||||||
small-gicp
|
|
||||||
@@ -1,5 +0,0 @@
|
|||||||
# results目录
|
|
||||||
|
|
||||||
`reference_data4/`是本仓库附带的精简参考结果。新的运行结果应写到仓库外目录或`outputs/`，不要覆盖参考结果。
|
|
||||||
|
|
||||||
参考结果保留最终矩阵、共识B、两后端精筛B、逐对CSV/筛选审计和地面平面；未保留原始点云、逐帧combined数据、冗长的初筛JSON和带本机绝对路径的过程文件。
|
|
||||||
@@ -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
|
|
||||||
]
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,27 +0,0 @@
|
|||||||
# run目录
|
|
||||||
|
|
||||||
根README包含完整复现命令；这里仅列入口职责。
|
|
||||||
|
|
||||||
| 脚本 | 用途 |
|
|
||||||
|---|---|
|
|
||||||
| `run_full_pipeline.ps1` | 调用一步导出得到 `combined/`，再跑到最终 `T_RTK_lidar` |
|
|
||||||
| `export_multisensor_stations.ps1` | 薄封装：调用 `tools/export_raw_to_combined.py` |
|
|
||||||
| `prepare_multisensor_dataset.ps1` | 每站选一帧，生成yaw-only RTK参考轨迹和`frames_all` |
|
|
||||||
| `run_direct_rtk_lidar.ps1` | 从combined数据运行RTK直接标定和最终结果封装 |
|
|
||||||
| `run_single_dataset.ps1` | 执行地面、两个GICP后端、精筛、共识和AX=XB求解 |
|
|
||||||
| `run_joint_rtk_lidar.ps1` | 合并多个独立批次的批内共识运动对和地面平面，求解共享外参 |
|
|
||||||
| `view_result.ps1` | 打开3D运动对对比并打印数值增量 |
|
|
||||||
|
|
||||||
原始→中间包请优先直接用 Python 一步导出（与 Lidar-IMU 用法对齐）：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu-rscap ... --out ... --overwrite
|
|
||||||
```
|
|
||||||
|
|
||||||
常用参数：
|
|
||||||
|
|
||||||
- `-LidarCaptureName h32.rscap`：每站雷达文件名（也接受 `lidar.rscap`）
|
|
||||||
- `-TimeBasis device_gnss`（默认）：雷达设备时 ↔ GNSS week/TOW
|
|
||||||
- `-TimeBasis host`：旧 dlog + 主机接收时间关联
|
|
||||||
|
|
||||||
所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度，避免静默使用与实车不符的默认值；默认生成目录`work/`和`outputs/`不会提交Git。
|
|
||||||
@@ -1,58 +0,0 @@
|
|||||||
param(
|
|
||||||
[Parameter(Mandatory = $true)][string]$DataRoot,
|
|
||||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
|
||||||
[Parameter(Mandatory = $true)][string]$RtkCapture,
|
|
||||||
[Parameter(Mandatory = $true)][string]$ImuCapture,
|
|
||||||
[string]$LidarObject = "frontlidar",
|
|
||||||
[string]$LidarCaptureName = "h32.rscap",
|
|
||||||
[string]$Timezone = "+08:00",
|
|
||||||
[string[]]$StationNames = @(),
|
|
||||||
[int]$Stride = 1,
|
|
||||||
[double]$RtkMaxDtMs = 150.0,
|
|
||||||
[double]$ImuBeforeMs = 100.0,
|
|
||||||
[double]$ImuAfterMs = 100.0,
|
|
||||||
[ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss",
|
|
||||||
[switch]$SkipLidarExport,
|
|
||||||
[switch]$SkipSerialParsing
|
|
||||||
)
|
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
|
||||||
$RepoRoot = Split-Path -Parent $PSScriptRoot
|
|
||||||
$Exporter = Join-Path $RepoRoot "tools\export_raw_to_combined.py"
|
|
||||||
|
|
||||||
foreach ($Path in @($DataRoot, $RtkCapture, $ImuCapture)) {
|
|
||||||
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
|
|
||||||
}
|
|
||||||
if ($Stride -lt 1) { throw "Stride must be at least 1" }
|
|
||||||
if ($SkipLidarExport -or $SkipSerialParsing) {
|
|
||||||
throw "Partial skip flags are no longer supported; use tools/export_raw_to_combined.py internals or run_direct_rtk_lidar.ps1 on an existing combined/"
|
|
||||||
}
|
|
||||||
|
|
||||||
$Args = @(
|
|
||||||
$Exporter,
|
|
||||||
"--stations-root", $DataRoot,
|
|
||||||
"--rtk-rscap", $RtkCapture,
|
|
||||||
"--imu-rscap", $ImuCapture,
|
|
||||||
"--out", $OutputRoot,
|
|
||||||
"--lidar-capture-name", $LidarCaptureName,
|
|
||||||
"--lidar-object", $LidarObject,
|
|
||||||
"--timezone", $Timezone,
|
|
||||||
"--stride", "$Stride",
|
|
||||||
"--rtk-max-dt-ms", "$RtkMaxDtMs",
|
|
||||||
"--imu-before-ms", "$ImuBeforeMs",
|
|
||||||
"--imu-after-ms", "$ImuAfterMs",
|
|
||||||
"--time-basis", $TimeBasis,
|
|
||||||
"--overwrite"
|
|
||||||
)
|
|
||||||
foreach ($Name in $StationNames) {
|
|
||||||
$Args += @("--station", $Name)
|
|
||||||
}
|
|
||||||
|
|
||||||
Write-Host "[raw → combined one-shot export]"
|
|
||||||
& python @Args
|
|
||||||
if ($LASTEXITCODE -ne 0) {
|
|
||||||
throw "export_raw_to_combined failed with Python exit code $LASTEXITCODE"
|
|
||||||
}
|
|
||||||
|
|
||||||
Write-Host "Combined NPZ: $(Join-Path $OutputRoot 'combined')"
|
|
||||||
Write-Host "Summary: $(Join-Path $OutputRoot 'export_summary.json')"
|
|
||||||
@@ -1,22 +0,0 @@
|
|||||||
param(
|
|
||||||
[Parameter(Mandatory = $true)][string]$CombinedRoot,
|
|
||||||
[Parameter(Mandatory = $true)][string]$Output,
|
|
||||||
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
|
|
||||||
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
|
|
||||||
[string]$PoseName = "rtk_gga_raw_heading",
|
|
||||||
[int]$MinStations = 30,
|
|
||||||
[int]$ExpectedStations = 0,
|
|
||||||
[double]$HeadingStdLimitDeg = 0.5,
|
|
||||||
[switch]$Overwrite
|
|
||||||
)
|
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
|
||||||
if ($AntennaLever.Count -ne 3) { throw "AntennaLever must contain X,Y,Z in body coordinates" }
|
|
||||||
$Repo = Split-Path -Parent $PSScriptRoot
|
|
||||||
$Args = @((Join-Path $Repo "tools\prepare_multisensor_station_dataset.py"), "--combined-root", $CombinedRoot,
|
|
||||||
"--output", $Output, "--pose-name", $PoseName, "--heading-offset-deg", "$HeadingOffsetDeg", "--antenna-lever") +
|
|
||||||
@($AntennaLever | ForEach-Object { "$_" }) + @("--min-stations", "$MinStations",
|
|
||||||
"--expected-stations", "$ExpectedStations", "--heading-std-limit-deg", "$HeadingStdLimitDeg")
|
|
||||||
if ($Overwrite) { $Args += "--overwrite" }
|
|
||||||
& python @Args
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Multisensor dataset preparation failed" }
|
|
||||||
@@ -1,36 +0,0 @@
|
|||||||
param(
|
|
||||||
[Parameter(Mandatory = $true)][string]$CombinedRoot,
|
|
||||||
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
|
||||||
[string]$OutputRoot = "",
|
|
||||||
[string]$WorkRoot = "",
|
|
||||||
[int]$ExpectedStations = 34,
|
|
||||||
[int]$MinPairs = 20,
|
|
||||||
[int]$Bootstrap = 200
|
|
||||||
)
|
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
|
||||||
$Repo = Split-Path -Parent $PSScriptRoot
|
|
||||||
if ([string]::IsNullOrWhiteSpace($OutputRoot)) { $OutputRoot = Join-Path $Repo "outputs\rtk_lidar_calibration" }
|
|
||||||
if ([string]::IsNullOrWhiteSpace($WorkRoot)) { $WorkRoot = Join-Path $Repo "work\prepared_rtk_direct" }
|
|
||||||
$Prepared = $WorkRoot
|
|
||||||
|
|
||||||
& (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") `
|
|
||||||
-CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg 0 `
|
|
||||||
-AntennaLever @(0.0,0.0,0.0) -PoseName "rtk_gga_raw_heading" -MinStations 30 -ExpectedStations $ExpectedStations -Overwrite
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }
|
|
||||||
|
|
||||||
& (Join-Path $Repo "run\run_single_dataset.ps1") `
|
|
||||||
-Prepared $Prepared -OutputRoot $OutputRoot `
|
|
||||||
-ReferencePoseFile "reference_poses_rtk_gga_raw_heading.csv" `
|
|
||||||
-ReferenceHeight $RtkReferenceHeightAboveGroundM -MinPairs $MinPairs -Bootstrap $Bootstrap
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct calibration failed" }
|
|
||||||
|
|
||||||
$Finalize = @(
|
|
||||||
(Join-Path $Repo "code\finalize_direct_rtk_lidar.py"),
|
|
||||||
"--result-root", $OutputRoot,
|
|
||||||
"--reference-height", "$RtkReferenceHeightAboveGroundM"
|
|
||||||
)
|
|
||||||
& python @Finalize
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
|
|
||||||
|
|
||||||
Write-Host "Final T_RTK_lidar: $(Join-Path $OutputRoot 'final_T_RTK_lidar.json')"
|
|
||||||
@@ -1,35 +0,0 @@
|
|||||||
param(
|
|
||||||
[Parameter(Mandatory = $true)][string]$DataRoot,
|
|
||||||
[Parameter(Mandatory = $true)][string]$RtkCapture,
|
|
||||||
[Parameter(Mandatory = $true)][string]$ImuCapture,
|
|
||||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
|
||||||
[string]$LidarObject = "frontlidar",
|
|
||||||
[string]$LidarCaptureName = "h32.rscap",
|
|
||||||
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
|
||||||
[string]$Timezone = "+08:00",
|
|
||||||
[ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss",
|
|
||||||
[int]$ExpectedStations = 34,
|
|
||||||
[int]$MinPairs = 20,
|
|
||||||
[int]$Bootstrap = 200
|
|
||||||
)
|
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
|
||||||
$ExportRoot = Join-Path $OutputRoot "exported"
|
|
||||||
$PreparedRoot = Join-Path $OutputRoot "prepared_rtk_direct"
|
|
||||||
$CalibrationRoot = Join-Path $OutputRoot "calibration"
|
|
||||||
|
|
||||||
& (Join-Path $PSScriptRoot "export_multisensor_stations.ps1") `
|
|
||||||
-DataRoot $DataRoot -RtkCapture $RtkCapture -ImuCapture $ImuCapture `
|
|
||||||
-OutputRoot $ExportRoot -LidarObject $LidarObject -LidarCaptureName $LidarCaptureName `
|
|
||||||
-Timezone $Timezone -TimeBasis $TimeBasis
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" }
|
|
||||||
|
|
||||||
& (Join-Path $PSScriptRoot "run_direct_rtk_lidar.ps1") `
|
|
||||||
-CombinedRoot (Join-Path $ExportRoot "combined") `
|
|
||||||
-WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
|
|
||||||
-RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM `
|
|
||||||
-ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" }
|
|
||||||
|
|
||||||
Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')"
|
|
||||||
Write-Host "Prepared frames: $(Join-Path $PreparedRoot 'frames_all')"
|
|
||||||
@@ -1,103 +0,0 @@
|
|||||||
param(
|
|
||||||
[Parameter(Mandatory = $true)][string[]]$BatchNames,
|
|
||||||
[Parameter(Mandatory = $true)][string[]]$Pairs,
|
|
||||||
[Parameter(Mandatory = $true)][string[]]$GroundPlanes,
|
|
||||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
|
||||||
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
|
||||||
[int]$Bootstrap = 200,
|
|
||||||
[double]$MaxBatchTranslationDifferenceM = 0.25,
|
|
||||||
[double]$MaxBatchRotationDifferenceDeg = 5.0
|
|
||||||
)
|
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
|
||||||
$Repo = Split-Path -Parent $PSScriptRoot
|
|
||||||
|
|
||||||
if ($BatchNames.Count -lt 2) {
|
|
||||||
throw "At least two independent batches are required"
|
|
||||||
}
|
|
||||||
if (($Pairs.Count -ne $BatchNames.Count) -or ($GroundPlanes.Count -ne $BatchNames.Count)) {
|
|
||||||
throw "BatchNames, Pairs, and GroundPlanes must have the same number of entries"
|
|
||||||
}
|
|
||||||
if ($RtkReferenceHeightAboveGroundM -le 0.0) {
|
|
||||||
throw "RtkReferenceHeightAboveGroundM must be greater than zero"
|
|
||||||
}
|
|
||||||
foreach ($Path in @($Pairs + $GroundPlanes)) {
|
|
||||||
if (-not (Test-Path -LiteralPath $Path)) {
|
|
||||||
throw "Input does not exist: $Path"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
$PreflightRoot = Join-Path $OutputRoot "preflight"
|
|
||||||
New-Item -ItemType Directory -Force -Path $PreflightRoot | Out-Null
|
|
||||||
$BatchExtrinsics = @()
|
|
||||||
for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
|
|
||||||
$SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
|
|
||||||
$BatchExtrinsic = Join-Path $PreflightRoot "$SafeName.json"
|
|
||||||
Write-Host "[preflight batch: $($BatchNames[$Index])]"
|
|
||||||
& python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
|
|
||||||
--pairs $Pairs[$Index] `
|
|
||||||
--ground-planes $GroundPlanes[$Index] `
|
|
||||||
--reference-height $RtkReferenceHeightAboveGroundM `
|
|
||||||
--bootstrap 0 `
|
|
||||||
--output $BatchExtrinsic | Out-Null
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Batch preflight failed: $($BatchNames[$Index])" }
|
|
||||||
$BatchExtrinsics += $BatchExtrinsic
|
|
||||||
}
|
|
||||||
|
|
||||||
for ($Index = 1; $Index -lt $BatchNames.Count; $Index++) {
|
|
||||||
$SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
|
|
||||||
$ComparisonPath = Join-Path $PreflightRoot "$SafeName-vs-batch0.json"
|
|
||||||
& python (Join-Path $Repo "code\compare_extrinsics.py") `
|
|
||||||
--reference $BatchExtrinsics[0] `
|
|
||||||
--candidate $BatchExtrinsics[$Index] `
|
|
||||||
--output $ComparisonPath | Out-Null
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Batch comparison failed: $($BatchNames[$Index])" }
|
|
||||||
|
|
||||||
$Comparison = Get-Content -LiteralPath $ComparisonPath -Raw | ConvertFrom-Json
|
|
||||||
$TranslationDifference = [double]$Comparison.relative_translation_norm_m
|
|
||||||
$RotationDifference = [double]$Comparison.relative_rotation_deg
|
|
||||||
Write-Host ("[preflight consistency] {0} vs {1}: {2:F4} m / {3:F3} deg" -f `
|
|
||||||
$BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference)
|
|
||||||
if (($TranslationDifference -gt $MaxBatchTranslationDifferenceM) -or
|
|
||||||
($RotationDifference -gt $MaxBatchRotationDifferenceDeg)) {
|
|
||||||
throw ("Batch extrinsics are inconsistent: {0} vs {1} = {2:F4} m / {3:F3} deg; " +
|
|
||||||
"check RTK heading/frame convention and sensor installation") -f `
|
|
||||||
$BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
$InputRoot = Join-Path $OutputRoot "inputs"
|
|
||||||
$JointPairs = Join-Path $InputRoot "joint_consensus_pairs.npz"
|
|
||||||
$JointGroundPlanes = Join-Path $InputRoot "joint_ground_planes.csv"
|
|
||||||
$InputSummary = Join-Path $InputRoot "joint_input_summary.json"
|
|
||||||
$Extrinsic = Join-Path $OutputRoot "shared_T_RTK_lidar.json"
|
|
||||||
|
|
||||||
$BuildArgs = @((Join-Path $Repo "code\build_joint_rtk_lidar_inputs.py"))
|
|
||||||
for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
|
|
||||||
$BuildArgs += @(
|
|
||||||
"--batch-name", $BatchNames[$Index],
|
|
||||||
"--pairs", $Pairs[$Index],
|
|
||||||
"--ground-planes", $GroundPlanes[$Index]
|
|
||||||
)
|
|
||||||
}
|
|
||||||
$BuildArgs += @(
|
|
||||||
"--output-pairs", $JointPairs,
|
|
||||||
"--output-ground-planes", $JointGroundPlanes,
|
|
||||||
"--summary", $InputSummary
|
|
||||||
)
|
|
||||||
|
|
||||||
Write-Host "[combine independent batches]"
|
|
||||||
& python @BuildArgs
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Combining independent batches failed" }
|
|
||||||
|
|
||||||
Write-Host "[solve shared T_RTK_lidar]"
|
|
||||||
& python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
|
|
||||||
--pairs $JointPairs `
|
|
||||||
--ground-planes $JointGroundPlanes `
|
|
||||||
--reference-height $RtkReferenceHeightAboveGroundM `
|
|
||||||
--bootstrap $Bootstrap `
|
|
||||||
--output $Extrinsic
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Shared RTK-LiDAR calibration failed" }
|
|
||||||
|
|
||||||
Write-Host "Shared T_RTK_lidar: $Extrinsic"
|
|
||||||
Write-Host "Joint input summary: $InputSummary"
|
|
||||||
@@ -1,70 +0,0 @@
|
|||||||
param(
|
|
||||||
[Parameter(Mandatory = $true)][string]$Prepared,
|
|
||||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
|
||||||
[Parameter(Mandatory = $true)][double]$ReferenceHeight,
|
|
||||||
[string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
|
|
||||||
[int]$MinPairs = 20,
|
|
||||||
[int]$Bootstrap = 100
|
|
||||||
)
|
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
|
||||||
$Repo = Split-Path -Parent $PSScriptRoot
|
|
||||||
$Code = Join-Path $Repo "code\rigorous_calibration.py"
|
|
||||||
$Refine = Join-Path $Repo "code\refine_pairs.py"
|
|
||||||
$Consensus = Join-Path $Repo "code\cross_backend_filter.py"
|
|
||||||
$Frames = Join-Path $Prepared "frames_all"
|
|
||||||
$ReferencePoses = Join-Path $Prepared $ReferencePoseFile
|
|
||||||
$Common = Join-Path $OutputRoot "common"
|
|
||||||
$Open = Join-Path $OutputRoot "open3d_gicp"
|
|
||||||
$Small = Join-Path $OutputRoot "small_gicp"
|
|
||||||
$ConsensusOut = Join-Path $OutputRoot "consensus"
|
|
||||||
|
|
||||||
function Run-Python {
|
|
||||||
param([string]$Stage, [string[]]$Arguments)
|
|
||||||
Write-Host "[$Stage]"
|
|
||||||
& python @Arguments
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "$Stage failed with Python exit code $LASTEXITCODE" }
|
|
||||||
}
|
|
||||||
|
|
||||||
foreach ($Path in @($Frames, $ReferencePoses)) {
|
|
||||||
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
|
|
||||||
}
|
|
||||||
New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null
|
|
||||||
|
|
||||||
$Ground = Join-Path $Common "ground_planes.csv"
|
|
||||||
Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground)
|
|
||||||
|
|
||||||
foreach ($Backend in @("small_gicp", "open3d")) {
|
|
||||||
$Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
|
|
||||||
$Raw = Join-Path $Directory "B_estimation.npz"
|
|
||||||
$QualityJson = Join-Path $Directory "B_quality.json"
|
|
||||||
$QualityCsv = Join-Path $Directory "B_quality.csv"
|
|
||||||
$PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--reference-poses", $ReferencePoses,
|
|
||||||
"--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv,
|
|
||||||
"--min-pairs", "$MinPairs")
|
|
||||||
if ($Backend -eq "open3d") { $PairArgs += @("--max-gap", "3", "--multistart", "1", "--iterations", "40") }
|
|
||||||
Run-Python "$Backend pairs" $PairArgs
|
|
||||||
Run-Python "$Backend X-independent refinement" @(
|
|
||||||
$Refine, "--pairs", $Raw, "--quality-json", $QualityJson,
|
|
||||||
"--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs"
|
|
||||||
)
|
|
||||||
Run-Python "$Backend calibration" @(
|
|
||||||
$Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
|
|
||||||
"--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
|
|
||||||
"--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
|
|
||||||
)
|
|
||||||
}
|
|
||||||
|
|
||||||
$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
|
|
||||||
Run-Python "cross-backend consensus" @(
|
|
||||||
$Consensus, "--open3d-pairs", (Join-Path $Open "B_refined.npz"),
|
|
||||||
"--small-pairs", (Join-Path $Small "B_refined.npz"),
|
|
||||||
"--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
|
|
||||||
)
|
|
||||||
Run-Python "consensus calibration" @(
|
|
||||||
$Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
|
|
||||||
"--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
|
|
||||||
"--output", (Join-Path $ConsensusOut "extrinsic.json")
|
|
||||||
)
|
|
||||||
|
|
||||||
Write-Host "Calibration results: $OutputRoot"
|
|
||||||
@@ -1,19 +0,0 @@
|
|||||||
param(
|
|
||||||
[Parameter(Mandatory = $true)][string]$Frames,
|
|
||||||
[Parameter(Mandatory = $true)][string]$Pairs,
|
|
||||||
[Parameter(Mandatory = $true)][string]$Extrinsic,
|
|
||||||
[int]$PairIndex = 0,
|
|
||||||
[double]$LeftRollDeg = 0.0,
|
|
||||||
[double]$LeftPitchDeg = 0.0,
|
|
||||||
[double]$LeftYawDeg = 0.0
|
|
||||||
)
|
|
||||||
|
|
||||||
$ErrorActionPreference = "Stop"
|
|
||||||
$Repo = Split-Path -Parent $PSScriptRoot
|
|
||||||
foreach ($Path in @($Frames, $Pairs, $Extrinsic)) {
|
|
||||||
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
|
|
||||||
}
|
|
||||||
& python (Join-Path $Repo "code\visualize_pair_3d.py") `
|
|
||||||
--frames $Frames --pairs $Pairs --extrinsic $Extrinsic --pair-index $PairIndex `
|
|
||||||
--left-rpy-deg $LeftRollDeg $LeftPitchDeg $LeftYawDeg
|
|
||||||
if ($LASTEXITCODE -ne 0) { throw "Visualization failed with Python exit code $LASTEXITCODE" }
|
|
||||||
+131
@@ -0,0 +1,131 @@
|
|||||||
|
# 测试说明
|
||||||
|
|
||||||
|
**用途：** 说明合成 pytest 与旧车 S2 线下试验的边界（避免误读）。日常跑通请先看根目录 [README](../README.md)。
|
||||||
|
|
||||||
|
| 试验 | 是否默认 pytest | 在证明什么 | 不在证明什么 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| **合成数据** | 是 | 算法链路正确、能收回已知 yaw / δt | 实车安装精度、平移可交付 |
|
||||||
|
| **旧车 S2** | 否（线下手工） | 主机时间旧数据上流水线能跑完；质量门会拒绝坏结果 | 外参真值；新车可用性 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 自动化测试（合成数据 / `pytest`）
|
||||||
|
|
||||||
|
入口：`tests/test_v1_pipeline.py`
|
||||||
|
命令：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
cd <仓库根目录>
|
||||||
|
python -m pytest -q
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
| 测试 | 输入 | 在测什么 | 期望结果 |
|
||||||
|
| ------------------------------------------------------------- | ------------ | ------------------- | ------------------------------------------ |
|
||||||
|
| `test_rotation_handeye_recovers_yaw` | 合成运动对（无点云） | 旋转手眼能否收回已知 yaw | 旋转误差 < 1° |
|
||||||
|
| `test_time_offset_on_synthetic` | 合成会话（故意加 δt） | 模长相关粗估时间偏置 | |δt 误差| < 0.05 s |
|
||||||
|
| `test_signed_time_offset_refine_improves_or_keeps` | 同上 + 真值 R | 有符号三轴 δt 精修 | 不比粗估明显更差 |
|
||||||
|
| `test_preintegration_bias_jacobian_matches_finite_difference` | 随机陀螺序列 | 旋转预积分 `J_bg` | 与有限差分一致（松阈值） |
|
||||||
|
| `test_imu_preintegration_recovers_constant_accel_translation` | 常值加速度 | 完整预积分 Δv/Δp | 接近解析值 |
|
||||||
|
| `test_imu_preintegration_bias_jacobian_finite_difference` | 随机 IMU | `J_bg`/`J_ba` 一阶修正 | 与重积分接近 |
|
||||||
|
| `test_synthetic_pipeline_rejects_noisy_icp_but_keeps_time_audit` | synthetic end-to-end | strict rotation quality gate + time audit | noisy ICP is blocked; delta-t remains accurate |
|
||||||
|
| `test_synthetic_pipeline_full_se3_smoke` | synthetic end-to-end | full-SE(3) smoke test | returns an explicit accepted/rejected/blocked status |
|
||||||
|
| `test_planar_yaw_is_not_full_rotation_or_translation_observable` | pure-yaw motion pairs | degeneracy detection | full rotation/translation observability is rejected |
|
||||||
|
| `test_multi_axis_motion_is_rotation_and_translation_observable` | multi-axis motion pairs | positive observability case | rotation and translation pass |
|
||||||
|
| `test_translation_prior_is_reported_but_not_accepted_when_unobservable` | planar motion + CAD prior | prior semantics | prior is reported but not accepted as calibration |
|
||||||
|
| `test_handeye_rejects_a_small_fraction_of_gross_rotation_outliers` | motion pairs with a gross outlier | residual-distribution gate | solve is rejected |
|
||||||
|
| `test_motion_pairs_reject_low_fitness` | low-fitness registration | fitness gate | no motion pair is emitted |
|
||||||
|
| `test_motion_pairs_reject_imu_and_lidar_discontinuities` | timestamp gaps | continuity gates | cross-gap pairs are rejected |
|
||||||
|
|
||||||
|
|
||||||
|
合成数据由 `tools/generate_synthetic_session.py` 生成（墙面点云 + 已知外参 yaw 与 δt）。
|
||||||
|
一键复现见根目录 README：`tools/reproduce_synthetic.py`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2. 旧车 S2 线下试验（不在默认 pytest 里）
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 用了什么数据
|
||||||
|
|
||||||
|
|
||||||
|
| 项 | 内容 |
|
||||||
|
| ------- | -------------------------------------------------------------------------- |
|
||||||
|
| 车辆 / 批次 | 旧 **S2** 验证集（`S2_scheme1_validation`）数据在网盘的“室外车数据\IMU-雷达标定数据” |
|
||||||
|
| 典型路径 | `D:\IMU_calibration\work\S2_scheme1_validation\`（历史目录名；含 `imu.csv` + 雷达会话） |
|
||||||
|
| IMU 时间 | **主机 UTC 接收时间**（串口块到达时刻），不是 IMU 设备时间 |
|
||||||
|
| 雷达时间 | dlog 导出的主机侧 `unix_time_ns`，不是 MSOP 设备时间 |
|
||||||
|
| 帧率特征 | 雷达约 **1 Hz** 量级，关键帧间隔偏长 |
|
||||||
|
| 配置烟测 | `config/s2_old_smoke.yaml`（仅声明为旧数据烟测，不当交付） |
|
||||||
|
|
||||||
|
|
||||||
|
这些数据**只能用来验证流水线能否跑通**，不能当作新车外参真值来源。
|
||||||
|
`blocked` 是质量门的**预期结果**，不是「算法突然坏了」。
|
||||||
|
|
||||||
|
### 做了什么测试
|
||||||
|
|
||||||
|
对同一批 S2 中间格式多次跑 `cli run`，例如：
|
||||||
|
|
||||||
|
- 预积分加强后的输出目录（本机历史名如 `out_scheme2_preint`）
|
||||||
|
- 有符号 δt / 联合精修后的输出（本机历史名如 `out_scheme2_phaseA`）
|
||||||
|
- 用 `tools/compare_s2_runs.py` 对比两次 `summary.json`
|
||||||
|
|
||||||
|
命令形态（路径按本机实际修改；雷达会话目录若仍叫 `scheme2_session` 为历史命名）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m imu_lidar.cli run `
|
||||||
|
--vehicle-config config\s2_old_smoke.yaml `
|
||||||
|
--imu D:\IMU_calibration\work\S2_scheme1_validation\imu.csv `
|
||||||
|
--lidar D:\IMU_calibration\work\S2_scheme1_validation\scheme2_session `
|
||||||
|
--output path\to\out_s2 `
|
||||||
|
--mode rotation_only `
|
||||||
|
--time-offset-search-s 2.0
|
||||||
|
|
||||||
|
python tools\compare_s2_runs.py path\to\out_old\summary.json path\to\out_new\summary.json
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 得到什么结果（记录摘要）
|
||||||
|
|
||||||
|
|
||||||
|
| 指标 | 预积分加强一轮 | 有符号 δt / 精修一轮 |
|
||||||
|
| -------- | ------------- | ------------------ |
|
||||||
|
| `status` | `blocked`（预期） | `blocked`（预期） |
|
||||||
|
| 手眼 RMS | 约 **15.0°** | 约 **14.5°** |
|
||||||
|
| 手眼中位数 | — | 约 **6.8°** |
|
||||||
|
| δt | 约 **−2.0 s** | 约 **−1.75 s**（有修正） |
|
||||||
|
| 相关峰 | 很弱（约 0.18） | 仍弱（约 0.13） |
|
||||||
|
| 结论 | 链路可跑 | 残差略降，但 **不当交付外参** |
|
||||||
|
|
||||||
|
|
||||||
|
原因归纳：
|
||||||
|
|
||||||
|
1. 时间戳是**主机时间**，相关峰弱，δt / yaw / 零偏互相耦合；
|
||||||
|
2. 雷达约 1 Hz，运动对间隔长，IMU 侧更易漂；
|
||||||
|
3. 质量门主动 `blocked`，避免把坏结果当成安装参数。
|
||||||
|
|
||||||
|
**正式标定**必须改用设备时间（IMU `device_timestamp`、雷达 MSOP 设备时）重新采集后再跑。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 3. 配准结果怎么目视检查
|
||||||
|
|
||||||
|
标定跑完后（合成或实车）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar examples\synthetic_session\lidar `
|
||||||
|
--imu examples\synthetic_session\imu.csv `
|
||||||
|
--summary examples\synthetic_session\out\summary.json `
|
||||||
|
--pair-index 0 `
|
||||||
|
--save-png examples\synthetic_session\out\pair0_overlay.png
|
||||||
|
```
|
||||||
|
|
||||||
|
交互窗口快捷键：`1`–`4` 切换叠点模式；`N`/`]` 下一运动对，`P`/`[` 上一运动对。
|
||||||
|
无显示器时加 `--no-gui --save-png ...` 只出俯视图 PNG。
|
||||||
@@ -0,0 +1,104 @@
|
|||||||
|
"""Unit tests for rscap → V1 export helpers (no large real captures)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import struct
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from tools.rscap_v2.h32_msop import (
|
||||||
|
CHANNELS,
|
||||||
|
PACKET_LENGTH,
|
||||||
|
decode_packet_points,
|
||||||
|
default_vertical_deg,
|
||||||
|
default_horizontal_deg,
|
||||||
|
device_timestamp_ms,
|
||||||
|
normalize_azimuth_deg,
|
||||||
|
)
|
||||||
|
from tools.rscap_v2.n300_imu import crc8_fdilink, crc16_fdilink, iter_n300_imu_samples
|
||||||
|
from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
|
||||||
|
|
||||||
|
|
||||||
|
def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
|
||||||
|
packet = bytearray(PACKET_LENGTH)
|
||||||
|
packet[17] = 1 # 2.5 mm unit
|
||||||
|
sec = seconds.to_bytes(6, "big")
|
||||||
|
packet[20:26] = sec
|
||||||
|
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):
|
||||||
|
# 4.0 m at 2.5 mm/unit => raw = 1600
|
||||||
|
packet[idx] = (1600 >> 8) & 0xFF
|
||||||
|
packet[idx + 1] = 1600 & 0xFF
|
||||||
|
packet[idx + 2] = 10
|
||||||
|
idx += 3
|
||||||
|
return bytes(packet)
|
||||||
|
|
||||||
|
|
||||||
|
def test_h32_device_timestamp_and_points():
|
||||||
|
packet = _make_msop_packet(seconds=1700000000, microseconds=123000)
|
||||||
|
assert device_timestamp_ms(packet) == 1700000000 * 1000 + 123
|
||||||
|
az_list, pts = decode_packet_points(
|
||||||
|
packet,
|
||||||
|
default_vertical_deg(),
|
||||||
|
default_horizontal_deg(),
|
||||||
|
min_range_m=0.1,
|
||||||
|
max_range_m=50.0,
|
||||||
|
)
|
||||||
|
assert len(az_list) == 12
|
||||||
|
assert pts.shape[0] == 12 * CHANNELS
|
||||||
|
assert np.allclose(np.linalg.norm(pts, axis=1), 4.0, atol=1e-3)
|
||||||
|
|
||||||
|
|
||||||
|
def test_normalize_azimuth():
|
||||||
|
assert abs(normalize_azimuth_deg(190.0) + 170.0) < 1e-9
|
||||||
|
|
||||||
|
|
||||||
|
def _n300_imu_frame(device_us: int = 123456) -> bytes:
|
||||||
|
payload = bytearray(56)
|
||||||
|
struct.pack_into("<3f", payload, 0, 0.1, -0.2, 0.3)
|
||||||
|
struct.pack_into("<3f", payload, 12, 0.0, 0.0, 9.81)
|
||||||
|
struct.pack_into("<q", payload, 48, device_us)
|
||||||
|
header = bytearray([0xFC, 0x40, 56, 7])
|
||||||
|
header.append(crc8_fdilink(header))
|
||||||
|
crc = crc16_fdilink(payload)
|
||||||
|
frame = bytes(header) + crc.to_bytes(2, "big") + bytes(payload) + b"\xFD"
|
||||||
|
return frame
|
||||||
|
|
||||||
|
|
||||||
|
def test_n300_imu_sample_from_capture_chunks():
|
||||||
|
frame = _n300_imu_frame(654321)
|
||||||
|
header = CaptureHeader(
|
||||||
|
sensor_kind="wheeltec-n300",
|
||||||
|
session_id="test",
|
||||||
|
session_start_utc_ticks=0,
|
||||||
|
session_start_monotonic_ticks=0,
|
||||||
|
monotonic_frequency=10_000_000,
|
||||||
|
port="COM1",
|
||||||
|
baud=921600,
|
||||||
|
file_start_utc_ticks=0,
|
||||||
|
)
|
||||||
|
chunk = RawChunk(
|
||||||
|
sequence=1,
|
||||||
|
receive_utc_ticks=100,
|
||||||
|
receive_monotonic_ticks=1,
|
||||||
|
raw=frame,
|
||||||
|
record_file_offset=0,
|
||||||
|
raw_file_offset=0,
|
||||||
|
record_crc32=0,
|
||||||
|
crc_valid=True,
|
||||||
|
)
|
||||||
|
capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
|
||||||
|
samples = iter_n300_imu_samples(capture)
|
||||||
|
assert len(samples) == 1
|
||||||
|
assert samples[0].device_timestamp_us == 654321
|
||||||
|
assert abs(samples[0].t_s - 654321e-6) < 1e-12
|
||||||
|
assert abs(samples[0].gyro_rad_s[0] - 0.1) < 1e-6
|
||||||
|
assert abs(samples[0].accel_m_s2[2] - 9.81) < 1e-5
|
||||||
@@ -0,0 +1,212 @@
|
|||||||
|
"""Unit tests for H32 Medulla dlog → V1 export helpers."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import struct
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from tools.h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles
|
||||||
|
from tools.h32_dlog.dobject import RECORD_RE, discover_records, iter_payloads, resolve_dlog_root
|
||||||
|
from tools.h32_dlog.timeutil import local_wall_to_dotnet_ticks
|
||||||
|
from tools.h32_dlog.load_session import load_h32_dlog_lidar
|
||||||
|
from tools.h32_dlog.payload_v1 import (
|
||||||
|
MsopPacketItem,
|
||||||
|
build_difop_payload,
|
||||||
|
build_msop_batch_payload,
|
||||||
|
parse_difop_payload,
|
||||||
|
parse_msop_batch_payload,
|
||||||
|
)
|
||||||
|
from tools.rscap_v2.h32_msop import PACKET_LENGTH, iter_h32_frames_from_packets
|
||||||
|
|
||||||
|
|
||||||
|
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,
|
||||||
|
offset: int = 0,
|
||||||
|
) -> int:
|
||||||
|
"""Append one DObject record; return file offset of the record start."""
|
||||||
|
|
||||||
|
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:
|
||||||
|
if offset:
|
||||||
|
handle.seek(offset)
|
||||||
|
start = handle.tell()
|
||||||
|
handle.write(blob)
|
||||||
|
return start
|
||||||
|
|
||||||
|
|
||||||
|
def test_recovered_index_line_and_local_ticks():
|
||||||
|
line = (
|
||||||
|
">DObject `frontlidar-msop-raw` post len=15532B, id:9CF1, "
|
||||||
|
"tic:639218060782100466, @data.bin:0"
|
||||||
|
)
|
||||||
|
match = RECORD_RE.search(line)
|
||||||
|
assert match is not None
|
||||||
|
assert match.group("name") == "frontlidar-msop-raw"
|
||||||
|
assert match.group("file") == "data.bin"
|
||||||
|
assert int(match.group("offset")) == 0
|
||||||
|
assert local_wall_to_dotnet_ticks("2026-08-08T17:14:38") == 639218060780000000
|
||||||
|
|
||||||
|
|
||||||
|
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].sequence == 7
|
||||||
|
assert batch.packets[0].device_timestamp_us == item.device_timestamp_us
|
||||||
|
assert batch.packets[0].device_timestamp_valid is True
|
||||||
|
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_payload = build_difop_payload(raw=difop_raw, sequence=3)
|
||||||
|
difop = parse_difop_payload(difop_payload)
|
||||||
|
assert difop.sequence == 3
|
||||||
|
angles = parse_difop_angles(difop.raw)
|
||||||
|
assert angles.vertical_deg.shape == (CHANNELS,)
|
||||||
|
assert np.allclose(angles.vertical_deg, vertical, atol=1e-2)
|
||||||
|
assert np.allclose(angles.horizontal_deg, 0.05, atol=1e-2)
|
||||||
|
|
||||||
|
|
||||||
|
def test_difop_signed_angle_negative():
|
||||||
|
packet = _make_difop_packet(vertical=[-5.25] + [0.0] * 31)
|
||||||
|
angles = parse_difop_angles(packet)
|
||||||
|
assert abs(angles.vertical_deg[0] + 5.25) < 1e-9
|
||||||
|
|
||||||
|
|
||||||
|
def test_load_h32_dlog_lidar_mini_session(tmp_path: Path):
|
||||||
|
dlog = tmp_path / "session" / "dlog"
|
||||||
|
dorec_name = "raw.dorec"
|
||||||
|
dorec_path = dlog / "dobject_recording" / dorec_name
|
||||||
|
log_path = dlog / "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=1,
|
||||||
|
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",
|
||||||
|
)
|
||||||
|
|
||||||
|
root = resolve_dlog_root(tmp_path / "session")
|
||||||
|
assert root == dlog
|
||||||
|
assert len(discover_records(root, "frontlidar-msop-raw")) == 1
|
||||||
|
payloads = list(iter_payloads(root, "frontlidar-msop-raw"))
|
||||||
|
assert len(payloads) == 1
|
||||||
|
|
||||||
|
session = load_h32_dlog_lidar(tmp_path / "session", require_difop=True)
|
||||||
|
assert session.angle_source == "difop_channel_angles"
|
||||||
|
assert len(session.msop_packets) == 1
|
||||||
|
assert np.allclose(session.vertical_deg, vertical, atol=1e-2)
|
||||||
|
|
||||||
|
frames = iter_h32_frames_from_packets(
|
||||||
|
session.msop_packets,
|
||||||
|
min_frame_points=1,
|
||||||
|
vertical_deg=session.vertical_deg,
|
||||||
|
horizontal_deg=session.horizontal_deg,
|
||||||
|
)
|
||||||
|
assert len(frames) == 1
|
||||||
|
assert frames[0].points_xyz.shape[0] > 0
|
||||||
@@ -0,0 +1,79 @@
|
|||||||
|
"""Unit tests for HI13 / HI91 IMU decoding."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import struct
|
||||||
|
|
||||||
|
from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
|
||||||
|
from tools.rscap_v2.hi13_imu import crc16_hi13, iter_hi13_imu_samples, parse_hi91_frame
|
||||||
|
|
||||||
|
|
||||||
|
def _hi91_frame(
|
||||||
|
*,
|
||||||
|
device_ms: int = 123456,
|
||||||
|
accel_g=(0.0, 0.0, 1.0),
|
||||||
|
gyro_dps=(1.0, -2.0, 3.0),
|
||||||
|
) -> bytes:
|
||||||
|
payload = bytearray(76)
|
||||||
|
payload[0] = 0x91
|
||||||
|
struct.pack_into("<H", payload, 1, 0) # pps
|
||||||
|
payload[3] = 25 # temp
|
||||||
|
struct.pack_into("<f", payload, 4, 101325.0)
|
||||||
|
struct.pack_into("<I", payload, 8, device_ms)
|
||||||
|
struct.pack_into("<fff", payload, 12, *accel_g)
|
||||||
|
struct.pack_into("<fff", payload, 24, *gyro_dps)
|
||||||
|
# remaining mag/rpy/quat left zero
|
||||||
|
payload_length = len(payload)
|
||||||
|
header = bytearray(6)
|
||||||
|
header[0] = 0x5A
|
||||||
|
header[1] = 0xA5
|
||||||
|
header[2] = payload_length & 0xFF
|
||||||
|
header[3] = (payload_length >> 8) & 0xFF
|
||||||
|
frame_wo_crc = bytes(header[:4]) + bytes(payload)
|
||||||
|
# crc over header[0:4] + payload
|
||||||
|
tmp = bytearray(6 + payload_length)
|
||||||
|
tmp[0:4] = header[0:4]
|
||||||
|
tmp[6:] = payload
|
||||||
|
crc = crc16_hi13(tmp, payload_length)
|
||||||
|
header[4] = crc & 0xFF
|
||||||
|
header[5] = (crc >> 8) & 0xFF
|
||||||
|
return bytes(header) + bytes(payload)
|
||||||
|
|
||||||
|
|
||||||
|
def test_parse_hi91_units():
|
||||||
|
frame = _hi91_frame(device_ms=5000, accel_g=(0.0, 0.0, 1.0), gyro_dps=(57.2957795, 0.0, 0.0))
|
||||||
|
parsed = parse_hi91_frame(frame)
|
||||||
|
assert parsed is not None
|
||||||
|
gyro, accel, device_ms = parsed
|
||||||
|
assert device_ms == 5000
|
||||||
|
assert abs(accel[2] - 9.80665) < 1e-4
|
||||||
|
assert abs(gyro[0] - 1.0) < 1e-5
|
||||||
|
|
||||||
|
|
||||||
|
def test_iter_hi13_from_capture():
|
||||||
|
frame = _hi91_frame(device_ms=42)
|
||||||
|
header = CaptureHeader(
|
||||||
|
sensor_kind="hi13r4-imu",
|
||||||
|
session_id="t",
|
||||||
|
session_start_utc_ticks=0,
|
||||||
|
session_start_monotonic_ticks=0,
|
||||||
|
monotonic_frequency=10_000_000,
|
||||||
|
port="COM1",
|
||||||
|
baud=115200,
|
||||||
|
file_start_utc_ticks=0,
|
||||||
|
)
|
||||||
|
chunk = RawChunk(
|
||||||
|
sequence=1,
|
||||||
|
receive_utc_ticks=100,
|
||||||
|
receive_monotonic_ticks=1,
|
||||||
|
raw=frame,
|
||||||
|
record_file_offset=0,
|
||||||
|
raw_file_offset=0,
|
||||||
|
record_crc32=0,
|
||||||
|
crc_valid=True,
|
||||||
|
)
|
||||||
|
capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
|
||||||
|
samples = iter_hi13_imu_samples(capture)
|
||||||
|
assert len(samples) == 1
|
||||||
|
assert samples[0].device_timestamp_us == 42_000
|
||||||
|
assert abs(samples[0].t_s - 0.042) < 1e-12
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
"""Tests for motion-pair cache IO."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.contracts import MotionPair
|
||||||
|
from imu_lidar.motion_pairs_io import (
|
||||||
|
build_motion_pairs_payload,
|
||||||
|
load_motion_pairs,
|
||||||
|
pair_from_dict,
|
||||||
|
pair_to_dict,
|
||||||
|
pairs_for_session,
|
||||||
|
save_motion_pairs,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_pair_roundtrip(tmp_path: Path) -> None:
|
||||||
|
pair = MotionPair(
|
||||||
|
session_id="s0",
|
||||||
|
i=1,
|
||||||
|
j=4,
|
||||||
|
t_i_s=1.0,
|
||||||
|
t_j_s=2.5,
|
||||||
|
R_A=np.eye(3),
|
||||||
|
R_B=np.eye(3),
|
||||||
|
t_A_m=np.array([0.1, 0.0, 0.0]),
|
||||||
|
t_B_m=np.array([0.1, 0.0, 0.0]),
|
||||||
|
fitness=0.8,
|
||||||
|
metadata={
|
||||||
|
"weight": 12.0,
|
||||||
|
"cov": (np.eye(3) * 1e-4).tolist(),
|
||||||
|
"J_bg": (-np.eye(3)).tolist(),
|
||||||
|
"cov9": [[0.0] * 9] * 9,
|
||||||
|
"backend": "test",
|
||||||
|
"gyro_bias0_rad_s": [0.01, -0.02, 0.03],
|
||||||
|
"accel_bias0_m_s2": [0.1, 0.2, -0.1],
|
||||||
|
"time_offset_s": 0.004,
|
||||||
|
"keyframe_span": 3,
|
||||||
|
"is_consecutive": False,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
encoded = pair_to_dict(pair)
|
||||||
|
assert "cov9" not in encoded["metadata"]
|
||||||
|
assert "cov" in encoded["metadata"]
|
||||||
|
assert "J_bg" in encoded["metadata"]
|
||||||
|
assert encoded["metadata"]["weight"] == 12.0
|
||||||
|
restored = pair_from_dict(encoded)
|
||||||
|
assert restored.i == 1 and restored.j == 4
|
||||||
|
np.testing.assert_allclose(restored.t_A_m, [0.1, 0.0, 0.0])
|
||||||
|
np.testing.assert_allclose(restored.metadata["gyro_bias0_rad_s"], [0.01, -0.02, 0.03])
|
||||||
|
assert restored.metadata["keyframe_span"] == 3
|
||||||
|
|
||||||
|
payload = build_motion_pairs_payload(
|
||||||
|
prepared_sessions=[
|
||||||
|
{
|
||||||
|
"session_id": "s0",
|
||||||
|
"time_offset_s": 0.0,
|
||||||
|
"gyro_bias_rad_s": np.zeros(3),
|
||||||
|
"pairs": (pair,),
|
||||||
|
}
|
||||||
|
]
|
||||||
|
)
|
||||||
|
path = save_motion_pairs(tmp_path / "motion_pairs.json", payload)
|
||||||
|
loaded = load_motion_pairs(path)
|
||||||
|
assert loaded["schema_version"] == 2
|
||||||
|
pairs = pairs_for_session(loaded, "s0")
|
||||||
|
assert len(pairs) == 1
|
||||||
|
assert pairs[0].session_id == "s0"
|
||||||
|
|
||||||
|
payload["schema_version"] = 1
|
||||||
|
legacy_path = save_motion_pairs(
|
||||||
|
tmp_path / "motion_pairs_v1.json", payload
|
||||||
|
)
|
||||||
|
legacy = load_motion_pairs(legacy_path)
|
||||||
|
assert legacy["schema_version"] == 1
|
||||||
@@ -0,0 +1,194 @@
|
|||||||
|
"""Tests for cached, session-balanced Phase-A comparison."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.contracts import ImuSeries, MotionPair
|
||||||
|
from imu_lidar.geometry import so3_exp, so3_log
|
||||||
|
from imu_lidar.imu_preintegration import preintegrate_gyro
|
||||||
|
from imu_lidar.phase_a import (
|
||||||
|
rehydrate_phase_a_pairs,
|
||||||
|
select_decorrelated_phase_a_pairs,
|
||||||
|
solve_phase_a_comparison,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _phase_a_pair(
|
||||||
|
session_id: str,
|
||||||
|
index: int,
|
||||||
|
r_true: np.ndarray,
|
||||||
|
vector_deg: tuple[float, float, float],
|
||||||
|
bias0: np.ndarray,
|
||||||
|
) -> MotionPair:
|
||||||
|
r_b = so3_exp(np.deg2rad(np.asarray(vector_deg, dtype=float)))
|
||||||
|
return MotionPair(
|
||||||
|
session_id=session_id,
|
||||||
|
i=index,
|
||||||
|
j=index + 1,
|
||||||
|
t_i_s=float(index),
|
||||||
|
t_j_s=float(index + 1),
|
||||||
|
R_A=r_true @ r_b @ r_true.T,
|
||||||
|
R_B=r_b,
|
||||||
|
t_A_m=np.zeros(3),
|
||||||
|
t_B_m=np.zeros(3),
|
||||||
|
metadata={
|
||||||
|
"J_bg": (-np.eye(3)).tolist(),
|
||||||
|
"cov": (np.eye(3) * 1e-5).tolist(),
|
||||||
|
"gyro_bias0_rad_s": bias0.tolist(),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_phase_a_reports_three_variants_and_leave_one_session_out() -> None:
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([3.0, -2.0, 25.0])))
|
||||||
|
prior = so3_exp(np.deg2rad(np.array([0.0, 0.0, 0.2]))) @ r_true
|
||||||
|
vectors = (
|
||||||
|
(12.0, 0.0, 0.0),
|
||||||
|
(0.0, 15.0, 0.0),
|
||||||
|
(0.0, 0.0, 18.0),
|
||||||
|
(10.0, 8.0, 0.0),
|
||||||
|
(0.0, 11.0, 9.0),
|
||||||
|
(7.0, 0.0, 13.0),
|
||||||
|
(9.0, -5.0, 6.0),
|
||||||
|
(-6.0, 8.0, 11.0),
|
||||||
|
(5.0, 7.0, -9.0),
|
||||||
|
)
|
||||||
|
biases = {
|
||||||
|
"s0": np.array([0.001, -0.0005, 0.0002]),
|
||||||
|
"s1": np.array([-0.0004, 0.0008, -0.0001]),
|
||||||
|
"s2": np.array([0.0002, 0.0001, -0.0006]),
|
||||||
|
}
|
||||||
|
pairs: list[MotionPair] = []
|
||||||
|
index = 0
|
||||||
|
for sid, count in (("s0", 18), ("s1", 9), ("s2", 6)):
|
||||||
|
for local_index in range(count):
|
||||||
|
pairs.append(
|
||||||
|
_phase_a_pair(
|
||||||
|
sid,
|
||||||
|
index,
|
||||||
|
r_true,
|
||||||
|
vectors[local_index % len(vectors)],
|
||||||
|
biases[sid],
|
||||||
|
)
|
||||||
|
)
|
||||||
|
index += 1
|
||||||
|
|
||||||
|
result = solve_phase_a_comparison(
|
||||||
|
pairs,
|
||||||
|
gyro_bias_rad_s_by_session=biases,
|
||||||
|
rotation_prior=prior,
|
||||||
|
rotation_prior_sigma_deg=15.0,
|
||||||
|
yaw_std_max_deg=1.0,
|
||||||
|
leave_one_out_yaw_range_max_deg=1.0,
|
||||||
|
data_prior_difference_max_deg=1.0,
|
||||||
|
decorrelation_block_s=0.0,
|
||||||
|
max_nfev=80,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert result.accepted
|
||||||
|
assert result.strong_pair_counts_per_session == {
|
||||||
|
"s0": 18,
|
||||||
|
"s1": 9,
|
||||||
|
"s2": 6,
|
||||||
|
}
|
||||||
|
assert len(result.leave_one_out) == 3
|
||||||
|
assert result.marginal_observability.rank == 3
|
||||||
|
assert result.leave_one_out_yaw_range_deg < 0.1
|
||||||
|
for variant in (
|
||||||
|
result.fixed_bg_data_only,
|
||||||
|
result.session_bg_data_only,
|
||||||
|
result.session_bg_with_rotation_prior,
|
||||||
|
):
|
||||||
|
error_deg = np.degrees(
|
||||||
|
np.linalg.norm(
|
||||||
|
so3_log(r_true.T @ variant.R_IMU_lidar)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
assert error_deg < 0.1
|
||||||
|
|
||||||
|
|
||||||
|
def test_rehydrate_phase_a_pairs_recovers_jacobian_without_lidar() -> None:
|
||||||
|
t_s = np.linspace(0.0, 1.0, 201)
|
||||||
|
gyro = np.tile(np.array([0.12, -0.04, 0.2]), (t_s.size, 1))
|
||||||
|
bias0 = np.array([0.01, -0.005, 0.002])
|
||||||
|
imu = ImuSeries(
|
||||||
|
t_s=t_s,
|
||||||
|
gyro_rad_s=gyro,
|
||||||
|
acc_m_s2=np.zeros((t_s.size, 3)),
|
||||||
|
)
|
||||||
|
preint = preintegrate_gyro(t_s, gyro, 0.1, 0.8, bias0)
|
||||||
|
pair = MotionPair(
|
||||||
|
session_id="s0",
|
||||||
|
i=0,
|
||||||
|
j=1,
|
||||||
|
t_i_s=0.1,
|
||||||
|
t_j_s=0.8,
|
||||||
|
R_A=preint.delta_R,
|
||||||
|
R_B=preint.delta_R,
|
||||||
|
metadata={
|
||||||
|
"t_i_imu_s": 0.1,
|
||||||
|
"t_j_imu_s": 0.8,
|
||||||
|
"gyro_bias0_rad_s": bias0.tolist(),
|
||||||
|
"preint_sigma_rad": preint.sigma_rad,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
enriched, report = rehydrate_phase_a_pairs(
|
||||||
|
[pair],
|
||||||
|
imu_by_session={"s0": imu},
|
||||||
|
bias0_by_session={"s0": bias0},
|
||||||
|
)
|
||||||
|
|
||||||
|
assert "J_bg" in enriched[0].metadata
|
||||||
|
assert "cov" in enriched[0].metadata
|
||||||
|
assert report["max_R_A_error_deg"] < 1e-8
|
||||||
|
|
||||||
|
|
||||||
|
def test_phase_a_time_blocks_do_not_count_overlapping_pairs_as_independent() -> None:
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([1.0, -2.0, 20.0])))
|
||||||
|
bias = np.zeros(3)
|
||||||
|
pairs = [
|
||||||
|
_phase_a_pair("s0", index, r_true, (5.0 + index, 2.0, 1.0), bias)
|
||||||
|
for index in range(9)
|
||||||
|
]
|
||||||
|
selected = select_decorrelated_phase_a_pairs(
|
||||||
|
pairs,
|
||||||
|
block_s=3.0,
|
||||||
|
max_pairs_per_block=1,
|
||||||
|
)
|
||||||
|
assert len(selected) == 3
|
||||||
|
assert all(pair in pairs for pair in selected)
|
||||||
|
|
||||||
|
|
||||||
|
def test_phase_a_planar_motion_is_partial_and_keeps_weak_direction_from_prior() -> None:
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([4.0, -3.0, 31.0])))
|
||||||
|
prior = so3_exp(np.deg2rad(np.array([0.2, -0.1, 0.4]))) @ r_true
|
||||||
|
biases = {"s0": np.zeros(3), "s1": np.zeros(3)}
|
||||||
|
pairs: list[MotionPair] = []
|
||||||
|
for session_index, sid in enumerate(biases):
|
||||||
|
for index in range(12):
|
||||||
|
pairs.append(
|
||||||
|
_phase_a_pair(
|
||||||
|
sid,
|
||||||
|
session_index * 100 + index,
|
||||||
|
r_true,
|
||||||
|
(0.0, 0.0, 8.0 + index),
|
||||||
|
biases[sid],
|
||||||
|
)
|
||||||
|
)
|
||||||
|
result = solve_phase_a_comparison(
|
||||||
|
pairs,
|
||||||
|
gyro_bias_rad_s_by_session=biases,
|
||||||
|
rotation_prior=prior,
|
||||||
|
decorrelation_block_s=0.0,
|
||||||
|
yaw_std_max_deg=0.5,
|
||||||
|
run_leave_one_out=False,
|
||||||
|
max_nfev=80,
|
||||||
|
)
|
||||||
|
assert not result.accepted
|
||||||
|
assert result.partial_accepted
|
||||||
|
assert result.solution_status == "phase_a_partial_accepted"
|
||||||
|
assert result.marginal_observability.precision_rank == 2
|
||||||
|
assert result.observable_subspace_with_prior is not None
|
||||||
|
assert np.isinf(result.marginal_observability.direction_std_deg[0])
|
||||||
@@ -0,0 +1,260 @@
|
|||||||
|
"""Regression tests for calibration quality, continuity, and observability gates."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.contracts import ImuSeries, LidarFrame, MotionPair
|
||||||
|
from imu_lidar.geometry import make_transform, so3_exp, so3_log
|
||||||
|
from imu_lidar.joint_optimizer import solve_joint_extrinsic
|
||||||
|
from imu_lidar.motion_pairs import build_motion_pairs
|
||||||
|
from imu_lidar.observability import analyze_observability
|
||||||
|
from imu_lidar.registration import RegistrationResult
|
||||||
|
from imu_lidar.rotation_handeye import solve_rotation_handeye
|
||||||
|
|
||||||
|
|
||||||
|
def _motion_pair(index: int, rotation_vector: np.ndarray) -> MotionPair:
|
||||||
|
rotation = so3_exp(np.asarray(rotation_vector, dtype=float))
|
||||||
|
return MotionPair(
|
||||||
|
session_id="synthetic",
|
||||||
|
i=index,
|
||||||
|
j=index + 1,
|
||||||
|
t_i_s=float(index),
|
||||||
|
t_j_s=float(index + 1),
|
||||||
|
R_A=rotation,
|
||||||
|
R_B=rotation,
|
||||||
|
t_A_m=np.zeros(3),
|
||||||
|
t_B_m=np.array([0.1, -0.03, 0.0]),
|
||||||
|
fitness=0.9,
|
||||||
|
metadata={
|
||||||
|
"J_bg": (-np.eye(3)).tolist(),
|
||||||
|
"cov": (np.eye(3) * 1e-4).tolist(),
|
||||||
|
"gyro_bias0_rad_s": [0.0, 0.0, 0.0],
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _frame(frame_id: str, mid_s: float) -> LidarFrame:
|
||||||
|
return LidarFrame(
|
||||||
|
frame_id=frame_id,
|
||||||
|
t_start_s=mid_s - 0.01,
|
||||||
|
t_end_s=mid_s + 0.01,
|
||||||
|
points_xyz=np.zeros((64, 3)),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _registration(*, fitness: float = 0.9) -> RegistrationResult:
|
||||||
|
rotation = so3_exp(np.deg2rad(np.array([0.0, 0.0, 10.0])))
|
||||||
|
return RegistrationResult(
|
||||||
|
transform=make_transform(np.array([0.4, 0.0, 0.0]), rotation),
|
||||||
|
fitness=fitness,
|
||||||
|
rotation_deg=10.0,
|
||||||
|
translation_m=0.4,
|
||||||
|
backend="test",
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_planar_yaw_is_not_full_rotation_or_translation_observable():
|
||||||
|
pairs = [
|
||||||
|
_motion_pair(i, np.deg2rad(np.array([0.0, 0.0, angle_deg])))
|
||||||
|
for i, angle_deg in enumerate((5.0, 8.0, 12.0, 17.0, 23.0, 31.0))
|
||||||
|
]
|
||||||
|
|
||||||
|
report = analyze_observability(pairs, np.eye(3))
|
||||||
|
|
||||||
|
assert not report.rotation_observable
|
||||||
|
assert not report.translation_observable
|
||||||
|
|
||||||
|
|
||||||
|
def test_multi_axis_motion_is_rotation_and_translation_observable():
|
||||||
|
vectors_deg = (
|
||||||
|
(12.0, 0.0, 0.0),
|
||||||
|
(0.0, 15.0, 0.0),
|
||||||
|
(0.0, 0.0, 18.0),
|
||||||
|
(10.0, 8.0, 0.0),
|
||||||
|
(0.0, 11.0, 9.0),
|
||||||
|
(7.0, 0.0, 13.0),
|
||||||
|
)
|
||||||
|
pairs = [
|
||||||
|
_motion_pair(i, np.deg2rad(np.asarray(vector_deg)))
|
||||||
|
for i, vector_deg in enumerate(vectors_deg)
|
||||||
|
]
|
||||||
|
|
||||||
|
report = analyze_observability(pairs, np.eye(3))
|
||||||
|
|
||||||
|
assert report.rotation_observable
|
||||||
|
assert report.translation_observable
|
||||||
|
|
||||||
|
|
||||||
|
def test_translation_prior_is_reported_but_not_accepted_when_unobservable():
|
||||||
|
pairs = [
|
||||||
|
_motion_pair(i, np.deg2rad(np.array([0.0, 0.0, angle_deg])))
|
||||||
|
for i, angle_deg in enumerate((5.0, 8.0, 12.0, 17.0, 23.0, 31.0))
|
||||||
|
]
|
||||||
|
prior = np.array([0.3, -0.2, 0.5])
|
||||||
|
|
||||||
|
result = solve_joint_extrinsic(
|
||||||
|
pairs,
|
||||||
|
np.eye(3),
|
||||||
|
force_rotation_only=False,
|
||||||
|
enable_phase_c=False,
|
||||||
|
t_prior_m=prior,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert not result.translation_accepted
|
||||||
|
np.testing.assert_allclose(result.T_IMU_lidar[:3, 3], prior)
|
||||||
|
assert any("prior only" in note for note in result.notes)
|
||||||
|
|
||||||
|
|
||||||
|
def test_handeye_rejects_a_small_fraction_of_gross_rotation_outliers():
|
||||||
|
rng = np.random.default_rng(7)
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([2.0, -3.0, 20.0])))
|
||||||
|
pairs: list[MotionPair] = []
|
||||||
|
for index in range(100):
|
||||||
|
axis = rng.normal(size=3)
|
||||||
|
axis /= np.linalg.norm(axis)
|
||||||
|
r_b = so3_exp(axis * np.deg2rad(rng.uniform(8.0, 30.0)))
|
||||||
|
r_a = r_true @ r_b @ r_true.T
|
||||||
|
if index == 0:
|
||||||
|
r_a = so3_exp(np.deg2rad(np.array([18.0, 0.0, 0.0]))) @ r_a
|
||||||
|
pairs.append(
|
||||||
|
MotionPair(
|
||||||
|
session_id="outlier",
|
||||||
|
i=index,
|
||||||
|
j=index + 1,
|
||||||
|
t_i_s=float(index),
|
||||||
|
t_j_s=float(index + 1),
|
||||||
|
R_A=r_a,
|
||||||
|
R_B=r_b,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
result = solve_rotation_handeye(pairs)
|
||||||
|
|
||||||
|
assert not result.ok
|
||||||
|
assert result.outlier_fraction_gt_5deg > 0.005
|
||||||
|
|
||||||
|
|
||||||
|
def test_motion_pairs_reject_low_fitness(monkeypatch):
|
||||||
|
monkeypatch.setattr(
|
||||||
|
"imu_lidar.motion_pairs.register_lidar_pair",
|
||||||
|
lambda *_args, **_kwargs: _registration(fitness=0.3),
|
||||||
|
)
|
||||||
|
imu = ImuSeries(
|
||||||
|
t_s=np.linspace(0.0, 1.2, 121),
|
||||||
|
gyro_rad_s=np.zeros((121, 3)),
|
||||||
|
acc_m_s2=np.zeros((121, 3)),
|
||||||
|
)
|
||||||
|
|
||||||
|
result = build_motion_pairs(
|
||||||
|
session_id="fitness",
|
||||||
|
keyframes=[_frame("0", 0.1), _frame("1", 1.1)],
|
||||||
|
keyframe_indices=[0, 1],
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=0.0,
|
||||||
|
min_registration_fitness=0.5,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert not result.pairs
|
||||||
|
assert any("fitness<0.50: 1" in note for note in result.notes)
|
||||||
|
|
||||||
|
|
||||||
|
def test_motion_pairs_reject_imu_and_lidar_discontinuities(monkeypatch):
|
||||||
|
monkeypatch.setattr(
|
||||||
|
"imu_lidar.motion_pairs.register_lidar_pair",
|
||||||
|
lambda *_args, **_kwargs: _registration(),
|
||||||
|
)
|
||||||
|
imu_with_gap = ImuSeries(
|
||||||
|
t_s=np.array([0.0, 0.1, 0.2, 0.3, 0.4, 0.8, 0.9, 1.0, 1.1, 1.2]),
|
||||||
|
gyro_rad_s=np.zeros((10, 3)),
|
||||||
|
acc_m_s2=np.zeros((10, 3)),
|
||||||
|
)
|
||||||
|
imu_result = build_motion_pairs(
|
||||||
|
session_id="imu-gap",
|
||||||
|
keyframes=[_frame("0", 0.1), _frame("1", 1.1)],
|
||||||
|
keyframe_indices=[0, 1],
|
||||||
|
imu=imu_with_gap,
|
||||||
|
delta_t_s=0.0,
|
||||||
|
max_imu_gap_s=0.2,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert not imu_result.pairs
|
||||||
|
assert any("IMU gap>0.200s: 1" in note for note in imu_result.notes)
|
||||||
|
|
||||||
|
continuous_imu = ImuSeries(
|
||||||
|
t_s=np.linspace(0.0, 2.2, 221),
|
||||||
|
gyro_rad_s=np.zeros((221, 3)),
|
||||||
|
acc_m_s2=np.zeros((221, 3)),
|
||||||
|
)
|
||||||
|
lidar_result = build_motion_pairs(
|
||||||
|
session_id="lidar-gap",
|
||||||
|
keyframes=[_frame("0", 0.1), _frame("2", 2.1)],
|
||||||
|
keyframe_indices=[0, 2],
|
||||||
|
imu=continuous_imu,
|
||||||
|
delta_t_s=0.0,
|
||||||
|
all_frame_times_s=np.array([0.1, 0.2, 2.1]),
|
||||||
|
max_lidar_gap_s=0.5,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert not lidar_result.pairs
|
||||||
|
assert any("LiDAR gap>0.500s: 1" in note for note in lidar_result.notes)
|
||||||
|
|
||||||
|
def test_phase_a_keeps_session_bias_linearization_points_independent():
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([2.0, -3.0, 20.0])))
|
||||||
|
bias0_by_session = {
|
||||||
|
"s0": np.array([0.010, -0.004, 0.002]),
|
||||||
|
"s1": np.array([-0.006, 0.008, -0.003]),
|
||||||
|
}
|
||||||
|
vectors_deg = (
|
||||||
|
(12.0, 0.0, 0.0),
|
||||||
|
(0.0, 15.0, 0.0),
|
||||||
|
(0.0, 0.0, 18.0),
|
||||||
|
(10.0, 8.0, 0.0),
|
||||||
|
(0.0, 11.0, 9.0),
|
||||||
|
(7.0, 0.0, 13.0),
|
||||||
|
)
|
||||||
|
pairs: list[MotionPair] = []
|
||||||
|
for session_index, (session_id, bias0) in enumerate(bias0_by_session.items()):
|
||||||
|
for pair_index, vector_deg in enumerate(vectors_deg):
|
||||||
|
r_b = so3_exp(np.deg2rad(np.asarray(vector_deg)))
|
||||||
|
r_a = r_true @ r_b @ r_true.T
|
||||||
|
index = session_index * 100 + pair_index
|
||||||
|
pairs.append(
|
||||||
|
MotionPair(
|
||||||
|
session_id=session_id,
|
||||||
|
i=index,
|
||||||
|
j=index + 1,
|
||||||
|
t_i_s=float(pair_index),
|
||||||
|
t_j_s=float(pair_index + 1),
|
||||||
|
R_A=r_a,
|
||||||
|
R_B=r_b,
|
||||||
|
t_A_m=np.zeros(3),
|
||||||
|
t_B_m=np.zeros(3),
|
||||||
|
metadata={
|
||||||
|
"J_bg": np.eye(3).tolist(),
|
||||||
|
"cov": (np.eye(3) * 1e-4).tolist(),
|
||||||
|
"gyro_bias0_rad_s": bias0.tolist(),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
result = solve_joint_extrinsic(
|
||||||
|
pairs,
|
||||||
|
r_true,
|
||||||
|
force_rotation_only=True,
|
||||||
|
gyro_bias_rad_s_by_session=bias0_by_session,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert result.phase_a_accepted
|
||||||
|
assert set(result.phase_a_comparison["variants"]) == {
|
||||||
|
"A0_fixed_bg_data_only",
|
||||||
|
"A1_session_bg_data_only",
|
||||||
|
"A2_session_bg_with_rotation_prior",
|
||||||
|
}
|
||||||
|
assert set(result.gyro_bias_rad_s_per_session) == {"s0", "s1"}
|
||||||
|
for session_id, bias0 in bias0_by_session.items():
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
result.gyro_bias_rad_s_per_session[session_id], bias0, atol=1e-8
|
||||||
|
)
|
||||||
|
assert np.linalg.norm(so3_log(r_true.T @ result.T_IMU_lidar[:3, :3])) < 1e-8
|
||||||
@@ -0,0 +1,221 @@
|
|||||||
|
"""Automated tests for imu_lidar (synthetic data).
|
||||||
|
|
||||||
|
See ``tests/README.md`` for:
|
||||||
|
- what each pytest covers;
|
||||||
|
- offline S2 host-time experiments (not run in default pytest) and recorded outcomes.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.contracts import CalibrationMode, CalibrationRequest, MotionPair, SessionInput
|
||||||
|
from imu_lidar.geometry import so3_exp
|
||||||
|
from imu_lidar.pipeline import run_calibration
|
||||||
|
from imu_lidar.rotation_handeye import solve_rotation_handeye
|
||||||
|
from imu_lidar.time_offset import estimate_time_offset
|
||||||
|
from imu_lidar.imu_io import load_imu_samples
|
||||||
|
from imu_lidar.lidar_io import load_lidar_frames
|
||||||
|
from tools.generate_synthetic_session import generate_synthetic_session
|
||||||
|
|
||||||
|
|
||||||
|
def test_rotation_handeye_recovers_yaw():
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([1.0, -2.0, 30.0])))
|
||||||
|
pairs = []
|
||||||
|
rng = np.random.default_rng(1)
|
||||||
|
for _ in range(20):
|
||||||
|
axis = rng.normal(size=3)
|
||||||
|
axis /= np.linalg.norm(axis)
|
||||||
|
angle = np.deg2rad(rng.uniform(8.0, 35.0))
|
||||||
|
r_b = so3_exp(axis * angle)
|
||||||
|
r_a = r_true @ r_b @ r_true.T
|
||||||
|
pairs.append(
|
||||||
|
MotionPair(
|
||||||
|
session_id="s",
|
||||||
|
i=0,
|
||||||
|
j=1,
|
||||||
|
t_i_s=0.0,
|
||||||
|
t_j_s=1.0,
|
||||||
|
R_A=r_a,
|
||||||
|
R_B=r_b,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
result = solve_rotation_handeye(pairs)
|
||||||
|
assert result.ok
|
||||||
|
err = np.linalg.norm(_log(r_true.T @ result.R_IMU_lidar))
|
||||||
|
assert np.degrees(err) < 1.0
|
||||||
|
|
||||||
|
|
||||||
|
def _log(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
return so3_log(rotation)
|
||||||
|
|
||||||
|
|
||||||
|
def test_synthetic_pipeline_rejects_noisy_icp_but_keeps_time_audit(tmp_path: Path):
|
||||||
|
meta = generate_synthetic_session(tmp_path, delta_t_s=0.17, yaw_extrinsic_deg=25.0)
|
||||||
|
config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
|
||||||
|
out = tmp_path / "out"
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=config,
|
||||||
|
sessions=(
|
||||||
|
SessionInput(
|
||||||
|
session_id="synth",
|
||||||
|
imu_source=tmp_path / "imu.csv",
|
||||||
|
lidar_source=tmp_path / "lidar",
|
||||||
|
),
|
||||||
|
),
|
||||||
|
requested_mode=CalibrationMode.ROTATION_ONLY,
|
||||||
|
output_directory=out,
|
||||||
|
max_iterations=1,
|
||||||
|
time_offset_search_s=0.5,
|
||||||
|
min_pair_rotation_deg=2.0,
|
||||||
|
min_pair_translation_m=0.05,
|
||||||
|
)
|
||||||
|
progress_events: list[dict] = []
|
||||||
|
result = run_calibration(request, progress_callback=progress_events.append)
|
||||||
|
# The lightweight synthetic point cloud uses approximate ICP and has a
|
||||||
|
# roughly 3-degree P95 residual. The production gate must reject it rather
|
||||||
|
# than expose a plausible-looking extrinsic.
|
||||||
|
assert result.status.value == "blocked"
|
||||||
|
assert result.T_IMU_lidar is None
|
||||||
|
session0 = result.details["sessions"][0]
|
||||||
|
assert abs(session0["time_offset_s"] - meta["delta_t_s"]) < 0.05
|
||||||
|
assert result.details["joint_handeye"]["residual_p95_deg"] > 1.5
|
||||||
|
assert not result.details["joint_handeye"]["ok"]
|
||||||
|
assert progress_events[0]["event"] == "pipeline_start"
|
||||||
|
assert any(
|
||||||
|
event["stage"] == "motion_pairs" and event["event"] == "complete"
|
||||||
|
for event in progress_events
|
||||||
|
)
|
||||||
|
assert any(
|
||||||
|
event["stage"] == "joint_optimizer" and event["event"] == "phase_a_complete"
|
||||||
|
for event in progress_events
|
||||||
|
)
|
||||||
|
assert progress_events[-1]["stage"] == "finalize"
|
||||||
|
assert progress_events[-1]["event"] == "complete"
|
||||||
|
|
||||||
|
|
||||||
|
def test_time_offset_on_synthetic(tmp_path: Path):
|
||||||
|
meta = generate_synthetic_session(tmp_path, delta_t_s=0.21, yaw_extrinsic_deg=15.0)
|
||||||
|
imu = load_imu_samples(tmp_path / "imu.csv")
|
||||||
|
frames = load_lidar_frames(tmp_path / "lidar")
|
||||||
|
offset = estimate_time_offset(imu, frames, search_s=0.5)
|
||||||
|
assert offset.ok
|
||||||
|
assert abs(offset.delta_t_s - meta["delta_t_s"]) < 0.05
|
||||||
|
|
||||||
|
|
||||||
|
def test_preintegration_bias_jacobian_matches_finite_difference():
|
||||||
|
from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
rng = np.random.default_rng(0)
|
||||||
|
t = np.linspace(0.0, 1.0, 200)
|
||||||
|
gyro = rng.normal(scale=0.2, size=(t.size, 3))
|
||||||
|
bias0 = np.array([0.01, -0.02, 0.005])
|
||||||
|
preint = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0)
|
||||||
|
db = np.array([1e-3, -2e-3, 5e-4])
|
||||||
|
approx = apply_bias_jacobian_correction(preint.delta_R, preint.J_bg, db)
|
||||||
|
exact = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0 + db).delta_R
|
||||||
|
err = np.linalg.norm(so3_log(approx.T @ exact))
|
||||||
|
assert err < 2e-3
|
||||||
|
|
||||||
|
|
||||||
|
def test_imu_preintegration_recovers_constant_accel_translation():
|
||||||
|
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
# Constant body accel (no gravity in preint body increments), zero gyro.
|
||||||
|
dt = 0.01
|
||||||
|
t = np.arange(0.0, 1.0 + 1e-9, dt)
|
||||||
|
gyro = np.zeros((t.size, 3))
|
||||||
|
acc = np.tile(np.array([0.5, -0.2, 0.1]), (t.size, 1))
|
||||||
|
preint = preintegrate_imu(t, gyro, acc, 0.0, 1.0, np.zeros(3), np.zeros(3))
|
||||||
|
assert np.linalg.norm(so3_log(preint.delta_R)) < 1e-9
|
||||||
|
# Δv ≈ a Δt, Δp ≈ 0.5 a Δt²
|
||||||
|
assert np.linalg.norm(preint.delta_v - acc[0] * 1.0) < 5e-3
|
||||||
|
assert np.linalg.norm(preint.delta_p - 0.5 * acc[0] * 1.0) < 1e-2
|
||||||
|
assert preint.cov.shape == (9, 9)
|
||||||
|
assert preint.J_bg.shape == (9, 3) and preint.J_ba.shape == (9, 3)
|
||||||
|
|
||||||
|
|
||||||
|
def test_imu_preintegration_bias_jacobian_finite_difference():
|
||||||
|
from imu_lidar.imu_preintegration import apply_bias_correction_imu, preintegrate_imu
|
||||||
|
|
||||||
|
rng = np.random.default_rng(2)
|
||||||
|
t = np.linspace(0.0, 0.8, 160)
|
||||||
|
gyro = rng.normal(scale=0.15, size=(t.size, 3))
|
||||||
|
acc = rng.normal(scale=0.5, size=(t.size, 3)) + np.array([0.0, 0.0, 9.8])
|
||||||
|
bg0 = np.array([0.01, -0.01, 0.0])
|
||||||
|
ba0 = np.array([0.02, 0.0, -0.01])
|
||||||
|
base = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0, ba0)
|
||||||
|
dbg = np.array([5e-4, -3e-4, 2e-4])
|
||||||
|
dba = np.array([1e-3, -5e-4, 0.0])
|
||||||
|
r_a, v_a, p_a = apply_bias_correction_imu(base, dbg, dba)
|
||||||
|
exact = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0 + dbg, ba0 + dba)
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
assert np.linalg.norm(so3_log(r_a.T @ exact.delta_R)) < 5e-3
|
||||||
|
assert np.linalg.norm(v_a - exact.delta_v) < 3e-2
|
||||||
|
assert np.linalg.norm(p_a - exact.delta_p) < 2e-2
|
||||||
|
|
||||||
|
|
||||||
|
def test_synthetic_pipeline_full_se3_smoke(tmp_path: Path):
|
||||||
|
generate_synthetic_session(tmp_path, delta_t_s=0.12, yaw_extrinsic_deg=18.0)
|
||||||
|
config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
|
||||||
|
out = tmp_path / "out_se3"
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=config,
|
||||||
|
sessions=(
|
||||||
|
SessionInput(
|
||||||
|
session_id="synth",
|
||||||
|
imu_source=tmp_path / "imu.csv",
|
||||||
|
lidar_source=tmp_path / "lidar",
|
||||||
|
),
|
||||||
|
),
|
||||||
|
requested_mode=CalibrationMode.FULL_SE3,
|
||||||
|
output_directory=out,
|
||||||
|
max_iterations=1,
|
||||||
|
time_offset_search_s=0.5,
|
||||||
|
min_pair_rotation_deg=2.0,
|
||||||
|
min_pair_translation_m=0.05,
|
||||||
|
)
|
||||||
|
result = run_calibration(request)
|
||||||
|
assert result.status.value in {
|
||||||
|
"full_se3_accepted",
|
||||||
|
"full_se3_rejected_due_to_observability",
|
||||||
|
"rotation_only_accepted",
|
||||||
|
"blocked",
|
||||||
|
}
|
||||||
|
session0 = result.details["sessions"][0]
|
||||||
|
assert "delta_v" in session0.get("pair_notes", []) or session0.get("pair_count", 0) >= 0
|
||||||
|
# Phase-C fields appear only when the strict rotation gate passed.
|
||||||
|
if result.status.value != "blocked":
|
||||||
|
assert result.T_IMU_lidar is not None
|
||||||
|
assert "gyro_bias_rad_s" in session0["joint"]
|
||||||
|
else:
|
||||||
|
assert result.T_IMU_lidar is None
|
||||||
|
assert not result.details["joint_handeye"]["ok"]
|
||||||
|
|
||||||
|
|
||||||
|
def test_signed_time_offset_refine_improves_or_keeps(tmp_path: Path):
|
||||||
|
from imu_lidar.geometry import so3_exp
|
||||||
|
from imu_lidar.time_offset import refine_time_offset_signed
|
||||||
|
|
||||||
|
meta = generate_synthetic_session(tmp_path, delta_t_s=0.18, yaw_extrinsic_deg=20.0)
|
||||||
|
imu = load_imu_samples(tmp_path / "imu.csv")
|
||||||
|
frames = load_lidar_frames(tmp_path / "lidar")
|
||||||
|
coarse = estimate_time_offset(imu, frames, search_s=0.5)
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
|
||||||
|
refined = refine_time_offset_signed(
|
||||||
|
imu,
|
||||||
|
frames,
|
||||||
|
delta_t_s=coarse.delta_t_s,
|
||||||
|
R_IMU_lidar=r_true,
|
||||||
|
search_s=0.08,
|
||||||
|
)
|
||||||
|
assert refined.ok
|
||||||
|
# Must not drift farther from truth than the coarse estimate by a large margin.
|
||||||
|
assert abs(refined.delta_t_s - meta["delta_t_s"]) <= abs(coarse.delta_t_s - meta["delta_t_s"]) + 0.01
|
||||||
@@ -1,28 +0,0 @@
|
|||||||
# tools目录
|
|
||||||
|
|
||||||
| 文件 | 输入→输出 |
|
|
||||||
|---|---|
|
|
||||||
| **`export_raw_to_combined.py`** | **一步导出**：逐站 H32 + 全程 G90/N300 `.rscap` → `combined/`（标定直接入口，对标 Lidar-IMU `export_rscap_to_v1`） |
|
|
||||||
| `export_h32_rscap_station.py` | 内部零件：单站 H32 → 雷达帧 NPZ（一般不必单独跑） |
|
|
||||||
| `frontlidar_dlog_export.py` | **旧数据** LiDAR dlog → 逐帧 NPZ；由一步导出在遇到 dlog 站时自动调用 |
|
|
||||||
| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU（调试用）；一步导出已内嵌同等逻辑 |
|
|
||||||
| `rscap_v2/h32_msop.py` | H32 MSOP 解码（XYZ / 极坐标 `points_raw`） |
|
|
||||||
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
|
|
||||||
| `rscap_v2/audit_capture_v2.py` | 检查rscap结构、时间范围和记录统计 |
|
|
||||||
| `build_multisensor_npz.py` | 关联雷达帧与 RTK/IMU → combined；一步导出内部调用 |
|
|
||||||
| `prepare_multisensor_station_dataset.py` | combined NPZ → 每站一帧`frames_all`和`reference_poses_*.csv` |
|
|
||||||
|
|
||||||
推荐用法：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
python tools\export_raw_to_combined.py `
|
|
||||||
--stations-root path\to\stations `
|
|
||||||
--rtk-rscap path\to\rtk.rscap `
|
|
||||||
--imu-rscap path\to\imu.rscap `
|
|
||||||
--out path\to\exported `
|
|
||||||
--overwrite
|
|
||||||
```
|
|
||||||
|
|
||||||
当前标定只使用LiDAR和RTK；IMU保持原始传感器坐标，不参与点云去畸变或外参求解。prepared阶段对站内有效RTK取平均、对heading取圆均值，并选择有效帧序列的中间LiDAR帧。
|
|
||||||
|
|
||||||
G90 `#PVTSLNA` 没有 NMEA `fix_quality` 字段时，解析会写入合成值 `4`，以便沿用 prepare 的固定解筛选（`{4,5}`）。
|
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
# Tools package for local scripts and tests.
|
||||||
@@ -1,401 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window.
|
|
||||||
|
|
||||||
Inputs are LiDAR frame NPZ files from ``export_h32_rscap_station.py`` (or legacy
|
|
||||||
``frontlidar_dlog_export.py``) and parsed RTK/IMU JSONL from
|
|
||||||
``parse_rtk_imu_v2.py``. Raw ``.rscap`` files remain the traceability source;
|
|
||||||
this script never modifies them.
|
|
||||||
|
|
||||||
Position rows may be NMEA ``GGA`` or G90 ``PVTSLNA`` (both expose ``lat_deg`` /
|
|
||||||
``lon_deg`` / ``altitude_m``). Default time basis is LiDAR device time vs GNSS
|
|
||||||
week/TOW; ``--time-basis host`` keeps the legacy host-receive nearest-neighbour
|
|
||||||
association for old dlog datasets.
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import csv
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
from typing import Any
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
|
|
||||||
POSITION_TYPES = {"GGA", "PVTSLNA"}
|
|
||||||
|
|
||||||
|
|
||||||
def parse_named_path(text: str) -> tuple[str, Path]:
|
|
||||||
if "=" not in text:
|
|
||||||
raise argparse.ArgumentTypeError("expected NAME=PATH")
|
|
||||||
name, raw_path = text.split("=", 1)
|
|
||||||
if not name.strip():
|
|
||||||
raise argparse.ArgumentTypeError("segment name is empty")
|
|
||||||
return name.strip(), Path(raw_path)
|
|
||||||
|
|
||||||
|
|
||||||
def parse_args() -> argparse.Namespace:
|
|
||||||
parser = argparse.ArgumentParser()
|
|
||||||
parser.add_argument(
|
|
||||||
"--lidar",
|
|
||||||
type=parse_named_path,
|
|
||||||
action="append",
|
|
||||||
required=True,
|
|
||||||
metavar="NAME=FRAMES_DIR",
|
|
||||||
help="Repeat for each LiDAR segment; directory contains exported *.npz frames.",
|
|
||||||
)
|
|
||||||
parser.add_argument("--rtk", type=Path, action="append", required=True, help="Parsed rtk.jsonl; repeat per session.")
|
|
||||||
parser.add_argument("--imu", type=Path, action="append", required=True, help="Parsed imu.jsonl; repeat per session.")
|
|
||||||
parser.add_argument("--out", type=Path, required=True)
|
|
||||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
|
||||||
parser.add_argument("--imu-before-ms", type=float, default=100.0)
|
|
||||||
parser.add_argument("--imu-after-ms", type=float, default=100.0)
|
|
||||||
parser.add_argument("--gps-utc-leap-seconds", type=int, default=18)
|
|
||||||
parser.add_argument(
|
|
||||||
"--time-basis",
|
|
||||||
choices=("device_gnss", "host"),
|
|
||||||
default="device_gnss",
|
|
||||||
help="device_gnss: LiDAR unix_time_ns ↔ GNSS week/TOW; host: legacy host-receive association.",
|
|
||||||
)
|
|
||||||
parser.add_argument("--overwrite", action="store_true")
|
|
||||||
return parser.parse_args()
|
|
||||||
|
|
||||||
|
|
||||||
def load_jsonl(paths: list[Path]) -> list[dict[str, Any]]:
|
|
||||||
rows: list[dict[str, Any]] = []
|
|
||||||
for source_index, path in enumerate(paths):
|
|
||||||
source_file = str(path.resolve())
|
|
||||||
with path.open("r", encoding="utf-8") as stream:
|
|
||||||
for line_number, line in enumerate(stream, start=1):
|
|
||||||
if not line.strip():
|
|
||||||
continue
|
|
||||||
row = json.loads(line)
|
|
||||||
row["_source_file"] = source_file
|
|
||||||
row["_source_index"] = source_index
|
|
||||||
row["_source_line"] = line_number
|
|
||||||
rows.append(row)
|
|
||||||
return rows
|
|
||||||
|
|
||||||
|
|
||||||
def utf8_array(value: Any) -> np.ndarray:
|
|
||||||
return np.frombuffer(str(value if value is not None else "").encode("utf-8"), dtype=np.uint8)
|
|
||||||
|
|
||||||
|
|
||||||
def scalar(array: np.ndarray) -> Any:
|
|
||||||
return array.reshape(-1)[0].item()
|
|
||||||
|
|
||||||
|
|
||||||
def nearest_index(times: np.ndarray, target: int) -> int:
|
|
||||||
if not len(times):
|
|
||||||
return -1
|
|
||||||
right = int(np.searchsorted(times, target, side="left"))
|
|
||||||
candidates = [index for index in (right - 1, right) if 0 <= index < len(times)]
|
|
||||||
return min(candidates, key=lambda index: abs(int(times[index]) - target))
|
|
||||||
|
|
||||||
|
|
||||||
def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
|
|
||||||
"""Recover timing inside each serial chunk from device timestamps.
|
|
||||||
|
|
||||||
A capture chunk has one host receive timestamp but may contain several IMU
|
|
||||||
frames. The last frame is anchored to the chunk receive time and earlier
|
|
||||||
frames are moved backwards by their device timestamp difference.
|
|
||||||
"""
|
|
||||||
groups: dict[tuple[int, int], list[dict[str, Any]]] = {}
|
|
||||||
for row in rows:
|
|
||||||
if not row.get("crc_valid") or row.get("device_timestamp_ms") is None:
|
|
||||||
continue
|
|
||||||
key = (int(row["_source_index"]), int(row.get("source_chunk_sequence_last", -1)))
|
|
||||||
groups.setdefault(key, []).append(row)
|
|
||||||
result: list[dict[str, Any]] = []
|
|
||||||
for group in groups.values():
|
|
||||||
group.sort(key=lambda row: (int(row["device_timestamp_ms"]), int(row["_source_line"])))
|
|
||||||
last_device = int(group[-1]["device_timestamp_ms"])
|
|
||||||
host_ns = int(group[-1]["host_receive_utc_ns"])
|
|
||||||
for row in group:
|
|
||||||
delta_ms = (last_device - int(row["device_timestamp_ms"])) & 0xFFFFFFFF
|
|
||||||
if delta_ms > 60_000:
|
|
||||||
delta_ms = 0
|
|
||||||
copied = dict(row)
|
|
||||||
copied["estimated_time_ns"] = host_ns - delta_ms * 1_000_000
|
|
||||||
result.append(copied)
|
|
||||||
result.sort(key=lambda row: int(row["estimated_time_ns"]))
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
|
|
||||||
week, tow_ms = row.get("gnss_week"), row.get("gnss_tow_ms")
|
|
||||||
if week is None or tow_ms is None:
|
|
||||||
return None
|
|
||||||
seconds = int(week) * 604800 + float(tow_ms) / 1000.0 - leap_seconds
|
|
||||||
return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
|
|
||||||
|
|
||||||
|
|
||||||
def association_time_ns(row: dict[str, Any], time_basis: str, leap_seconds: int) -> int | None:
|
|
||||||
if time_basis == "host":
|
|
||||||
host = row.get("host_receive_utc_ns")
|
|
||||||
return int(host) if host is not None else None
|
|
||||||
device = gnss_utc_ns(row, leap_seconds)
|
|
||||||
if device is not None:
|
|
||||||
return device
|
|
||||||
host = row.get("host_receive_utc_ns")
|
|
||||||
return int(host) if host is not None else None
|
|
||||||
|
|
||||||
|
|
||||||
def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray:
|
|
||||||
return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype)
|
|
||||||
|
|
||||||
|
|
||||||
def raw_frame_matrix(rows: list[dict[str, Any]]) -> tuple[np.ndarray, np.ndarray]:
|
|
||||||
frames = [bytes.fromhex(str(row.get("raw_frame_hex", ""))) for row in rows]
|
|
||||||
lengths = np.asarray([len(frame) for frame in frames], dtype=np.int32)
|
|
||||||
width = max(lengths, default=0)
|
|
||||||
matrix = np.zeros((len(frames), width), dtype=np.uint8)
|
|
||||||
for index, frame in enumerate(frames):
|
|
||||||
matrix[index, : len(frame)] = np.frombuffer(frame, dtype=np.uint8)
|
|
||||||
return matrix, lengths
|
|
||||||
|
|
||||||
|
|
||||||
def add_rtk(values: dict[str, np.ndarray], prefix: str, row: dict[str, Any] | None, dt_ns: int | None) -> None:
|
|
||||||
values[f"{prefix}_valid"] = np.asarray([row is not None], dtype=np.uint8)
|
|
||||||
values[f"{prefix}_dt_ns"] = np.asarray([dt_ns or 0], dtype=np.int64)
|
|
||||||
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([0], dtype=np.int64)
|
|
||||||
values[f"{prefix}_raw_utf8"] = utf8_array("")
|
|
||||||
values[f"{prefix}_source_file_utf8"] = utf8_array("")
|
|
||||||
values[f"{prefix}_source_raw_file_offset"] = np.asarray([-1], dtype=np.int64)
|
|
||||||
values[f"{prefix}_source_raw_byte_length"] = np.asarray([0], dtype=np.int32)
|
|
||||||
if row is None:
|
|
||||||
return
|
|
||||||
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([row.get("host_receive_utc_ns", 0)], dtype=np.int64)
|
|
||||||
values[f"{prefix}_raw_utf8"] = utf8_array(row.get("raw_line", ""))
|
|
||||||
values[f"{prefix}_source_file_utf8"] = utf8_array(row.get("_source_file", ""))
|
|
||||||
values[f"{prefix}_source_raw_file_offset"] = np.asarray([row.get("source_raw_file_offset", -1)], dtype=np.int64)
|
|
||||||
values[f"{prefix}_source_raw_byte_length"] = np.asarray([row.get("source_raw_byte_length", 0)], dtype=np.int32)
|
|
||||||
|
|
||||||
|
|
||||||
def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
|
|
||||||
for key, dtype, default in (
|
|
||||||
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
|
|
||||||
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
|
|
||||||
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
|
|
||||||
("differential_age_s", np.float64, np.nan),
|
|
||||||
("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),
|
|
||||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
|
||||||
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
|
|
||||||
("solution_satellites", np.int32, -1),
|
|
||||||
):
|
|
||||||
values[f"rtk_{key}"] = np.asarray([default], dtype=dtype)
|
|
||||||
values["rtk_fixed"] = np.asarray([0], dtype=np.uint8)
|
|
||||||
values["rtk_heading_solution_utf8"] = utf8_array("")
|
|
||||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64)
|
|
||||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64)
|
|
||||||
|
|
||||||
|
|
||||||
def build_combined(
|
|
||||||
lidar_segments: list[tuple[str, Path]],
|
|
||||||
rtk_paths: list[Path],
|
|
||||||
imu_paths: list[Path],
|
|
||||||
out: Path,
|
|
||||||
*,
|
|
||||||
rtk_max_dt_ms: float = 150.0,
|
|
||||||
imu_before_ms: float = 100.0,
|
|
||||||
imu_after_ms: float = 100.0,
|
|
||||||
gps_utc_leap_seconds: int = 18,
|
|
||||||
time_basis: str = "device_gnss",
|
|
||||||
overwrite: bool = False,
|
|
||||||
) -> dict[str, Any]:
|
|
||||||
"""Associate LiDAR frames with RTK/IMU and write ``out/`` combined package."""
|
|
||||||
|
|
||||||
if out.exists() and any(out.iterdir()) and not overwrite:
|
|
||||||
raise FileExistsError(f"{out} is non-empty; pass overwrite=True")
|
|
||||||
frames_out = out / "frames"
|
|
||||||
frames_out.mkdir(parents=True, exist_ok=True)
|
|
||||||
|
|
||||||
rtk_rows = load_jsonl(rtk_paths)
|
|
||||||
positions = []
|
|
||||||
for row in rtk_rows:
|
|
||||||
if row.get("type") not in POSITION_TYPES or not row.get("checksum_valid"):
|
|
||||||
continue
|
|
||||||
if row.get("lat_deg") is None or row.get("lon_deg") is None:
|
|
||||||
continue
|
|
||||||
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
|
|
||||||
if assoc is None:
|
|
||||||
continue
|
|
||||||
copied = dict(row)
|
|
||||||
copied["_assoc_time_ns"] = assoc
|
|
||||||
positions.append(copied)
|
|
||||||
positions.sort(key=lambda row: int(row["_assoc_time_ns"]))
|
|
||||||
|
|
||||||
heading = []
|
|
||||||
for row in rtk_rows:
|
|
||||||
if row.get("type") != "UNIHEADINGA" or not row.get("checksum_valid") or not row.get("heading_valid"):
|
|
||||||
continue
|
|
||||||
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
|
|
||||||
if assoc is None:
|
|
||||||
continue
|
|
||||||
copied = dict(row)
|
|
||||||
copied["_assoc_time_ns"] = assoc
|
|
||||||
heading.append(copied)
|
|
||||||
heading.sort(key=lambda row: int(row["_assoc_time_ns"]))
|
|
||||||
|
|
||||||
imu = estimate_imu_times(load_jsonl(imu_paths))
|
|
||||||
position_times = np.asarray([int(row["_assoc_time_ns"]) for row in positions], dtype=np.int64)
|
|
||||||
heading_times = np.asarray([int(row["_assoc_time_ns"]) for row in heading], dtype=np.int64)
|
|
||||||
imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64)
|
|
||||||
|
|
||||||
manifest: list[dict[str, Any]] = []
|
|
||||||
global_index = 0
|
|
||||||
max_rtk_ns = int(rtk_max_dt_ms * 1_000_000)
|
|
||||||
before_ns = int(imu_before_ms * 1_000_000)
|
|
||||||
after_ns = int(imu_after_ms * 1_000_000)
|
|
||||||
|
|
||||||
for segment_name, frame_dir in lidar_segments:
|
|
||||||
frame_paths = sorted(frame_dir.glob("*.npz"))
|
|
||||||
if not frame_paths:
|
|
||||||
raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
|
|
||||||
for segment_index, source in enumerate(frame_paths):
|
|
||||||
with np.load(source, allow_pickle=False) as frame:
|
|
||||||
values = {key: np.asarray(frame[key]) for key in frame.files}
|
|
||||||
lidar_time_ns = int(scalar(values["unix_time_ns"]))
|
|
||||||
|
|
||||||
position_index = nearest_index(position_times, lidar_time_ns)
|
|
||||||
heading_index = nearest_index(heading_times, lidar_time_ns)
|
|
||||||
position_row = positions[position_index] if position_index >= 0 else None
|
|
||||||
heading_row = heading[heading_index] if heading_index >= 0 else None
|
|
||||||
position_dt = int(position_times[position_index]) - lidar_time_ns if position_index >= 0 else None
|
|
||||||
heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None
|
|
||||||
position_ok = position_row is not None and abs(position_dt or 0) <= max_rtk_ns
|
|
||||||
heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns
|
|
||||||
add_rtk(values, "rtk_gga", position_row if position_ok else None, position_dt)
|
|
||||||
add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt)
|
|
||||||
initialize_rtk_measurements(values)
|
|
||||||
|
|
||||||
if position_ok and position_row:
|
|
||||||
for key, dtype, default in (
|
|
||||||
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
|
|
||||||
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
|
|
||||||
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
|
|
||||||
("differential_age_s", np.float64, np.nan),
|
|
||||||
):
|
|
||||||
values[f"rtk_{key}"] = np.asarray([position_row.get(key, default)], dtype=dtype)
|
|
||||||
values["rtk_gga_satellites"] = np.asarray([position_row.get("satellites", -1)], dtype=np.int32)
|
|
||||||
if position_row.get("gnss_week") is not None:
|
|
||||||
values["rtk_gnss_week"] = np.asarray([position_row.get("gnss_week", -1)], dtype=np.int32)
|
|
||||||
values["rtk_gnss_tow_ms"] = np.asarray([position_row.get("gnss_tow_ms", -1)], dtype=np.int64)
|
|
||||||
values["rtk_fixed"] = np.asarray([int(position_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
|
|
||||||
if heading_ok and heading_row:
|
|
||||||
for key, dtype, default in (
|
|
||||||
("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),
|
|
||||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
|
||||||
("pitch_stddev_deg", np.float64, np.nan),
|
|
||||||
("solution_satellites", np.int32, -1),
|
|
||||||
):
|
|
||||||
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
|
|
||||||
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", ""))
|
|
||||||
device_ns = gnss_utc_ns(heading_row, gps_utc_leap_seconds)
|
|
||||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64)
|
|
||||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray(
|
|
||||||
[int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64
|
|
||||||
)
|
|
||||||
|
|
||||||
left = int(np.searchsorted(imu_times, lidar_time_ns - before_ns, side="left"))
|
|
||||||
right = int(np.searchsorted(imu_times, lidar_time_ns + after_ns, side="right"))
|
|
||||||
window = imu[left:right]
|
|
||||||
values["imu_window_count"] = np.asarray([len(window)], dtype=np.int32)
|
|
||||||
values["imu_valid"] = np.asarray([bool(window)], dtype=np.uint8)
|
|
||||||
values["imu_time_ns"] = numeric_array(window, "estimated_time_ns", np.int64, 0)
|
|
||||||
values["imu_host_receive_utc_ns"] = numeric_array(window, "host_receive_utc_ns", np.int64, 0)
|
|
||||||
for key in ("device_timestamp_ms", "pps_sync_stamp_ms", "tag"):
|
|
||||||
values[f"imu_{key}"] = numeric_array(window, key, np.int64, -1)
|
|
||||||
for key in (
|
|
||||||
"temperature_c", "air_pressure_pa", "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",
|
|
||||||
"roll_deg", "pitch_deg", "yaw_deg", "quaternion_w", "quaternion_x", "quaternion_y", "quaternion_z",
|
|
||||||
):
|
|
||||||
values[f"imu_{key}"] = numeric_array(window, key, np.float64, np.nan)
|
|
||||||
values["imu_source_index"] = numeric_array(window, "_source_index", np.int32, -1)
|
|
||||||
values["imu_source_raw_file_offset"] = numeric_array(window, "source_raw_file_offset", np.int64, -1)
|
|
||||||
raw_matrix, raw_lengths = raw_frame_matrix(window)
|
|
||||||
values["imu_raw_frame_bytes"] = raw_matrix
|
|
||||||
values["imu_raw_frame_length"] = raw_lengths
|
|
||||||
values["imu_source_files_json_utf8"] = utf8_array(
|
|
||||||
json.dumps([str(path.resolve()) for path in imu_paths], ensure_ascii=False)
|
|
||||||
)
|
|
||||||
values["source_lidar_file_utf8"] = utf8_array(source.resolve())
|
|
||||||
values["segment_name_utf8"] = utf8_array(segment_name)
|
|
||||||
|
|
||||||
output = frames_out / f"{segment_name}_{segment_index:06d}.npz"
|
|
||||||
np.savez_compressed(output, **values)
|
|
||||||
manifest.append({
|
|
||||||
"global_index": global_index,
|
|
||||||
"segment": segment_name,
|
|
||||||
"segment_index": segment_index,
|
|
||||||
"output": str(output.relative_to(out)),
|
|
||||||
"source_lidar": str(source.resolve()),
|
|
||||||
"lidar_time_ns": lidar_time_ns,
|
|
||||||
"rtk_gga_dt_ns": position_dt,
|
|
||||||
"rtk_heading_dt_ns": heading_dt,
|
|
||||||
"rtk_valid": position_ok,
|
|
||||||
"heading_valid": heading_ok,
|
|
||||||
"rtk_fix_quality": position_row.get("fix_quality") if position_ok and position_row else None,
|
|
||||||
"rtk_fixed": bool(position_ok and position_row and int(position_row.get("fix_quality", -1)) in {4, 5}),
|
|
||||||
"imu_window_count": len(window),
|
|
||||||
})
|
|
||||||
global_index += 1
|
|
||||||
|
|
||||||
fields = sorted({key for row in manifest for key in row})
|
|
||||||
with (out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
|
||||||
writer.writeheader()
|
|
||||||
writer.writerows(manifest)
|
|
||||||
if time_basis == "device_gnss":
|
|
||||||
time_basis_text = (
|
|
||||||
"LiDAR MSOP/device unix_time_ns ↔ RTK GNSS week/TOW (fallback host receive); "
|
|
||||||
"IMU still windowed on host-anchored device deltas"
|
|
||||||
)
|
|
||||||
else:
|
|
||||||
time_basis_text = (
|
|
||||||
"LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement"
|
|
||||||
)
|
|
||||||
summary = {
|
|
||||||
"frames": len(manifest),
|
|
||||||
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in lidar_segments},
|
|
||||||
"rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest),
|
|
||||||
"heading_valid": sum(bool(row["heading_valid"]) for row in manifest),
|
|
||||||
"rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest),
|
|
||||||
"imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest),
|
|
||||||
"rtk_max_dt_ms": rtk_max_dt_ms,
|
|
||||||
"imu_window_ms": [-imu_before_ms, imu_after_ms],
|
|
||||||
"time_basis": time_basis_text,
|
|
||||||
"time_basis_mode": time_basis,
|
|
||||||
"position_message_types": sorted(POSITION_TYPES),
|
|
||||||
"imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied",
|
|
||||||
}
|
|
||||||
(out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
return summary
|
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
|
||||||
args = parse_args()
|
|
||||||
summary = build_combined(
|
|
||||||
args.lidar,
|
|
||||||
args.rtk,
|
|
||||||
args.imu,
|
|
||||||
args.out,
|
|
||||||
rtk_max_dt_ms=args.rtk_max_dt_ms,
|
|
||||||
imu_before_ms=args.imu_before_ms,
|
|
||||||
imu_after_ms=args.imu_after_ms,
|
|
||||||
gps_utc_leap_seconds=args.gps_utc_leap_seconds,
|
|
||||||
time_basis=args.time_basis,
|
|
||||||
overwrite=args.overwrite,
|
|
||||||
)
|
|
||||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
|
||||||
return 0
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
raise SystemExit(main())
|
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Compare two S2 offline summary.json runs (real artifacts only)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.geometry import rpy_deg_xyz, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
def dig(path: Path):
|
||||||
|
data = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
session = data["details"]["sessions"][0]
|
||||||
|
return data, session, session.get("handeye", {}), session.get("time_offset", {})
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
old_path = Path(sys.argv[1])
|
||||||
|
new_path = Path(sys.argv[2])
|
||||||
|
old, so, heo, too = dig(old_path)
|
||||||
|
new, sn, hen, ton = dig(new_path)
|
||||||
|
|
||||||
|
print("==== COMPARISON (real summary.json artifacts) ====")
|
||||||
|
print(f"{'metric':28s} {'run_a':28s} {'run_b':28s}")
|
||||||
|
rows = [
|
||||||
|
("status", old.get("status"), new.get("status")),
|
||||||
|
("stage", so.get("stage"), sn.get("stage")),
|
||||||
|
("delta_t_s", f"{too.get('delta_t_s'):.6f}", f"{ton.get('delta_t_s'):.6f}"),
|
||||||
|
(
|
||||||
|
"corr/mag_peak",
|
||||||
|
f"{too.get('correlation_peak'):.6f}",
|
||||||
|
f"{ton.get('correlation_peak'):.6f}",
|
||||||
|
),
|
||||||
|
("keyframes", so.get("keyframes"), sn.get("keyframes")),
|
||||||
|
("handeye_pairs", heo.get("pair_count"), hen.get("pair_count")),
|
||||||
|
("handeye_ok", heo.get("ok"), hen.get("ok")),
|
||||||
|
("rms_deg", f"{heo.get('residual_rms_deg'):.4f}", f"{hen.get('residual_rms_deg'):.4f}"),
|
||||||
|
(
|
||||||
|
"median_deg",
|
||||||
|
f"{heo.get('residual_median_deg'):.4f}",
|
||||||
|
f"{hen.get('residual_median_deg'):.4f}",
|
||||||
|
),
|
||||||
|
]
|
||||||
|
for key, a, b in rows:
|
||||||
|
print(f"{key:28s} {str(a):28s} {str(b):28s}")
|
||||||
|
|
||||||
|
print("run_a pair_notes:", so.get("pair_notes"))
|
||||||
|
print("run_b pair_notes:", sn.get("pair_notes"))
|
||||||
|
print("run_a time notes:", too.get("notes"))
|
||||||
|
print("run_b time notes:", ton.get("notes"))
|
||||||
|
print("run_a handeye notes:", heo.get("notes"))
|
||||||
|
print("run_b handeye notes:", hen.get("notes"))
|
||||||
|
|
||||||
|
r_old = np.asarray(heo["R_IMU_lidar"], dtype=float)
|
||||||
|
r_new = np.asarray(hen["R_IMU_lidar"], dtype=float)
|
||||||
|
print("R relative change deg:", float(np.degrees(np.linalg.norm(so3_log(r_old.T @ r_new)))))
|
||||||
|
print("RPY run_a deg:", rpy_deg_xyz(r_old))
|
||||||
|
print("RPY run_b deg:", rpy_deg_xyz(r_new))
|
||||||
|
print("delta rms (b-a):", hen.get("residual_rms_deg") - heo.get("residual_rms_deg"))
|
||||||
|
print(
|
||||||
|
"delta median (b-a):",
|
||||||
|
hen.get("residual_median_deg") - heo.get("residual_median_deg"),
|
||||||
|
)
|
||||||
|
print("artifacts:")
|
||||||
|
print(" run_a:", old_path)
|
||||||
|
print(" run_b:", new_path)
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -1,172 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Export one static-station H32 V2 .rscap into LiDAR frame NPZs.
|
|
||||||
|
|
||||||
This is an **internal** helper used by ``export_raw_to_combined.py``.
|
|
||||||
For RTK–LiDAR calibration, prefer the one-shot exporter that writes ``combined/``.
|
|
||||||
|
|
||||||
Output frame contract (consumed by ``build_multisensor_npz.py``):
|
|
||||||
|
|
||||||
- ``points_raw``: (N, 5) polar ``d_mm, azimuth_deg, altitude_deg, intensity, progression``
|
|
||||||
- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns)
|
|
||||||
- ``frame_counter``, ``point_count``, optional host receive stamp
|
|
||||||
|
|
||||||
Raw ``.rscap`` files are never modified.
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import csv
|
|
||||||
import json
|
|
||||||
import sys
|
|
||||||
from pathlib import Path
|
|
||||||
from typing import Any
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
ROOT = Path(__file__).resolve().parent
|
|
||||||
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
|
||||||
|
|
||||||
from capture_format_v2 import file_summary, read_capture # noqa: E402
|
|
||||||
from h32_msop import iter_h32_frames_polar # noqa: E402
|
|
||||||
|
|
||||||
|
|
||||||
def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> Path:
|
|
||||||
candidates = [
|
|
||||||
station_dir / capture_name,
|
|
||||||
station_dir / "h32.rscap",
|
|
||||||
station_dir / "lidar.rscap",
|
|
||||||
]
|
|
||||||
for path in candidates:
|
|
||||||
if path.is_file():
|
|
||||||
return path
|
|
||||||
raise FileNotFoundError(
|
|
||||||
f"no LiDAR .rscap under {station_dir}; tried {[str(p.name) for p in candidates]}"
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def export_station_h32(
|
|
||||||
station: Path,
|
|
||||||
out: Path,
|
|
||||||
*,
|
|
||||||
capture_name: str = "h32.rscap",
|
|
||||||
stride: int = 1,
|
|
||||||
min_frame_points: int = 100,
|
|
||||||
min_range_m: float = 0.3,
|
|
||||||
max_range_m: float = 120.0,
|
|
||||||
compress: bool = True,
|
|
||||||
write_reports: bool = False,
|
|
||||||
resume: bool = False,
|
|
||||||
) -> dict[str, Any]:
|
|
||||||
"""Decode one station H32 capture into ``out/frames/*.npz``. Returns metadata."""
|
|
||||||
|
|
||||||
rscap = station if station.is_file() and station.suffix.lower() == ".rscap" else resolve_lidar_rscap(station, capture_name)
|
|
||||||
frames_dir = out / "frames"
|
|
||||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
|
||||||
|
|
||||||
capture = read_capture(rscap)
|
|
||||||
frames = iter_h32_frames_polar(
|
|
||||||
capture,
|
|
||||||
min_frame_points=min_frame_points,
|
|
||||||
frame_stride=max(1, stride),
|
|
||||||
min_range_m=min_range_m,
|
|
||||||
max_range_m=max_range_m,
|
|
||||||
)
|
|
||||||
if not frames:
|
|
||||||
raise RuntimeError(f"no H32 frames decoded from {rscap}")
|
|
||||||
|
|
||||||
saver = np.savez_compressed if compress else np.savez
|
|
||||||
manifest_rows: list[dict[str, Any]] = []
|
|
||||||
written = 0
|
|
||||||
for index, frame in enumerate(frames):
|
|
||||||
unix_time_ns = int(round(frame.t_start_s * 1_000_000_000))
|
|
||||||
name = f"h32_{index:06d}_{unix_time_ns}_frame{index}.npz"
|
|
||||||
destination = frames_dir / name
|
|
||||||
if resume and destination.exists():
|
|
||||||
continue
|
|
||||||
points = np.asarray(frame.points_raw, dtype=np.float32)
|
|
||||||
payload = {
|
|
||||||
"points_raw": points,
|
|
||||||
"frame_counter": np.asarray([index], dtype=np.int32),
|
|
||||||
"point_count": np.asarray([points.shape[0]], dtype=np.int32),
|
|
||||||
"unix_time_ns": np.asarray([unix_time_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(rscap.resolve()).encode("utf-8"), dtype=np.uint8),
|
|
||||||
}
|
|
||||||
saver(destination, **payload)
|
|
||||||
written += 1
|
|
||||||
manifest_rows.append(
|
|
||||||
{
|
|
||||||
"index": index,
|
|
||||||
"output": name,
|
|
||||||
"unix_time_ns": unix_time_ns,
|
|
||||||
"point_count": int(points.shape[0]),
|
|
||||||
"host_receive_utc_ns": int(frame.host_receive_utc_ns),
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
metadata: dict[str, Any] = {
|
|
||||||
"source_rscap": str(rscap.resolve()),
|
|
||||||
"capture": file_summary(capture),
|
|
||||||
"frames_decoded": len(frames),
|
|
||||||
"frames_written": written,
|
|
||||||
"frames_dir": str(frames_dir.resolve()),
|
|
||||||
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
|
|
||||||
"points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
|
|
||||||
}
|
|
||||||
(out / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
(out / "README.md").write_text(
|
|
||||||
"# H32 station export (internal)\n\n"
|
|
||||||
f"- source: `{rscap}`\n"
|
|
||||||
f"- frames: `{frames_dir}`\n"
|
|
||||||
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
|
|
||||||
encoding="utf-8",
|
|
||||||
)
|
|
||||||
if write_reports:
|
|
||||||
reports = out / "reports"
|
|
||||||
reports.mkdir(parents=True, exist_ok=True)
|
|
||||||
with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
writer = csv.DictWriter(stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"])
|
|
||||||
writer.writeheader()
|
|
||||||
writer.writerows(manifest_rows)
|
|
||||||
(reports / "export_summary.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
return metadata
|
|
||||||
|
|
||||||
|
|
||||||
def parse_args() -> argparse.Namespace:
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--station", type=Path, required=True, help="Station directory or .rscap file")
|
|
||||||
parser.add_argument("--out", type=Path, required=True)
|
|
||||||
parser.add_argument("--capture-name", default="h32.rscap")
|
|
||||||
parser.add_argument("--stride", type=int, default=1)
|
|
||||||
parser.add_argument("--min-frame-points", type=int, default=100)
|
|
||||||
parser.add_argument("--min-range-m", type=float, default=0.3)
|
|
||||||
parser.add_argument("--max-range-m", type=float, default=120.0)
|
|
||||||
parser.add_argument("--compress", action="store_true", default=True)
|
|
||||||
parser.add_argument("--write-reports", action="store_true")
|
|
||||||
parser.add_argument("--resume", action="store_true", help="Skip frames that already exist")
|
|
||||||
return parser.parse_args()
|
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
|
||||||
args = parse_args()
|
|
||||||
metadata = export_station_h32(
|
|
||||||
args.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,
|
|
||||||
)
|
|
||||||
print(json.dumps(metadata, ensure_ascii=False, indent=2))
|
|
||||||
return 0
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
raise SystemExit(main())
|
|
||||||
@@ -0,0 +1,111 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Build motion_pairs.json next to an existing summary without re-solving extrinsic.
|
||||||
|
|
||||||
|
Use this once for older calibration outputs that predate automatic pair caching.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
ROOT = Path(__file__).resolve().parents[1]
|
||||||
|
if str(ROOT) not in sys.path:
|
||||||
|
sys.path.insert(0, str(ROOT))
|
||||||
|
|
||||||
|
from imu_lidar.imu_audit import audit_imu
|
||||||
|
from imu_lidar.imu_io import load_imu_samples
|
||||||
|
from imu_lidar.keyframes import build_keyframes
|
||||||
|
from imu_lidar.lidar_io import load_lidar_frames
|
||||||
|
from imu_lidar.motion_pairs import build_motion_pairs
|
||||||
|
from imu_lidar.motion_pairs_io import build_motion_pairs_payload, save_motion_pairs
|
||||||
|
|
||||||
|
|
||||||
|
def _load_summary_meta(summary_path: Path) -> tuple[float, np.ndarray, str]:
|
||||||
|
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||||
|
delta_t = float(summary.get("time_offset_s") or 0.0)
|
||||||
|
session = (summary.get("details") or {}).get("sessions", [{}])[0]
|
||||||
|
session_id = str(session.get("session_id") or summary_path.parent.name)
|
||||||
|
bias = np.asarray(
|
||||||
|
(session.get("imu_audit") or {}).get("gyro_bias_rad_s")
|
||||||
|
or (session.get("joint") or {}).get("gyro_bias_rad_s")
|
||||||
|
or [0.0, 0.0, 0.0],
|
||||||
|
dtype=float,
|
||||||
|
).reshape(3)
|
||||||
|
return delta_t, bias, session_id
|
||||||
|
|
||||||
|
|
||||||
|
def export_one(
|
||||||
|
*,
|
||||||
|
lidar: Path,
|
||||||
|
imu: Path,
|
||||||
|
summary: Path,
|
||||||
|
output: Path | None,
|
||||||
|
min_rotation_deg: float,
|
||||||
|
min_translation_m: float,
|
||||||
|
) -> Path:
|
||||||
|
delta_t, bias_from_summary, session_id = _load_summary_meta(summary)
|
||||||
|
imu_series = load_imu_samples(imu)
|
||||||
|
# Prefer freshly audited bias if summary bias is missing/zeros.
|
||||||
|
if float(np.linalg.norm(bias_from_summary)) < 1e-12:
|
||||||
|
bias = audit_imu(imu_series).gyro_bias_rad_s
|
||||||
|
else:
|
||||||
|
bias = bias_from_summary
|
||||||
|
|
||||||
|
frames = load_lidar_frames(lidar)
|
||||||
|
keyframes = build_keyframes(
|
||||||
|
frames,
|
||||||
|
min_translation_m=min_translation_m,
|
||||||
|
min_rotation_deg=min_rotation_deg,
|
||||||
|
)
|
||||||
|
pair_set = build_motion_pairs(
|
||||||
|
session_id=session_id,
|
||||||
|
keyframes=list(keyframes.frames),
|
||||||
|
keyframe_indices=keyframes.indices,
|
||||||
|
imu=imu_series,
|
||||||
|
delta_t_s=delta_t,
|
||||||
|
gyro_bias_rad_s=bias,
|
||||||
|
min_rotation_deg=min_rotation_deg,
|
||||||
|
min_translation_m=min_translation_m,
|
||||||
|
)
|
||||||
|
prepared = [
|
||||||
|
{
|
||||||
|
"session_id": session_id,
|
||||||
|
"time_offset_s": delta_t,
|
||||||
|
"gyro_bias_rad_s": np.asarray(bias, dtype=float).reshape(3),
|
||||||
|
"pairs": pair_set.pairs,
|
||||||
|
}
|
||||||
|
]
|
||||||
|
payload = build_motion_pairs_payload(prepared_sessions=prepared)
|
||||||
|
out = output or (summary.parent / "motion_pairs.json")
|
||||||
|
save_motion_pairs(out, payload)
|
||||||
|
print(f"wrote {out} ({len(pair_set.pairs)} pairs, session={session_id}, dt={delta_t:.6f})")
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument("--lidar", type=Path, required=True)
|
||||||
|
parser.add_argument("--imu", type=Path, required=True)
|
||||||
|
parser.add_argument("--summary", type=Path, required=True)
|
||||||
|
parser.add_argument("--output", type=Path, default=None, help="Default: <summary_dir>/motion_pairs.json")
|
||||||
|
parser.add_argument("--min-pair-rotation-deg", type=float, default=2.0)
|
||||||
|
parser.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
||||||
|
args = parser.parse_args()
|
||||||
|
export_one(
|
||||||
|
lidar=args.lidar,
|
||||||
|
imu=args.imu,
|
||||||
|
summary=args.summary,
|
||||||
|
output=args.output,
|
||||||
|
min_rotation_deg=args.min_pair_rotation_deg,
|
||||||
|
min_translation_m=args.min_pair_translation_m,
|
||||||
|
)
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -1,322 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package.
|
|
||||||
|
|
||||||
Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw ``.rscap`` in,
|
|
||||||
calibration-ready intermediate out. Downstream prepare/solve consume ``combined/``
|
|
||||||
only (``manifest.csv`` + associated frame NPZs).
|
|
||||||
|
|
||||||
Expected raw layout:
|
|
||||||
|
|
||||||
stations/
|
|
||||||
001/h32.rscap
|
|
||||||
002/h32.rscap
|
|
||||||
...
|
|
||||||
captures/ (paths passed explicitly)
|
|
||||||
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
|
|
||||||
imu.rscap # N300 (associated only; not used in AX=XB)
|
|
||||||
|
|
||||||
Output under ``--out``:
|
|
||||||
|
|
||||||
export/<station>/frames/*.npz # internal LiDAR frames
|
|
||||||
parsed/rtk.jsonl, imu.jsonl
|
|
||||||
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
|
||||||
export_summary.json
|
|
||||||
|
|
||||||
Legacy dlog stations (``dobject`` + ``dobject_recording``) are still accepted;
|
|
||||||
use ``--time-basis host`` for those datasets.
|
|
||||||
|
|
||||||
Raw ``.rscap`` / dlog files are never modified.
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
import shutil
|
|
||||||
import subprocess
|
|
||||||
import sys
|
|
||||||
from pathlib import Path
|
|
||||||
from typing import Any
|
|
||||||
|
|
||||||
ROOT = Path(__file__).resolve().parent
|
|
||||||
REPO = ROOT.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 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 pipeline_common_corrected import ( # noqa: E402
|
|
||||||
parse_imu_capture,
|
|
||||||
parse_rtk_capture,
|
|
||||||
write_json,
|
|
||||||
write_jsonl,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def is_h32_station(station: Path, capture_name: str) -> bool:
|
|
||||||
try:
|
|
||||||
resolve_lidar_rscap(station, capture_name)
|
|
||||||
return True
|
|
||||||
except FileNotFoundError:
|
|
||||||
return False
|
|
||||||
|
|
||||||
|
|
||||||
def is_dlog_station(station: Path) -> bool:
|
|
||||||
return (station / "dobject").is_dir() and (station / "dobject_recording").is_dir()
|
|
||||||
|
|
||||||
|
|
||||||
def discover_stations(stations_root: Path, names: list[str], capture_name: str) -> list[Path]:
|
|
||||||
if names:
|
|
||||||
stations = [stations_root / name for name in names]
|
|
||||||
missing = [str(path) for path in stations if not path.is_dir()]
|
|
||||||
if missing:
|
|
||||||
raise FileNotFoundError(f"station directories missing: {missing}")
|
|
||||||
return stations
|
|
||||||
stations = sorted(
|
|
||||||
[
|
|
||||||
path
|
|
||||||
for path in stations_root.iterdir()
|
|
||||||
if path.is_dir() and (is_h32_station(path, capture_name) or is_dlog_station(path))
|
|
||||||
],
|
|
||||||
key=lambda path: path.name,
|
|
||||||
)
|
|
||||||
if not stations:
|
|
||||||
raise FileNotFoundError(
|
|
||||||
f"no station with {capture_name}/lidar.rscap or dobject+dobject_recording under {stations_root}"
|
|
||||||
)
|
|
||||||
return stations
|
|
||||||
|
|
||||||
|
|
||||||
def export_legacy_dlog_station(
|
|
||||||
station: Path,
|
|
||||||
out: Path,
|
|
||||||
*,
|
|
||||||
lidar_object: str,
|
|
||||||
timezone: str,
|
|
||||||
stride: int,
|
|
||||||
) -> None:
|
|
||||||
exporter = ROOT / "frontlidar_dlog_export.py"
|
|
||||||
command = [
|
|
||||||
sys.executable,
|
|
||||||
str(exporter),
|
|
||||||
"--dlog",
|
|
||||||
str(station),
|
|
||||||
"--out",
|
|
||||||
str(out),
|
|
||||||
"--object",
|
|
||||||
lidar_object,
|
|
||||||
"--format",
|
|
||||||
"npz",
|
|
||||||
"--timezone",
|
|
||||||
timezone,
|
|
||||||
"--stride",
|
|
||||||
str(stride),
|
|
||||||
"--compress",
|
|
||||||
"--skip-rtk",
|
|
||||||
"--write-reports",
|
|
||||||
"--resume",
|
|
||||||
]
|
|
||||||
completed = subprocess.run(command, check=False)
|
|
||||||
if completed.returncode != 0:
|
|
||||||
raise RuntimeError(f"legacy dlog export failed for {station} (exit {completed.returncode})")
|
|
||||||
|
|
||||||
|
|
||||||
def parse_serial(rtk_rscap: Path, imu_rscap: Path, parsed_root: Path) -> dict[str, Any]:
|
|
||||||
parsed_root.mkdir(parents=True, exist_ok=True)
|
|
||||||
rtk_capture = read_capture(rtk_rscap)
|
|
||||||
imu_capture = read_capture(imu_rscap)
|
|
||||||
rtk_rows = parse_rtk_capture(rtk_capture)
|
|
||||||
imu_rows = parse_imu_capture(imu_capture)
|
|
||||||
write_jsonl(parsed_root / "rtk.jsonl", rtk_rows)
|
|
||||||
write_jsonl(parsed_root / "imu.jsonl", imu_rows)
|
|
||||||
summary = {
|
|
||||||
"rtk_capture": file_summary(rtk_capture),
|
|
||||||
"imu_capture": file_summary(imu_capture),
|
|
||||||
"rtk_records": len(rtk_rows),
|
|
||||||
"rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows),
|
|
||||||
"rtk_pvtslna": sum(row.get("type") == "PVTSLNA" and row.get("checksum_valid") for row in rtk_rows),
|
|
||||||
"rtk_gga": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in rtk_rows),
|
|
||||||
"rtk_heading_valid": sum(row.get("type") == "UNIHEADINGA" and row.get("heading_valid") for row in rtk_rows),
|
|
||||||
"imu_frames": len(imu_rows),
|
|
||||||
"imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows),
|
|
||||||
"imu_types": sorted({str(row.get("type")) for row in imu_rows}),
|
|
||||||
}
|
|
||||||
write_json(parsed_root / "parse_summary.json", summary)
|
|
||||||
return summary
|
|
||||||
|
|
||||||
|
|
||||||
def export_raw_to_combined(
|
|
||||||
*,
|
|
||||||
stations_root: Path,
|
|
||||||
rtk_rscap: Path,
|
|
||||||
imu_rscap: Path,
|
|
||||||
out: Path,
|
|
||||||
station_names: list[str] | None = None,
|
|
||||||
lidar_capture_name: str = "h32.rscap",
|
|
||||||
lidar_object: str = "frontlidar",
|
|
||||||
timezone: str = "+08:00",
|
|
||||||
stride: int = 1,
|
|
||||||
rtk_max_dt_ms: float = 150.0,
|
|
||||||
imu_before_ms: float = 100.0,
|
|
||||||
imu_after_ms: float = 100.0,
|
|
||||||
time_basis: str = "device_gnss",
|
|
||||||
overwrite: bool = False,
|
|
||||||
) -> dict[str, Any]:
|
|
||||||
"""Full raw → combined export. Returns ``export_summary`` dict."""
|
|
||||||
|
|
||||||
if not stations_root.is_dir():
|
|
||||||
raise FileNotFoundError(f"stations root does not exist: {stations_root}")
|
|
||||||
if not rtk_rscap.is_file():
|
|
||||||
raise FileNotFoundError(f"RTK capture missing: {rtk_rscap}")
|
|
||||||
if not imu_rscap.is_file():
|
|
||||||
raise FileNotFoundError(f"IMU capture missing: {imu_rscap}")
|
|
||||||
if out.exists() and any(out.iterdir()) and not overwrite:
|
|
||||||
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
|
||||||
if overwrite and out.exists():
|
|
||||||
# Keep out root but clear known children so rebuild is deterministic.
|
|
||||||
for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
|
|
||||||
target = out / child
|
|
||||||
if target.is_dir():
|
|
||||||
shutil.rmtree(target)
|
|
||||||
elif target.is_file():
|
|
||||||
target.unlink()
|
|
||||||
|
|
||||||
out.mkdir(parents=True, exist_ok=True)
|
|
||||||
export_root = out / "export"
|
|
||||||
parsed_root = out / "parsed"
|
|
||||||
combined_root = out / "combined"
|
|
||||||
|
|
||||||
stations = discover_stations(stations_root, station_names or [], lidar_capture_name)
|
|
||||||
parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root)
|
|
||||||
|
|
||||||
station_meta: list[dict[str, Any]] = []
|
|
||||||
lidar_segments: list[tuple[str, Path]] = []
|
|
||||||
saw_dlog = False
|
|
||||||
for station in stations:
|
|
||||||
station_out = export_root / station.name
|
|
||||||
if is_h32_station(station, lidar_capture_name):
|
|
||||||
meta = export_station_h32(
|
|
||||||
station,
|
|
||||||
station_out,
|
|
||||||
capture_name=lidar_capture_name,
|
|
||||||
stride=stride,
|
|
||||||
write_reports=True,
|
|
||||||
resume=False,
|
|
||||||
)
|
|
||||||
kind = "h32_rscap"
|
|
||||||
elif is_dlog_station(station):
|
|
||||||
saw_dlog = True
|
|
||||||
export_legacy_dlog_station(
|
|
||||||
station,
|
|
||||||
station_out,
|
|
||||||
lidar_object=lidar_object,
|
|
||||||
timezone=timezone,
|
|
||||||
stride=stride,
|
|
||||||
)
|
|
||||||
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
|
||||||
kind = "legacy_dlog"
|
|
||||||
else:
|
|
||||||
raise RuntimeError(f"station {station.name} has neither H32 .rscap nor dlog layout")
|
|
||||||
frames_dir = station_out / "frames"
|
|
||||||
if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")):
|
|
||||||
raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}")
|
|
||||||
lidar_segments.append((station.name, frames_dir))
|
|
||||||
station_meta.append({"station": station.name, "kind": kind, "frames_dir": str(frames_dir), **meta})
|
|
||||||
|
|
||||||
if saw_dlog and time_basis == "device_gnss":
|
|
||||||
print(
|
|
||||||
"[warn] legacy dlog stations use host/DObject time; prefer --time-basis host",
|
|
||||||
file=sys.stderr,
|
|
||||||
)
|
|
||||||
|
|
||||||
combined_summary = build_combined(
|
|
||||||
lidar_segments,
|
|
||||||
[parsed_root / "rtk.jsonl"],
|
|
||||||
[parsed_root / "imu.jsonl"],
|
|
||||||
combined_root,
|
|
||||||
rtk_max_dt_ms=rtk_max_dt_ms,
|
|
||||||
imu_before_ms=imu_before_ms,
|
|
||||||
imu_after_ms=imu_after_ms,
|
|
||||||
time_basis=time_basis,
|
|
||||||
overwrite=True,
|
|
||||||
)
|
|
||||||
|
|
||||||
summary = {
|
|
||||||
"role": "RTK-LiDAR one-shot raw export (like Lidar-IMU export_rscap_to_v1)",
|
|
||||||
"stations_root": str(stations_root.resolve()),
|
|
||||||
"rtk_rscap": str(rtk_rscap.resolve()),
|
|
||||||
"imu_rscap": str(imu_rscap.resolve()),
|
|
||||||
"out": str(out.resolve()),
|
|
||||||
"station_count": len(stations),
|
|
||||||
"stations": station_meta,
|
|
||||||
"parsed": parse_summary,
|
|
||||||
"combined": combined_summary,
|
|
||||||
"outputs": {
|
|
||||||
"combined": str(combined_root.resolve()),
|
|
||||||
"manifest": str((combined_root / "manifest.csv").resolve()),
|
|
||||||
"parsed": str(parsed_root.resolve()),
|
|
||||||
"export": str(export_root.resolve()),
|
|
||||||
},
|
|
||||||
"timestamp_policy": {
|
|
||||||
"default_time_basis": time_basis,
|
|
||||||
"lidar_h32": "MSOP device timestamp → unix_time_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",
|
|
||||||
"host_utc": "kept for audit; not the default calibration timeline for new captures",
|
|
||||||
},
|
|
||||||
"next_step": "run/run_direct_rtk_lidar.ps1 -CombinedRoot <out>/combined ...",
|
|
||||||
}
|
|
||||||
(out / "export_summary.json").write_text(
|
|
||||||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n",
|
|
||||||
encoding="utf-8",
|
|
||||||
)
|
|
||||||
return summary
|
|
||||||
|
|
||||||
|
|
||||||
def parse_args() -> argparse.Namespace:
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
|
||||||
parser.add_argument("--stations-root", type=Path, required=True, help="Directory of per-station folders")
|
|
||||||
parser.add_argument("--rtk-rscap", type=Path, required=True, help="Continuous G90/RTK 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("--station", action="append", default=[], help="Optional station name filter; repeatable")
|
|
||||||
parser.add_argument("--lidar-capture-name", default="h32.rscap")
|
|
||||||
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy dlog DObject name")
|
|
||||||
parser.add_argument("--timezone", default="+08:00", help="Legacy dlog tick timezone")
|
|
||||||
parser.add_argument("--stride", type=int, default=1)
|
|
||||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
|
||||||
parser.add_argument("--imu-before-ms", type=float, default=100.0)
|
|
||||||
parser.add_argument("--imu-after-ms", type=float, default=100.0)
|
|
||||||
parser.add_argument("--time-basis", choices=("device_gnss", "host"), default="device_gnss")
|
|
||||||
parser.add_argument("--overwrite", action="store_true")
|
|
||||||
return parser.parse_args()
|
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
|
||||||
args = parse_args()
|
|
||||||
if args.stride < 1:
|
|
||||||
raise SystemExit("stride must be >= 1")
|
|
||||||
summary = export_raw_to_combined(
|
|
||||||
stations_root=args.stations_root,
|
|
||||||
rtk_rscap=args.rtk_rscap,
|
|
||||||
imu_rscap=args.imu_rscap,
|
|
||||||
out=args.out,
|
|
||||||
station_names=args.station,
|
|
||||||
lidar_capture_name=args.lidar_capture_name,
|
|
||||||
lidar_object=args.lidar_object,
|
|
||||||
timezone=args.timezone,
|
|
||||||
stride=args.stride,
|
|
||||||
rtk_max_dt_ms=args.rtk_max_dt_ms,
|
|
||||||
imu_before_ms=args.imu_before_ms,
|
|
||||||
imu_after_ms=args.imu_after_ms,
|
|
||||||
time_basis=args.time_basis,
|
|
||||||
overwrite=args.overwrite,
|
|
||||||
)
|
|
||||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
|
||||||
print(f"\nCombined package ready: {summary['outputs']['combined']}")
|
|
||||||
return 0
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
raise SystemExit(main())
|
|
||||||
@@ -0,0 +1,430 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Export IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
|
||||||
|
|
||||||
|
IMU sources:
|
||||||
|
- ``--imu-kind hi13`` (HI13R4 / HI91) or ``n300`` or ``auto``
|
||||||
|
- one or more ``--imu-rscap`` files (concatenated)
|
||||||
|
|
||||||
|
LiDAR sources (exactly one):
|
||||||
|
- ``--lidar-dlog``: Medulla dlog dir **or recovered zip** (MSOP + DIFOP)
|
||||||
|
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap``
|
||||||
|
|
||||||
|
Optional host-time window (local wall clock, DateTime.Now.Ticks convention):
|
||||||
|
- ``--host-start`` / ``--host-end`` e.g. ``2026-08-08T17:40:05``
|
||||||
|
|
||||||
|
Output under ``--out``:
|
||||||
|
|
||||||
|
imu.csv
|
||||||
|
lidar/
|
||||||
|
frames_index.csv
|
||||||
|
frames/frame_XXXXX.npz
|
||||||
|
export_summary.json
|
||||||
|
|
||||||
|
Device times stay in ``t`` / ``t_start``/``t_end``. Host UTC receive times are
|
||||||
|
also written so LiDAR–IMU alignment can bridge clocks without forcing first-frame
|
||||||
|
device coincidence.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import csv
|
||||||
|
import json
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
ROOT = Path(__file__).resolve().parents[1]
|
||||||
|
if str(ROOT) not in sys.path:
|
||||||
|
sys.path.insert(0, str(ROOT))
|
||||||
|
|
||||||
|
from tools.h32_dlog.load_session import load_h32_dlog_lidar
|
||||||
|
from tools.h32_dlog.timeutil import (
|
||||||
|
local_wall_to_dotnet_ticks,
|
||||||
|
local_wall_to_utc_dotnet_ticks,
|
||||||
|
utc_dotnet_ticks_to_unix_s,
|
||||||
|
)
|
||||||
|
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||||
|
from tools.rscap_v2.h32_msop import iter_h32_frames, iter_h32_frames_from_packets
|
||||||
|
from tools.rscap_v2.hi13_imu import iter_hi13_imu_samples
|
||||||
|
from tools.rscap_v2.n300_imu import ImuSample, iter_n300_imu_samples, samples_to_arrays
|
||||||
|
|
||||||
|
|
||||||
|
def write_imu_csv(path: Path, samples: list[ImuSample]) -> None:
|
||||||
|
path.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
with path.open("w", newline="", encoding="utf-8") as handle:
|
||||||
|
writer = csv.writer(handle)
|
||||||
|
writer.writerow(
|
||||||
|
[
|
||||||
|
"t",
|
||||||
|
"gx",
|
||||||
|
"gy",
|
||||||
|
"gz",
|
||||||
|
"ax",
|
||||||
|
"ay",
|
||||||
|
"az",
|
||||||
|
"t_host_utc_s",
|
||||||
|
"receive_utc_ticks",
|
||||||
|
]
|
||||||
|
)
|
||||||
|
for sample in samples:
|
||||||
|
ticks = int(sample.host_receive_utc_ticks)
|
||||||
|
t_host = utc_dotnet_ticks_to_unix_s(ticks) if ticks > 0 else float("nan")
|
||||||
|
writer.writerow(
|
||||||
|
[
|
||||||
|
f"{sample.t_s:.9f}",
|
||||||
|
f"{sample.gyro_rad_s[0]:.12g}",
|
||||||
|
f"{sample.gyro_rad_s[1]:.12g}",
|
||||||
|
f"{sample.gyro_rad_s[2]:.12g}",
|
||||||
|
f"{sample.accel_m_s2[0]:.12g}",
|
||||||
|
f"{sample.accel_m_s2[1]:.12g}",
|
||||||
|
f"{sample.accel_m_s2[2]:.12g}",
|
||||||
|
f"{t_host:.9f}" if ticks > 0 else "",
|
||||||
|
ticks,
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def write_lidar_session(root: Path, frames) -> dict:
|
||||||
|
frames_dir = root / "frames"
|
||||||
|
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||||
|
index_path = root / "frames_index.csv"
|
||||||
|
with index_path.open("w", newline="", encoding="utf-8") as handle:
|
||||||
|
writer = csv.writer(handle)
|
||||||
|
writer.writerow(
|
||||||
|
[
|
||||||
|
"frame_id",
|
||||||
|
"filename",
|
||||||
|
"t_start",
|
||||||
|
"t_end",
|
||||||
|
"host_receive_utc_ticks",
|
||||||
|
"t_host_utc_s",
|
||||||
|
"host_receive_utc_end_ticks",
|
||||||
|
"t_host_utc_end_s",
|
||||||
|
]
|
||||||
|
)
|
||||||
|
point_counts = []
|
||||||
|
host_ok = 0
|
||||||
|
for index, frame in enumerate(frames):
|
||||||
|
rel = f"frames/frame_{index:05d}.npz"
|
||||||
|
np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32))
|
||||||
|
h0 = int(getattr(frame, "host_receive_utc_ticks_start", 0) or 0)
|
||||||
|
h1 = int(getattr(frame, "host_receive_utc_ticks_end", 0) or 0)
|
||||||
|
t_host0 = utc_dotnet_ticks_to_unix_s(h0) if h0 > 0 else float("nan")
|
||||||
|
t_host1 = utc_dotnet_ticks_to_unix_s(h1) if h1 > 0 else float("nan")
|
||||||
|
if h0 > 0:
|
||||||
|
host_ok += 1
|
||||||
|
writer.writerow(
|
||||||
|
[
|
||||||
|
index,
|
||||||
|
rel,
|
||||||
|
f"{frame.t_start_s:.9f}",
|
||||||
|
f"{frame.t_end_s:.9f}",
|
||||||
|
h0,
|
||||||
|
f"{t_host0:.9f}" if h0 > 0 else "",
|
||||||
|
h1,
|
||||||
|
f"{t_host1:.9f}" if h1 > 0 else "",
|
||||||
|
]
|
||||||
|
)
|
||||||
|
point_counts.append(int(frame.points_xyz.shape[0]))
|
||||||
|
return {
|
||||||
|
"frames": len(frames),
|
||||||
|
"frames_with_host_utc": host_ok,
|
||||||
|
"points_min": int(min(point_counts)) if point_counts else 0,
|
||||||
|
"points_max": int(max(point_counts)) if point_counts else 0,
|
||||||
|
"points_mean": float(np.mean(point_counts)) if point_counts else 0.0,
|
||||||
|
"t_start": float(frames[0].t_start_s) if frames else None,
|
||||||
|
"t_end": float(frames[-1].t_end_s) if frames else None,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def detect_imu_kind(paths: list[Path], explicit: str) -> str:
|
||||||
|
if explicit != "auto":
|
||||||
|
return explicit
|
||||||
|
joined = " ".join(path.name.lower() for path in paths)
|
||||||
|
if "hi13" in joined or "hipnuc" in joined:
|
||||||
|
return "hi13"
|
||||||
|
if "n300" in joined or "wheeltec" in joined:
|
||||||
|
return "n300"
|
||||||
|
return "hi13"
|
||||||
|
|
||||||
|
|
||||||
|
def load_imu_samples(
|
||||||
|
paths: list[Path],
|
||||||
|
*,
|
||||||
|
kind: str,
|
||||||
|
host_ticks_min: int | None,
|
||||||
|
host_ticks_max: int | None,
|
||||||
|
) -> tuple[list[ImuSample], list[dict], str]:
|
||||||
|
samples: list[ImuSample] = []
|
||||||
|
captures_meta: list[dict] = []
|
||||||
|
for path in paths:
|
||||||
|
capture = read_capture(path)
|
||||||
|
captures_meta.append(file_summary(capture))
|
||||||
|
if kind == "hi13":
|
||||||
|
part = iter_hi13_imu_samples(
|
||||||
|
capture,
|
||||||
|
host_utc_ticks_min=host_ticks_min,
|
||||||
|
host_utc_ticks_max=host_ticks_max,
|
||||||
|
)
|
||||||
|
elif kind == "n300":
|
||||||
|
part = iter_n300_imu_samples(capture)
|
||||||
|
if host_ticks_min is not None or host_ticks_max is not None:
|
||||||
|
part = [
|
||||||
|
sample
|
||||||
|
for sample in part
|
||||||
|
if (host_ticks_min is None or sample.host_receive_utc_ticks >= host_ticks_min)
|
||||||
|
and (host_ticks_max is None or sample.host_receive_utc_ticks <= host_ticks_max)
|
||||||
|
]
|
||||||
|
else:
|
||||||
|
raise ValueError(f"unsupported imu kind: {kind}")
|
||||||
|
samples.extend(part)
|
||||||
|
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
||||||
|
return samples, captures_meta, kind
|
||||||
|
|
||||||
|
|
||||||
|
def export_session(
|
||||||
|
*,
|
||||||
|
imu_rscap: list[Path] | Path,
|
||||||
|
out: Path,
|
||||||
|
lidar_rscap: Path | None = None,
|
||||||
|
lidar_dlog: Path | None = None,
|
||||||
|
imu_kind: str = "auto",
|
||||||
|
msop_object: str = "frontlidar-msop-raw",
|
||||||
|
difop_object: str = "frontlidar-difop-raw",
|
||||||
|
require_difop: bool = False,
|
||||||
|
host_start: str | None = None,
|
||||||
|
host_end: str | None = None,
|
||||||
|
frame_stride: int = 1,
|
||||||
|
max_points_per_frame: int | None = 80000,
|
||||||
|
min_range_m: float = 0.3,
|
||||||
|
max_range_m: float = 120.0,
|
||||||
|
min_frame_points: int = 100,
|
||||||
|
) -> dict:
|
||||||
|
if (lidar_rscap is None) == (lidar_dlog is None):
|
||||||
|
raise ValueError("provide exactly one of lidar_rscap or lidar_dlog")
|
||||||
|
|
||||||
|
imu_paths = [imu_rscap] if isinstance(imu_rscap, Path) else list(imu_rscap)
|
||||||
|
if not imu_paths:
|
||||||
|
raise ValueError("at least one --imu-rscap is required")
|
||||||
|
|
||||||
|
# LiDAR DObject tic uses DateTime.Now; IMU/MSOP host fields use UTC.
|
||||||
|
lidar_ticks_min = local_wall_to_dotnet_ticks(host_start) if host_start else None
|
||||||
|
lidar_ticks_max = local_wall_to_dotnet_ticks(host_end) if host_end else None
|
||||||
|
imu_ticks_min = local_wall_to_utc_dotnet_ticks(host_start) if host_start else None
|
||||||
|
imu_ticks_max = local_wall_to_utc_dotnet_ticks(host_end) if host_end else None
|
||||||
|
kind = detect_imu_kind(imu_paths, imu_kind)
|
||||||
|
|
||||||
|
out.mkdir(parents=True, exist_ok=True)
|
||||||
|
samples, imu_captures, kind = load_imu_samples(
|
||||||
|
imu_paths,
|
||||||
|
kind=kind,
|
||||||
|
host_ticks_min=imu_ticks_min,
|
||||||
|
host_ticks_max=imu_ticks_max,
|
||||||
|
)
|
||||||
|
t, _gyro, _accel = samples_to_arrays(samples)
|
||||||
|
imu_csv = out / "imu.csv"
|
||||||
|
write_imu_csv(imu_csv, samples)
|
||||||
|
imu_host_ok = sum(1 for sample in samples if sample.host_receive_utc_ticks > 0)
|
||||||
|
|
||||||
|
if lidar_dlog is not None:
|
||||||
|
session = load_h32_dlog_lidar(
|
||||||
|
lidar_dlog,
|
||||||
|
msop_object=msop_object,
|
||||||
|
difop_object=difop_object,
|
||||||
|
require_difop=require_difop,
|
||||||
|
host_ticks_min=lidar_ticks_min,
|
||||||
|
host_ticks_max=lidar_ticks_max,
|
||||||
|
)
|
||||||
|
frames = iter_h32_frames_from_packets(
|
||||||
|
session.msop_packets,
|
||||||
|
host_utc_ticks=session.msop_host_utc_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=session.vertical_deg,
|
||||||
|
horizontal_deg=session.horizontal_deg,
|
||||||
|
)
|
||||||
|
lidar_meta = {
|
||||||
|
"source": "dlog",
|
||||||
|
"lidar_dlog": session.dlog_root,
|
||||||
|
"msop_object": session.msop_object,
|
||||||
|
"difop_object": session.difop_object,
|
||||||
|
"msop_packets": len(session.msop_packets),
|
||||||
|
"msop_packets_with_host_utc": sum(1 for ticks in session.msop_host_utc_ticks if ticks > 0),
|
||||||
|
"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,
|
||||||
|
"timestamp_note": (
|
||||||
|
"device: h32_msop_device_timestamp -> seconds; "
|
||||||
|
"host: MSOP HostReceiveUtcTicks -> unix seconds"
|
||||||
|
),
|
||||||
|
}
|
||||||
|
else:
|
||||||
|
assert lidar_rscap is not None
|
||||||
|
lidar_capture = read_capture(lidar_rscap)
|
||||||
|
frames = iter_h32_frames(
|
||||||
|
lidar_capture,
|
||||||
|
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,
|
||||||
|
)
|
||||||
|
lidar_meta = {
|
||||||
|
"source": "rscap_v2",
|
||||||
|
"lidar_rscap": str(lidar_rscap),
|
||||||
|
"capture": file_summary(lidar_capture),
|
||||||
|
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
|
||||||
|
"timestamp_note": (
|
||||||
|
"device: h32_msop_device_timestamp_ms -> seconds; "
|
||||||
|
"host: rscap receive_utc_ticks -> unix seconds"
|
||||||
|
),
|
||||||
|
}
|
||||||
|
|
||||||
|
lidar_dir = out / "lidar"
|
||||||
|
lidar_stats = write_lidar_session(lidar_dir, frames)
|
||||||
|
imu_time_note = (
|
||||||
|
"hi13_device_timestamp_ms -> seconds"
|
||||||
|
if kind == "hi13"
|
||||||
|
else "n300_device_timestamp_us -> seconds"
|
||||||
|
)
|
||||||
|
|
||||||
|
summary = {
|
||||||
|
"imu_rscap": [str(path) for path in imu_paths],
|
||||||
|
"imu_kind": kind,
|
||||||
|
"out": str(out),
|
||||||
|
"host_window": {
|
||||||
|
"host_start": host_start,
|
||||||
|
"host_end": host_end,
|
||||||
|
"lidar_ticks_min": lidar_ticks_min,
|
||||||
|
"lidar_ticks_max": lidar_ticks_max,
|
||||||
|
"imu_ticks_min": imu_ticks_min,
|
||||||
|
"imu_ticks_max": imu_ticks_max,
|
||||||
|
"note": "local wall cut; lidar DObject tic=DateTime.Now, IMU/MSOP host=UTC",
|
||||||
|
},
|
||||||
|
"timestamp_policy": {
|
||||||
|
"imu_device": imu_time_note,
|
||||||
|
"imu_host": "rscap receive_utc_ticks -> t_host_utc_s",
|
||||||
|
"lidar_device": "MSOP device timestamp -> t_start/t_end",
|
||||||
|
"lidar_host": "MSOP HostReceiveUtcTicks -> t_host_utc_s",
|
||||||
|
"calibration_align": "bridge via host UTC; do not force first device samples to coincide",
|
||||||
|
},
|
||||||
|
"imu": {
|
||||||
|
"samples": int(t.shape[0]),
|
||||||
|
"samples_with_host_utc": imu_host_ok,
|
||||||
|
"t_start": float(t[0]) if t.size else None,
|
||||||
|
"t_end": float(t[-1]) if t.size else None,
|
||||||
|
"captures": imu_captures,
|
||||||
|
},
|
||||||
|
"lidar": {
|
||||||
|
**lidar_stats,
|
||||||
|
"frame_stride": int(frame_stride),
|
||||||
|
"max_points_per_frame": max_points_per_frame,
|
||||||
|
**lidar_meta,
|
||||||
|
},
|
||||||
|
"outputs": {
|
||||||
|
"imu_csv": str(imu_csv),
|
||||||
|
"lidar_session": str(lidar_dir),
|
||||||
|
},
|
||||||
|
}
|
||||||
|
(out / "export_summary.json").write_text(
|
||||||
|
json.dumps(summary, indent=2, ensure_ascii=False) + "\n",
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
return summary
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument(
|
||||||
|
"--imu-rscap",
|
||||||
|
type=Path,
|
||||||
|
action="append",
|
||||||
|
required=True,
|
||||||
|
help="IMU V2 .rscap (repeatable)",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--imu-kind",
|
||||||
|
choices=("auto", "hi13", "n300"),
|
||||||
|
default="auto",
|
||||||
|
help="IMU decoder (default: auto from filename)",
|
||||||
|
)
|
||||||
|
lidar = parser.add_mutually_exclusive_group(required=True)
|
||||||
|
lidar.add_argument(
|
||||||
|
"--lidar-dlog",
|
||||||
|
type=Path,
|
||||||
|
help="H32 dlog directory or recovered zip (indices.log + data.bin)",
|
||||||
|
)
|
||||||
|
lidar.add_argument(
|
||||||
|
"--lidar-rscap",
|
||||||
|
type=Path,
|
||||||
|
help="Legacy H32 MSOP V2 .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="store_true")
|
||||||
|
parser.add_argument("--host-start", type=str, default=None, help="Local wall start, e.g. 2026-08-08T17:40:05")
|
||||||
|
parser.add_argument("--host-end", type=str, default=None, help="Local wall end, e.g. 2026-08-08T17:45:15")
|
||||||
|
parser.add_argument("--out", type=Path, required=True)
|
||||||
|
parser.add_argument("--frame-stride", type=int, default=1)
|
||||||
|
parser.add_argument("--max-points-per-frame", type=int, default=80000)
|
||||||
|
parser.add_argument("--min-range-m", type=float, default=0.3)
|
||||||
|
parser.add_argument("--max-range-m", type=float, default=120.0)
|
||||||
|
parser.add_argument("--min-frame-points", type=int, default=100)
|
||||||
|
args = parser.parse_args()
|
||||||
|
max_points = None if args.max_points_per_frame <= 0 else args.max_points_per_frame
|
||||||
|
summary = export_session(
|
||||||
|
imu_rscap=args.imu_rscap,
|
||||||
|
lidar_rscap=args.lidar_rscap,
|
||||||
|
lidar_dlog=args.lidar_dlog,
|
||||||
|
imu_kind=args.imu_kind,
|
||||||
|
msop_object=args.msop_object,
|
||||||
|
difop_object=args.difop_object,
|
||||||
|
require_difop=args.require_difop,
|
||||||
|
host_start=args.host_start,
|
||||||
|
host_end=args.host_end,
|
||||||
|
out=args.out,
|
||||||
|
frame_stride=args.frame_stride,
|
||||||
|
max_points_per_frame=max_points,
|
||||||
|
min_range_m=args.min_range_m,
|
||||||
|
max_range_m=args.max_range_m,
|
||||||
|
min_frame_points=args.min_frame_points,
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
json.dumps(
|
||||||
|
{
|
||||||
|
"imu_kind": summary["imu_kind"],
|
||||||
|
"imu_samples": summary["imu"]["samples"],
|
||||||
|
"imu_host_utc": summary["imu"]["samples_with_host_utc"],
|
||||||
|
"lidar_frames": summary["lidar"]["frames"],
|
||||||
|
"lidar_host_utc": summary["lidar"]["frames_with_host_utc"],
|
||||||
|
"lidar_source": summary["lidar"]["source"],
|
||||||
|
"angle_source": summary["lidar"]["angle_source"],
|
||||||
|
"host_window": summary["host_window"],
|
||||||
|
"imu_csv": summary["outputs"]["imu_csv"],
|
||||||
|
"lidar_session": summary["outputs"]["lidar_session"],
|
||||||
|
"export_summary": str(Path(args.out) / "export_summary.json"),
|
||||||
|
},
|
||||||
|
ensure_ascii=False,
|
||||||
|
indent=2,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
if summary["imu"]["samples"] == 0:
|
||||||
|
raise SystemExit("no valid IMU samples decoded in window")
|
||||||
|
if summary["lidar"]["frames"] == 0:
|
||||||
|
raise SystemExit("no valid H32 frames decoded in window")
|
||||||
|
if summary["lidar"]["frames_with_host_utc"] == 0:
|
||||||
|
raise SystemExit("no LiDAR frames with MSOP HostReceiveUtcTicks; cannot host-bridge align")
|
||||||
|
if summary["imu"]["samples_with_host_utc"] == 0:
|
||||||
|
raise SystemExit("no IMU samples with host receive UTC; cannot host-bridge align")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,125 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Export priority LiDAR–IMU windows from calibration_usable_20260808.
|
||||||
|
|
||||||
|
Does not push anything; writes local V1 sessions under --out-root.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
ROOT = Path(__file__).resolve().parents[1]
|
||||||
|
if str(ROOT) not in sys.path:
|
||||||
|
sys.path.insert(0, str(ROOT))
|
||||||
|
|
||||||
|
from tools.export_rscap_to_v1 import export_session
|
||||||
|
|
||||||
|
DEFAULT_DATA = Path(r"D:\data\calibration_usable_20260808")
|
||||||
|
LIDAR_ZIP = "lidar_dlog/dorec_recovered_20260808_171438_181422.zip"
|
||||||
|
IMU_MAIN = "imu_rscap/hi13r4-imu_20260808-092827.638_39783edb-e46e-4b28-a5e8-427b981c2fce.rscap"
|
||||||
|
IMU_TAIL = "imu_rscap/hi13r4-imu_20260808-101022.036_87ea5edc-cd3d-4192-809a-469fbc8cac01.rscap"
|
||||||
|
|
||||||
|
# From usable-segment chart (local wall clock).
|
||||||
|
WINDOWS = [
|
||||||
|
{
|
||||||
|
"name": "priority_174005_174515",
|
||||||
|
"host_start": "2026-08-08T17:40:05",
|
||||||
|
"host_end": "2026-08-08T17:45:15",
|
||||||
|
"imu": [IMU_MAIN],
|
||||||
|
"priority": True,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "priority_174905_175450",
|
||||||
|
"host_start": "2026-08-08T17:49:05",
|
||||||
|
"host_end": "2026-08-08T17:54:50",
|
||||||
|
"imu": [IMU_MAIN],
|
||||||
|
"priority": True,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "priority_175910_180530",
|
||||||
|
"host_start": "2026-08-08T17:59:10",
|
||||||
|
"host_end": "2026-08-08T18:05:30",
|
||||||
|
"imu": [IMU_MAIN],
|
||||||
|
"priority": True,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "usable_181035_181050",
|
||||||
|
"host_start": "2026-08-08T18:10:35",
|
||||||
|
"host_end": "2026-08-08T18:10:50",
|
||||||
|
"imu": [IMU_TAIL],
|
||||||
|
"priority": False,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "usable_181225_181300",
|
||||||
|
"host_start": "2026-08-08T18:12:25",
|
||||||
|
"host_end": "2026-08-08T18:13:00",
|
||||||
|
"imu": [IMU_TAIL],
|
||||||
|
"priority": False,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "usable_181350_181410",
|
||||||
|
"host_start": "2026-08-08T18:13:50",
|
||||||
|
"host_end": "2026-08-08T18:14:10",
|
||||||
|
"imu": [IMU_TAIL],
|
||||||
|
"priority": False,
|
||||||
|
},
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument("--data-root", type=Path, default=DEFAULT_DATA)
|
||||||
|
parser.add_argument(
|
||||||
|
"--out-root",
|
||||||
|
type=Path,
|
||||||
|
default=DEFAULT_DATA / "sessions_v1",
|
||||||
|
)
|
||||||
|
parser.add_argument("--priority-only", action="store_true", default=True)
|
||||||
|
parser.add_argument("--all-windows", action="store_true")
|
||||||
|
parser.add_argument("--frame-stride", type=int, default=5)
|
||||||
|
parser.add_argument("--max-points-per-frame", type=int, default=40000)
|
||||||
|
args = parser.parse_args()
|
||||||
|
priority_only = not args.all_windows
|
||||||
|
lidar = args.data_root / LIDAR_ZIP
|
||||||
|
if not lidar.is_file():
|
||||||
|
raise SystemExit(f"missing lidar zip: {lidar}")
|
||||||
|
|
||||||
|
selected = [w for w in WINDOWS if (not priority_only) or w["priority"]]
|
||||||
|
results = []
|
||||||
|
for window in selected:
|
||||||
|
out = args.out_root / window["name"]
|
||||||
|
imu_paths = [args.data_root / rel for rel in window["imu"]]
|
||||||
|
print(f"=== exporting {window['name']} ===", flush=True)
|
||||||
|
summary = export_session(
|
||||||
|
imu_rscap=imu_paths,
|
||||||
|
lidar_dlog=lidar,
|
||||||
|
imu_kind="hi13",
|
||||||
|
require_difop=True,
|
||||||
|
host_start=window["host_start"],
|
||||||
|
host_end=window["host_end"],
|
||||||
|
out=out,
|
||||||
|
frame_stride=args.frame_stride,
|
||||||
|
max_points_per_frame=args.max_points_per_frame,
|
||||||
|
)
|
||||||
|
brief = {
|
||||||
|
"name": window["name"],
|
||||||
|
"imu_samples": summary["imu"]["samples"],
|
||||||
|
"lidar_frames": summary["lidar"]["frames"],
|
||||||
|
"angle_source": summary["lidar"]["angle_source"],
|
||||||
|
"out": str(out),
|
||||||
|
}
|
||||||
|
results.append(brief)
|
||||||
|
print(json.dumps(brief, ensure_ascii=False, indent=2), flush=True)
|
||||||
|
|
||||||
|
manifest = args.out_root / "export_windows_manifest.json"
|
||||||
|
args.out_root.mkdir(parents=True, exist_ok=True)
|
||||||
|
manifest.write_text(json.dumps(results, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
||||||
|
print(f"manifest: {manifest}")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,114 @@
|
|||||||
|
"""Generate a tiny synthetic session for V1 smoke tests."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.contracts import ImuSeries, LidarFrame
|
||||||
|
from imu_lidar.geometry import so3_exp
|
||||||
|
from imu_lidar.imu_io import save_imu_csv
|
||||||
|
from imu_lidar.lidar_io import save_lidar_session
|
||||||
|
|
||||||
|
|
||||||
|
def _wall_cloud(rng: np.random.Generator, n: int = 800) -> np.ndarray:
|
||||||
|
yz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
|
||||||
|
wall_x = np.column_stack([np.full(n // 3, 8.0), yz[:, 0], yz[:, 1]])
|
||||||
|
xz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
|
||||||
|
wall_y = np.column_stack([xz[:, 0], np.full(n // 3, 6.0), xz[:, 1]])
|
||||||
|
xy = rng.uniform([-5, -5], [5, 5], size=(n - 2 * (n // 3), 2))
|
||||||
|
ground = np.column_stack([xy[:, 0], xy[:, 1], np.full(xy.shape[0], -1.0)])
|
||||||
|
return np.vstack([wall_x, wall_y, ground])
|
||||||
|
|
||||||
|
|
||||||
|
def generate_synthetic_session(
|
||||||
|
output_root: Path,
|
||||||
|
*,
|
||||||
|
delta_t_s: float = 0.17,
|
||||||
|
yaw_extrinsic_deg: float = 25.0,
|
||||||
|
seed: int = 0,
|
||||||
|
) -> dict[str, float]:
|
||||||
|
"""Write IMU CSV + LiDAR frames with known extrinsic rotation and time offset."""
|
||||||
|
|
||||||
|
rng = np.random.default_rng(seed)
|
||||||
|
output_root = Path(output_root)
|
||||||
|
output_root.mkdir(parents=True, exist_ok=True)
|
||||||
|
|
||||||
|
r_x = so3_exp(np.deg2rad(np.array([2.0, -1.5, yaw_extrinsic_deg])))
|
||||||
|
map_points = _wall_cloud(rng)
|
||||||
|
|
||||||
|
lidar_hz = 10.0
|
||||||
|
duration = 8.0
|
||||||
|
lidar_times = np.arange(0.0, duration, 1.0 / lidar_hz)
|
||||||
|
# Non-yaw excitation is required for unique SO(3) hand-eye observability.
|
||||||
|
yaw = 0.5 * np.sin(0.8 * lidar_times) + 0.12 * lidar_times
|
||||||
|
pitch = 0.18 * np.sin(1.3 * lidar_times + 0.4)
|
||||||
|
roll = 0.12 * np.sin(1.7 * lidar_times + 1.0)
|
||||||
|
yaw_rate = np.gradient(yaw, lidar_times)
|
||||||
|
pitch_rate = np.gradient(pitch, lidar_times)
|
||||||
|
roll_rate = np.gradient(roll, lidar_times)
|
||||||
|
|
||||||
|
frames: list[LidarFrame] = []
|
||||||
|
for index, (t, yaw_i, pitch_i, roll_i) in enumerate(zip(lidar_times, yaw, pitch, roll)):
|
||||||
|
r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
|
||||||
|
t_wl = np.array([0.4 * t, 0.05 * np.sin(0.5 * t), 0.0])
|
||||||
|
points = (map_points - t_wl) @ r_wl
|
||||||
|
points = points + rng.normal(0.0, 0.01, size=points.shape)
|
||||||
|
frames.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=str(index),
|
||||||
|
t_start_s=float(t),
|
||||||
|
t_end_s=float(t + 0.08),
|
||||||
|
points_xyz=points.astype(float),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
save_lidar_session(output_root / "lidar", frames)
|
||||||
|
|
||||||
|
imu_hz = 100.0
|
||||||
|
t_lidar_grid = np.arange(0.0, duration, 1.0 / imu_hz)
|
||||||
|
omega_lidar = np.column_stack(
|
||||||
|
[
|
||||||
|
np.interp(t_lidar_grid, lidar_times, roll_rate),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, pitch_rate),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, yaw_rate),
|
||||||
|
]
|
||||||
|
)
|
||||||
|
omega_imu = omega_lidar @ r_x.T
|
||||||
|
|
||||||
|
g_world = np.array([0.0, 0.0, 9.80665])
|
||||||
|
acc_rows = []
|
||||||
|
for yaw_i, pitch_i, roll_i in zip(
|
||||||
|
np.interp(t_lidar_grid, lidar_times, yaw),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, pitch),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, roll),
|
||||||
|
):
|
||||||
|
r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
|
||||||
|
g_in_lidar = r_wl.T @ g_world
|
||||||
|
acc_rows.append(r_x @ g_in_lidar)
|
||||||
|
acc = np.asarray(acc_rows, dtype=float)
|
||||||
|
|
||||||
|
static_t = np.arange(-1.0, 0.0, 1.0 / imu_hz)
|
||||||
|
static_gyro = np.zeros((static_t.size, 3))
|
||||||
|
static_acc = np.tile(r_x @ g_world, (static_t.size, 1))
|
||||||
|
|
||||||
|
t_imu = np.concatenate([static_t + delta_t_s, t_lidar_grid + delta_t_s])
|
||||||
|
gyro = np.vstack([static_gyro, omega_imu]) + rng.normal(0.0, 0.001, size=(t_imu.size, 3))
|
||||||
|
acc_all = np.vstack([static_acc, acc]) + rng.normal(0.0, 0.01, size=(t_imu.size, 3))
|
||||||
|
imu = ImuSeries(t_s=t_imu, gyro_rad_s=gyro, acc_m_s2=acc_all)
|
||||||
|
save_imu_csv(output_root / "imu.csv", imu)
|
||||||
|
|
||||||
|
return {
|
||||||
|
"delta_t_s": float(delta_t_s),
|
||||||
|
"yaw_extrinsic_deg": float(yaw_extrinsic_deg),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
import json
|
||||||
|
|
||||||
|
out = Path("examples/synthetic_session")
|
||||||
|
meta = generate_synthetic_session(out)
|
||||||
|
(out / "meta.json").write_text(json.dumps(meta, indent=2), encoding="utf-8")
|
||||||
|
print(f"wrote {out}")
|
||||||
|
print(meta)
|
||||||
@@ -0,0 +1,20 @@
|
|||||||
|
"""Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP."""
|
||||||
|
|
||||||
|
from .difop import parse_difop_angles
|
||||||
|
from .dobject import discover_records, iter_payloads, open_dlog_source, resolve_dlog_root
|
||||||
|
from .load_session import H32DlogLidarSession, load_h32_dlog_lidar
|
||||||
|
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||||
|
from .timeutil import local_wall_to_dotnet_ticks
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
"H32DlogLidarSession",
|
||||||
|
"discover_records",
|
||||||
|
"iter_payloads",
|
||||||
|
"load_h32_dlog_lidar",
|
||||||
|
"local_wall_to_dotnet_ticks",
|
||||||
|
"open_dlog_source",
|
||||||
|
"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,381 @@
|
|||||||
|
"""Index and read Medulla DObject recordings.
|
||||||
|
|
||||||
|
Supports:
|
||||||
|
|
||||||
|
- standard layout: ``dobject/**/*.log`` + ``dobject_recording/**/*.dorec``
|
||||||
|
- recovered layout: ``dobject/all/indices.log`` + ``dobject_recording/data.bin``
|
||||||
|
- either as an extracted directory or a zip containing those paths
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import re
|
||||||
|
import struct
|
||||||
|
import zipfile
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import BinaryIO, Iterator
|
||||||
|
|
||||||
|
|
||||||
|
RECORD_RE = re.compile(
|
||||||
|
r"^(?:\[(?P<log_time>[^]]+)\])?>?\s*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
|
||||||
|
|
||||||
|
|
||||||
|
class _ZipStoredMemberIO:
|
||||||
|
"""Random-access reader for a ZIP_STORED member via the underlying zip file.
|
||||||
|
|
||||||
|
``ZipExtFile.seek`` on multi-GB members is far too slow for per-record reads.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, zip_path: Path, member_name: str, data_offset: int, data_size: int):
|
||||||
|
self._path = zip_path
|
||||||
|
self._member_name = member_name
|
||||||
|
self._data_offset = data_offset
|
||||||
|
self._data_size = data_size
|
||||||
|
self._fh = zip_path.open("rb")
|
||||||
|
self._pos = 0
|
||||||
|
|
||||||
|
def seek(self, offset: int, whence: int = 0) -> int:
|
||||||
|
if whence == 0:
|
||||||
|
self._pos = offset
|
||||||
|
elif whence == 1:
|
||||||
|
self._pos += offset
|
||||||
|
elif whence == 2:
|
||||||
|
self._pos = self._data_size + offset
|
||||||
|
else:
|
||||||
|
raise ValueError(f"invalid whence: {whence}")
|
||||||
|
if self._pos < 0:
|
||||||
|
raise ValueError("negative seek")
|
||||||
|
return self._pos
|
||||||
|
|
||||||
|
def read(self, size: int = -1) -> bytes:
|
||||||
|
if size is None or size < 0:
|
||||||
|
size = self._data_size - self._pos
|
||||||
|
if size <= 0 or self._pos >= self._data_size:
|
||||||
|
return b""
|
||||||
|
size = min(size, self._data_size - self._pos)
|
||||||
|
self._fh.seek(self._data_offset + self._pos)
|
||||||
|
data = self._fh.read(size)
|
||||||
|
self._pos += len(data)
|
||||||
|
return data
|
||||||
|
|
||||||
|
def close(self) -> None:
|
||||||
|
self._fh.close()
|
||||||
|
|
||||||
|
|
||||||
|
def _zip_stored_member_offset(zip_path: Path, info: zipfile.ZipInfo) -> int:
|
||||||
|
if info.compress_type != zipfile.ZIP_STORED:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"member {info.filename!r} is compressed (type={info.compress_type}); "
|
||||||
|
"extract it first or store uncompressed"
|
||||||
|
)
|
||||||
|
with zip_path.open("rb") as handle:
|
||||||
|
handle.seek(info.header_offset)
|
||||||
|
header = handle.read(30)
|
||||||
|
if len(header) != 30 or header[:4] != b"PK\x03\x04":
|
||||||
|
raise RuntimeError(f"bad local zip header for {info.filename!r}")
|
||||||
|
name_len, extra_len = struct.unpack("<HH", header[26:30])
|
||||||
|
return info.header_offset + 30 + name_len + extra_len
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class DlogSource:
|
||||||
|
"""Opened dlog directory or recovered zip."""
|
||||||
|
|
||||||
|
label: str
|
||||||
|
directory: Path | None = None
|
||||||
|
zip_path: Path | None = None
|
||||||
|
_zip: zipfile.ZipFile | None = None
|
||||||
|
_log_cache: dict[str, str] | None = None
|
||||||
|
_member_offsets: dict[str, tuple[int, int]] | None = None
|
||||||
|
|
||||||
|
def close(self) -> None:
|
||||||
|
if self._zip is not None:
|
||||||
|
self._zip.close()
|
||||||
|
self._zip = None
|
||||||
|
|
||||||
|
def __enter__(self) -> "DlogSource":
|
||||||
|
return self
|
||||||
|
|
||||||
|
def __exit__(self, exc_type, exc, tb) -> None:
|
||||||
|
self.close()
|
||||||
|
|
||||||
|
def iter_log_texts(self) -> Iterator[tuple[str, str]]:
|
||||||
|
if self.zip_path is not None:
|
||||||
|
assert self._zip is not None
|
||||||
|
if self._log_cache is None:
|
||||||
|
self._log_cache = {}
|
||||||
|
names = sorted(
|
||||||
|
name
|
||||||
|
for name in self._zip.namelist()
|
||||||
|
if name.replace("\\", "/").startswith("dobject/")
|
||||||
|
and name.replace("\\", "/").endswith(".log")
|
||||||
|
)
|
||||||
|
for name in names:
|
||||||
|
key = name.replace("\\", "/")
|
||||||
|
self._log_cache[key] = self._zip.read(name).decode("utf-8", errors="replace")
|
||||||
|
for name, text in self._log_cache.items():
|
||||||
|
yield name, text
|
||||||
|
return
|
||||||
|
assert self.directory is not None
|
||||||
|
for log_path in sorted((self.directory / "dobject").rglob("*.log")):
|
||||||
|
relative = log_path.relative_to(self.directory).as_posix()
|
||||||
|
yield relative, log_path.read_text(encoding="utf-8", errors="replace")
|
||||||
|
|
||||||
|
def open_recording(self, name: str) -> tuple[object, BinaryIO]:
|
||||||
|
"""Return (owner, binary stream) supporting seek/read of one recording member."""
|
||||||
|
|
||||||
|
base = Path(name).name
|
||||||
|
if self.zip_path is not None:
|
||||||
|
assert self._zip is not None
|
||||||
|
candidates = [
|
||||||
|
n
|
||||||
|
for n in self._zip.namelist()
|
||||||
|
if Path(n.replace("\\", "/")).name.casefold() == base.casefold()
|
||||||
|
and "dobject_recording/" in n.replace("\\", "/")
|
||||||
|
]
|
||||||
|
if not candidates:
|
||||||
|
alt = name.replace("\\", "/")
|
||||||
|
if alt in self._zip.namelist():
|
||||||
|
candidates = [alt]
|
||||||
|
elif f"dobject_recording/{base}" in self._zip.namelist():
|
||||||
|
candidates = [f"dobject_recording/{base}"]
|
||||||
|
if not candidates:
|
||||||
|
raise FileNotFoundError(f"missing recording in zip: {name}")
|
||||||
|
if len(candidates) > 1:
|
||||||
|
raise RuntimeError(f"ambiguous recording in zip {name}: {candidates}")
|
||||||
|
member = candidates[0].replace("\\", "/")
|
||||||
|
if self._member_offsets is None:
|
||||||
|
self._member_offsets = {}
|
||||||
|
if member not in self._member_offsets:
|
||||||
|
info = self._zip.getinfo(member)
|
||||||
|
self._member_offsets[member] = (
|
||||||
|
_zip_stored_member_offset(self.zip_path, info),
|
||||||
|
info.file_size,
|
||||||
|
)
|
||||||
|
data_offset, data_size = self._member_offsets[member]
|
||||||
|
stream = _ZipStoredMemberIO(self.zip_path, member, data_offset, data_size)
|
||||||
|
return stream, stream
|
||||||
|
|
||||||
|
assert self.directory is not None
|
||||||
|
index = index_dorec_files(self.directory)
|
||||||
|
if base.casefold() == "data.bin":
|
||||||
|
path = self.directory / "dobject_recording" / "data.bin"
|
||||||
|
if not path.is_file():
|
||||||
|
matches = list((self.directory / "dobject_recording").rglob("data.bin"))
|
||||||
|
if not matches:
|
||||||
|
raise FileNotFoundError(f"missing recording file: {name}")
|
||||||
|
path = matches[0]
|
||||||
|
stream = path.open("rb")
|
||||||
|
return stream, stream
|
||||||
|
path = choose_dorec(index, name)
|
||||||
|
stream = path.open("rb")
|
||||||
|
return stream, stream
|
||||||
|
|
||||||
|
|
||||||
|
def open_dlog_source(value: Path | str) -> DlogSource:
|
||||||
|
path = Path(value).expanduser().resolve()
|
||||||
|
if path.is_file() and path.suffix.lower() == ".zip":
|
||||||
|
zf = zipfile.ZipFile(path, "r")
|
||||||
|
names = {n.replace("\\", "/") for n in zf.namelist()}
|
||||||
|
has_log = any(n.startswith("dobject/") and n.endswith(".log") for n in names)
|
||||||
|
has_rec = any(n.startswith("dobject_recording/") for n in names)
|
||||||
|
if not (has_log and has_rec):
|
||||||
|
zf.close()
|
||||||
|
raise FileNotFoundError(f"{path} is not a recovered/standard dlog zip")
|
||||||
|
return DlogSource(label=str(path), zip_path=path, _zip=zf)
|
||||||
|
|
||||||
|
root = path
|
||||||
|
if not ((root / "dobject").is_dir() and (root / "dobject_recording").is_dir()):
|
||||||
|
child = root / "dlog"
|
||||||
|
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
|
||||||
|
root = child
|
||||||
|
else:
|
||||||
|
raise FileNotFoundError(f"{path} does not contain dobject and dobject_recording")
|
||||||
|
return DlogSource(label=str(root), directory=root)
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_dlog_root(value: Path | str) -> Path:
|
||||||
|
"""Backward-compatible helper: directory roots only (not zip)."""
|
||||||
|
|
||||||
|
source = open_dlog_source(value)
|
||||||
|
try:
|
||||||
|
if source.directory is None:
|
||||||
|
raise FileNotFoundError(
|
||||||
|
f"{value} is a zip; use open_dlog_source()/iter_payloads_from_source()"
|
||||||
|
)
|
||||||
|
return source.directory
|
||||||
|
finally:
|
||||||
|
source.close()
|
||||||
|
|
||||||
|
|
||||||
|
def discover_records_from_source(
|
||||||
|
source: DlogSource,
|
||||||
|
object_name: str,
|
||||||
|
*,
|
||||||
|
host_ticks_min: int | None = None,
|
||||||
|
host_ticks_max: int | None = None,
|
||||||
|
) -> list[RecordRef]:
|
||||||
|
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
|
||||||
|
name_key = object_name.casefold()
|
||||||
|
for relative_log, text in source.iter_log_texts():
|
||||||
|
for line in text.splitlines():
|
||||||
|
match = RECORD_RE.search(line.strip())
|
||||||
|
if not match or match.group("name").casefold() != name_key:
|
||||||
|
continue
|
||||||
|
ticks = int(match.group("tic"))
|
||||||
|
if host_ticks_min is not None and ticks < host_ticks_min:
|
||||||
|
continue
|
||||||
|
if host_ticks_max is not None and ticks > host_ticks_max:
|
||||||
|
continue
|
||||||
|
pending.append(
|
||||||
|
(
|
||||||
|
match.group("name"),
|
||||||
|
match.group("log_time") or "",
|
||||||
|
relative_log,
|
||||||
|
int(match.group("offset")),
|
||||||
|
int(match.group("len")),
|
||||||
|
match.group("id").upper(),
|
||||||
|
ticks,
|
||||||
|
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 discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
|
||||||
|
with open_dlog_source(dlog_root) as source:
|
||||||
|
return discover_records_from_source(source, object_name)
|
||||||
|
|
||||||
|
|
||||||
|
def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
|
||||||
|
result: dict[str, list[Path]] = {}
|
||||||
|
recording = dlog_root / "dobject_recording"
|
||||||
|
if not recording.is_dir():
|
||||||
|
return result
|
||||||
|
for path in recording.rglob("*"):
|
||||||
|
if path.is_file() and path.suffix.lower() in {".dorec", ".bin"}:
|
||||||
|
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_payload_at(stream: BinaryIO, record: RecordRef) -> bytes:
|
||||||
|
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_from_source(
|
||||||
|
source: DlogSource,
|
||||||
|
object_name: str,
|
||||||
|
*,
|
||||||
|
host_ticks_min: int | None = None,
|
||||||
|
host_ticks_max: int | None = None,
|
||||||
|
) -> Iterator[tuple[RecordRef, bytes]]:
|
||||||
|
records = discover_records_from_source(
|
||||||
|
source,
|
||||||
|
object_name,
|
||||||
|
host_ticks_min=host_ticks_min,
|
||||||
|
host_ticks_max=host_ticks_max,
|
||||||
|
)
|
||||||
|
if not records:
|
||||||
|
return
|
||||||
|
open_files: dict[str, BinaryIO] = {}
|
||||||
|
try:
|
||||||
|
for record in records:
|
||||||
|
key = Path(record.source_dorec).name.casefold()
|
||||||
|
stream = open_files.get(key)
|
||||||
|
if stream is None:
|
||||||
|
_owner, stream = source.open_recording(record.source_dorec)
|
||||||
|
open_files[key] = stream
|
||||||
|
yield record, _read_payload_at(stream, record)
|
||||||
|
finally:
|
||||||
|
for stream in open_files.values():
|
||||||
|
stream.close()
|
||||||
|
|
||||||
|
|
||||||
|
def iter_payloads(
|
||||||
|
dlog_root: Path | str,
|
||||||
|
object_name: str,
|
||||||
|
*,
|
||||||
|
host_ticks_min: int | None = None,
|
||||||
|
host_ticks_max: int | None = None,
|
||||||
|
) -> Iterator[tuple[RecordRef, bytes]]:
|
||||||
|
with open_dlog_source(dlog_root) as source:
|
||||||
|
yield from iter_payloads_from_source(
|
||||||
|
source,
|
||||||
|
object_name,
|
||||||
|
host_ticks_min=host_ticks_min,
|
||||||
|
host_ticks_max=host_ticks_max,
|
||||||
|
)
|
||||||
@@ -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,140 @@
|
|||||||
|
"""Load H32 MSOP packets and DIFOP angles from a Medulla dlog session."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from tools.rscap_v2.h32_msop import default_horizontal_deg, default_vertical_deg
|
||||||
|
|
||||||
|
from .difop import DifopAngles, parse_difop_angles
|
||||||
|
from .dobject import (
|
||||||
|
discover_records_from_source,
|
||||||
|
iter_payloads_from_source,
|
||||||
|
open_dlog_source,
|
||||||
|
)
|
||||||
|
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class H32DlogLidarSession:
|
||||||
|
dlog_root: str
|
||||||
|
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
|
||||||
|
host_ticks_min: int | None = None
|
||||||
|
host_ticks_max: int | 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,
|
||||||
|
host_ticks_min: int | None = None,
|
||||||
|
host_ticks_max: int | None = None,
|
||||||
|
) -> H32DlogLidarSession:
|
||||||
|
with open_dlog_source(dlog_root) as source:
|
||||||
|
# DIFOP angles: prefer packets inside the window, else any in the capture.
|
||||||
|
angles: DifopAngles | None = None
|
||||||
|
difop_count = 0
|
||||||
|
session_id: str | None = None
|
||||||
|
lidar_ip: str | None = None
|
||||||
|
for _record, payload in iter_payloads_from_source(
|
||||||
|
source,
|
||||||
|
difop_object,
|
||||||
|
host_ticks_min=host_ticks_min,
|
||||||
|
host_ticks_max=host_ticks_max,
|
||||||
|
):
|
||||||
|
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 angles is None:
|
||||||
|
for _record, payload in iter_payloads_from_source(source, 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 angles is not None:
|
||||||
|
break
|
||||||
|
|
||||||
|
msop_packets: list[bytes] = []
|
||||||
|
msop_host_utc_ticks: list[int] = []
|
||||||
|
batch_count = 0
|
||||||
|
for record, payload in iter_payloads_from_source(
|
||||||
|
source,
|
||||||
|
msop_object,
|
||||||
|
host_ticks_min=host_ticks_min,
|
||||||
|
host_ticks_max=host_ticks_max,
|
||||||
|
):
|
||||||
|
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)
|
||||||
|
# Per-packet UTC host receive from MSOP DLog payload only.
|
||||||
|
# Do NOT fall back to DObject tic (DateTime.Now / local).
|
||||||
|
msop_host_utc_ticks.append(int(item.host_receive_utc_ticks))
|
||||||
|
|
||||||
|
if not msop_packets:
|
||||||
|
msop_records = discover_records_from_source(source, msop_object)
|
||||||
|
raise RuntimeError(
|
||||||
|
f"no MSOP packets from DObject {msop_object!r} under {source.label} "
|
||||||
|
f"(log records={len(msop_records)}, "
|
||||||
|
f"host_ticks=[{host_ticks_min}, {host_ticks_max}])"
|
||||||
|
)
|
||||||
|
|
||||||
|
if angles is None:
|
||||||
|
if require_difop:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"no valid DIFOP calibration from DObject {difop_object!r} under {source.label}"
|
||||||
|
)
|
||||||
|
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=source.label,
|
||||||
|
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,
|
||||||
|
host_ticks_min=host_ticks_min,
|
||||||
|
host_ticks_max=host_ticks_max,
|
||||||
|
)
|
||||||
@@ -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()
|
||||||
@@ -0,0 +1,56 @@
|
|||||||
|
"""Wall-clock helpers for Medulla tick filtering.
|
||||||
|
|
||||||
|
Two tick conventions appear in this dataset:
|
||||||
|
|
||||||
|
- LiDAR DObject ``tic`` / recovered ``indices.log``: ``DateTime.Now.Ticks`` (local)
|
||||||
|
- IMU / MSOP payload host receive fields: UTC ``DateTime.UtcNow.Ticks``
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from datetime import datetime, timedelta, timezone
|
||||||
|
|
||||||
|
TICKS_PER_SECOND = 10_000_000
|
||||||
|
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||||
|
|
||||||
|
|
||||||
|
def _parse_local_wall(text: str) -> datetime:
|
||||||
|
normalized = text.strip().replace(" ", "T")
|
||||||
|
if normalized.endswith("Z"):
|
||||||
|
raise ValueError("expected local wall time without Z; got UTC marker")
|
||||||
|
if "+" in normalized[10:]:
|
||||||
|
idx = normalized.find("+", 10)
|
||||||
|
normalized = normalized[:idx]
|
||||||
|
elif normalized.count("-") > 2:
|
||||||
|
# timezone like -08:00 after the date
|
||||||
|
idx = normalized.find("-", 10)
|
||||||
|
if idx > 0 and ":" in normalized[idx + 1 :]:
|
||||||
|
normalized = normalized[:idx]
|
||||||
|
return datetime.fromisoformat(normalized).replace(tzinfo=None)
|
||||||
|
|
||||||
|
|
||||||
|
def local_wall_to_dotnet_ticks(text: str) -> int:
|
||||||
|
"""Local wall time → ``DateTime.Now.Ticks`` (LiDAR DObject tic)."""
|
||||||
|
|
||||||
|
dt = _parse_local_wall(text)
|
||||||
|
delta = dt - datetime(1, 1, 1)
|
||||||
|
return int(delta.total_seconds() * TICKS_PER_SECOND)
|
||||||
|
|
||||||
|
|
||||||
|
def local_wall_to_utc_dotnet_ticks(text: str, *, tz_hours: float = 8.0) -> int:
|
||||||
|
"""Local wall time in ``tz_hours`` → UTC ``DateTime.UtcNow.Ticks`` (IMU host)."""
|
||||||
|
|
||||||
|
dt = _parse_local_wall(text).replace(tzinfo=timezone(timedelta(hours=tz_hours)))
|
||||||
|
unix = dt.timestamp()
|
||||||
|
return int(round(unix * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS
|
||||||
|
|
||||||
|
|
||||||
|
def dotnet_ticks_to_local_iso(ticks: int) -> str:
|
||||||
|
dt = datetime(1, 1, 1) + timedelta(microseconds=ticks / 10.0)
|
||||||
|
return dt.isoformat(timespec="milliseconds")
|
||||||
|
|
||||||
|
|
||||||
|
def utc_dotnet_ticks_to_unix_s(ticks: int) -> float:
|
||||||
|
"""UTC ``DateTime.UtcNow.Ticks`` → Unix seconds."""
|
||||||
|
|
||||||
|
return (float(ticks) - float(DOTNET_UNIX_EPOCH_TICKS)) / float(TICKS_PER_SECOND)
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user