Compare commits
18
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 lichun.qu
|
3c8e4f92f6 | ||
 lichun.quandCursor
|
5f59bcd795 | ||
|
lichun.quandCursor
|
6242fd1081 | ||
|
lichun.quandCursor
|
3b8282353c | ||
|
lichun.quandCursor
|
e33a7a7657 | ||
|
lichun.quandCursor
|
68cb5eaf90 | ||
|
lichun.quandCursor
|
46d2fa1d69 | ||
|
lichun.quandCursor
|
69bb44bccd | ||
|
lichun.qu
|
b2271d05ba | ||
|
lichun.quandCursor
|
8477935ad2 | ||
|
lichun.quandCursor
|
13624b0be8 | ||
|
lichun.qu
|
24eaa8508e | ||
|
lichun.qu
|
e847cd4590 | ||
|
lichun.qu
|
6d87b6ba9c | ||
|
lichun.qu
|
d2aae6177e | ||
|
lichun.qu
|
f72fcb71cc | ||
|
lichun.qu
|
6b7844a977 | ||
|
lichun.qu
|
62c7ab2e98 |
+17
-22
@@ -1,29 +1,24 @@
|
||||
# Python
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.pytest_cache/
|
||||
*.egg-info/
|
||||
.eggs/
|
||||
dist/
|
||||
build/
|
||||
examples/synthetic_session/
|
||||
.venv/
|
||||
venv/
|
||||
|
||||
# Local IDE state
|
||||
.vs/
|
||||
# IDE / OS
|
||||
.idea/
|
||||
.vscode/
|
||||
.DS_Store
|
||||
Thumbs.db
|
||||
|
||||
# RTK-IMU calibration process artifacts stay local. Keep only the reviewed
|
||||
# V3 result bundle explicitly listed below under version control.
|
||||
artifacts/rtk_imu_calibration_v2/
|
||||
artifacts/rtk_imu_calibration_v3/*
|
||||
!artifacts/rtk_imu_calibration_v3/README.md
|
||||
!artifacts/rtk_imu_calibration_v3/engineering_release_decision.json
|
||||
!artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json
|
||||
!artifacts/rtk_imu_calibration_v3/heldout_nonconverged_retry.json
|
||||
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection.json
|
||||
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection_refined.json
|
||||
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json
|
||||
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json
|
||||
!artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json
|
||||
!artifacts/rtk_imu_calibration_v3/node_graph_free_information_selected_mechanical.json
|
||||
!artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json
|
||||
# Raw data and generated outputs
|
||||
data/raw/
|
||||
work/
|
||||
outputs/
|
||||
*.rscap
|
||||
*.dorec
|
||||
*.log
|
||||
|
||||
# Large generated point clouds outside the archived reference result
|
||||
**/frames/
|
||||
**/frames_all/
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"ExpandedNodes": [
|
||||
""
|
||||
],
|
||||
"PreviewInSolutionExplorer": false
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,49 @@
|
||||
{
|
||||
"Version": 1,
|
||||
"WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\",
|
||||
"Documents": [
|
||||
{
|
||||
"AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}",
|
||||
"RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}"
|
||||
}
|
||||
],
|
||||
"DocumentGroupContainers": [
|
||||
{
|
||||
"Orientation": 0,
|
||||
"VerticalTabListWidth": 256,
|
||||
"DocumentGroups": [
|
||||
{
|
||||
"DockedWidth": 200,
|
||||
"SelectedChildIndex": 3,
|
||||
"Children": [
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
|
||||
},
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:0:0:{1c4feeaa-4718-4aa9-859d-94ce25d182ba}"
|
||||
},
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}"
|
||||
},
|
||||
{
|
||||
"$type": "Document",
|
||||
"DocumentIndex": 0,
|
||||
"Title": "README.md",
|
||||
"DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeDocumentMoniker": "README.md",
|
||||
"ToolTip": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeToolTip": "README.md",
|
||||
"ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==",
|
||||
"Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|",
|
||||
"WhenOpened": "2026-07-24T03:20:21.56Z",
|
||||
"EditorCaption": ""
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
{
|
||||
"Version": 1,
|
||||
"WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\",
|
||||
"Documents": [
|
||||
{
|
||||
"AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}",
|
||||
"RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}"
|
||||
}
|
||||
],
|
||||
"DocumentGroupContainers": [
|
||||
{
|
||||
"Orientation": 0,
|
||||
"VerticalTabListWidth": 256,
|
||||
"DocumentGroups": [
|
||||
{
|
||||
"DockedWidth": 200,
|
||||
"SelectedChildIndex": 2,
|
||||
"Children": [
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}"
|
||||
},
|
||||
{
|
||||
"$type": "Bookmark",
|
||||
"Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}"
|
||||
},
|
||||
{
|
||||
"$type": "Document",
|
||||
"DocumentIndex": 0,
|
||||
"Title": "README.md",
|
||||
"DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeDocumentMoniker": "README.md",
|
||||
"ToolTip": "D:\\First-dev-dept\\calibration\\README.md",
|
||||
"RelativeToolTip": "README.md",
|
||||
"ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==",
|
||||
"Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|",
|
||||
"WhenOpened": "2026-07-24T03:20:21.56Z",
|
||||
"EditorCaption": ""
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Binary file not shown.
@@ -1,69 +1,292 @@
|
||||
# 车辆多传感器外参标定
|
||||
# 双天线RTK—3D LiDAR直接手眼标定
|
||||
|
||||
本仓库同时维护两条独立的外参标定链路：LiDAR–IMU 与 RTK–IMU。两条链路共享通用的几何和 IMU 预积分基础能力，但各自的观测模型、求解器、工具入口和验收结论独立管理。
|
||||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **车头向前的 RTK 车体系**（主天线原点）。
|
||||
求解不使用 RTK 到后轮轴的 XY 杆臂；与雷达–IMU 外参对照时旋转系一致，平移仍差天线原点。
|
||||
|
||||
## 当前应如何使用
|
||||
当前交付标定（2026-08 室外车，27 站）约定如下：
|
||||
|
||||
| 目标 | 推荐链路 | 当前可交付状态 |
|
||||
| --- | --- | --- |
|
||||
| LiDAR 与 IMU 的旋转、时间对齐 | LiDAR–IMU | 旋转与主机桥接时间对齐可冻结；完整平移不可交付。 |
|
||||
| RTK 天线相位中心与 IMU 的工程外参 | RTK–IMU | 机械杆臂经动态数据一致性验证；data-only 平移及正式 engineering 放行尚未通过。 |
|
||||
| 需要完整 RTK–IMU 工程候选和复现方式 | RTK–IMU | 先阅读 [README_RTK_IMU.md](README_RTK_IMU.md)。 |
|
||||
| 项 | 值 |
|
||||
|---|---|
|
||||
| RTK 坐标系 | **车头向前**（`HeadingOffsetDeg = -90`；主从装反、基线朝右） |
|
||||
| 天线相位中心离地高 | **1.9165 m**（1916.5 mm） |
|
||||
| 机械初值（车头系） | \(t=(+0.21086,-0.41418,+0.07850)\) m，yaw=**0°**（CAD 纵向已按车头正向取 +X） |
|
||||
| 物理基线 | `baseline_points=vehicle_right`（主天线车左，从天线车右，后轴中心左右对称） |
|
||||
| 姿态 | 双天线 pitch/roll：`R = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(+90°)` |
|
||||
| 地面点 ROI | LiDAR 系 **`z ∈ [-2.5, -1.5]`**（约 2 m 车顶安装） |
|
||||
| pair 配准 | **禁止**使用外参 seed；B 与 X 独立 |
|
||||
|
||||
不要把任何一条链路未通过的平移结果当作已标定的 6DoF 外参。结果的接受状态以对应 JSON 中的 gate 为准。
|
||||
数据下载：https://fs.fairylandtech.com:5001/FRLD/#file_id=966776353886090246
|
||||
账号：lichun.qu@fairylandtech.com 密码：lichun.qu
|
||||
|
||||
## 为什么同时保留两条链路
|
||||
---
|
||||
|
||||
LiDAR–IMU 链路通过点云相对运动与 IMU 预积分求手眼外参，适合旋转与时间对齐；但当前实车数据的加速度预积分位移，尤其 Z 分量，不足以支持可靠平移求解，且主要采集运动缺乏竖直激励。因此它的 `full_se3` 门禁未通过。
|
||||
## 1. 输出坐标约定（车头向前）
|
||||
|
||||
RTK–IMU 链路直接利用双天线基线、GNSS 位置和 Doppler 速度，并以机械测量给出天线相位中心相对 IMU 的绝对杆臂。它不是“用 RTK 替代 LiDAR”，而是针对 LiDAR–IMU 平移不可观的问题，采用更直接的天线位置/速度观测做工程外参验证。
|
||||
|
||||
完整理由、两条模型的差异及当前结果见 [LiDAR→RTK–IMU 决策说明](docs/lidar_imu_to_rtk_imu.md)。
|
||||
|
||||
## 仓库结构
|
||||
统一约定 `T_A_B` 把 B 系点变换到 A 系：
|
||||
|
||||
```text
|
||||
calibration/
|
||||
├─ imu_lidar/ LiDAR–IMU 专用流程，以及双方共用基础模块
|
||||
├─ rtk_imu/ RTK–IMU 专用算法包
|
||||
├─ tools/ 两条链路的命令入口、导出和审计工具
|
||||
├─ tests/ 自动化测试
|
||||
├─ docs/ 详细方法、状态、采集与决策说明
|
||||
├─ artifacts/ 受审核的结果快照；大过程文件仅保留本地
|
||||
├─ config/ 车辆安装与流程配置
|
||||
├─ README.md 本页：仓库总览
|
||||
└─ README_RTK_IMU.md RTK–IMU 当前工程流程、结果与复现
|
||||
p_RTK = T_RTK_lidar · p_lidar
|
||||
```
|
||||
|
||||
`rtk_imu/` 只依赖下列 `imu_lidar/` 通用模块：
|
||||
本仓库默认 RTK 导航系（**车头向前 / vehicle_forward_heading_offset**）：
|
||||
|
||||
- `contracts.py`：数据契约。
|
||||
- `geometry.py`：SO(3)、变换和坐标运算。
|
||||
- `geodesy.py`：地理坐标到 ENU。
|
||||
- `imu_io.py`：IMU 读取。
|
||||
- `imu_preintegration.py`：IMU 预积分、bias 修正与 whitening。
|
||||
- `rotation_handeye.py`：通用旋转手眼初值器。
|
||||
- 原点：GGA 位置参考点（主天线 / ANT1 相位中心）；
|
||||
- X 轴：车头向前（`rawHeading + HeadingOffsetDeg`，本车 `HeadingOffsetDeg = -90`）；
|
||||
- Y 轴：左；
|
||||
- Z 轴：上；
|
||||
- 姿态：先在基线系应用双天线 pitch/roll，再乘固定 `Rz(-heading_offset)`；不是 IMU 融合姿态。
|
||||
|
||||
RTK–IMU 不得依赖 LiDAR 专用的 `lidar_io.py`、`lidar_deskew.py`、`registration.py`、`pipeline.py`、`phase_a.py` 或 `joint_optimizer.py`。
|
||||
> 改 `HeadingOffsetDeg` 或姿态模型后必须从 **prepare** 起重跑；禁止事后只改 JSON 里的 yaw。
|
||||
> 旧基线系结果（`HeadingOffsetDeg = 0`）与车头系外参不可混用。
|
||||
|
||||
## 文档入口
|
||||
机械初值文件：[`run/rtk_lidar_mechanical_initial.json`](run/rtk_lidar_mechanical_initial.json)
|
||||
**仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。**
|
||||
|
||||
| 文档 | 内容 |
|
||||
---
|
||||
|
||||
## 2. 算法流程
|
||||
|
||||
```text
|
||||
原始雷达 + RTK（+ 可选 IMU）
|
||||
→ combined/（按站关联的多传感器 NPZ）
|
||||
→ 每站选一帧静态点云 + RTK pose（车头向前，含双天线 pitch/roll）
|
||||
→ Open3D GICP 与 small_gicp 分别求 B_ij = T_Li_Lj（无外参 seed）
|
||||
→ 留出点、正反向、旋转共轭不变量等精筛
|
||||
→ 双后端共识边 → consensus B
|
||||
→ A_ij X = X B_ij + 地面法向/高度约束 → X = T_RTK_lidar
|
||||
→ bootstrap、双后端差异、逐对残差与 3D 可视化
|
||||
```
|
||||
|
||||
```text
|
||||
A_ij = inv(T_W_Ri) · T_W_Rj = T_Ri_Rj
|
||||
B_ij = T_Li_Lj
|
||||
A_ij · X = X · B_ij
|
||||
X = T_RTK_lidar
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. 原始数据与导出
|
||||
|
||||
大体积数据不提交 Git。常见两种采集形态：
|
||||
|
||||
### 3.1 每站独立雷达目录（旧/标准站目录）
|
||||
|
||||
```text
|
||||
raw_dataset/
|
||||
├── stations/001|002|.../ # H32 dlog 或 h32.rscap
|
||||
└── captures/
|
||||
├── rtk.rscap
|
||||
└── imu.rscap # 仅关联，不参与外参求解
|
||||
```
|
||||
|
||||
```powershell
|
||||
python tools\export_raw_to_combined.py `
|
||||
--stations-root "$Raw\stations" `
|
||||
--rtk-rscap "$Raw\captures\rtk.rscap" `
|
||||
--imu-rscap "$Raw\captures\imu.rscap" `
|
||||
--out "$Out\exported" `
|
||||
--overwrite
|
||||
```
|
||||
|
||||
默认时间基：`-TimeBasis device_gnss`（雷达设备时 ↔ GNSS week/TOW）。
|
||||
|
||||
### 3.2 G90 连续录制 + H32 DLog 按站时间窗（本次 27 站）
|
||||
|
||||
站不在独立目录，而在多个 Medulla DLog ZIP 与 G90 `.rscap` 中时：
|
||||
|
||||
```powershell
|
||||
python tools\export_g90_h32_windows_to_combined.py `
|
||||
--segments-csv <rtk_lidar_station_segments.csv> `
|
||||
--lidar-dlog <dump_1.zip> --lidar-dlog <dump_2.zip> `
|
||||
--rtk-rscap <g90_1.rscap> --rtk-rscap <g90_2.rscap> `
|
||||
--out <output_root> --expected-stations 27 --frame-stride 5
|
||||
```
|
||||
|
||||
该入口用 **主机接收 UTC** 做近邻关联（`time_basis_mode: host`），并保留设备时间供审计。
|
||||
可加 `--reuse-export` 在已有 `export/` 上续跑。
|
||||
|
||||
采集建议：有效静站 ≥30（更好 40～60）；相邻站转角约 **15°～30°**；避免一长串同朝向停车；场内宜有墙/立柱及 2～3 块法向不同的固定平面板。
|
||||
|
||||
---
|
||||
|
||||
## 4. 环境安装
|
||||
|
||||
Windows + PowerShell + Python 3.11：
|
||||
|
||||
```powershell
|
||||
python -m pip install -r requirements.txt
|
||||
```
|
||||
|
||||
依赖：NumPy、SciPy、Open3D、small_gicp。完整共识需要两个配准后端。
|
||||
|
||||
---
|
||||
|
||||
## 5. 一键复现（匹配本次标定）
|
||||
|
||||
### 5.1 已有 `combined/`（推荐复现本次结果）
|
||||
|
||||
```powershell
|
||||
$Repo = (Resolve-Path ".").Path
|
||||
$Data = "D:\data\rtk_lidar_run" # 含 combined/
|
||||
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
|
||||
-CombinedRoot "$Data\combined" `
|
||||
-WorkRoot "$Data\prepared_vehicle_h19165" `
|
||||
-OutputRoot "$Data\outputs_vehicle_h19165" `
|
||||
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||
-HeadingOffsetDeg -90 `
|
||||
-ExpectedStations 27 `
|
||||
-MinStations 20 `
|
||||
-GroundZMin -2.5 `
|
||||
-GroundZMax -1.5 `
|
||||
-Bootstrap 200
|
||||
```
|
||||
|
||||
关键参数：
|
||||
|
||||
| 参数 | 本次取值 | 说明 |
|
||||
|---|---|---|
|
||||
| `-RtkReferenceHeightAboveGroundM` | **1.9165** | GGA/ANT1 相位中心离地高（m），必填 |
|
||||
| `-HeadingOffsetDeg` | **-90** | 车头向前（主从装反、基线朝右）；`0` 才是基线系 |
|
||||
| `-GroundZMin/Max` | **-2.5 / -1.5** | 约 2 m 车顶雷达；旧默认 `[-1.4,-0.4]` 会拟合到墙 |
|
||||
| `-ExpectedStations` | **27** | 本批站数 |
|
||||
| `-MinStations` | **20** | 远程旧脚本曾写死 30，会跑不了本批 |
|
||||
|
||||
pair 阶段**不会**传入 `--initial-extrinsic`；机械初值只进最终 AX=XB。
|
||||
|
||||
### 5.2 站目录原始数据一键（导出 + 求解）
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" `
|
||||
-DataRoot "$Raw\stations" `
|
||||
-RtkCapture "$Raw\captures\rtk.rscap" `
|
||||
-ImuCapture "$Raw\captures\imu.rscap" `
|
||||
-OutputRoot $Out `
|
||||
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||
-ExpectedStations 27 `
|
||||
-GroundZMin -2.5 `
|
||||
-GroundZMax -1.5
|
||||
```
|
||||
|
||||
主要输出：
|
||||
|
||||
```text
|
||||
$Out/
|
||||
├── exported/combined/ # 或外部已有 combined/
|
||||
├── prepared_*/frames_all/
|
||||
├── prepared_*/reference_poses_rtk_gga_raw_heading.csv
|
||||
└── calibration/ 或 outputs_*/
|
||||
├── open3d_gicp/ small_gicp/ consensus/
|
||||
├── common/ground_planes.csv
|
||||
├── summary.json
|
||||
└── final_T_RTK_lidar.json
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 6. 3D 可视化
|
||||
|
||||
查看本次结果：
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\First-dev-dept\calibration-rtk-run"
|
||||
$Out = "D:\data\rtk_lidar_run\outputs_vehicle_h19165"
|
||||
$Work = "D:\data\rtk_lidar_run\prepared_vehicle_h19165"
|
||||
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||||
-Frames "$Work\frames_all" `
|
||||
-Pairs "$Out\consensus\B_consensus.npz" `
|
||||
-Extrinsic "$Out\final_T_RTK_lidar.json" `
|
||||
-PairIndex 0
|
||||
```
|
||||
|
||||
通用模板（把路径换成你的 `WorkRoot` / `OutputRoot`）：
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||||
-Frames "$WorkRoot\frames_all" `
|
||||
-Pairs "$OutputRoot\consensus\B_consensus.npz" `
|
||||
-Extrinsic "$OutputRoot\final_T_RTK_lidar.json" `
|
||||
-PairIndex 0
|
||||
```
|
||||
|
||||
| 按键 | 含义 |
|
||||
|---|---|
|
||||
| [README_RTK_IMU.md](README_RTK_IMU.md) | RTK–IMU 当前候选、状态、流程、审核证据和复现命令。 |
|
||||
| [docs/lidar_imu_to_rtk_imu.md](docs/lidar_imu_to_rtk_imu.md) | 为什么 LiDAR–IMU 暂不能交平移、为什么引入 RTK–IMU、两者边界与共享代码。 |
|
||||
| [rtk_imu/README.md](rtk_imu/README.md) | RTK–IMU 包的代码职责与 import 边界。 |
|
||||
| [docs/rtk_imu_multisource_v3.md](docs/rtk_imu_multisource_v3.md) | G90/HI13 统一导出及 R1b/R2V/R2G/R3 旋转链路。 |
|
||||
| [docs/rtk_imu_engineering_6dof.md](docs/rtk_imu_engineering_6dof.md) | node-state factor graph、机械先验、验收门禁。 |
|
||||
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | LiDAR–IMU 方法与采集要求。 |
|
||||
| [docs/20260808_LiDAR-IMU标定现状与问题.md](docs/20260808_LiDAR-IMU标定现状与问题.md) | LiDAR–IMU 当前问题、证据和历史处置记录。 |
|
||||
| `1` | 原始点云 |
|
||||
| `2` | 仅用 RTK 运动作初值 |
|
||||
| `3` | GICP 测得的 B |
|
||||
| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） |
|
||||
| `N` / `]` | 下一运动对 |
|
||||
| `P` / `[` | 上一运动对 |
|
||||
| `Q` / `Esc` | 退出 |
|
||||
|
||||
## 结果与过程文件
|
||||
蓝 = 站 i，橙 = 站 j。请用 `N`/`P` **多看大转角对**，不要只看前几对同朝向站。
|
||||
|
||||
`artifacts/rtk_imu_calibration_v3/` 中只提交审核结论及其关键证据。原始 `.rscap`、统一导出数据、节点状态、checkpoint、调试 JSON 与旧版本过程产物均通过 `.gitignore` 保持本地，不应随提交或推送传播。
|
||||
---
|
||||
|
||||
每次改动前先确认使用哪一条链路；每次交付前先读取相应 release JSON 中的 accepted gate，而不是仅凭优化器收敛、残差下降或一个看似合理的 4×4 矩阵下结论。
|
||||
## 7. 当前标定结果（车头向前，h = 1.9165 m）
|
||||
|
||||
## LiDAR–IMU 专用运行指南
|
||||
> **状态：可作车头系候选交付**（`recommended_for_deployment: true`）。
|
||||
> 约定：`HeadingOffsetDeg=-90`，双天线 pitch/roll，机械初值 \(t=(+0.21086,-0.41418,+0.07850)\)，yaw=0。
|
||||
|
||||
[README_Lidar_IMU.md](README_Lidar_IMU.md) 保留 LiDAR–IMU 的独立运行命令、合成复现与输入输出说明；它是远端原 README 的专用化重命名，不再承担仓库总导航职责。
|
||||
仓库内结果：[`results/vehicle_20260808/`](results/vehicle_20260808/)（来自本机 `outputs_vehicle_h19165`）。
|
||||
|
||||
```text
|
||||
translation_m = [0.217822250, -0.411347802, 0.106542337]
|
||||
RPY_deg_xyz = [0.066239, 0.809662, -0.551322]
|
||||
|
||||
T_RTK_lidar ≈
|
||||
0.999854 0.009639 0.014119 0.217822
|
||||
-0.009621 0.999953 -0.001292 -0.411348
|
||||
-0.014131 0.001156 0.999899 0.106542
|
||||
0 0 0 1
|
||||
```
|
||||
|
||||
| 指标 | 值 |
|
||||
|---|---:|
|
||||
| 有效站点 / 共识对 | 27 / 20 |
|
||||
| 平移残差 RMS | ≈ 0.071 m |
|
||||
| 旋转残差 RMS | ≈ 0.982 ° |
|
||||
| 双后端差 | ≈ 3.1 mm / 0.12° |
|
||||
| `frame_mode` | `vehicle_forward_heading_offset` |
|
||||
| 相对机械初值 | XY 近机械杆臂；yaw≈0；无近 180° 冲突 |
|
||||
|
||||
与机械平移初值 XY 相差约数毫米；z 由天线高度约束，CAD 的 4 mm 不能代替实测 1.9165 m。
|
||||
|
||||
### 为何 RMS 尚可、尾部（P95/max）较差？
|
||||
|
||||
1. **前段多站几乎同航向**（STATION-01～05 约 250°～255°）。最差对（如 2→4）站间转角仅约 5°，小转角对平均平移残差约 7.4 cm，大转角对约 3.7 cm。
|
||||
2. **GICP heldout RMSE** 本身多在 0.11～0.14 m，场景重叠/结构限制了配准下限。
|
||||
3. 本批导出为 **host 时间关联**，静站可用，但仍可能引入厘米级位姿—点云错位。
|
||||
4. AX 残差衡量的是「RTK 运动 A」与「外参预测 XBX」的一致性，**不是**相对 CAD 的毫米误差，也不能单独证明 ±3 cm 绝对真值。
|
||||
|
||||
改进方向：相邻站转角 15°～30°、站数 ≥40、固定平面板、有条件改用 `device_gnss`。
|
||||
|
||||
---
|
||||
|
||||
## 8. z 与精度限制
|
||||
|
||||
平面阿克曼运动不能独立观测 z。z 由「LiDAR 地面平面 + 外供 RTK 参考点离地高」约束：
|
||||
|
||||
- 本次：**1.9165 m**（相位中心离地）；
|
||||
- 不得复用其他车辆或历史采集的天线离地高度。
|
||||
|
||||
更改高度后必须重新求解，禁止只改 JSON 里的 z。
|
||||
GGA 对应哪根天线、`rawHeading` 方向须现场确认；搞反会导致 yaw 差约 180°。
|
||||
|
||||
---
|
||||
|
||||
|
||||
## 9. 仓库目录
|
||||
|
||||
| 目录 | 职责 |
|
||||
|---|---|
|
||||
| [`code/`](code/) | GICP、运动对质量、AX=XB、结果封装、3D 可视化 |
|
||||
| [`tools/`](tools/) | dlog/rscap 解析、G90 窗导出、combined / prepared |
|
||||
| [`run/`](run/) | PowerShell 入口；路径与高度均由参数传入 |
|
||||
| [`results/`](results/) | 当前车辆的最终外参与质量摘要；不含原始数据和中间点云 |
|
||||
| `tests/` | 坐标契约、G90 host 关联等回归 |
|
||||
| `work/`、`outputs/` | 本地生成物（`.gitignore`） |
|
||||
|
||||
命令索引见 [`run/README.md`](run/README.md)，工具说明见 [`tools/README.md`](tools/README.md)，操作手册见 [`雷达与RTK标定说明书.md`](雷达与RTK标定说明书.md)。
|
||||
|
||||
@@ -1,184 +0,0 @@
|
||||
# LiDAR–IMU 外参标定
|
||||
|
||||
用连续行驶中的 LiDAR 与 IMU 相对运动，估计安装外参与时间偏置：
|
||||
|
||||
```text
|
||||
p_IMU = T_IMU_lidar · p_lidar
|
||||
```
|
||||
|
||||
**当前阶段：** 算法与合成自检已闭环；已提供 `tools/export_rscap_to_v1.py`（N300 `.rscap` + H32 dlog/MSOP → 中间格式）；**合格实车验收尚未完成**，故正式外参尚未对实车落盘交付。
|
||||
|
||||
---
|
||||
|
||||
## 先看什么（对外三份就够）
|
||||
|
||||
|
||||
| 顺序 | 文档 | 用途 |
|
||||
| --- | -------------------------------------- | -------------------- |
|
||||
| 1 | **本 README** | 做什么、怎么跑、结果怎么判 |
|
||||
| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + 原始数据导出命令 |
|
||||
| 3 | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采（合格数据要求） |
|
||||
|
||||
|
||||
其余（方法细述、测试说明、源码职责、CHANGELOG）给深入阅读 / 改代码时用，见文末。
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
## 1. 输入 / 输出
|
||||
|
||||
|
||||
| 输入 | 说明 |
|
||||
| --------- | ----------------------------------------------------------- |
|
||||
| `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`） |
|
||||
|
||||
|
||||
|
||||
| 输出 | 说明 |
|
||||
| ------------------ | ---------------------- |
|
||||
| `T_IMU_lidar.json` | 外参 |
|
||||
| `time_offset.json` | `t_imu = t_lidar + δt` |
|
||||
| `summary.json` | 状态、残差、可观性 |
|
||||
|
||||
|
||||
新车原始数据导出（H32 dlog/zip + HI13 rscap）：
|
||||
|
||||
```powershell
|
||||
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
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
## 2. 一键复现（合成，不需实车）
|
||||
|
||||
```powershell
|
||||
cd <本仓库根目录>
|
||||
python -m pip install -e ".[dev]"
|
||||
python -m pip install -e ".[open3d]" # 推荐
|
||||
powershell -File tools\reproduce_synthetic.ps1
|
||||
```
|
||||
|
||||
证明：链路可跑通，能收回已知 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
|
||||
imu_lidar/ 算法与 CLI
|
||||
config/ 车辆配置模板
|
||||
docs/ 采集清单、数据格式、方法细述
|
||||
tools/ 导出、合成复现、可视化
|
||||
tests/ 自动化测试
|
||||
```
|
||||
|
||||
|
||||
| 文档 | 何时看 |
|
||||
| ------------------------------------------------ | ---------------- |
|
||||
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 要看方法约定与实现状态表 |
|
||||
| [tests/README.md](tests/README.md) | 要看合成用例 / S2 烟测记录 |
|
||||
| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 要改源码 |
|
||||
| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 要查改动史 |
|
||||
|
||||
|
||||
改算法请同步职责说明与 CHANGELOG；改对外用法请更新本 README。
|
||||
@@ -1,177 +0,0 @@
|
||||
# RTK-IMU 标定：流程、结果与复现
|
||||
|
||||
本文是本次 G90 双天线 RTK 与 HI13 IMU 标定的工程入口。算法代码在 `rtk_imu/`，命令入口在 `tools/`，正式审核结果在 `artifacts/rtk_imu_calibration_v3/`。原始 `.rscap`、统一导出数据、状态快照与 checkpoint 均只保留本地，不提交仓库。
|
||||
|
||||
## 一句话结论
|
||||
|
||||
RTK-IMU 旋转外参由双天线基线方向与水平静止重力约束获得；平移外参以机械测量为绝对基准，再通过 RTK+IMU 联合状态图、高动态转弯/坡道数据、独立 held-out 数据和旋转扰动测试进行一致性验证。
|
||||
|
||||
当前动态数据不足以独立高精度求出完整 XYZ 杆臂，因此它不是 data-only translation calibration。机械测量给出了杆臂的绝对值；在未参与标定的数据、转弯等杆臂敏感运动以及旋转外参扰动测试中，均未发现该机械外参存在明显矛盾或不稳定性。当前结果的正确表述是：**机械杆臂锚定，并经动态数据一致性验证的工程候选**。
|
||||
|
||||
## 当前工程候选与状态
|
||||
|
||||
坐标约定：`l_I = p_ANT1^I`，即 ANT1（主天线、左侧）相位中心在 IMU 坐标系中的位置。车体/RTK 安装坐标为：+X 为 ANT1（左）到 ANT2（右），+Y 为车辆前进方向且与 IMU +Y 同向，+Z 向上；GGA 参考点为 ANT1 相位中心，离地 `1.916499878 m`。
|
||||
|
||||
- 固定旋转来源：`R2G_gravity_level_prior`。
|
||||
- 固定旋转近似 RPY（X/Y/Z）：`[0.454°, -0.003°, 0.012°]`。
|
||||
- 当前工程候选杆臂：`l_I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||
- 变换约定：`p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU`。
|
||||
- 对应候选 `T_RTK_IMU`（四舍五入到 6 位小数）：
|
||||
|
||||
```text
|
||||
[[ 1.000000, -0.000214, -0.000044, 0.451777],
|
||||
[ 0.000214, 0.999969, -0.007929, 0.270363],
|
||||
[ 0.000046, 0.007929, 0.999969, -0.729325],
|
||||
[ 0.000000, 0.000000, 0.000000, 1.000000]]
|
||||
```
|
||||
|
||||
当前门禁状态必须同时保留：
|
||||
|
||||
- `data_only_translation_accepted=false`：数据本身没有提供足够稳定的完整平移可观性。
|
||||
- `engineering_translation_accepted=false`：独立传播验证仍存在公共加速度偏差，不能声称正式工程放行。
|
||||
- `independent_extrinsic_sensitive_validation_passed=true`：杆臂敏感的高动态验证未发现机械杆臂冲突。
|
||||
- `heldout_physical_validation_passed=true`、`rotation_sensitivity_passed=true`：固定候选在 held-out 物理残差和旋转扰动中保持一致。
|
||||
|
||||
因此不得描述为“data-only 标定平移”或“动态数据已精确细化机械杆臂”。完整、机器可读的结论见 [engineering_release_decision.json](artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。
|
||||
|
||||
## 求解流程
|
||||
|
||||
1. **统一原始数据导出。** 使用 G90/HI13 设备时间作为主时间轴；host receive time 仅用于诊断。G90 保留 GGA 质量、GNHPR 基线及质量、BESTNAVA Doppler velocity，必要时 PVTSLNA；HI13 保留 system time、gyro、accel、姿态/四元数和 host receive time。
|
||||
2. **R0 连续性与质量控制。** 校验 checksum、RTK Fixed、设备时间单调性、IMU 覆盖、baseline jump 与测量间隔。轨迹连续性由 IMU 设备时间和预积分覆盖决定；孤立 HPR 缺失/Q5 只禁用或降权 HPR factor，不切断 IMU 轨迹。
|
||||
3. **旋转外参。** R1b 从基线与 IMU 动态估计 ANT1→ANT2 在 IMU 中的 2DoF 方向；R2V 用基线与高质量 Doppler 速度作独立诊断；R2G 使用水平静止场地中的基线＋重力＋地面水平先验补齐完整旋转。R2G 是本次正式固定旋转来源，旧 GNHPR 三轴手眼只作诊断。
|
||||
4. **节点状态图。** 每个 GNSS node 包含 `R,p,v,bg,ba` 的 15DoF 状态；相邻 node 由 covariance-whitened IMU preintegration 和 bias random walk 连接。BEST 约束 XYZ，GGA 仅在 BEST 缺失时约束 XY，Doppler 约束速度，HPR 是可选姿态/基线因子。
|
||||
5. **机械锚定的平移验证。** 杆臂以机械值为基准，分别比较无先验 free、固定机械杆臂和软先验解。free 解只用于观测性诊断，不能因为数值收敛就替代机械值。
|
||||
6. **独立验证。** 使用 circle、left-right、slope 的高动态非重叠窗口；再使用剩余 held-out 窗口、旋转 ±扰动敏感性、独立 innovation 和 propagation-bias root-cause audit 复核。
|
||||
|
||||
## 证据与限制
|
||||
|
||||
47 个非重叠标定窗口上的 free/fixed/prior 对比表明机械先验与数据拟合相容，但 posterior/prior 方差比没有显示足够的数据驱动细化，因此 `translation_refined_by_data=false`。剩余 267 个 frozen held-out 窗口的物理验证通过；然而独立传播创新在低速、低角速度区间同时出现位置和 Doppler 的同向偏差，等效为约 `0.20 m/s²` 的公共传播加速度误差。
|
||||
|
||||
该误差在 `|omega|` 很小时不能优先归因于杆臂速度项 `R*(omega × l)`，因此它不单独否决高动态杆臂敏感验证；但在传播模型根因关闭前，也不能把候选杆臂标为正式工程已放行。
|
||||
|
||||
主要审核证据：
|
||||
|
||||
- [47 窗口机械分支](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json)
|
||||
- [held-out 验证](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json)
|
||||
- [独立创新审计](artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json)
|
||||
- [传播偏差根因审计](artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json)
|
||||
- [旋转敏感性](artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json)
|
||||
|
||||
## 如何复现
|
||||
|
||||
### 1. 准备环境和原始数据
|
||||
|
||||
```powershell
|
||||
cd <repository-root>
|
||||
python -m pip install -e ".[dev]"
|
||||
```
|
||||
|
||||
将三批 G90/HI13 原始 `.rscap` 会话放到本机的数据位置。数据位置不写入仓库；`tools/export_rtk_imu_unified.py` 中的会话配对清单必须与实际采集文件一致。
|
||||
|
||||
```powershell
|
||||
$OUT = "artifacts\rtk_imu_calibration_v3\reproduce"
|
||||
python tools\export_rtk_imu_unified.py --output-root "$OUT\unified" --overwrite
|
||||
$MANIFEST = "$OUT\unified\manifest.json"
|
||||
```
|
||||
|
||||
导出后应检查每个会话目录中的 `export_summary.json`，并确认 `manifest.json` 中记录的传感器时间轴没有被 host receive time 替换。
|
||||
|
||||
### 2. 复现固定旋转
|
||||
|
||||
`<flat-static-session-id>` 必须是已确认地面水平、车辆静止的会话；可多次传入 `--level-static`。R2V 只作独立诊断，不替代 R2G。
|
||||
|
||||
```powershell
|
||||
python tools\run_rtk_imu_multisource.py `
|
||||
--manifest $MANIFEST `
|
||||
--level-static <flat-static-session-id> `
|
||||
--output "$OUT\r2g_multisource.json"
|
||||
```
|
||||
|
||||
检查输出的 R2G 旋转与本 README 的近似 RPY 一致后，将该旋转固定为后续 node graph 的 `--rotation-rpy-deg 0.454 -0.003 0.012`。若 R2G 不一致，应停止，先复核天线方向、场地水平和时间/轴定义，不应继续求杆臂。
|
||||
|
||||
### 3. 复现窗口选择与无先验诊断
|
||||
|
||||
从 circle、left-right、slope 三类会话中选择高质量窗口；窗口不可共享 IMU/GNSS/HPR 样本。仓库提交的 `lever_information_window_selection*.json` 是本次审核所用选择结果，可用于对照。
|
||||
|
||||
```powershell
|
||||
python tools\select_rtk_imu_windows_by_lever_information.py `
|
||||
--manifest $MANIFEST `
|
||||
--circle-session <circle-session-id> `
|
||||
--left-right-session <left-right-session-id> `
|
||||
--slope-session <slope-session-id> `
|
||||
--output "$OUT\selection.json" `
|
||||
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||
|
||||
python tools\run_rtk_imu_node_graph_free_selected.py `
|
||||
--manifest $MANIFEST `
|
||||
--selection "$OUT\selection.json" `
|
||||
--output "$OUT\free_baseline.json" `
|
||||
--start-name all `
|
||||
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||
```
|
||||
|
||||
free solve 的作用是输出边缘化杆臂信息、协方差、最弱方向和多初值稳定性；本次数据若仍未达到完整 XYZ 可观性，不得扩大无先验求解规模来强行放行。
|
||||
|
||||
### 4. 复现机械杆臂分支和 held-out 验证
|
||||
|
||||
固定工程候选杆臂并使用同一批非重叠标定窗口。`states.npz` 和 checkpoint-dir 是本地过程产物，应保持被 `.gitignore` 排除。
|
||||
|
||||
```powershell
|
||||
python tools\run_rtk_imu_mechanical_prior_branch.py `
|
||||
--manifest $MANIFEST `
|
||||
--selection "$OUT\selection.json" `
|
||||
--free-baseline "$OUT\free_baseline.json" `
|
||||
--output "$OUT\mechanical_47_window.json" `
|
||||
--state-output "$OUT\states.npz" `
|
||||
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||
|
||||
python tools\run_rtk_imu_mechanical_prior_heldout.py `
|
||||
--manifest $MANIFEST `
|
||||
--calibration-selection "$OUT\selection.json" `
|
||||
--all-selection <all-nonoverlapping-selection.json> `
|
||||
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||
--output "$OUT\heldout.json" `
|
||||
--checkpoint-dir "$OUT\heldout_checkpoints" `
|
||||
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||
```
|
||||
|
||||
随后运行独立 innovation、传播根因审计和旋转敏感性。它们不重新优化杆臂，不应被用于调 covariance、R2G 或机械先验。
|
||||
|
||||
```powershell
|
||||
python tools\audit_rtk_imu_heldout_innovation.py `
|
||||
--manifest $MANIFEST `
|
||||
--calibration-selection "$OUT\selection.json" `
|
||||
--all-selection <all-nonoverlapping-selection.json> `
|
||||
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||
--output "$OUT\heldout_innovation.json" `
|
||||
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||
|
||||
python tools\audit_rtk_imu_propagation_bias_root_cause.py `
|
||||
--manifest $MANIFEST `
|
||||
--calibration-selection "$OUT\selection.json" `
|
||||
--all-selection <all-nonoverlapping-selection.json> `
|
||||
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||
--output "$OUT\propagation_bias_root_cause.json" `
|
||||
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||
```
|
||||
|
||||
最终仅汇总已生成的结果，不在 release 阶段重新拟合：
|
||||
|
||||
```powershell
|
||||
python tools\finalize_rtk_imu_engineering_release.py `
|
||||
--calibration "$OUT\mechanical_47_window.json" `
|
||||
--heldout-postfit "$OUT\heldout.json" `
|
||||
--innovation "$OUT\heldout_innovation.json" `
|
||||
--sensitivity <rotation_sensitivity.json> `
|
||||
--convergence-retry <heldout_retry.json> `
|
||||
--propagation-root-cause "$OUT\propagation_bias_root_cause.json" `
|
||||
--output "$OUT\engineering_release_decision.json"
|
||||
```
|
||||
|
||||
## 相关文件
|
||||
|
||||
- [RTK-IMU 代码包](rtk_imu/README.md)
|
||||
- [多源导出与旋转 V3 说明](docs/rtk_imu_multisource_v3.md)
|
||||
- [engineering 6DoF 说明](docs/rtk_imu_engineering_6dof.md)
|
||||
- [历史链路审计](docs/rtk_imu_calibration.md)
|
||||
- [正式结果索引](artifacts/rtk_imu_calibration_v3/README.md)
|
||||
@@ -1,17 +0,0 @@
|
||||
# RTK–IMU 标定产物说明
|
||||
|
||||
- `all_sessions/`：8 会话、5 s 平移节点、带 conditional rotation LOO 和 translation LOO 的当前完整基线。
|
||||
- `rotation_hpr_time/`：改用 GNHPR 自带测量时刻后的全量 rotation-only 对照。
|
||||
- `rotation_smoke/`：较早的 GGA 最近邻姿态时刻对照，不作为当前结果。
|
||||
- `batch_0808_full_smoke/`：0808 三会话完整诊断。
|
||||
- `batch_0815_rotation/`：0815 四会话 rotation-only 诊断。
|
||||
- `single_smoke/`：早期单会话性能/数值冒烟，不作为当前结果。
|
||||
|
||||
每个正式运行目录包含：
|
||||
|
||||
- `dataset_audit.json`：样本数、固定解比例、共同时间范围和 ENU 原点。
|
||||
- `rotation_result.json`：旋转、RPY、时间审计、GNHPR 候选、偏置、残差、协方差、逐会话指标和 LOO。
|
||||
- `translation_result.json`：杆臂、平移、齐次矩阵、协方差/秩、位置/速度残差、偏置和 LOO。
|
||||
- `summary.json`：供程序读取的最终状态和候选矩阵。
|
||||
|
||||
当前 `all_sessions/summary.json` 是 `diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性，禁止用于车辆配置。完整解释见 `docs/rtk_imu_calibration.md`。
|
||||
@@ -1,157 +0,0 @@
|
||||
{
|
||||
"session_count": 8,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "motion_sms_154023_154359",
|
||||
"batch_id": "0819",
|
||||
"imu_samples": 21556,
|
||||
"rtk_samples": 3294,
|
||||
"fixed_position_ratio": 0.49271402550091076,
|
||||
"fixed_attitude_ratio": 0.4344262295081967,
|
||||
"common_time_span_s": [
|
||||
25829.8725525,
|
||||
26027.4217529
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4651580863,
|
||||
114.090773052,
|
||||
36.5626
|
||||
],
|
||||
"imu_source": "21556 normalized samples",
|
||||
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,130 +0,0 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
0.1028243313393031,
|
||||
-0.1450716664951083,
|
||||
-1.43269836393699
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-8.90944066727157e-05,
|
||||
7.078609864979291e-05,
|
||||
0.0001460390779870233
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-4.327374730750894e-05,
|
||||
-0.0001097548390022348,
|
||||
3.000549725593387e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-7.036551812132934e-05,
|
||||
-0.0008054961048184173,
|
||||
1.3026193503057623e-05
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
6.035222904568755e-05,
|
||||
-8.913884157613711e-06,
|
||||
0.00015470949872434692
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-3.696799268832588e-05,
|
||||
-0.00019200076224600124,
|
||||
-5.9567987995056394e-05
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.0001979655352189002,
|
||||
-0.00015937450336643818,
|
||||
-4.343089303706036e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
-7.841839817698704e-05,
|
||||
-2.604932294598783e-06,
|
||||
5.8586555655011475e-05
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
0.00011745666467620586,
|
||||
0.00021434926285955486,
|
||||
-4.5147354343760625e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.04000000000000031,
|
||||
"peak_correlation": 0.5788788671828708,
|
||||
"second_best_correlation": 0.5773967219219845,
|
||||
"evaluated_samples": 12394,
|
||||
"reliable": false
|
||||
},
|
||||
"applied_time_offset_s": 0.0,
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
|
||||
"north_cw__pitch_opposite": 1.737582794831228,
|
||||
"heading_opposite__pitch_nose_up": 7.263259116648845,
|
||||
"heading_opposite__pitch_opposite": 17.716319437929055
|
||||
},
|
||||
"pair_count": 731,
|
||||
"residual_rms_deg": 1.6276839301413086,
|
||||
"residual_median_deg": 0.5695938849052221,
|
||||
"residual_p95_deg": 3.0903953005641345,
|
||||
"rotation_std_deg": [
|
||||
0.2340126126406699,
|
||||
0.20048574131667432,
|
||||
2.314692068045375
|
||||
],
|
||||
"information_singular_values": [
|
||||
82178.77940373585,
|
||||
60441.86300479194,
|
||||
612.6358640387364
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.6134232617562615,
|
||||
"priority_174905_175450": 2.2485932625979106,
|
||||
"priority_175910_180530": 3.095861798757508,
|
||||
"slope_190548_190730": 2.388478834012087,
|
||||
"circle_193412_193642": 0.5629885032374518,
|
||||
"loop_194223_195003": 0.6250976606325254,
|
||||
"accel_195608_195958": 0.975856888271022,
|
||||
"motion_sms_154023_154359": 1.3698317592842066
|
||||
},
|
||||
"loo_delta_deg": {
|
||||
"priority_174005_174515": 0.7895996403426686,
|
||||
"priority_174905_175450": 0.9224102138653388,
|
||||
"priority_175910_180530": 5.144190932547379,
|
||||
"slope_190548_190730": 0.24769087330915346,
|
||||
"circle_193412_193642": 1.2637729378484346,
|
||||
"loop_194223_195003": 0.4844375238260164,
|
||||
"accel_195608_195958": 1.0976412402728102,
|
||||
"motion_sms_154023_154359": 0.6896079810115167
|
||||
},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1063 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,65 +0,0 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"rtk_frame_definition": "",
|
||||
"rtk_reference_point": "",
|
||||
"interpretation_blockers": [
|
||||
"rotation quality gates failed",
|
||||
"translation quality gates failed or were not run",
|
||||
"RTK frame_definition is empty",
|
||||
"RTK reference_point is empty"
|
||||
],
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
0.7710937276592442,
|
||||
0.5693018309069646,
|
||||
-21.07303743539233
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804,
|
||||
0.7710937276592442
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697,
|
||||
0.5693018309069646
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289,
|
||||
-21.07303743539233
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"rotation_ok": false,
|
||||
"translation_ok": false,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": "translation_result.json",
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -1,161 +0,0 @@
|
||||
{
|
||||
"lever_IMU_to_RTK_in_IMU_m": [
|
||||
-0.7032597491310882,
|
||||
-0.5505808768918061,
|
||||
21.07590765040047
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
0.7710937276592442,
|
||||
0.5693018309069646,
|
||||
-21.07303743539233
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804,
|
||||
0.7710937276592442
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697,
|
||||
0.5693018309069646
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289,
|
||||
-21.07303743539233
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"translation_std_m": [
|
||||
0.367657527272358,
|
||||
0.36764001580136974,
|
||||
2.960858932902556
|
||||
],
|
||||
"lever_information_singular_values": [
|
||||
7.467287688797444,
|
||||
7.419771560016465,
|
||||
0.11404687553142893
|
||||
],
|
||||
"lever_precision_rank": 3,
|
||||
"position_residual_rms_xyz_m": [
|
||||
0.0026057774092802665,
|
||||
0.0026172203224947795,
|
||||
0.0008707288139887198
|
||||
],
|
||||
"velocity_residual_rms_xyz_m_s": [
|
||||
1.0344084464154262,
|
||||
1.0018456371907998,
|
||||
0.08189262443294992
|
||||
],
|
||||
"accel_bias_by_session_m_s2": {
|
||||
"priority_174005_174515": [
|
||||
-0.0033446448235936025,
|
||||
0.04850567116751809,
|
||||
0.014864908749227664
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
0.051484854191344014,
|
||||
0.009750775340658668,
|
||||
0.016837766756117954
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-0.0012098409852818557,
|
||||
0.04428237322299069,
|
||||
0.015044444493724668
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
0.007720083607057322,
|
||||
-0.07045791547639069,
|
||||
0.011096872492579776
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
0.010695150095323893,
|
||||
0.07124837152622766,
|
||||
0.012181970165526993
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
0.017798177348527063,
|
||||
0.026531175431114495,
|
||||
0.013517325106459492
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
0.010843370014374661,
|
||||
0.0021847264023688797,
|
||||
0.012720554759472001
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
-0.01917068926914634,
|
||||
0.04878246850510192,
|
||||
0.011986246228615108
|
||||
]
|
||||
},
|
||||
"knot_count_by_session": {
|
||||
"priority_174005_174515": 62,
|
||||
"priority_174905_175450": 69,
|
||||
"priority_175910_180530": 76,
|
||||
"slope_190548_190730": 21,
|
||||
"circle_193412_193642": 30,
|
||||
"loop_194223_195003": 92,
|
||||
"accel_195608_195958": 46,
|
||||
"motion_sms_154023_154359": 20
|
||||
},
|
||||
"loo_delta_m": {
|
||||
"priority_174005_174515": [
|
||||
-0.08631101169626099,
|
||||
-0.16878866579372004,
|
||||
-0.1969063529933237
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
0.052982832020427084,
|
||||
0.23694572858391594,
|
||||
0.5921699991558107
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-0.03728037456994515,
|
||||
-0.0587695607071852,
|
||||
0.36745157813128415
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
0.06820599394639204,
|
||||
0.13646009238200618,
|
||||
0.8245922313333871
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-0.06745036261863124,
|
||||
-0.18645839932341524,
|
||||
-0.8300513550738842
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
0.12045919921905746,
|
||||
0.06572884705689208,
|
||||
-1.774476169745249
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
0.019243586803303625,
|
||||
-0.002985337002138544,
|
||||
0.3653882998138158
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
-0.0383153365081248,
|
||||
0.027686850248512473,
|
||||
0.507160468738487
|
||||
]
|
||||
},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||
"upstream rotation is not accepted, so translation is diagnostic only",
|
||||
"translation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,62 +0,0 @@
|
||||
{
|
||||
"session_count": 3,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,89 +0,0 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
-0.23265982849493025,
|
||||
-0.03971564182674239,
|
||||
-1.5635621825359411
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-9.355487703345326e-05,
|
||||
6.022278219673199e-05,
|
||||
0.0001599879605718389
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-6.763782096065313e-05,
|
||||
-0.00013271246982721386,
|
||||
2.7304435743171578e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-4.9469524877665416e-05,
|
||||
-0.0005605818923478396,
|
||||
4.72341323992995e-06
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.07500000000000034,
|
||||
"peak_correlation": 0.2075894836191556,
|
||||
"second_best_correlation": 0.2075830757517665,
|
||||
"evaluated_samples": 6286,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 2.124836124403544,
|
||||
"north_cw__pitch_opposite": 2.246244073697405,
|
||||
"heading_opposite__pitch_nose_up": 14.081722937345578,
|
||||
"heading_opposite__pitch_opposite": 14.187723789068325
|
||||
},
|
||||
"pair_count": 347,
|
||||
"residual_rms_deg": 2.121956852743709,
|
||||
"residual_median_deg": 0.6620410270629032,
|
||||
"residual_p95_deg": 4.932454679529714,
|
||||
"rotation_std_deg": [
|
||||
0.5378381823216046,
|
||||
0.43991569563643235,
|
||||
3.604673882923754
|
||||
],
|
||||
"information_singular_values": [
|
||||
17058.156967522238,
|
||||
11392.471369813427,
|
||||
252.6038959367189
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.6074812737155098,
|
||||
"priority_174905_175450": 2.2498284381524494,
|
||||
"priority_175910_180530": 3.09642157433063
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1214 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,57 +0,0 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
1.017036626166409,
|
||||
0.7992624136748191,
|
||||
-22.250154010194304
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719,
|
||||
1.017036626166409
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135,
|
||||
0.7992624136748191
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744,
|
||||
-22.250154010194304
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"rotation_ok": false,
|
||||
"translation_ok": false,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": "translation_result.json",
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -1,90 +0,0 @@
|
||||
{
|
||||
"lever_IMU_to_RTK_in_IMU_m": [
|
||||
-0.979425993211181,
|
||||
-0.9170620761729408,
|
||||
22.247297796687818
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
1.017036626166409,
|
||||
0.7992624136748191,
|
||||
-22.250154010194304
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719,
|
||||
1.017036626166409
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135,
|
||||
0.7992624136748191
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744,
|
||||
-22.250154010194304
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"translation_std_m": [
|
||||
0.6410402171684149,
|
||||
0.6447113787519164,
|
||||
3.8963000265795396
|
||||
],
|
||||
"lever_information_singular_values": [
|
||||
2.4406600443109308,
|
||||
2.412462748498746,
|
||||
0.06586096829714262
|
||||
],
|
||||
"lever_precision_rank": 3,
|
||||
"position_residual_rms_xyz_m": [
|
||||
0.0023101392989052683,
|
||||
0.002794889168410421,
|
||||
0.0005246719035281052
|
||||
],
|
||||
"velocity_residual_rms_xyz_m_s": [
|
||||
1.1059333756244145,
|
||||
1.368704336610928,
|
||||
0.11603076744633863
|
||||
],
|
||||
"accel_bias_by_session_m_s2": {
|
||||
"priority_174005_174515": [
|
||||
0.015165392523580496,
|
||||
0.10623623265620695,
|
||||
0.015221547657584966
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
0.06725309871307823,
|
||||
0.06785033078439083,
|
||||
0.0171641139816057
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
0.015302724485125336,
|
||||
0.10160587655627046,
|
||||
0.015281702682303734
|
||||
]
|
||||
},
|
||||
"knot_count_by_session": {
|
||||
"priority_174005_174515": 62,
|
||||
"priority_174905_175450": 69,
|
||||
"priority_175910_180530": 76
|
||||
},
|
||||
"loo_delta_m": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||
"upstream rotation is not accepted, so translation is diagnostic only",
|
||||
"translation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,81 +0,0 @@
|
||||
{
|
||||
"session_count": 4,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,96 +0,0 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9999453769836999,
|
||||
0.010155336209751328,
|
||||
-0.002472285459453144
|
||||
],
|
||||
[
|
||||
-0.010163396322394882,
|
||||
0.9999430054296535,
|
||||
-0.003269750373547743
|
||||
],
|
||||
[
|
||||
0.002438939138240274,
|
||||
0.003294698586865735,
|
||||
0.9999915982332558
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
0.18877322677308359,
|
||||
-0.13974105765052158,
|
||||
-0.5823314723802753
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"slope_190548_190730": [
|
||||
5.517563410757097e-05,
|
||||
-1.0668622945796592e-05,
|
||||
0.00015474647141927933
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-4.092656265416791e-05,
|
||||
-0.00017676217236757262,
|
||||
-5.914138553067106e-05
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.000196149370561509,
|
||||
-0.0001486662046316796,
|
||||
-3.6375768145656054e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
-7.483644204529091e-05,
|
||||
-4.17320520562614e-06,
|
||||
5.861303135056825e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.04000000000000031,
|
||||
"peak_correlation": 0.7578237026149359,
|
||||
"second_best_correlation": 0.757682618781891,
|
||||
"evaluated_samples": 5443,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 0.9457675569035681,
|
||||
"north_cw__pitch_opposite": 1.064327146928918,
|
||||
"heading_opposite__pitch_nose_up": 5.753424726280977,
|
||||
"heading_opposite__pitch_opposite": 13.362267974377751
|
||||
},
|
||||
"pair_count": 351,
|
||||
"residual_rms_deg": 0.9428350359826574,
|
||||
"residual_median_deg": 0.4993317818354644,
|
||||
"residual_p95_deg": 1.5023594730593923,
|
||||
"rotation_std_deg": [
|
||||
0.191917244945292,
|
||||
0.16245924204992213,
|
||||
2.5710977852526464
|
||||
],
|
||||
"information_singular_values": [
|
||||
125441.66767899474,
|
||||
90572.5161990362,
|
||||
496.5407001788204
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"slope_190548_190730": 2.386329048920402,
|
||||
"circle_193412_193642": 0.560395991888806,
|
||||
"loop_194223_195003": 0.6239401903763562,
|
||||
"accel_195608_195958": 0.9797575195873669
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1186 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,28 +0,0 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9999453769836999,
|
||||
0.010155336209751328,
|
||||
-0.002472285459453144
|
||||
],
|
||||
[
|
||||
-0.010163396322394882,
|
||||
0.9999430054296535,
|
||||
-0.003269750373547743
|
||||
],
|
||||
[
|
||||
0.002438939138240274,
|
||||
0.003294698586865735,
|
||||
0.9999915982332558
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -1,157 +0,0 @@
|
||||
{
|
||||
"session_count": 8,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "motion_sms_154023_154359",
|
||||
"batch_id": "0819",
|
||||
"imu_samples": 21556,
|
||||
"rtk_samples": 3294,
|
||||
"fixed_position_ratio": 0.49271402550091076,
|
||||
"fixed_attitude_ratio": 0.4344262295081967,
|
||||
"common_time_span_s": [
|
||||
25829.8725525,
|
||||
26027.4217529
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4651580863,
|
||||
114.090773052,
|
||||
36.5626
|
||||
],
|
||||
"imu_source": "21556 normalized samples",
|
||||
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,119 +0,0 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
0.1028243313393031,
|
||||
-0.1450716664951083,
|
||||
-1.43269836393699
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-8.90944066727157e-05,
|
||||
7.078609864979291e-05,
|
||||
0.0001460390779870233
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-4.327374730750894e-05,
|
||||
-0.0001097548390022348,
|
||||
3.000549725593387e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-7.036551812132934e-05,
|
||||
-0.0008054961048184173,
|
||||
1.3026193503057623e-05
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
6.035222904568755e-05,
|
||||
-8.913884157613711e-06,
|
||||
0.00015470949872434692
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-3.696799268832588e-05,
|
||||
-0.00019200076224600124,
|
||||
-5.9567987995056394e-05
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.0001979655352189002,
|
||||
-0.00015937450336643818,
|
||||
-4.343089303706036e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
-7.841839817698704e-05,
|
||||
-2.604932294598783e-06,
|
||||
5.8586555655011475e-05
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
0.00011745666467620586,
|
||||
0.00021434926285955486,
|
||||
-4.5147354343760625e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.04000000000000031,
|
||||
"peak_correlation": 0.5788788671828708,
|
||||
"second_best_correlation": 0.5773967219219845,
|
||||
"evaluated_samples": 12394,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
|
||||
"north_cw__pitch_opposite": 1.737582794831228,
|
||||
"heading_opposite__pitch_nose_up": 7.263259116648845,
|
||||
"heading_opposite__pitch_opposite": 17.716319437929055
|
||||
},
|
||||
"pair_count": 731,
|
||||
"residual_rms_deg": 1.6276839301413086,
|
||||
"residual_median_deg": 0.5695938849052221,
|
||||
"residual_p95_deg": 3.0903953005641345,
|
||||
"rotation_std_deg": [
|
||||
0.2340126126406699,
|
||||
0.20048574131667432,
|
||||
2.314692068045375
|
||||
],
|
||||
"information_singular_values": [
|
||||
82178.77940373585,
|
||||
60441.86300479194,
|
||||
612.6358640387364
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.6134232617562615,
|
||||
"priority_174905_175450": 2.2485932625979106,
|
||||
"priority_175910_180530": 3.095861798757508,
|
||||
"slope_190548_190730": 2.388478834012087,
|
||||
"circle_193412_193642": 0.5629885032374518,
|
||||
"loop_194223_195003": 0.6250976606325254,
|
||||
"accel_195608_195958": 0.975856888271022,
|
||||
"motion_sms_154023_154359": 1.3698317592842066
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1063 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,28 +0,0 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -1,157 +0,0 @@
|
||||
{
|
||||
"session_count": 8,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "motion_sms_154023_154359",
|
||||
"batch_id": "0819",
|
||||
"imu_samples": 21556,
|
||||
"rtk_samples": 3294,
|
||||
"fixed_position_ratio": 0.49271402550091076,
|
||||
"fixed_attitude_ratio": 0.4344262295081967,
|
||||
"common_time_span_s": [
|
||||
25829.8725525,
|
||||
26027.4217529
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4651580863,
|
||||
114.090773052,
|
||||
36.5626
|
||||
],
|
||||
"imu_source": "21556 normalized samples",
|
||||
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,119 +0,0 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
-0.999754381938718,
|
||||
0.021803011844914608,
|
||||
0.003975483470215314
|
||||
],
|
||||
[
|
||||
0.021793866945340735,
|
||||
0.9997597719617833,
|
||||
-0.0023293197487744975
|
||||
],
|
||||
[
|
||||
-0.0040253146336934244,
|
||||
-0.002242106467980424,
|
||||
-0.9999893848440022
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
-179.8715356137635,
|
||||
0.23063416255936714,
|
||||
178.75119441523574
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-0.00018079953472427648,
|
||||
-9.3706764772758e-05,
|
||||
-1.6471539455612585e-05
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-0.0001527801061740737,
|
||||
-0.0001164065722445753,
|
||||
-5.5002757975692905e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
0.00010732176614734042,
|
||||
-9.217694744503191e-05,
|
||||
0.00037171055222583177
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
-0.0001400513048024026,
|
||||
-9.490085614046164e-05,
|
||||
-7.313488292136488e-05
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-5.9488404294769446e-05,
|
||||
-0.00029434501957892675,
|
||||
0.000102661594942897
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.0002980828626939094,
|
||||
9.137138703447405e-05,
|
||||
4.430930647242609e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
5.9388956272647935e-05,
|
||||
9.900621296014341e-06,
|
||||
8.065768586636302e-05
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
0.0016442968972331072,
|
||||
0.00012387305081956675,
|
||||
8.435528364463815e-08
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.08500000000000035,
|
||||
"peak_correlation": 0.46191352508231565,
|
||||
"second_best_correlation": 0.460889710037625,
|
||||
"evaluated_samples": 12384,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "heading_opposite__pitch_nose_up",
|
||||
"heading_sign": 1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 1.6671304616155285,
|
||||
"north_cw__pitch_opposite": 1.7955511909491206,
|
||||
"heading_opposite__pitch_nose_up": 1.6669796415371239,
|
||||
"heading_opposite__pitch_opposite": 19.709790964275243
|
||||
},
|
||||
"pair_count": 6164,
|
||||
"residual_rms_deg": 1.6669796415371239,
|
||||
"residual_median_deg": 0.6097138083592458,
|
||||
"residual_p95_deg": 2.9952512236242987,
|
||||
"rotation_std_deg": [
|
||||
1.0204560247927847,
|
||||
0.06460681216547948,
|
||||
0.11838973994567728
|
||||
],
|
||||
"information_singular_values": [
|
||||
818353.8002481281,
|
||||
235774.70975965378,
|
||||
3151.7390576048515
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.7068100152136895,
|
||||
"priority_174905_175450": 1.7590065687347056,
|
||||
"priority_175910_180530": 3.0674316757114104,
|
||||
"slope_190548_190730": 2.07580304513421,
|
||||
"circle_193412_193642": 0.6574485751543728,
|
||||
"loop_194223_195003": 0.6594363752510715,
|
||||
"accel_195608_195958": 0.7631418065216562,
|
||||
"motion_sms_154023_154359": 3.709386878989403
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.0002 deg",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"empirical best GNHPR convention differs from protocol expectation; manual verification required",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,28 +0,0 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
-0.999754381938718,
|
||||
0.021803011844914608,
|
||||
0.003975483470215314
|
||||
],
|
||||
[
|
||||
0.021793866945340735,
|
||||
0.9997597719617833,
|
||||
-0.0023293197487744975
|
||||
],
|
||||
[
|
||||
-0.0040253146336934244,
|
||||
-0.002242106467980424,
|
||||
-0.9999893848440022
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -1,24 +0,0 @@
|
||||
{
|
||||
"session_count": 1,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,76 +0,0 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9974493913373024,
|
||||
-0.07135460605626225,
|
||||
0.0017977528752911507
|
||||
],
|
||||
[
|
||||
0.0713431523484532,
|
||||
0.997435050761847,
|
||||
0.005785682733905998
|
||||
],
|
||||
[
|
||||
-0.0022059768426676615,
|
||||
-0.00564266836413856,
|
||||
0.9999816468115312
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
-0.32330358477785043,
|
||||
0.1263932653005637,
|
||||
4.0911470587568495
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_175910_180530": [
|
||||
-0.00010509442179307857,
|
||||
-0.0001426783269500731,
|
||||
3.086292346981605e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": -0.0949999999999998,
|
||||
"peak_correlation": 0.1329249352233157,
|
||||
"second_best_correlation": 0.13247995527618098,
|
||||
"evaluated_samples": 2316,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 2.8406184024910064,
|
||||
"north_cw__pitch_opposite": 2.8942770966870084,
|
||||
"heading_opposite__pitch_nose_up": 3.8836333647028383,
|
||||
"heading_opposite__pitch_opposite": 8.244842702849073
|
||||
},
|
||||
"pair_count": 91,
|
||||
"residual_rms_deg": 2.8406184024910064,
|
||||
"residual_median_deg": 1.624215282655001,
|
||||
"residual_p95_deg": 6.087913500787023,
|
||||
"rotation_std_deg": [
|
||||
1.6029337786454214,
|
||||
1.0761604678920118,
|
||||
6.305203755276785
|
||||
],
|
||||
"information_singular_values": [
|
||||
2941.184567996004,
|
||||
1268.1548949190246,
|
||||
82.52753961323997
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_175910_180530": 2.8406184024910064
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.0537 deg",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -1,28 +0,0 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9974493913373024,
|
||||
-0.07135460605626225,
|
||||
0.0017977528752911507
|
||||
],
|
||||
[
|
||||
0.0713431523484532,
|
||||
0.997435050761847,
|
||||
0.005785682733905998
|
||||
],
|
||||
[
|
||||
-0.0022059768426676615,
|
||||
-0.00564266836413856,
|
||||
0.9999816468115312
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -1,24 +0,0 @@
|
||||
# RTK-IMU V3 正式结果集
|
||||
|
||||
本目录只提交复现工程结论所需的审核结果。运行过程中的调试输出、状态快照、逐窗口 checkpoint 和旧版结果保留在本机，由仓库根目录 `.gitignore` 排除。
|
||||
|
||||
## 当前结论
|
||||
|
||||
- 固定旋转来源：`R2G_gravity_level_prior`。
|
||||
- 工程候选杆臂：`l_I = p_ANT1^I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||
- `data_only_translation_accepted=false`。
|
||||
- `engineering_translation_accepted=false`，原因是独立传播验证仍受公共加速度偏差影响。
|
||||
- `independent_extrinsic_sensitive_validation_passed=true`；当前结果可描述为机械杆臂经动态数据一致性验证的工程候选，不得描述为 data-only 平移标定结果。
|
||||
|
||||
## 文件说明
|
||||
|
||||
- `engineering_release_decision.json`：最终状态、变换和放行判定。
|
||||
- `mechanical_prior_engineering_47_window.json`：相同 47 个非重叠窗口上的 free/fixed/prior 对比。
|
||||
- `node_graph_free_information_selected_mechanical.json`：冻结的无先验 free-solve 基线。
|
||||
- `lever_information_window_selection*.json`：窗口选择及实际边缘信息复核。
|
||||
- `mechanical_prior_engineering_heldout.json`：未参与标定窗口的 held-out 验证。
|
||||
- `heldout_independent_innovation.json`、`heldout_nonconverged_retry.json`：独立创新与失败窗口重试结果。
|
||||
- `mechanical_prior_rotation_sensitivity.json`：固定候选杆臂的旋转扰动敏感性结果。
|
||||
- `propagation_bias_root_cause_audit.json`：独立传播公共加速度误差根因审计。
|
||||
|
||||
结果 JSON 是审核快照；需要重新生成时应通过 `tools/` 中相应入口运行，且不得提交运行产生的 checkpoint 或 `.npz` 状态文件。
|
||||
@@ -1,128 +0,0 @@
|
||||
{
|
||||
"scope": "final mechanical-prior RTK-IMU engineering release decision",
|
||||
"no_refit_performed": true,
|
||||
"data_only_full_free_called": false,
|
||||
"bootstrap_called": false,
|
||||
"loo_called": false,
|
||||
"covariance_retuned": false,
|
||||
"new_window_selection_called": false,
|
||||
"parser_R0_modified": false,
|
||||
"data_only_translation_accepted": false,
|
||||
"translation_refined_by_data": false,
|
||||
"mechanical_prior_consistent_with_calibration": true,
|
||||
"heldout_physical_validation_passed": true,
|
||||
"heldout_physical_gate_checks": {
|
||||
"all_267_converged_after_retry": true,
|
||||
"BEST_position_vector_p95_le_0p20_m": true,
|
||||
"Doppler_vector_p95_le_0p50_m_s": true,
|
||||
"HPR_normalized_p95_le_4": true,
|
||||
"preintegration_normalized_p95_le_3": true
|
||||
},
|
||||
"heldout_statistical_scale_passed": false,
|
||||
"heldout_postfit_chi_square_per_dof": 0.12733913101711092,
|
||||
"heldout_covariance_underdispersion_warning": true,
|
||||
"independent_heldout_innovation_passed": false,
|
||||
"common_constant_acceleration_error_detected": true,
|
||||
"independent_propagation_validation_passed": false,
|
||||
"independent_extrinsic_sensitive_validation_passed": true,
|
||||
"rotation_sensitivity_passed": true,
|
||||
"engineering_translation_acceptance_formula": "mechanical_prior_consistent_with_calibration AND heldout_physical_validation_passed AND independent_heldout_innovation_passed AND rotation_sensitivity_passed",
|
||||
"engineering_translation_accepted": false,
|
||||
"result_nature": "mechanically anchored + dynamically validated",
|
||||
"summary": "Translation is mechanically anchored and dynamically validated. The current dataset does not independently observe translation accurately enough for data-only calibration, and does not provide meaningful refinement beyond the mechanical prior.",
|
||||
"forbidden_descriptions": [
|
||||
"data-only calibrated translation",
|
||||
"dynamically refined mechanical lever"
|
||||
],
|
||||
"candidate_l_I_engineering_m": [
|
||||
-0.45180151590212486,
|
||||
-0.26447498198536895,
|
||||
0.7314656114613277
|
||||
],
|
||||
"candidate_T_RTK_IMU": [
|
||||
[
|
||||
0.9999999761265028,
|
||||
-0.00021395911860711003,
|
||||
-4.436766104011944e-05,
|
||||
0.45177737170031523
|
||||
],
|
||||
[
|
||||
0.00021360059847267874,
|
||||
0.9999685415936667,
|
||||
-0.007929072948303898,
|
||||
0.27036301129056084
|
||||
],
|
||||
[
|
||||
4.606276276360097e-05,
|
||||
0.007929063282050246,
|
||||
0.9999685634227163,
|
||||
-0.7293247665913783
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"candidate_T_IMU_RTK": [
|
||||
[
|
||||
0.9999999761265029,
|
||||
0.00021360059847267876,
|
||||
4.606276276360098e-05,
|
||||
-0.45180151590212486
|
||||
],
|
||||
[
|
||||
-0.00021395911860711008,
|
||||
0.9999685415936669,
|
||||
0.007929063282050248,
|
||||
-0.264474981985369
|
||||
],
|
||||
[
|
||||
-4.436766104011945e-05,
|
||||
-0.0079290729483039,
|
||||
0.9999685634227164,
|
||||
0.7314656114613278
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"candidate_transform_inverse_error_norm": 1.3597553244868544e-16,
|
||||
"l_I_engineering_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"T_IMU_RTK": null,
|
||||
"transform_convention": {
|
||||
"equation": "p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU",
|
||||
"translation": "t_RTK_IMU = -R_RTK_IMU * l_I",
|
||||
"RTK_origin": "ANT1 phase center"
|
||||
},
|
||||
"rotation_source": "R2G_gravity_level_prior",
|
||||
"translation_conditional_on_rotation": true,
|
||||
"evidence": {
|
||||
"calibration_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_47_window.json",
|
||||
"heldout_postfit_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_heldout.json",
|
||||
"innovation_path": "artifacts\\rtk_imu_calibration_v3\\heldout_independent_innovation.json",
|
||||
"sensitivity_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_rotation_sensitivity.json",
|
||||
"convergence_retry_path": "artifacts\\rtk_imu_calibration_v3\\heldout_nonconverged_retry.json",
|
||||
"propagation_root_cause_path": "artifacts\\rtk_imu_calibration_v3\\propagation_bias_root_cause_audit.json",
|
||||
"posterior_prior_variance_ratio": [
|
||||
0.9841028247436912,
|
||||
0.9831529061304226,
|
||||
0.9921155953185713
|
||||
],
|
||||
"heldout_convergence_after_retry": 1.0,
|
||||
"rotation_sensitivity_summary": {
|
||||
"max_abs_delta_l_xyz_m": [
|
||||
0.0024980444199615426,
|
||||
0.0006588674774769543,
|
||||
0.0007172660986609625
|
||||
],
|
||||
"max_delta_l_norm_m": 0.0025582764807350012,
|
||||
"max_transform_translation_delta_norm_m": 0.00686033304738171
|
||||
}
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
-1302
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,52 +0,0 @@
|
||||
# RTK–IMU 候选数据清点（2026-08-20）
|
||||
|
||||
本目录记录 0808、0815、0819 三批 LiDAR/IMU 会话所对应的 G90 RTK 原始记录与当前导出状态。此处只做数据血缘和可用性评估，尚未求解 `T_RTK_IMU`。
|
||||
|
||||
## 时间与质量约定
|
||||
|
||||
- 原始 RTK 为 Wheeltec G90 V2 `.rscap`，包含 `$GNGGA/$GPGGA` 位置和 `$GNHPR` heading/pitch/roll。
|
||||
- 切窗使用 NMEA 报文自带的测量 UTC，再通过每个会话的 IMU device→host affine clock 映射到 IMU 设备时间。
|
||||
- 主机接收时间比 NMEA 测量时间晚约 3–4 s，且有波动；不能按接收时间直接切窗。
|
||||
- 当前项目门控按 GGA `fix_quality=4` 和 HPR `heading_quality∈{4,5}` 判断固定位置/有效航向。
|
||||
- `heading_valid` 未覆盖全部 GGA 行主要因为 GGA 与 HPR 频率不同、最近邻匹配阈值为 80 ms；不代表该会话航向整体失效。
|
||||
|
||||
## 数据映射与质量
|
||||
|
||||
详细机器可读清单见 `rtk_session_inventory.csv`。
|
||||
|
||||
| 会话 | RTK结论 | 适合的标定作用 |
|
||||
| --- | --- | --- |
|
||||
| `priority_174005_174515` | 100% GGA质量4；有效航向约309 s；yaw变化约485° | 多圈 yaw 与 XY 杠杆臂 |
|
||||
| `priority_174905_175450` | 100% GGA质量4；Pitch跨度约11.4°；XY约58×22 m | 强候选：yaw、pitch、XY/Z耦合解除 |
|
||||
| `priority_175910_180530` | 100% GGA质量4；Pitch跨度约13.6°；XY约144×59 m | 最强候选：长基线、pitch、平移 |
|
||||
| `slope_190548_190730` | 100% GGA质量4；Pitch跨度约7.4°；约102 s | 坡度/Pitch补充 |
|
||||
| `circle_193412_193642` | 100% GGA质量4；yaw变化约445° | 平面旋转与XY杠杆臂 |
|
||||
| `loop_194223_195003` | 仅1个异常GGA；yaw累计变化约1203°；约460 s | 最强 yaw/多圈转弯候选 |
|
||||
| `accel_195608_195958` | 100% GGA质量4；XY约46×23 m；约230 s | 加减速、速度和水平杠杆臂 |
|
||||
| `motion_sms_154023_154359` | 全窗仅约49%为质量4；可用连续子段约100 s | 仅用15:41:28.2–15:43:08.15固定解/有效航向段 |
|
||||
|
||||
## 导出状态
|
||||
|
||||
- 0808 当前 `sessions_v2_device_affine` 原先没有 RTK CSV，本次已从原始 G90 `.rscap` 按 NMEA 测量 UTC补导三个会话，未覆盖旧文件。
|
||||
- 0815 `sessions_v2_device_affine` 与 dense5 slope 已经采用同一测量时间导出规则，无需重导。
|
||||
- 0819 `sessions_v2_device_affine` 与 dense5 motion 已经采用同一规则；需要在求解器中按质量与时间连续段过滤，而不是重新解释为全窗固定解。
|
||||
- 0808 旧 `sessions_v1_host_aligned_00` RTK CSV 不含 `t_measurement_utc_s` 字段，只保留作历史对照；新求解应使用 `sessions_v2_device_affine`。
|
||||
|
||||
## 已发现的 LiDAR–RTK 资料边界
|
||||
|
||||
`D:\data\calibration_usable_20260808\rtk_lidar_station_report*` 保存的是静止站点候选：27个站点、29个候选段，并非包含 `T_RTK_lidar`、协方差和留一验证的正式手眼结果。本轮在 0808 数据目录的 JSON/YAML/Markdown/CSV/日志中没有找到 `T_RTK_lidar` 矩阵。若要通过链式关系得到 LiDAR–IMU，需要继续定位原手眼结果及其坐标约定：
|
||||
|
||||
```text
|
||||
T_IMU_lidar = inverse(T_RTK_IMU) @ T_RTK_lidar
|
||||
```
|
||||
|
||||
## 初步可行性判断
|
||||
|
||||
这些数据足以启动直接 RTK–IMU 标定，且比当前纯 LiDAR–IMU Phase-B 更有希望约束 XY：RTK 提供绝对位置，GNHPR 提供航向和 Pitch，多会话包含长基线、转弯、加减速与坡度。仍需注意：
|
||||
|
||||
1. GNHPR roll 的变化仅约0.006°–0.065°，不能指望它提供有效 roll 激励。
|
||||
2. 应先用 RTK heading/pitch 角速度与 IMU gyro 做残余时间偏置和坐标轴验证，再求旋转。
|
||||
3. 平移应使用 RTK绝对位置 + IMU预积分的联合状态模型，估计共享 `T_RTK_IMU`、每会话速度/bias；不应把RTK轨迹简单二次差分后直接最小二乘。
|
||||
4. `motion_sms` 必须仅使用其连续固定解子段。
|
||||
5. 跨0808/0815/0819时应使用每会话IMU bias，外参共享，并检查安装期间是否发生机械变动。
|
||||
|
||||
@@ -1,9 +0,0 @@
|
||||
session,batch,raw_rtk_rscap,current_rtk_csv,rows,fixed_gga_ratio,fixed_heading_valid_rows,valid_duration_s,xy_robust_span_x_m,xy_robust_span_y_m,altitude_robust_span_m,heading_unwrapped_span_deg,pitch_robust_span_deg,roll_robust_span_deg,notes
|
||||
priority_174005_174515,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174005_174515\rtk.csv,4780,1.0000,4085,309.45,12.045,17.380,0.112,485.411,3.462,0.012,newly exported from NMEA measurement UTC
|
||||
priority_174905_175450,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174905_175450\rtk.csv,5211,1.0000,4454,344.90,58.436,21.923,0.277,350.616,11.379,0.008,newly exported from NMEA measurement UTC
|
||||
priority_175910_180530,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_175910_180530\rtk.csv,5786,1.0000,4957,379.85,143.775,58.657,0.971,260.213,13.574,0.065,newly exported from NMEA measurement UTC
|
||||
slope_190548_190730,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-110519.251_e0edf32b-c82b-4a38-b5df-2c0e4cac364f.rscap,D:\data\0815\sessions_v2_device_affine\slope_190548_190730\rtk.csv,1578,1.0000,1382,101.90,11.354,18.567,0.714,155.356,7.399,0.026,root is named 0815 but raw measurement date is 2026-08-14 local
|
||||
circle_193412_193642,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-113355.356_b4b37794-d4d6-433e-87e9-037cad5517d1.rscap,D:\data\0815\sessions_v2_device_affine\circle_193412_193642\rtk.csv,2162,1.0000,1792,147.25,10.105,10.277,0.097,444.994,2.834,0.006,raw capture has truncated tail but target messages are checksum-valid
|
||||
loop_194223_195003,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-114200.914_36911c82-f8c0-451b-99e3-f5b663ec6115.rscap,D:\data\0815\sessions_v2_device_affine\loop_194223_195003\rtk.csv,6741,0.9999,5647,459.90,11.555,18.800,0.104,1202.601,2.727,0.009,one malformed/non-fixed GGA excluded
|
||||
accel_195608_195958,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-115547.472_8722f326-8314-4db1-9ec4-bce185c54a78.rscap,D:\data\0815\sessions_v2_device_affine\accel_195608_195958\rtk.csv,3456,1.0000,2923,229.90,45.542,22.610,0.130,188.236,3.248,0.010,measurement-time export already present
|
||||
motion_sms_154023_154359,0819,D:\data\0819\raw_serial_capture_v2\wheeltec-g90_20260819-074023.329_3d9da6eb-7ef8-4f7b-9262-9193328238b0.rscap,D:\data\0819\dense5\sessions_v2_device_affine\motion_sms_154023_154359\rtk.csv,3294,0.4924,1431,99.95,12.679,17.356,0.839,308.864,8.985,0.020,use only 15:41:28.200-15:43:08.150 fixed+valid sub-window
|
||||
|
@@ -0,0 +1,13 @@
|
||||
# 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帧。
|
||||
@@ -0,0 +1,119 @@
|
||||
#!/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())
|
||||
@@ -0,0 +1,43 @@
|
||||
#!/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())
|
||||
@@ -0,0 +1,81 @@
|
||||
#!/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()
|
||||
@@ -0,0 +1,349 @@
|
||||
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)
|
||||
|
||||
def default(obj):
|
||||
if isinstance(obj, (np.bool_, np.integer)):
|
||||
return obj.item()
|
||||
if isinstance(obj, np.floating):
|
||||
return float(obj)
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
raise TypeError(f"Object of type {type(obj).__name__} is not JSON serializable")
|
||||
|
||||
path.write_text(json.dumps(document, ensure_ascii=False, indent=2, default=default), encoding="utf-8")
|
||||
|
||||
|
||||
def inverse(t: np.ndarray) -> np.ndarray:
|
||||
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 wrap180(deg: float) -> float:
|
||||
return (deg + 180.0) % 360.0 - 180.0
|
||||
|
||||
|
||||
def yaw_deg_of(transform: np.ndarray) -> float:
|
||||
return float(Rotation.from_matrix(transform[:3, :3]).as_euler("xyz", degrees=True)[2])
|
||||
|
||||
|
||||
def mechanical_self_consistency(document: dict) -> dict:
|
||||
"""Reject mechanical JSON that mixes incompatible baseline / body definitions."""
|
||||
translation = np.asarray(document["translation_m"], float)
|
||||
yaw = float(document["rotation_rpy_deg_xyz"][2])
|
||||
side = str(document.get("baseline_points", "")).strip().lower()
|
||||
frame_mode = str(document.get("frame_mode", "")).strip().lower()
|
||||
heading_offset = float(document.get("heading_offset_deg", 0.0) or 0.0)
|
||||
vehicle_forward = (
|
||||
frame_mode == "vehicle_forward_heading_offset"
|
||||
or abs(heading_offset) > 1e-6
|
||||
)
|
||||
issues: list[str] = []
|
||||
|
||||
if vehicle_forward:
|
||||
if abs(wrap180(yaw)) > 15.0:
|
||||
issues.append(
|
||||
f"vehicle-forward mechanical initial requires yaw≈0°, got {yaw:g}°"
|
||||
)
|
||||
lever = document.get("vehicle_flu_lever_master_to_lidar_m")
|
||||
if lever is not None:
|
||||
if float(np.linalg.norm(translation - np.asarray(lever, float))) > 0.05:
|
||||
issues.append(
|
||||
"vehicle-forward translation_m must match vehicle_flu_lever_master_to_lidar_m"
|
||||
)
|
||||
if abs(heading_offset + 90.0) > 1e-6 and abs(heading_offset - 90.0) > 1e-6:
|
||||
issues.append(
|
||||
f"vehicle-forward heading_offset_deg should be ±90 for left/right baseline, got {heading_offset:g}"
|
||||
)
|
||||
elif side in {"vehicle_left", "left"}:
|
||||
if abs(wrap180(yaw - (-90.0))) > 15.0:
|
||||
issues.append(
|
||||
f"baseline_points=vehicle_left requires yaw≈-90°, got {yaw:g}°"
|
||||
)
|
||||
if translation[0] <= 0.0 or translation[1] <= 0.0:
|
||||
issues.append(
|
||||
"baseline_points=vehicle_left expects +X/+Y lever in RTK baseline frame "
|
||||
f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
|
||||
)
|
||||
elif side in {"vehicle_right", "right"}:
|
||||
if abs(wrap180(yaw - 90.0)) > 15.0:
|
||||
issues.append(
|
||||
f"baseline_points=vehicle_right requires yaw≈+90°, got {yaw:g}°"
|
||||
)
|
||||
# Swapped but centerline-symmetric master (vehicle left): +X / -Y in baseline frame.
|
||||
if translation[0] <= 0.0 or translation[1] >= 0.0:
|
||||
issues.append(
|
||||
"baseline_points=vehicle_right (master on vehicle left, baseline to the right) "
|
||||
"expects +X/-Y lever in RTK baseline frame "
|
||||
f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
|
||||
)
|
||||
else:
|
||||
left_xy = translation[0] > 0.05 and translation[1] > 0.05
|
||||
right_xy = translation[0] < -0.05 and translation[1] < -0.05
|
||||
swapped_right_xy = translation[0] > 0.05 and translation[1] < -0.05
|
||||
if left_xy and abs(wrap180(yaw - 90.0)) <= 15.0:
|
||||
issues.append(
|
||||
"mixed baseline definition: +X/+Y translation (left-baseline) combined with yaw≈+90° (right-baseline)"
|
||||
)
|
||||
if right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
|
||||
issues.append(
|
||||
"mixed baseline definition: -X/-Y translation combined with yaw≈-90°"
|
||||
)
|
||||
if swapped_right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
|
||||
issues.append(
|
||||
"mixed baseline definition: +X/-Y translation (swapped-master right-baseline) "
|
||||
"combined with yaw≈-90° (left-baseline)"
|
||||
)
|
||||
|
||||
return {
|
||||
"baseline_points": side or None,
|
||||
"frame_mode": frame_mode or None,
|
||||
"heading_offset_deg": heading_offset,
|
||||
"consistent": not issues,
|
||||
"issues": issues,
|
||||
}
|
||||
|
||||
|
||||
def solution_matches_declared_side(solution: np.ndarray, document: dict) -> dict:
|
||||
"""Check whether the solved extrinsic agrees with the mechanical baseline side."""
|
||||
side = str(document.get("baseline_points", "")).strip().lower()
|
||||
yaw = yaw_deg_of(solution)
|
||||
t = solution[:3, 3]
|
||||
expected_yaw = float(document["rotation_rpy_deg_xyz"][2])
|
||||
yaw_err = abs(wrap180(yaw - expected_yaw))
|
||||
xy_err = float(np.linalg.norm(t[:2] - np.asarray(document["translation_m"][:2], float)))
|
||||
z_err = float(abs(t[2] - float(document["translation_m"][2])))
|
||||
|
||||
opposite_yaw = abs(wrap180(yaw - expected_yaw) - 180.0) <= 15.0 or abs(
|
||||
wrap180(yaw - expected_yaw) + 180.0
|
||||
) <= 15.0
|
||||
# Same XY sign as mechanical but yaw flipped ~180° (classic mixed inheritance).
|
||||
same_xy_sign = (t[0] * float(document["translation_m"][0]) > 0.0) and (
|
||||
t[1] * float(document["translation_m"][1]) > 0.0
|
||||
)
|
||||
mixed_inheritance = same_xy_sign and opposite_yaw
|
||||
|
||||
return {
|
||||
"baseline_points": side or None,
|
||||
"solution_yaw_deg": yaw,
|
||||
"expected_yaw_deg": expected_yaw,
|
||||
"yaw_error_deg": yaw_err,
|
||||
"xy_error_m": xy_err,
|
||||
"z_error_m": z_err,
|
||||
"mixed_translation_rotation_inheritance": bool(mixed_inheritance),
|
||||
"near_expected_pose": bool(yaw_err <= 15.0 and xy_err <= 0.25),
|
||||
}
|
||||
|
||||
|
||||
def coordinate_contract_audit(raw: dict) -> dict:
|
||||
"""Audit mechanical self-consistency and solution agreement.
|
||||
|
||||
A near-180-degree disagreement is not auto-corrected: it normally means
|
||||
that one physical forward-axis / baseline-direction statement is reversed.
|
||||
"""
|
||||
path_text = raw.get("solver_initial_extrinsic")
|
||||
if not path_text:
|
||||
return {
|
||||
"status": "mechanical_initial_not_available",
|
||||
"requires_physical_axis_confirmation": False,
|
||||
}
|
||||
path = Path(path_text)
|
||||
if not path.exists():
|
||||
return {
|
||||
"status": "mechanical_initial_file_missing",
|
||||
"requires_physical_axis_confirmation": False,
|
||||
"mechanical_initial_path": str(path),
|
||||
}
|
||||
initial_document = load(path)
|
||||
initial = np.asarray(initial_document["matrix_4x4"], float)
|
||||
solution = np.asarray(raw["matrix_4x4"], float)
|
||||
comparison = delta(initial, solution)
|
||||
near_180 = abs(comparison["rotation_deg"] - 180.0) <= 15.0
|
||||
mech_check = mechanical_self_consistency(initial_document)
|
||||
match = solution_matches_declared_side(solution, initial_document)
|
||||
|
||||
if not mech_check["consistent"]:
|
||||
status = "mechanical_initial_inconsistent"
|
||||
elif match["mixed_translation_rotation_inheritance"] or near_180:
|
||||
status = "near_180_degree_axis_conflict"
|
||||
elif not match["near_expected_pose"]:
|
||||
status = "solution_disagrees_with_mechanical_baseline_side"
|
||||
else:
|
||||
status = "no_near_180_degree_axis_conflict"
|
||||
|
||||
requires = status != "no_near_180_degree_axis_conflict"
|
||||
return {
|
||||
"status": status,
|
||||
"requires_physical_axis_confirmation": requires,
|
||||
"mechanical_initial_path": str(path.resolve()),
|
||||
"mechanical_self_consistency": mech_check,
|
||||
"solution_vs_declared_baseline_side": match,
|
||||
"solution_relative_to_mechanical_initial": comparison,
|
||||
"note": (
|
||||
"No automatic 180-degree correction was applied. Confirm static GNHPR "
|
||||
"left/right vs vehicle heading and Helios +X vs vehicle forward before deployment."
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
def corrected(raw: dict, backend: str, reference_height: float, heading_offset_deg: float) -> dict:
|
||||
baseline_frame = abs(heading_offset_deg) <= 1e-12
|
||||
x_axis = (
|
||||
"horizontal projection of the rawHeading baseline direction reported by the receiver"
|
||||
if baseline_frame else
|
||||
"vehicle forward after applying the configured G90 heading offset"
|
||||
)
|
||||
return {
|
||||
"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": x_axis,
|
||||
"y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": f"90 - (rawHeadingDeg + {heading_offset_deg:g})",
|
||||
"frame_mode": "baseline_raw_heading" if baseline_frame else "vehicle_forward_heading_offset",
|
||||
},
|
||||
"LiDAR": {
|
||||
"description": "raw Helios sensor frame from points_raw polar decode",
|
||||
"x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
|
||||
"y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
|
||||
"z_axis": "up",
|
||||
"origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ",
|
||||
},
|
||||
},
|
||||
"backend": backend,
|
||||
"measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")),
|
||||
"solver_initial_extrinsic": raw.get("solver_initial_extrinsic"),
|
||||
"body_heading_offset_deg": heading_offset_deg,
|
||||
"body_heading_offset_used": abs(heading_offset_deg) > 1e-12,
|
||||
"body_antenna_lever_xy_used": False,
|
||||
"translation_m": raw["translation_m"],
|
||||
"rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
|
||||
"quaternion_xyzw": raw["quaternion_xyzw"],
|
||||
"coordinate_contract_audit": coordinate_contract_audit(raw),
|
||||
"matrix_4x4": raw["matrix_4x4"],
|
||||
"quality": {
|
||||
"stations": raw["estimation"]["stations"],
|
||||
"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)
|
||||
parser.add_argument("--heading-offset-deg", 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, args.heading_offset_deg
|
||||
)
|
||||
write(path.with_name("extrinsic_rtk_lidar.json"), document)
|
||||
docs[backend] = document
|
||||
|
||||
open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
|
||||
small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
|
||||
final = dict(docs["consensus"])
|
||||
needs_axis_confirmation = bool(
|
||||
final["coordinate_contract_audit"]["requires_physical_axis_confirmation"]
|
||||
)
|
||||
status = final["coordinate_contract_audit"]["status"]
|
||||
reason_map = {
|
||||
"mechanical_initial_inconsistent": (
|
||||
"Mechanical initial mixes incompatible baseline-left/right translation and yaw; "
|
||||
"fix run/rtk_lidar_mechanical_initial.json before trusting deployment"
|
||||
),
|
||||
"near_180_degree_axis_conflict": (
|
||||
"Physical axis confirmation is required because the data-driven solution differs "
|
||||
"from the declared mechanical initial by approximately 180 degrees "
|
||||
"(or inherits mixed translation/rotation signs)"
|
||||
),
|
||||
"solution_disagrees_with_mechanical_baseline_side": (
|
||||
"Solution yaw/XY disagree with the declared mechanical baseline side; "
|
||||
"confirm static GNHPR direction before deployment"
|
||||
),
|
||||
}
|
||||
final["selection"] = {
|
||||
"recommended": not needs_axis_confirmation,
|
||||
"reason": (
|
||||
reason_map.get(
|
||||
status,
|
||||
"Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
||||
)
|
||||
),
|
||||
"open3d_vs_small_gicp": delta(open_t, small_t),
|
||||
}
|
||||
|
||||
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"],
|
||||
"coordinate_contract_status": final["coordinate_contract_audit"]["status"],
|
||||
"recommended_for_deployment": final["selection"]["recommended"],
|
||||
},
|
||||
"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()
|
||||
@@ -0,0 +1,87 @@
|
||||
#!/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()
|
||||
@@ -0,0 +1,875 @@
|
||||
#!/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 transform_params(transform):
|
||||
from scipy.spatial.transform import Rotation
|
||||
transform = np.asarray(transform, float)
|
||||
return np.r_[transform[:3, 3], Rotation.from_matrix(transform[:3, :3]).as_rotvec()]
|
||||
|
||||
|
||||
def load_extrinsic_matrix(path):
|
||||
document = json.loads(Path(path).read_text(encoding="utf-8-sig"))
|
||||
transform = np.asarray(document["matrix_4x4"], dtype=float)
|
||||
if transform.shape != (4, 4):
|
||||
raise ValueError("initial extrinsic matrix_4x4 must be 4x4")
|
||||
return transform
|
||||
|
||||
|
||||
def inverse_transform(transform):
|
||||
answer = np.eye(4)
|
||||
answer[:3, :3] = transform[:3, :3].T
|
||||
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)
|
||||
time_key = "lidar_association_time_ns" if "lidar_association_time_ns" in data else "unix_time_ns"
|
||||
timestamp = float(np.ravel(data[time_key])[0]) / 1e9
|
||||
counter = int(np.ravel(data["frame_counter"])[0])
|
||||
distance = raw[:, 0] * 0.001
|
||||
azimuth = np.deg2rad(raw[:, 1])
|
||||
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 make_global_features(points, voxel):
|
||||
import open3d as o3d
|
||||
cloud = make_o3d_cloud(points, voxel)
|
||||
cloud.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(
|
||||
radius=voxel * 2.5, max_nn=50
|
||||
))
|
||||
features = o3d.pipelines.registration.compute_fpfh_feature(
|
||||
cloud,
|
||||
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel * 5.0, max_nn=100),
|
||||
)
|
||||
return cloud, features
|
||||
|
||||
|
||||
def global_lidar_initialization(target_features, source_features, args, pair_seed):
|
||||
"""Estimate source-to-target motion from LiDAR geometry without RTK or an extrinsic."""
|
||||
import open3d as o3d
|
||||
registration = o3d.pipelines.registration
|
||||
target_cloud, target_fpfh = target_features
|
||||
source_cloud, source_fpfh = source_features
|
||||
attempts = []
|
||||
for attempt in range(args.global_ransac_attempts):
|
||||
o3d.utility.random.seed(int(pair_seed + attempt))
|
||||
answer = registration.registration_ransac_based_on_feature_matching(
|
||||
source_cloud,
|
||||
target_cloud,
|
||||
source_fpfh,
|
||||
target_fpfh,
|
||||
True,
|
||||
args.global_correspondence,
|
||||
registration.TransformationEstimationPointToPoint(False),
|
||||
4,
|
||||
[
|
||||
registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
|
||||
registration.CorrespondenceCheckerBasedOnDistance(args.global_correspondence),
|
||||
],
|
||||
registration.RANSACConvergenceCriteria(
|
||||
args.global_ransac_iterations, args.global_ransac_confidence
|
||||
),
|
||||
)
|
||||
attempts.append({
|
||||
"transform": np.asarray(answer.transformation, float),
|
||||
"fitness": float(answer.fitness),
|
||||
"inlier_rmse_m": float(answer.inlier_rmse),
|
||||
})
|
||||
best = max(attempts, key=lambda item: (item["fitness"], -item["inlier_rmse_m"]))
|
||||
return {
|
||||
"transform": best["transform"],
|
||||
"method": "LiDAR-only FPFH RANSAC",
|
||||
"fitness": best["fitness"],
|
||||
"inlier_rmse_m": best["inlier_rmse_m"],
|
||||
"attempts": [
|
||||
{key: value for key, value in item.items() if key != "transform"}
|
||||
for item in attempts
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def align_open3d(target, source, initial, voxels, correspondences, iterations):
|
||||
import open3d as o3d
|
||||
registration = o3d.pipelines.registration
|
||||
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)]
|
||||
global_features = [make_global_features(points[0], args.global_voxel)
|
||||
for points in split]
|
||||
rng = np.random.default_rng(args.seed)
|
||||
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
|
||||
accepted_transforms = {}
|
||||
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 args.max_reference_translation is not None and translation > args.max_reference_translation:
|
||||
continue
|
||||
if translation < args.min_translation and rotation < args.min_rotation:
|
||||
continue
|
||||
global_initial = global_lidar_initialization(
|
||||
global_features[i], global_features[j], args,
|
||||
args.seed + i * 1009 + j * 9176,
|
||||
)
|
||||
initial_b = global_initial["transform"]
|
||||
target_fit, target_holdout = split[i]
|
||||
source_fit, source_holdout = split[j]
|
||||
forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
|
||||
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": global_initial["method"],
|
||||
"global_lidar_initialization": {
|
||||
key: value for key, value in global_initial.items() if key != "transform"
|
||||
},
|
||||
"B_ij_4x4": forward["transform"].tolist(),
|
||||
"backend": args.backend, "backend_converged": forward["converged"],
|
||||
"backend_iterations": forward["iterations"],
|
||||
"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",
|
||||
"registration_initial_extrinsic": None,
|
||||
"selection_is_X_independent": True,
|
||||
"B_estimation_is_RTK_independent": True,
|
||||
"candidate_pair_selection_uses_reference_motion": True,
|
||||
"initialization_warning": None,
|
||||
"stations": len(stations), "candidate_pairs": len(reports),
|
||||
"accepted_pairs": len(accepted_a),
|
||||
"parameters": vars(args),
|
||||
"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)
|
||||
center = (transform_params(load_extrinsic_matrix(args.initial_extrinsic))
|
||||
if args.initial_extrinsic else np.zeros(6))
|
||||
starts = [center]
|
||||
for _ in range(args.solver_multistart - 1):
|
||||
starts.append(center + 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": bool(args.initial_extrinsic),
|
||||
"solver_initial_extrinsic": (
|
||||
str(Path(args.initial_extrinsic).resolve()) if args.initial_extrinsic else None
|
||||
),
|
||||
"translation_m": x[:3, 3].tolist(),
|
||||
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
|
||||
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
|
||||
"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)
|
||||
# Default ROI for ~2 m roof LiDAR (Z-up). Override for other mounting heights.
|
||||
ground.add_argument("--z-min", type=float, default=-2.5); ground.add_argument("--z-max", type=float, default=-1.5)
|
||||
ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025)
|
||||
ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500)
|
||||
ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground)
|
||||
|
||||
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("--max-reference-translation", type=float)
|
||||
pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0)
|
||||
pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0)
|
||||
pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0)
|
||||
pairs.add_argument("--min-roi-points", type=int, default=1000)
|
||||
pairs.add_argument("--global-voxel", type=float, default=0.50)
|
||||
pairs.add_argument("--global-correspondence", type=float, default=1.25)
|
||||
pairs.add_argument("--global-ransac-attempts", type=int, default=3)
|
||||
pairs.add_argument("--global-ransac-iterations", type=int, default=100000)
|
||||
pairs.add_argument("--global-ransac-confidence", type=float, default=0.999)
|
||||
pairs.add_argument("--holdout-fraction", type=float, default=0.20)
|
||||
pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08])
|
||||
pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25])
|
||||
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("--initial-extrinsic")
|
||||
calibrate.add_argument("--translation-sigma", type=float, default=0.05)
|
||||
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
|
||||
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
|
||||
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()
|
||||
@@ -0,0 +1,294 @@
|
||||
#!/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()
|
||||
@@ -1,51 +0,0 @@
|
||||
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
|
||||
@@ -1,86 +0,0 @@
|
||||
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]]
|
||||
@@ -1,51 +0,0 @@
|
||||
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,209 +0,0 @@
|
||||
# 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/交付物同步 |
|
||||
@@ -1,195 +0,0 @@
|
||||
# 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
@@ -1,109 +0,0 @@
|
||||
# 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 帧，场景有墙/柱等结构，含转弯
|
||||
@@ -1,92 +0,0 @@
|
||||
# 从 LiDAR–IMU 平移困难到 RTK–IMU 工程外参
|
||||
|
||||
本文解释为什么仓库同时保留 LiDAR–IMU 与 RTK–IMU 两条链路、为什么本轮将天线–IMU 外参的工程验证重心转向 RTK–IMU，以及两条链路如何避免互相混淆。
|
||||
|
||||
## 决策结论
|
||||
|
||||
保留两条链路，不删除 LiDAR–IMU。LiDAR–IMU 仍然是雷达与 IMU 旋转/时间关系的有效工具，也记录了平移失败的具体证据；RTK–IMU 是针对“天线相位中心到 IMU 的杆臂”采用的独立观测模型。它们共享基础数学与 IMU 预积分，但不共享求解器或验收结论。
|
||||
|
||||
当前 RTK–IMU 结果应描述为：**机械测量锚定的杆臂，经 RTK+IMU 动态数据一致性验证的工程候选**。它不是 data-only 平移标定，也不是已正式 engineering 放行的结果。
|
||||
|
||||
## LiDAR–IMU 为什么暂时不能交平移
|
||||
|
||||
LiDAR–IMU 链路将相邻关键帧的点云配准相对运动，与同一区间 IMU 预积分运动做手眼/联合优化。当前实车数据中旋转和时间关系可得到稳定结果，但平移受两个独立问题限制：
|
||||
|
||||
1. **IMU 位移预积分不可靠。** 加速度积分对重力与加速度零偏敏感；当前 `Δp` 尤其 Z 分量会发散。已有证据显示点云相对旋转残差中位约 `0.16°`，而 IMU 位移增量模长常大于 `1 m`，说明这不是单纯旋转误差。
|
||||
2. **运动激励不足。** 现有重点窗口以水平转弯为主，缺少连续坡道/俯仰激励，竖直杆臂弱可观。即使加入很强的平移先验，求解也可能只是先验回显，而非数据观测。
|
||||
|
||||
因此 LiDAR–IMU 的状态是：旋转与时间对齐可用，`full_se3`/平移不可作为安装参数交付。详细证据在 [LiDAR–IMU 现状与问题](20260808_LiDAR-IMU标定现状与问题.md) 和 [问题清单](问题清单.md)。
|
||||
|
||||
## 为什么 RTK–IMU 更适合验证天线杆臂
|
||||
|
||||
RTK 直接给出天线相位中心的 GNSS 位置与 Doppler 速度；双天线还给出 ANT1→ANT2 基线方向。对固定旋转 `R_WI` 和杆臂 `l_I = p_ANT1^I`，观测模型为：
|
||||
|
||||
```text
|
||||
p_ANT1^W = p_IMU^W + R_WI * l_I
|
||||
v_ANT1^W = v_IMU^W + R_WI * ((omega - b_g) × l_I)
|
||||
```
|
||||
|
||||
转弯时的 `omega × l_I`、加减速和坡道中的姿态变化，都会直接增强杆臂可观性；这比把加速度二次积分当作唯一位移来源更适合做天线–IMU 一致性检查。
|
||||
|
||||
双天线基线本身只有 2DoF 方向信息，不能凭空构造完整三轴姿态。本项目采用：
|
||||
|
||||
1. R1b：以基线和 IMU 动态估计 ANT1→ANT2 在 IMU 中的 2DoF 方向。
|
||||
2. R2V：以基线和高质量 Doppler velocity 做独立诊断。
|
||||
3. R2G：在确认水平的静止场地，以基线 + 重力 + 地面水平先验补齐完整旋转。
|
||||
4. R3：跨会话、block-out、协方差和可观性复核。
|
||||
|
||||
正式固定旋转来源是 `R2G_gravity_level_prior`；旧式 GNHPR 三轴姿态手眼只保留为诊断，不作为正式外参。
|
||||
|
||||
## RTK–IMU 平移如何求解和验证
|
||||
|
||||
每个 GNSS node 使用 15DoF 状态 `(R,p,v,bg,ba)`，相邻 node 由 covariance-whitened IMU preintegration 与 bias random-walk 连接。因子包括：
|
||||
|
||||
- BESTNAVA：天线相位中心 XYZ；
|
||||
- GGA：仅在 BEST 缺失时约束 XY，不混用 MSL 高度到 ENU Z；
|
||||
- Doppler：天线速度；
|
||||
- HPR：可选基线/姿态观测，direct 与短 bridge 使用不同协方差；
|
||||
- 静止段：gravity candidate 与 ZUPT 分离，禁止将匀速直线误作 ZUPT。
|
||||
|
||||
杆臂的判断流程不是“优化器有数值就算成功”：
|
||||
|
||||
1. 先用无机械先验的 free solve 评估 Schur 边缘化后的 3×3 杆臂信息、协方差、最弱方向和多初值稳定性。
|
||||
2. 若完整 XYZ 不可观，冻结 free 结果为诊断基线，不继续无限增加非线性 free solve。
|
||||
3. 以机械测量作为绝对杆臂来源，比较 fixed-mechanical、soft-prior 与 free 的代价和物理残差。
|
||||
4. 在 circle、left-right、slope 高动态窗口，以及与标定窗口不重叠的 held-out 数据上固定杆臂验证。
|
||||
5. 做旋转扰动敏感性、独立 innovation 与 propagation root-cause audit；这些审计不重新优化杆臂。
|
||||
|
||||
## 本次结果应如何理解
|
||||
|
||||
- 当前固定工程候选：`l_I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||
- 机械测量决定杆臂绝对值；RTK+IMU 动态数据用于独立一致性和稳定性验证。
|
||||
- 47 个非重叠标定窗口表明机械先验与数据拟合相容，但没有足够的数据驱动信息去有意义地细化机械值。
|
||||
- held-out 物理验证和旋转敏感性通过；杆臂敏感的高动态数据没有发现机械外参明显冲突。
|
||||
- 低速、低角速度独立传播仍有约 `0.20 m/s²` 的公共加速度偏差；这更像 propagation nuisance，而不是由 `omega × l` 消失后的杆臂错误造成。该问题尚未关闭，故 `engineering_translation_accepted=false`。
|
||||
|
||||
这解释了两个同时成立的事实：机械外参没有被动态数据否定，但整个传播模型尚未达到正式放行标准。机器可读结果见 [工程 release 决策](../artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。
|
||||
|
||||
## 代码边界和共享模块
|
||||
|
||||
```text
|
||||
imu_lidar/
|
||||
lidar_io.py, lidar_deskew.py, registration.py, keyframes.py
|
||||
motion_pairs.py, phase_a.py, pipeline.py, joint_optimizer.py
|
||||
-> LiDAR–IMU 专用
|
||||
|
||||
rtk_imu/
|
||||
rtk_io.py, rtk_attitude.py, rtk_imu_rotation.py
|
||||
rtk_imu_multisource.py, rtk_imu_engineering.py, rtk_imu_node_graph.py
|
||||
-> RTK–IMU 专用
|
||||
|
||||
imu_lidar/contracts.py, geometry.py, geodesy.py, imu_io.py,
|
||||
imu_preintegration.py, rotation_handeye.py
|
||||
-> 两条链路可复用的基础能力
|
||||
```
|
||||
|
||||
RTK–IMU 只能 import 这 6 个共享模块，不应 import LiDAR 专用求解文件。命令入口都在 `tools/`，并按 `run_rtk_imu_*`、`audit_rtk_imu_*` 与 LiDAR 工具名称区分。
|
||||
|
||||
## 进一步工作
|
||||
|
||||
在宣称正式工程放行前，应优先关闭低速传播公共加速度偏差：区分图优化 nuisance accel bias 与可迁移物理 bias，复核静态 bias、重力泄漏和短时传播。不要用调机械先验、调 covariance 或扩大 free solve 来掩盖该问题。
|
||||
|
||||
RTK–IMU 的完整工程流程、当前变换和复现命令见 [README_RTK_IMU.md](../README_RTK_IMU.md)。
|
||||
@@ -1,168 +0,0 @@
|
||||
# RTK–IMU 标定链路审计与当前方案
|
||||
|
||||
> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
|
||||
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
|
||||
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。
|
||||
|
||||
## 结论
|
||||
|
||||
当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。
|
||||
|
||||
G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。
|
||||
|
||||
双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。
|
||||
|
||||
当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。
|
||||
|
||||
## 坐标系和参考点
|
||||
|
||||
统一输出约定：
|
||||
|
||||
```text
|
||||
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
|
||||
```
|
||||
|
||||
RTK 车固坐标定义：
|
||||
|
||||
- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
|
||||
- +Y：车辆前进方向，与 IMU +Y 同向。
|
||||
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
|
||||
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。
|
||||
|
||||
位置参考点：
|
||||
|
||||
- GGA 为 ANT1/主天线相位中心。
|
||||
- 相位中心离地高度为 1.916499878 m。
|
||||
|
||||
## 求解链路
|
||||
|
||||
### R0：协议、数据质量和时间
|
||||
|
||||
1. 校验 NMEA checksum。
|
||||
2. GGA 位置只接收 Q=4 固定解。
|
||||
3. GNHPR 姿态只接收 Q=4 固定解；Q=5 浮点解仅进入诊断统计，不参与标定。
|
||||
4. 保留 GNHPR 自己的卫星数、差分龄期和基站号，不能再用 GGA 卫星数替代 HPR 质量。
|
||||
5. 姿态使用 `hpr_measurement_utc_s` 映射后的 IMU 设备时间。
|
||||
6. 遇到 HPR 无效、浮点、时间间隔异常或基线跳变时切断连续段，运动对不得跨断点。
|
||||
|
||||
### T0：残余时间偏移
|
||||
|
||||
只使用可观测的有符号 heading 角速度与 IMU `gyro_z` 做相关扫描。角速度模长会混入不可观的绕基线旋转，不再作为时间审计依据。
|
||||
|
||||
时间偏移只用于初始化/诊断。只有相关峰足够高、与次峰分离且峰宽足够窄时才应用，否则保持 0 s。
|
||||
|
||||
### R1：双天线基线一致性
|
||||
|
||||
对每个连续固定解片段构造 0.75 s、1.5 s、3.0 s 的相对运动。以安装先验 `u_IMU=[1,0,0]` 检验：
|
||||
|
||||
```text
|
||||
angle(u_RTK(t0), u_RTK(t1))
|
||||
≈ angle(u_IMU, ΔR_IMU(t0,t1) u_IMU)
|
||||
```
|
||||
|
||||
该标量约束不虚构 RTK 的前向轴和上向轴。输出整体、逐会话 RMS/P95、分轴诊断及最坏时间区间。所有会话的总权重归一，避免高激励或样本更多的会话支配结果。
|
||||
|
||||
### R2：旧式完整 HPR 诊断
|
||||
|
||||
为兼容历史输出，将基线补成零 roll 的数学坐标架，再运行完整手眼。这个结果仅用于暴露符号错误、异常会话和旧结果变化，不能作为可交付外参。
|
||||
|
||||
LOO 删除一个会话后，会重新优化剩余会话的陀螺零偏，不再固定全量数据的 nuisance 参数。
|
||||
|
||||
### T1：平移
|
||||
|
||||
只有完整三自由度旋转“可观且通过”时，才允许进入杆臂和平移求解。当前条件不满足，因此：
|
||||
|
||||
- 不运行平移优化；
|
||||
- 不输出 `translation_result.json`；
|
||||
- `t_RTK_IMU_m` 和 `T_RTK_IMU` 为 null；
|
||||
- 历史巨大 Z、米级不确定度和约 1 m/s 速度残差不再消耗优化时间。
|
||||
|
||||
## 当前 8 会话结果
|
||||
|
||||
结果目录：`artifacts/rtk_imu_calibration_v2/all_sessions`。
|
||||
|
||||
```text
|
||||
可观测基线一致性：
|
||||
RMS / median / P95 = 0.593665 / 0.146040 / 1.212972 deg
|
||||
priority_175910 RMS / P95 = 1.316292 / 2.472570 deg
|
||||
slope RMS / P95 = 1.264514 / 2.475899 deg
|
||||
最坏区间 = priority_175910，约 5.084 deg
|
||||
baseline gate = passed
|
||||
|
||||
时间偏移：
|
||||
全局候选 = +0.005 s
|
||||
峰值相关 = 0.909019
|
||||
近峰宽度 = [-0.110, +0.170] s
|
||||
实际应用 = 0 s
|
||||
|
||||
旧式完整 HPR 诊断：
|
||||
RPY = [-0.132765, -0.297433, -1.558358] deg
|
||||
RMS / P95 = 1.172397 / 2.404977 deg
|
||||
std = [0.615416, 0.437419, 3.687145] deg
|
||||
priority_175910 re-optimized LOO = 1.698555 deg
|
||||
max re-optimized LOO = 1.699427 deg（删除 slope）
|
||||
legacy numeric gate = failed
|
||||
full attitude observable = false
|
||||
|
||||
平移：
|
||||
frozen / not run
|
||||
```
|
||||
|
||||
旧结果中的 `priority_175910` 条件 LOO 为 5.14°。严格剔除 Q5、会话等权、断段保护以及 LOO 重估零偏后，该会话完整诊断 LOO 降为 1.699°。它和坡道会话仍是主要异常源，但现在异常集中在基线仰角通道，而不是 heading：这更符合原始 HPR 中 Q5、低卫星数和 pitch 大幅波动的事实。
|
||||
|
||||
## 已排除或仍存在的漏洞
|
||||
|
||||
- 已修复：heading 误当车前方向。
|
||||
- 已修复：GNHPR pitch 误当纵向车体 pitch。
|
||||
- 已修复：不可观 roll 注入完整姿态。
|
||||
- 已修复：Q5 浮点 HPR 进入标定。
|
||||
- 已修复：HPR 质量字段在导出时丢失。
|
||||
- 已修复：相对运动跨越无效段或跳变。
|
||||
- 已修复：样本多的会话权重过大。
|
||||
- 已修复：LOO 固定全量会话零偏。
|
||||
- 已修复：时间相关使用三轴角速度模长。
|
||||
- 已修复：旋转未通过仍继续优化平移。
|
||||
- 仍存在：当前运动不能稳定地从 GGA 速度补全车前/车上方向。
|
||||
- 仍存在：基线绕轴自由度没有独立传感器观测。
|
||||
- 仍存在：`priority_175910` 和坡道的基线仰角存在局部异常。
|
||||
|
||||
## 如何得到可交付的完整旋转
|
||||
|
||||
按优先级建议：
|
||||
|
||||
1. 采一组专用数据：空旷区域、全程 RTK fixed、较长直线加减速、左右转、坡道上下行，保留原始 GGA/GNHPR/IMU 时间和全部质量字段。
|
||||
2. 用高质量前向速度补第二根轴。只在速度足够高、航向变化平缓的区间用 GGA course，并在同一优化中建模 ANT1 杆臂、非完整车辆侧向速度约束和时间偏移。
|
||||
3. 用静止重力补上向轴时，必须把“地面水平/车辆静止”写成显式先验，并将结果标记为安装先验约束解，而不是双天线数据独立解。
|
||||
4. 若能取得 G90 内部融合后的完整 INS 姿态、第三天线、轮速/转角或可靠车体姿态源，优先作为第二独立方向。
|
||||
5. 完整旋转通过留出验证后，才恢复平移；平移应同时估计杆臂、速度、加计偏置，并检查垂向高程基准。
|
||||
|
||||
当前数据上尝试用平滑 GGA 速度直接补全姿态，结果随平滑窗口明显变化，受低速、转弯杆臂和高程差分噪声影响，不能进入正式结果。
|
||||
|
||||
## 与主流开源方法的对应
|
||||
|
||||
- Kalibr：角速度相关适合作为时间偏移初始化，不应在宽峰时强行采用候选值。
|
||||
- iKalibr：采用连续时间轨迹联合估计时空参数，并强调充分激励；适合后续专用数据。
|
||||
- MINS：异步测量插值并把传感器外参、时间和 nuisance 状态一起估计。
|
||||
- GICI-LIB：因子图中显式进行 GNSS/INS 初始化、质量控制和异常值处理。
|
||||
|
||||
本项目暂不直接引入这些大型框架，而是吸收其原则：先保证物理可观测性和数据质量，再进行联合优化；不能用自由状态吸收错误模型。
|
||||
|
||||
参考：
|
||||
|
||||
- Unicore N4 Reference Commands Manual
|
||||
- Unicore UM982 User Manual
|
||||
- https://github.com/Unsigned-Long/iKalibr
|
||||
- https://github.com/ethz-asl/kalibr
|
||||
- https://github.com/rpng/MINS
|
||||
- https://github.com/chichengcn/gici-open
|
||||
|
||||
## 代码入口
|
||||
|
||||
- `rtk_imu/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。
|
||||
- `rtk_imu/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。
|
||||
- `rtk_imu/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。
|
||||
- `rtk_imu/rtk_imu_replay.py`：坐标定义、参考点、平移冻结和 JSON 输出。
|
||||
- `tools/rscap_v2/g90_rtk.py`：原始 GNHPR 解析及质量字段。
|
||||
- `tools/export_g90_rtk_to_sessions.py`：GNHPR 质量字段导出。
|
||||
- `tools/run_rtk_imu_calibration.py`：端到端命令行入口。
|
||||
- `tests/test_rtk_imu_calibration.py`：轴定义、Q4/Q5、时间和预积分回归测试。
|
||||
@@ -1,108 +0,0 @@
|
||||
# RTK–IMU Engineering 6DoF 分支
|
||||
|
||||
该分支与 V3 `data_only` 严格链路并列，不改变其门禁结论：
|
||||
|
||||
```text
|
||||
data_only_6dof_accepted = false
|
||||
```
|
||||
|
||||
工程分支固定使用真实水平静止场地得到的 R2G 旋转，默认参考值为
|
||||
`RPY=[0.4543066, -0.0026392, 0.0122384] deg`，输出始终记录：
|
||||
|
||||
```text
|
||||
rotation_source = R2G_gravity_level_prior
|
||||
translation_conditional_on_rotation = true
|
||||
```
|
||||
|
||||
## 条件模型
|
||||
|
||||
估计量是 `l_I = p_ANT1^I`。每个连续质量段使用 HI13 设备时间、raw gyro/acc 动态预积分，融合 RTK Fixed 的 GGA/BESTNAVA 位置、BESTNAVA Doppler velocity 和 Q4 GNHPR 的 ANT1→ANT2 基线。逐段 nuisance state 包含初始姿态、IMU 原点初始位置、速度、gyro bias 和 acc bias。HI13 absolute quaternion/RPY（尤其 absolute yaw）不进入机械外参因子；R2V 仅作诊断。
|
||||
|
||||
静止相关因子分为两类：
|
||||
|
||||
- `gravity_candidate`：连续约 1.5 s 的低角速度、gyro/acc 方差稳定且加速度模长接近重力；它不能等价为静止。
|
||||
- `zupt_static`：在 gravity candidate 基础上，必须同时有至少约 1 s 的连续 BESTNAVA Doppler 速度接近 0。高速或匀速直线不会加入 ZUPT。
|
||||
|
||||
连续段复用 R0 质量断点：checksum 无效、定位非 Fixed、GNHPR 非 Q4、设备时间回跳、HPR 测量间隙、baseline jump、位置测量间隙或 IMU gap 都会切段，禁止跨断点预积分。 断后片段还必须至少包含 6 个求解节点且持续不少于 5 s；更短的局部欠约束微段不会跨断点拼接，而是直接不进入杆臂优化。
|
||||
|
||||
## 高程与残差口径
|
||||
|
||||
- GGA 只约束 XY；GGA MSL altitude 不定义也不参与 ENU-Z。
|
||||
- ENU-Z reference 只来自有效 Fixed BESTNAVA altitude；BESTNAVA 才约束 XYZ。
|
||||
- 输出分别为 `gga_xy_residual`、`bestnava_xyz_residual` 和 `doppler_velocity_residual`。未进入 Z factor 的 GGA 高度不进入垂向残差统计。
|
||||
|
||||
## 可观性与验收
|
||||
|
||||
杆臂可观性不再使用全状态最小奇异向量。状态分为杆臂 `l` 与 nuisance state，对优化 Hessian 计算 Schur complement：
|
||||
|
||||
```text
|
||||
H_l_marg = H_ll - H_ln pinv(H_nn) H_nl
|
||||
```
|
||||
|
||||
只对该 3×3 marginal lever information 做 SVD，并输出：
|
||||
|
||||
- `l_I_marginal_covariance_m2`、`l_I_std_m`
|
||||
- `lever_information_singular_values`
|
||||
- `lever_information_condition_number`
|
||||
- `lever_precision_rank`
|
||||
- `weakest_lever_direction_I`
|
||||
|
||||
门禁分为两层：
|
||||
|
||||
- `solver_health_gates` 只判断优化是否收敛、数值是否有限且残差未发散。
|
||||
- `engineering_acceptance_gates` 使用更严格的杆臂 marginal std/information/rank/condition、BESTNAVA XYZ、GGA XY、Doppler velocity、LOO、bootstrap、旋转敏感性和可选手量一致性。
|
||||
|
||||
任一核心门禁失败时，`engineering_6dof_accepted=false`。没有完整执行 bootstrap 和 18 组旋转敏感性时，两项门禁明确为 false，不会把阶段性 base/LOO 结果误标为正式放行。
|
||||
|
||||
Bootstrap 按 session 有放回抽样，并保留重复 session 的 multiplicity；重复抽中的 session 会重复贡献其全部连续段。
|
||||
|
||||
## 机械杆臂软先验与双解输出
|
||||
|
||||
`--manual-l-i-m` 仅在同时提供 `--manual-l-i-std-m` 或完整
|
||||
`--manual-l-i-covariance-m2` 时才成为白化高斯软因子。求解器始终先运行无先验
|
||||
`free_solution`，再运行 `prior_constrained_solution`；输出还包含机械参考及两者到
|
||||
参考的差值。当前活动 engineering 解为 prior-constrained 解（若启用），但 acceptance
|
||||
额外要求 free-solve 也与机械参考一致，软先验不能掩盖不可观或数据矛盾。
|
||||
|
||||
示例（数值需使用实际机械测量的 1σ，不可把示例值当作默认）：
|
||||
|
||||
```powershell
|
||||
--manual-l-i-m -0.45072 -0.25682 0.73208 `
|
||||
--manual-l-i-std-m <sigma_x_m> <sigma_y_m> <sigma_z_m>
|
||||
```
|
||||
## 坐标转换
|
||||
|
||||
```text
|
||||
T_RTK_IMU: p_RTK = R_RTK_IMU p_IMU - R_RTK_IMU l_I
|
||||
T_IMU_RTK: p_IMU = R_RTK_IMU^T p_RTK + l_I
|
||||
```
|
||||
|
||||
RTK 原点是 ANT1，因此 `T_IMU_RTK.translation == l_I`，两矩阵必须互逆。
|
||||
|
||||
## 分阶段运行
|
||||
|
||||
第一阶段默认只运行 base fit、marginal observability、residual audit 和 LOO：
|
||||
|
||||
```powershell
|
||||
python tools/run_rtk_imu_engineering_6dof.py `
|
||||
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_base_loo.json `
|
||||
--session <session-a> `
|
||||
--session <session-b> `
|
||||
--session <session-c>
|
||||
```
|
||||
|
||||
只有 base/LOO 合理后，才显式启动全量验证：
|
||||
|
||||
```powershell
|
||||
python tools/run_rtk_imu_engineering_6dof.py `
|
||||
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_full.json `
|
||||
--session <session-a> `
|
||||
--session <session-b> `
|
||||
--session <session-c> `
|
||||
--run-bootstrap --bootstrap-repetitions 40 --bootstrap-seed 0 `
|
||||
--run-rotation-sensitivity
|
||||
```
|
||||
|
||||
正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。
|
||||
@@ -1,175 +0,0 @@
|
||||
# RTK–IMU 多源原始数据与旋转标定 V3
|
||||
|
||||
## 可行性结论
|
||||
|
||||
重构方向正确且必要。原始捕获中确实存在旧导出链路忽略的 BESTNAVA、
|
||||
PVTSLNA 和 HI13 姿态/四元数。它们可以补充速度、质量、绝对姿态和静止
|
||||
重力约束，但本批数据尚不能通过完整旋转门禁：
|
||||
|
||||
- BESTNAVA 约 0.8–1 Hz，严格高速样本很少。
|
||||
- 当前动态主要是平面 yaw，R1b 的两个倾斜自由度仍弱可观。
|
||||
- HI13 四元数与 GNSS 真北存在稳定但非机械的 yaw 偏差，可能来自磁偏角或
|
||||
内部导航融合。
|
||||
- 两个明确水平静止会话的 R2G 很稳定，但它是水平/重力先验约束解。
|
||||
|
||||
因此，本次重构显著提高了诊断能力，也防止错误外参进入平移；它没有把信息
|
||||
不足包装成“标定成功”。
|
||||
|
||||
## 三批原始数据
|
||||
|
||||
扫描根目录：
|
||||
|
||||
- 0808：D:\data\raw_serial_capture_v2，仅选 20260808。
|
||||
- 0815：D:\data\0815\raw_serial_capture_v2；目录实际包含 20260812–14。
|
||||
- 0819：D:\data\0819\raw_serial_capture_v2。
|
||||
|
||||
按文件名捕获开始时间在 1.5 s 内一一配对：
|
||||
|
||||
| 批次 | G90/HI13 配对 | GNHPR | BESTNAVA | PVTSLNA | Q4 HPR | Fixed Doppler |
|
||||
|---|---:|---:|---:|---:|---:|---:|
|
||||
| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 |
|
||||
| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 |
|
||||
| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 |
|
||||
|
||||
共完成 55 组统一导出。0815 有一组 G90 没有匹配 HI13，manifest 将其列为
|
||||
unmatched。发现一条 NMEA 时间为 0913A：保留原始行，设备时间留空并隔离，
|
||||
没有回退到 host receive time。
|
||||
|
||||
速度不等于高速激励。增加速度 ≥1.5 m/s、水平速度标准差 ≤0.25 m/s 后，
|
||||
三个批次分别只剩 44、8、0 条候选。
|
||||
|
||||
## 统一导出
|
||||
|
||||
输出根目录：D:\data\rtk_imu_unified_v3。
|
||||
|
||||
每组捕获：
|
||||
|
||||
- imu.npz：HI13 system_time、gyro、accel、RPY、WXYZ quaternion、磁场、
|
||||
PPS stamp、温度、气压和 host receive UTC。
|
||||
- rtk.csv：每条原生异步 GGA/GNHPR/BESTNAVA/PVTSLNA 单独成行，不再把
|
||||
HPR/BEST/PVT 最近邻挂到 GGA。
|
||||
- export_summary.json：原始捕获摘要、报文数量、四元数范数和设备到 host
|
||||
的仿射时钟诊断。
|
||||
- manifest.json：全部会话、批次、目录和 unmatched 记录。
|
||||
|
||||
时间规则：
|
||||
|
||||
1. HI13 system_time 是 IMU 主时间轴。
|
||||
2. BESTNAVA/PVTSLNA 使用 GNSS week/TOW 和 leap seconds。
|
||||
3. GGA/GNHPR 使用报文自己的 UTC time-of-day。
|
||||
4. RTK GNSS 测量时刻通过 HI13 device→host 仿射模型映射进 HI13 设备时钟。
|
||||
5. host receive time 只用于跨时钟桥接、延迟和抖动诊断，绝不替代采样时刻。
|
||||
6. checksum 无效、时间畸形或非固定解记录保留，但不进入求解。
|
||||
|
||||
## 新旋转链路
|
||||
|
||||
### R1b：基线在 IMU 中的 2DoF 方向
|
||||
|
||||
对连续 Q4 GNHPR 基线和 HI13 陀螺相对旋转使用不变量：
|
||||
|
||||
angle(b_ENU(t0), b_ENU(t1))
|
||||
= angle(b_IMU, DeltaR_IMU b_IMU)
|
||||
|
||||
估计 b_IMU 的两个倾斜自由度及逐会话陀螺零偏。安装信息只用于选择 +X
|
||||
半球，并施加明确记录的弱 20° 先验。按会话等权，输出残差、协方差和信息
|
||||
奇异值。
|
||||
|
||||
### R2V：基线 + Doppler velocity
|
||||
|
||||
筛选条件：
|
||||
|
||||
- BESTNAVA position 为 SOL_COMPUTED/NARROW_INT；
|
||||
- velocity 为 SOL_COMPUTED/DOPPLER_VELOCITY；
|
||||
- checksum 有效；
|
||||
- 水平速度 ≥1.5 m/s、速度标准差 ≤0.25 m/s；
|
||||
- |IMU gyro_z| ≤3°/s；
|
||||
- 速度方向与横向基线接近正交；
|
||||
- GNHPR Q4，并有时间邻近的合法 HI13 quaternion。
|
||||
|
||||
基线给车右，Doppler velocity 给车前，叉积给车上。HI13 quaternion 的
|
||||
body/world 和 ENU/NED 候选全部评分，只保留残差最小者。该方法会把 HI13
|
||||
导航 yaw 偏差带入候选，因此必须和 R2G 交叉验证。
|
||||
|
||||
### R2G：基线 + 水平静止重力
|
||||
|
||||
只允许调用方明确标记的水平静止会话。本次使用：
|
||||
|
||||
- 0819_20260819_072130（flat_static_hdg207）
|
||||
- 0819_20260819_073045（flat_static_hdg082）
|
||||
|
||||
每 10 s 分块，要求 gyro norm ≤0.35°/s，且加速度模长距标准重力不超过
|
||||
0.15 m/s²。R1b 提供车右，加速度中值提供车上，叉积得到车前。
|
||||
|
||||
### R3：稳定性与正式门禁
|
||||
|
||||
R2V/R2G 都输出：
|
||||
|
||||
- 样本和会话数量；
|
||||
- SO(3) RMS/P95；
|
||||
- 旋转向量样本标准差和均值协方差；
|
||||
- leave-one-session；
|
||||
- 10 样本或 10 s block-out。
|
||||
|
||||
只有 R1b 可观、R2V 和 R2G 各自稳定、二者差异 ≤2° 时，才将
|
||||
translation_unlocked 设为 true。旧 GNHPR 三轴手眼不参与正式门禁。
|
||||
|
||||
## 首轮实测
|
||||
|
||||
R1b：
|
||||
|
||||
b_IMU = [0.999999976, -0.000213959, -0.000044368]
|
||||
tilt_yz = [-0.0123, -0.0025] deg
|
||||
pairs = 2091
|
||||
RMS / P95 = 0.8086 / 1.7105 deg
|
||||
information singular values = [1.176e-2, 8.476e-4]
|
||||
std = [27.77, 7.47] deg
|
||||
gate = failed
|
||||
|
||||
点估计接近 +X 是安装先验与名义轴一致的结果；巨大协方差说明不能宣称
|
||||
数据独立估出了这两个小角。
|
||||
|
||||
R2V：
|
||||
|
||||
qualifying samples / sessions = 41 / 2
|
||||
selected quaternion convention = HI13_q_body_to_ENU
|
||||
RPY = [0.7532, 0.0708, -9.2944] deg
|
||||
RMS / P95 = 1.8436 / 3.4975 deg
|
||||
max leave-one-session = 3.4519 deg
|
||||
gate = failed
|
||||
|
||||
R2G：
|
||||
|
||||
level-static blocks / sessions = 53 / 2
|
||||
RPY = [0.4543, -0.0026, 0.0122] deg
|
||||
RMS / P95 = 0.2542 / 0.2732 deg
|
||||
max leave-one-session = 0.2688 deg
|
||||
gate = passed (level/gravity-prior constrained)
|
||||
|
||||
R2V 与 R2G 的 SO(3) 差异为 9.3119°。R3 最终：
|
||||
|
||||
full_rotation_accepted = false
|
||||
translation_unlocked = false
|
||||
|
||||
## 下一轮数据要求
|
||||
|
||||
1. BESTNAVA 改为至少 10 Hz，并确认 Doppler velocity 与 GNHPR 使用同一
|
||||
GNSS week/TOW 输出周期。
|
||||
2. 每个日期都录制多段 ≥3 m/s、持续 20–30 s 的正向直线；包含不同方位，
|
||||
避免单一磁环境和单一会话支配。
|
||||
3. 为 R1b 增加可控的 roll/pitch 激励；只有平面 yaw 无法稳定估出横向
|
||||
基线的两个微小倾斜角。
|
||||
4. 每个日期至少录两种车头方位的明确水平静止段，检验 HI13 重力和绝对
|
||||
quaternion 的跨日期稳定性。
|
||||
5. 若 HI13 quaternion 的 yaw 来自磁融合，应获取其导航坐标定义、磁偏角
|
||||
设置和融合状态；否则 R2V 只使用其 roll/pitch，yaw 由 GNSS 基线和速度
|
||||
决定。
|
||||
6. 上述门禁通过前继续冻结杆臂和平移。
|
||||
|
||||
## 实现入口
|
||||
|
||||
- tools/rscap_v2/g90_rtk.py：GGA/GNHPR/BESTNAVA/PVTSLNA 和双 checksum。
|
||||
- tools/rscap_v2/hi13_imu.py：HI91 system_time、惯性、姿态和四元数。
|
||||
- tools/export_rtk_imu_unified.py：55 组配对、原生异步导出和断点续导。
|
||||
- rtk_imu/rtk_imu_multisource.py：R1b/R2V/R2G/R3。
|
||||
- tools/run_rtk_imu_multisource.py：多源旋转命令行入口。
|
||||
- artifacts/rtk_imu_calibration_v3/multisource_result.json：首轮 R3 结果。
|
||||
@@ -1,157 +0,0 @@
|
||||
# 纯 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
@@ -1,209 +0,0 @@
|
||||
# 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/交付物同步 |
|
||||
@@ -1,270 +0,0 @@
|
||||
# `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)
|
||||
|
||||
### 标题
|
||||
|
||||
- **原本**：...
|
||||
- **改成**：...
|
||||
```
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
"""LiDAR–IMU calibration package (V1 runnable pipeline)."""
|
||||
|
||||
from .contracts import CalibrationMode, CalibrationStatus, TransformConvention
|
||||
|
||||
__all__ = ["CalibrationMode", "CalibrationStatus", "TransformConvention"]
|
||||
@@ -1,345 +0,0 @@
|
||||
"""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())
|
||||
@@ -1,130 +0,0 @@
|
||||
"""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)
|
||||
@@ -1,82 +0,0 @@
|
||||
"""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
|
||||
@@ -1,74 +0,0 @@
|
||||
"""Small WGS84 geodesy helpers used by the RTK--IMU calibration path."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
WGS84_A_M = 6378137.0
|
||||
WGS84_F = 1.0 / 298.257223563
|
||||
WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
|
||||
|
||||
|
||||
def geodetic_to_ecef(
|
||||
latitude_deg: np.ndarray,
|
||||
longitude_deg: np.ndarray,
|
||||
altitude_m: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""Convert WGS84 latitude/longitude/ellipsoidal height to ECEF metres."""
|
||||
|
||||
latitude = np.deg2rad(np.asarray(latitude_deg, dtype=float))
|
||||
longitude = np.deg2rad(np.asarray(longitude_deg, dtype=float))
|
||||
altitude = np.asarray(altitude_m, dtype=float)
|
||||
latitude, longitude, altitude = np.broadcast_arrays(latitude, longitude, altitude)
|
||||
sin_lat = np.sin(latitude)
|
||||
cos_lat = np.cos(latitude)
|
||||
radius = WGS84_A_M / np.sqrt(1.0 - WGS84_E2 * sin_lat**2)
|
||||
x = (radius + altitude) * cos_lat * np.cos(longitude)
|
||||
y = (radius + altitude) * cos_lat * np.sin(longitude)
|
||||
z = (radius * (1.0 - WGS84_E2) + altitude) * sin_lat
|
||||
return np.stack([x, y, z], axis=-1)
|
||||
|
||||
|
||||
def geodetic_to_enu(
|
||||
latitude_deg: np.ndarray,
|
||||
longitude_deg: np.ndarray,
|
||||
altitude_m: np.ndarray,
|
||||
*,
|
||||
origin_latitude_deg: float | None = None,
|
||||
origin_longitude_deg: float | None = None,
|
||||
origin_altitude_m: float | None = None,
|
||||
) -> tuple[np.ndarray, tuple[float, float, float]]:
|
||||
"""Convert WGS84 samples to a local east/north/up frame.
|
||||
|
||||
When no origin is supplied, the first finite sample is used. The returned
|
||||
origin tuple is ``(latitude_deg, longitude_deg, altitude_m)``.
|
||||
"""
|
||||
|
||||
lat = np.asarray(latitude_deg, dtype=float).reshape(-1)
|
||||
lon = np.asarray(longitude_deg, dtype=float).reshape(-1)
|
||||
alt = np.asarray(altitude_m, dtype=float).reshape(-1)
|
||||
if not (lat.size == lon.size == alt.size):
|
||||
raise ValueError("latitude, longitude and altitude must have equal length")
|
||||
finite = np.isfinite(lat) & np.isfinite(lon) & np.isfinite(alt)
|
||||
if not np.any(finite):
|
||||
raise ValueError("no finite geodetic sample")
|
||||
first = int(np.flatnonzero(finite)[0])
|
||||
lat0 = float(lat[first] if origin_latitude_deg is None else origin_latitude_deg)
|
||||
lon0 = float(lon[first] if origin_longitude_deg is None else origin_longitude_deg)
|
||||
alt0 = float(alt[first] if origin_altitude_m is None else origin_altitude_m)
|
||||
|
||||
ecef = geodetic_to_ecef(lat, lon, alt)
|
||||
ecef0 = geodetic_to_ecef(np.array(lat0), np.array(lon0), np.array(alt0)).reshape(3)
|
||||
delta = ecef - ecef0
|
||||
phi = np.deg2rad(lat0)
|
||||
lam = np.deg2rad(lon0)
|
||||
rotation = np.array(
|
||||
[
|
||||
[-np.sin(lam), np.cos(lam), 0.0],
|
||||
[-np.sin(phi) * np.cos(lam), -np.sin(phi) * np.sin(lam), np.cos(phi)],
|
||||
[np.cos(phi) * np.cos(lam), np.cos(phi) * np.sin(lam), np.sin(phi)],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
return delta @ rotation.T, (lat0, lon0, alt0)
|
||||
@@ -1,207 +0,0 @@
|
||||
"""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))
|
||||
@@ -1,86 +0,0 @@
|
||||
"""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),
|
||||
)
|
||||
@@ -1,74 +0,0 @@
|
||||
"""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
|
||||
|
||||
import csv
|
||||
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:
|
||||
required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
|
||||
with path.open("r", encoding="utf-8-sig", newline="") as handle:
|
||||
header = next(csv.reader(handle), [])
|
||||
names = set(header)
|
||||
if not set(required_order).issubset(names):
|
||||
raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
|
||||
usecols = [header.index(name) for name in required_order]
|
||||
data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
|
||||
t = np.asarray(data[:, 0], dtype=float).reshape(-1)
|
||||
gyro = np.asarray(data[:, 1:4], dtype=float)
|
||||
acc = np.asarray(data[:, 4:7], dtype=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="")
|
||||
@@ -1,460 +0,0 @@
|
||||
"""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 local endpoint interpolation. ``seg0`` and ``seg1`` are inside
|
||||
# this adjacent sample interval, so scanning the full series with
|
||||
# np.interp here would turn pair construction into quadratic work.
|
||||
sample_dt = max(t_b - t_a, 1e-12)
|
||||
u0 = (seg0 - t_a) / sample_dt
|
||||
u1 = (seg1 - t_a) / sample_dt
|
||||
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
|
||||
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
|
||||
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
|
||||
|
||||
sample_dt = max(t_b - t_a, 1e-12)
|
||||
u0 = (seg0 - t_a) / sample_dt
|
||||
u1 = (seg1 - t_a) / sample_dt
|
||||
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
|
||||
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
|
||||
a_a = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
|
||||
a_b = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
|
||||
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
|
||||
@@ -1,977 +0,0 @@
|
||||
"""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),
|
||||
)
|
||||
@@ -1,50 +0,0 @@
|
||||
"""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))
|
||||
@@ -1,59 +0,0 @@
|
||||
"""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
|
||||
@@ -1,127 +0,0 @@
|
||||
"""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")
|
||||
@@ -1,220 +0,0 @@
|
||||
"""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
|
||||
@@ -1,154 +0,0 @@
|
||||
"""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
|
||||
@@ -1,114 +0,0 @@
|
||||
"""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
@@ -1,290 +0,0 @@
|
||||
"""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
|
||||
@@ -1,941 +0,0 @@
|
||||
"""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
|
||||
@@ -1,159 +0,0 @@
|
||||
"""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
|
||||
@@ -1,217 +0,0 @@
|
||||
"""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),
|
||||
)
|
||||
@@ -1,332 +0,0 @@
|
||||
"""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,
|
||||
)
|
||||
@@ -1,78 +0,0 @@
|
||||
"""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,
|
||||
)
|
||||
@@ -1,83 +0,0 @@
|
||||
"""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))
|
||||
@@ -1,82 +0,0 @@
|
||||
# `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)
|
||||
@@ -1,28 +0,0 @@
|
||||
[project]
|
||||
name = "lidar-imu-calibration"
|
||||
version = "0.3.0"
|
||||
description = "LiDAR-IMU and RTK-IMU extrinsic calibration algorithms"
|
||||
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", "rtk_imu", "tools"]
|
||||
|
||||
[tool.pytest.ini_options]
|
||||
testpaths = ["tests"]
|
||||
pythonpath = ["."]
|
||||
@@ -0,0 +1,4 @@
|
||||
numpy>=1.26
|
||||
scipy>=1.11
|
||||
open3d>=0.18
|
||||
small-gicp
|
||||
@@ -0,0 +1,15 @@
|
||||
# 当前车辆标定结果
|
||||
|
||||
本目录只保存当前车辆、当前传感器安装条件下的最终可交付结果；不保存历史车辆数据、原始采集包、点云帧或中间配准产物。
|
||||
|
||||
## 2026-08 车辆 / 27 个静止站点
|
||||
|
||||
目录 [`vehicle_20260808/`](vehicle_20260808/) 对应本机运行目录 `D:\data\rtk_lidar_run\outputs_vehicle_h19165`。
|
||||
|
||||
- 外参：`final_T_RTK_lidar.json`
|
||||
- 质量摘要：`summary.json`
|
||||
- 坐标约定：`p_RTK = T_RTK_lidar · p_lidar`，RTK 为车头向前坐标系（`HeadingOffsetDeg=-90`）。
|
||||
- RTK 参考点高度：1.9165 m（ANT1 相位中心）。
|
||||
- 质量：27 个站点、20 个共识运动对、平移 RMS 0.07116 m、旋转 RMS 0.98209°。
|
||||
|
||||
这些文件记录的是 host 时间关联版本的现有最终结果。后续采用 RTK 测量时间重新导出后，应写入新的结果目录，不能覆盖本目录。
|
||||
@@ -0,0 +1,337 @@
|
||||
{
|
||||
"schema_version": 1,
|
||||
"success": true,
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
||||
"frames": {
|
||||
"RTK": {
|
||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||
"x_axis": "vehicle forward after applying the configured G90 heading offset",
|
||||
"y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": "90 - (rawHeadingDeg + -90)",
|
||||
"frame_mode": "vehicle_forward_heading_offset"
|
||||
},
|
||||
"LiDAR": {
|
||||
"description": "raw Helios sensor frame from points_raw polar decode",
|
||||
"x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
|
||||
"y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
|
||||
"z_axis": "up",
|
||||
"origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ"
|
||||
}
|
||||
},
|
||||
"backend": "consensus",
|
||||
"measured_lidar_extrinsic_used_as_initial": true,
|
||||
"solver_initial_extrinsic": "D:\\First-dev-dept\\calibration-rtk-run\\run\\rtk_lidar_mechanical_initial.json",
|
||||
"body_heading_offset_deg": -90.0,
|
||||
"body_heading_offset_used": true,
|
||||
"body_antenna_lever_xy_used": false,
|
||||
"translation_m": [
|
||||
0.21782224963960972,
|
||||
-0.41134780227275347,
|
||||
0.1065423366878719
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
0.06623859235262805,
|
||||
0.8096624730962496,
|
||||
-0.5513220563826681
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
0.00061201333859398,
|
||||
0.007062715262034178,
|
||||
-0.004815137247499642,
|
||||
0.9999632782988027
|
||||
],
|
||||
"coordinate_contract_audit": {
|
||||
"status": "no_near_180_degree_axis_conflict",
|
||||
"requires_physical_axis_confirmation": false,
|
||||
"mechanical_initial_path": "D:\\First-dev-dept\\calibration-rtk-run\\run\\rtk_lidar_mechanical_initial.json",
|
||||
"mechanical_self_consistency": {
|
||||
"baseline_points": "vehicle_right",
|
||||
"frame_mode": "vehicle_forward_heading_offset",
|
||||
"heading_offset_deg": -90.0,
|
||||
"consistent": true,
|
||||
"issues": []
|
||||
},
|
||||
"solution_vs_declared_baseline_side": {
|
||||
"baseline_points": "vehicle_right",
|
||||
"solution_yaw_deg": -0.5513220563826681,
|
||||
"expected_yaw_deg": 0.0,
|
||||
"yaw_error_deg": 0.5513220563826735,
|
||||
"xy_error_m": 0.007516661287052368,
|
||||
"z_error_m": 0.0280423356878719,
|
||||
"mixed_translation_rotation_inheritance": false,
|
||||
"near_expected_pose": true
|
||||
},
|
||||
"solution_relative_to_mechanical_initial": {
|
||||
"translation_m": 0.029032271487700816,
|
||||
"rotation_deg": 0.9820426535863694,
|
||||
"delta_matrix_4x4": [
|
||||
[
|
||||
0.9998538650128304,
|
||||
0.009638565806830942,
|
||||
0.014119017957784103,
|
||||
0.006962889639609726
|
||||
],
|
||||
[
|
||||
-0.009621275903042714,
|
||||
0.9999528797859223,
|
||||
-0.0012919976154994906,
|
||||
0.002831671727246521
|
||||
],
|
||||
[
|
||||
-0.014130805670674627,
|
||||
0.0011559658421926345,
|
||||
0.9998994869856016,
|
||||
0.0280423356878719
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
]
|
||||
},
|
||||
"note": "No automatic 180-degree correction was applied. Confirm static GNHPR left/right vs vehicle heading and Helios +X vs vehicle forward before deployment."
|
||||
},
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9998538650128304,
|
||||
0.009638565806830942,
|
||||
0.014119017957784103,
|
||||
0.21782224963960972
|
||||
],
|
||||
[
|
||||
-0.009621275903042714,
|
||||
0.9999528797859223,
|
||||
-0.0012919976154994906,
|
||||
-0.41134780227275347
|
||||
],
|
||||
[
|
||||
-0.014130805670674627,
|
||||
0.0011559658421926345,
|
||||
0.9998994869856016,
|
||||
0.1065423366878719
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"quality": {
|
||||
"stations": 27,
|
||||
"pairs": 20,
|
||||
"residuals": {
|
||||
"pairs": 20,
|
||||
"translation_m": {
|
||||
"rms": 0.07116027693011169,
|
||||
"median": 0.048258124379040895,
|
||||
"p90": 0.10707349630410129,
|
||||
"p95": 0.12100345665884242,
|
||||
"max": 0.1859157912711514
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 0.9820870524210346,
|
||||
"median": 0.6015253089423158,
|
||||
"p90": 1.626986719630708,
|
||||
"p95": 1.7485896823567941,
|
||||
"max": 2.6898815510329674
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.10590532722733068,
|
||||
"rotation_deg": 0.5565962122599133
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.11758701799503664,
|
||||
"rotation_deg": 0.36543717366192036
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.07568813061789224,
|
||||
"rotation_deg": 1.6990480050580474
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.07918615709103373,
|
||||
"rotation_deg": 0.33723851079591255
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.04618587731787657,
|
||||
"rotation_deg": 0.21044665580544528
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.1859157912711514,
|
||||
"rotation_deg": 2.6898815510329674
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.015032558607699278,
|
||||
"rotation_deg": 0.2283712127949818
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.013237326674396453,
|
||||
"rotation_deg": 1.6189799101387812
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.07003654121179845,
|
||||
"rotation_deg": 0.6464544056247182
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.093450766078183,
|
||||
"rotation_deg": 0.6476769159531274
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.015054656507824072,
|
||||
"rotation_deg": 1.041955302231996
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.02338150242985638,
|
||||
"rotation_deg": 0.9212619385214977
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.01577916809950981,
|
||||
"rotation_deg": 0.5021648028537498
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.05033037144020521,
|
||||
"rotation_deg": 0.31352985458971655
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.059761151158357104,
|
||||
"rotation_deg": 0.8078349560587313
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.04160156756834453,
|
||||
"rotation_deg": 0.32936437427192145
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.02424387832291359,
|
||||
"rotation_deg": 0.430799739443549
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.05938006964566194,
|
||||
"rotation_deg": 0.3855347569405268
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.003922740066294881,
|
||||
"rotation_deg": 0.8803664894801937
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.02258390614293418,
|
||||
"rotation_deg": 0.9489255319885953
|
||||
}
|
||||
]
|
||||
},
|
||||
"weighted_jacobian_condition_number": 7.551077537197385,
|
||||
"linearized_one_sigma": {
|
||||
"translation_m": [
|
||||
0.01109415838733884,
|
||||
0.011043247019191056,
|
||||
0.0049617743728656676
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.08809558088476348,
|
||||
0.08691570892756702,
|
||||
0.13552099560800782
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 200,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.006235825930578342,
|
||||
0.004851737742140203,
|
||||
0.0004474159829207979,
|
||||
0.07186455153709871,
|
||||
0.11762318545143821,
|
||||
0.061763211598034336
|
||||
],
|
||||
"p025": [
|
||||
0.20955504469154063,
|
||||
-0.4174678620205485,
|
||||
0.10568919463688603,
|
||||
-0.06202286115194262,
|
||||
0.5207464314096704,
|
||||
-0.6639417037512721
|
||||
],
|
||||
"p975": [
|
||||
0.23211472114546824,
|
||||
-0.4001091835786546,
|
||||
0.1074353326322909,
|
||||
0.23746760471402314,
|
||||
1.0462704287205609,
|
||||
-0.43455346485967655
|
||||
]
|
||||
}
|
||||
},
|
||||
"z_constraint": {
|
||||
"observable_from_planar_AX_XB": false,
|
||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
||||
"rtk_reference_height_above_ground_m": 1.9165,
|
||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
||||
},
|
||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
|
||||
"selection": {
|
||||
"recommended": true,
|
||||
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
||||
"open3d_vs_small_gicp": {
|
||||
"translation_m": 0.00312519750472982,
|
||||
"rotation_deg": 0.11907006217018351,
|
||||
"delta_matrix_4x4": [
|
||||
[
|
||||
0.9999996872332217,
|
||||
-0.00030752729270551075,
|
||||
-0.0007286703114418758,
|
||||
0.000533303946557151
|
||||
],
|
||||
[
|
||||
0.00030892706731572284,
|
||||
0.9999981058814015,
|
||||
0.0019216653392719056,
|
||||
-0.002909732881513971
|
||||
],
|
||||
[
|
||||
0.0007280779667146176,
|
||||
-0.0019218898442212235,
|
||||
0.9999978881187204,
|
||||
0.001007919095162138
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
{
|
||||
"final": {
|
||||
"translation_m": [
|
||||
0.21782224963960972,
|
||||
-0.41134780227275347,
|
||||
0.1065423366878719
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
0.06623859235262805,
|
||||
0.8096624730962496,
|
||||
-0.5513220563826681
|
||||
],
|
||||
"pairs": 20,
|
||||
"translation_rms_m": 0.07116027693011169,
|
||||
"rotation_rms_deg": 0.9820870524210346,
|
||||
"condition_number": 7.551077537197385,
|
||||
"coordinate_contract_status": "no_near_180_degree_axis_conflict",
|
||||
"recommended_for_deployment": true
|
||||
},
|
||||
"backend_difference": {
|
||||
"translation_m": 0.00312519750472982,
|
||||
"rotation_deg": 0.11907006217018351,
|
||||
"delta_matrix_4x4": [
|
||||
[
|
||||
0.9999996872332217,
|
||||
-0.00030752729270551075,
|
||||
-0.0007286703114418758,
|
||||
0.000533303946557151
|
||||
],
|
||||
[
|
||||
0.00030892706731572284,
|
||||
0.9999981058814015,
|
||||
0.0019216653392719056,
|
||||
-0.002909732881513971
|
||||
],
|
||||
[
|
||||
0.0007280779667146176,
|
||||
-0.0019218898442212235,
|
||||
0.9999978881187204,
|
||||
0.001007919095162138
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
# RTK-IMU Calibration Package
|
||||
|
||||
`rtk_imu/` contains the independent RTK-IMU calibration pipeline: G90 observation I/O, antenna-baseline rotation, V1 replay, V3 multisource stages, engineering 6DoF, and the node-state factor graph.
|
||||
|
||||
## Boundary
|
||||
|
||||
This package may import only shared `imu_lidar` utilities: `contracts`, `geometry`, `geodesy`, `imu_io`, `imu_preintegration`, and the generic `rotation_handeye` initializer.
|
||||
|
||||
It must not import LiDAR-specific modules such as `lidar_io`, `lidar_deskew`, `registration`, `pipeline`, `phase_a`, or `joint_optimizer`. The LiDAR-IMU pipeline remains in `imu_lidar/`.
|
||||
|
||||
## Entrypoints
|
||||
Use `tools/run_rtk_imu_*.py` and `tools/audit_rtk_imu_*.py`; these scripts import from `rtk_imu.*`.
|
||||
For the engineering procedure, current result, acceptance boundary, and reproduction commands, see [`README_RTK_IMU.md`](../README_RTK_IMU.md).
|
||||
@@ -1 +0,0 @@
|
||||
"""RTK-IMU extrinsic calibration algorithms and data interfaces."""
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user