添加 LiDAR-IMU 外参标定流水线与说明文档
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
+10
@@ -0,0 +1,10 @@
|
|||||||
|
__pycache__/
|
||||||
|
*.py[cod]
|
||||||
|
.pytest_cache/
|
||||||
|
*.egg-info/
|
||||||
|
.eggs/
|
||||||
|
dist/
|
||||||
|
build/
|
||||||
|
examples/synthetic_session/
|
||||||
|
.venv/
|
||||||
|
venv/
|
||||||
@@ -0,0 +1,350 @@
|
|||||||
|
# LiDAR–IMU 外参标定
|
||||||
|
|
||||||
|
用车上的**激光雷达（LiDAR）**与**惯性测量单元（IMU）**在连续行驶中的相对运动，估计二者之间的安装关系（外参），并估计两路传感器的时间差。
|
||||||
|
|
||||||
|
```text
|
||||||
|
p_IMU = T_IMU_lidar · p_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
实现包：`[imu_lidar/](imu_lidar/)`。文件职责说明：`[imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md)`。
|
||||||
|
|
||||||
|
本仓库**只有这一条标定路径**（连续运动关键帧 + IMU 预积分）。不直接解析原始 `.rscap` / dlog，需先导出为中间格式。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 0. 现状一览（先读）
|
||||||
|
|
||||||
|
|
||||||
|
| 项 | 说明 |
|
||||||
|
| ------------- | ------------------------------------------------------------------------ |
|
||||||
|
| **仓库做什么** | 输入标准中间格式（IMU CSV + 雷达会话），输出 LiDAR–IMU **正式安装外参** `T_IMU_lidar` 与时间偏置 `δt`。合格数据且过质量门 → 可交付（常规先交旋转 + δt；激励够再交可观平移）；数据不合格或未过门 → `blocked`，不交外参 |
|
||||||
|
| **当前阶段** | 算法链路已闭环，合成自检通过；**尚缺合格实车数据验收**，故正式外参尚未对实车落盘交付（生产级原始录制→中间格式导出仍待完善） |
|
||||||
|
| **合成数据测什么** | 证明链路能跑通，并能收回**已知**旋转（yaw）与 δt；**不证明**实车安装精度或平移交付 |
|
||||||
|
| **旧车 S2 测什么** | 仅线下烟测：主机时间旧数据上流水线能否跑完；结果预期为 `blocked`，**不当作外参真值** |
|
||||||
|
| **合格数据** | 见下方定义；拿到后即可按正式标定流程跑通，期望得到可交付外参（先旋转 + δt） |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 什么叫合格数据
|
||||||
|
|
||||||
|
同时满足下列条件，才视为可用于正式标定（与旧车主机时间烟测数据区分）：
|
||||||
|
|
||||||
|
|
||||||
|
| 类别 | 要求 |
|
||||||
|
| ------------ | ----------------------------------------------------------------------------- |
|
||||||
|
| **时间戳** | IMU、雷达均用**设备时间**写入中间格式的 `t` / `t_start`/`t_end`（可另留主机接收时间作排查，但标定主轴不用主机时） |
|
||||||
|
| **会话结构** | 单趟：`静止 20–30 s` → `低速连续运动 3–8 min` → `再静止 10–20 s`；运动含明显左右转（如「8」字 / 圆）及适量加减速 |
|
||||||
|
| **场景** | 有墙、柱、路缘等结构，避免空旷无特征；雷达帧率与点数正常、无明显大面积丢帧 |
|
||||||
|
| **覆盖** | 两路传感器覆盖**同一段**物理行驶；安装全程不得改动 |
|
||||||
|
| **格式** | 已导出本仓库中间格式：`imu.csv` + `lidar_session/`（见 [docs/V1_数据格式.md](docs/V1_数据格式.md)） |
|
||||||
|
| **验证会话（建议）** | 另录一段独立路线 B，只做叠点/一致性检查，不参与求外参 |
|
||||||
|
|
||||||
|
|
||||||
|
**不算合格（举例）：** 仅主机接收时间；只有静止、无转弯；IMU 与雷达不是同一趟；未导出中间格式；时间相关极弱仍强行当安装参数。
|
||||||
|
|
||||||
|
完整现场清单见 [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md)。
|
||||||
|
|
||||||
|
### 合格数据拿到后 → 正式外参与预期效果
|
||||||
|
|
||||||
|
仓库目标产物就是**可安装使用的外参**（再加 δt）。合格数据过门后：
|
||||||
|
|
||||||
|
|
||||||
|
| 模式 | 跑法 | 正式交付内容 | 较现实预期 | 成功标志 |
|
||||||
|
| ----------------------- | ---------------------- | ----------- | --------------------------------- | ----------------------------------------- |
|
||||||
|
| `rotation_only`（第一步，必做） | `--mode rotation_only` | 旋转外参 + δt | 旋转约 **0.5°–2°**；δt 随同步与激励质量变化 | `summary.json` → `rotation_only_accepted` |
|
||||||
|
| `full_se3`（激励够再试） | `--mode full_se3` | 上一项 + 可观的平移 | 水平平移约数厘米～十几厘米；竖直常更差或不可观（需缓坡或垂直先验） | `full_se3_accepted`；否则旋转仍可交付、平移拒绝 |
|
||||||
|
|
||||||
|
|
||||||
|
交付前须叠点云 / 跨会话独立验证；`blocked` 一律不可用作安装参数。
|
||||||
|
**当前尚未完成的是「用合格实车数据跑出并验收这份外参」**，不是「合格数据也交不出外参」。
|
||||||
|
|
||||||
|
### 两类试验对照
|
||||||
|
|
||||||
|
|
||||||
|
| | 合成数据（默认 pytest / 一键复现） | 旧车 S2（线下，非默认测试） |
|
||||||
|
| ------- | -------------------------------------- | ---------------- |
|
||||||
|
| 目的 | 回归算法正确性 | 验证实车 IO/链路可跑 |
|
||||||
|
| 时间戳 | 可控、带已知 δt | 主机接收时间（非设备时间） |
|
||||||
|
| 是否合格数据 | 否（合成，仅对照） | **否**（主机时间，预期拒绝） |
|
||||||
|
| 期望结果 | `rotation_only_accepted`；δt / yaw 接近真值 | `blocked`（质量门拒绝） |
|
||||||
|
| 能否当安装参数 | 否 | **否** |
|
||||||
|
|
||||||
|
|
||||||
|
细节见 [tests/README.md](tests/README.md)。实现状态细表见 [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) §8。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 目录
|
||||||
|
|
||||||
|
1. [现状一览（先读）](#0-现状一览先读)
|
||||||
|
2. [系统输入 / 输出](#1-系统输入--输出)
|
||||||
|
3. [一键复现（合成数据）](#2-一键复现合成数据)
|
||||||
|
4. [能求出什么](#3-能求出什么)
|
||||||
|
5. [方法概要](#4-方法概要)
|
||||||
|
6. [分步求值](#5-分步求值)
|
||||||
|
7. [程序流程](#6-程序流程)
|
||||||
|
8. [真实数据怎么跑](#7-真实数据怎么跑)
|
||||||
|
9. [结果判读](#8-结果判读)
|
||||||
|
10. [仓库结构与文档](#9-仓库结构与文档)
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 1. 系统输入 / 输出
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 输入
|
||||||
|
|
||||||
|
|
||||||
|
| 输入 | 说明 |
|
||||||
|
| --------- | --------------------------------------------------------------------- |
|
||||||
|
| `imu.csv` | 列：`t,gx,gy,gz,ax,ay,az`（秒；陀螺 rad/s；加速度 m/s²） |
|
||||||
|
| 雷达点云 | `frames_index.csv` + `frames/frame_XXXXX.npz`（点云米制） |
|
||||||
|
| 车辆配置 YAML | 轴向与时间语义；**不必事先填外参真值**（模板：`config/vehicle_installation.template.yaml`） |
|
||||||
|
|
||||||
|
|
||||||
|
中间格式细节见 [docs/V1_数据格式.md](docs/V1_数据格式.md)。原始 `.rscap` 等需先导出。
|
||||||
|
|
||||||
|
### 输出（`--output` 目录）
|
||||||
|
|
||||||
|
|
||||||
|
| 文件 | 内容 |
|
||||||
|
| -------------------- | ---------------------------- |
|
||||||
|
| `T_IMU_lidar.json` | 外参矩阵、平移、四元数、欧拉角 |
|
||||||
|
| `time_offset.json` | δt，定义 `t_imu = t_lidar + δt` |
|
||||||
|
| `summary.json` | 状态、残差、可观性、各阶段细节 |
|
||||||
|
| `report_preview.png` | 一键复现时生成的简易指标图（可选） |
|
||||||
|
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2. 一键复现（合成数据）
|
||||||
|
|
||||||
|
不需要实车数据。脚本会：**生成合成 IMU/雷达 → 跑标定 → 打印报告并出预览图 → 跑 pytest**。
|
||||||
|
|
||||||
|
**这一趟证明什么：** 全流程可复现；能收回合成真值中的旋转（yaw）与时间偏置。
|
||||||
|
**不证明什么：** 实车安装精度、平移外参可交付、主机时间旧数据可用。
|
||||||
|
|
||||||
|
### 推荐（Windows）
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
cd <本仓库根目录>
|
||||||
|
python -m pip install -e ".[dev]"
|
||||||
|
python -m pip install -e ".[open3d]" # 可选，配准更稳；未装则用内置 ICP
|
||||||
|
# 预览图需要 matplotlib（没有则跳过绑图，不影响标定）
|
||||||
|
python -m pip install matplotlib
|
||||||
|
|
||||||
|
powershell -File tools\reproduce_synthetic.ps1
|
||||||
|
```
|
||||||
|
|
||||||
|
只要标定、跳过测试：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
powershell -File tools\reproduce_synthetic.ps1 -SkipPytest
|
||||||
|
```
|
||||||
|
|
||||||
|
尝试完整六自由度模式：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
powershell -File tools\reproduce_synthetic.ps1 -Mode full_se3 -SkipPytest
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 等价 Python 入口
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\reproduce_synthetic.py
|
||||||
|
python tools\reproduce_synthetic.py --mode full_se3 --skip-pytest
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 这一趟用了什么、产出什么
|
||||||
|
|
||||||
|
```text
|
||||||
|
INPUT
|
||||||
|
examples/synthetic_session/imu.csv
|
||||||
|
examples/synthetic_session/lidar/
|
||||||
|
config/vehicle_installation.template.yaml
|
||||||
|
examples/synthetic_session/meta.json # 合成真值（δt、yaw），仅用于对照
|
||||||
|
|
||||||
|
OUTPUT
|
||||||
|
examples/synthetic_session/out/T_IMU_lidar.json
|
||||||
|
examples/synthetic_session/out/time_offset.json
|
||||||
|
examples/synthetic_session/out/summary.json
|
||||||
|
examples/synthetic_session/out/report_preview.png
|
||||||
|
```
|
||||||
|
|
||||||
|
单独查看已有结果：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\show_calibration_report.py `
|
||||||
|
--summary examples\synthetic_session\out\summary.json `
|
||||||
|
--truth-meta examples\synthetic_session\meta.json `
|
||||||
|
--plot examples\synthetic_session\out\report_preview.png
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 配准结果 3D / 俯视可视化
|
||||||
|
|
||||||
|
蓝=目标帧，橙=源帧。
|
||||||
|
|
||||||
|
|
||||||
|
| 按键 | 作用 |
|
||||||
|
| --------------------- | ------------------------------------- |
|
||||||
|
| `1` / `2` / `3` / `4` | 原始 / IMU(X=I) / 雷达配准 B / 标定预测 `X⁻¹AX` |
|
||||||
|
| `N` 或 `]` | 下一运动对 |
|
||||||
|
| `P` 或 `[` | 上一运动对 |
|
||||||
|
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar examples\synthetic_session\lidar `
|
||||||
|
--imu examples\synthetic_session\imu.csv `
|
||||||
|
--summary examples\synthetic_session\out\summary.json `
|
||||||
|
--pair-index 0 `
|
||||||
|
--save-png examples\synthetic_session\out\pair0_overlay.png
|
||||||
|
```
|
||||||
|
|
||||||
|
`--pair-index` 只决定起始对；窗口内可继续按 `N`/`P` 切换。无 GUI 时加 `--no-gui`。封装：`tools\view_pair.ps1`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 3. 能求出什么
|
||||||
|
|
||||||
|
|
||||||
|
| 结果 | 含义 | 说明 |
|
||||||
|
| ------- | ------------- | ------------------ |
|
||||||
|
| 旋转外参 | 雷达相对 IMU 的朝向差 | 常规主交付 |
|
||||||
|
| 时间偏置 δt | 两路时钟差（秒） | 与旋转一并给出 |
|
||||||
|
| 平移外参 | 安装位置差（米） | 激励足够且质量门通过才接受，否则置零 |
|
||||||
|
| 零偏等 | 辅助量 | 改善积分/优化，一般不当安装外参交付 |
|
||||||
|
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 4. 方法概要
|
||||||
|
|
||||||
|
1. 低速多转弯行驶（如「8」字），首尾各留静止段。
|
||||||
|
2. 雷达关键帧配准得相对运动 **B**。
|
||||||
|
3. 同一时段 IMU 预积分得 **A**（先用旋转）。
|
||||||
|
4. 手眼关系：`R_A · R_X ≈ R_X · R_B`。
|
||||||
|
5. 时间与旋转稳定后，若 `--mode full_se3` 且可观，再估计平移（用加速度预积分、重力、速度与时变零偏）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 5. 分步求值
|
||||||
|
|
||||||
|
|
||||||
|
| 步骤 | 交付 | 典型要求 |
|
||||||
|
| --- | ------- | ---------------------- |
|
||||||
|
| 第一步 | 旋转 + δt | 设备时间戳；静止 + 低速转弯；有结构的场景 |
|
||||||
|
| 第二步 | + 水平平移 | 更多转弯半径与加减速 |
|
||||||
|
| 第三步 | 完整六自由度 | 缓坡等俯仰激励，或垂直安装尺寸先验 |
|
||||||
|
|
||||||
|
|
||||||
|
- `--mode rotation_only`：第一步
|
||||||
|
- `--mode full_se3`：尝试完整刚体；平移仍可能被拒绝
|
||||||
|
|
||||||
|
质量门未通过时，不能当正式安装参数。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 6. 程序流程
|
||||||
|
|
||||||
|
```text
|
||||||
|
读入 → 时间/IMU 质检 → 粗估 δt
|
||||||
|
→ 关键帧配准 B + IMU 预积分 A → 旋转手眼 R
|
||||||
|
→ 用 R 精修 δt（可交替组对）
|
||||||
|
→ 联合精修 R / 常值陀螺零偏
|
||||||
|
→（full_se3 且可观）重力、速度、时变零偏、平移 t
|
||||||
|
→ 写出 JSON
|
||||||
|
```
|
||||||
|
|
||||||
|
「可观」：程序检查转轴是否够多样、相对位移是否够丰富；不够则只保留旋转。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 7. 真实数据怎么跑
|
||||||
|
|
||||||
|
```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
|
||||||
|
```
|
||||||
|
|
||||||
|
采集与现场清单：[docs/标定流程与采集清单.md](docs/标定流程与采集清单.md)。
|
||||||
|
|
||||||
|
旧车 S2 主机时间烟测（预期 `blocked`，不当交付）：配置见 `config/s2_old_smoke.yaml`，步骤见 `[tests/README.md](tests/README.md)`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 8. 结果判读
|
||||||
|
|
||||||
|
|
||||||
|
| `summary.json` 状态 | 含义 |
|
||||||
|
| ---------------------------------------- | ----------------- |
|
||||||
|
| `rotation_only_accepted` | 旋转可用（该模式下平移置零） |
|
||||||
|
| `full_se3_accepted` | 含平移的完整外参通过 |
|
||||||
|
| `full_se3_rejected_due_to_observability` | 旋转可用，平移未接受 |
|
||||||
|
| `blocked` | 前序失败，**不可用作安装参数** |
|
||||||
|
|
||||||
|
|
||||||
|
请同时看手眼残差、时间相关、可观性说明，并做点云叠加等独立验证。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 9. 仓库结构与文档
|
||||||
|
|
||||||
|
```text
|
||||||
|
imu_lidar/ 标定算法与 CLI
|
||||||
|
config/ 车辆配置模板
|
||||||
|
docs/ 流程、数据格式、方法说明
|
||||||
|
tools/ 合成数据、一键复现、报告查看
|
||||||
|
tests/ 自动化测试
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
| 文档 | 内容 |
|
||||||
|
| ------------------------------------------------ | ---------------------- |
|
||||||
|
| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 各源文件职责 |
|
||||||
|
| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 算法改动记录 |
|
||||||
|
| [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场采集与分步流程（与现行实现对齐） |
|
||||||
|
| [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间数据格式 |
|
||||||
|
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 方法约定与实现状态 |
|
||||||
|
| [tests/README.md](tests/README.md) | 自动化测试说明 + S2 旧数据线下试验结果 |
|
||||||
|
|
||||||
|
|
||||||
|
改算法请同步更新 `imu_lidar/文件职责说明.md` 与 `CHANGELOG.md`；改安装/运行/对外说明时更新本文件。
|
||||||
@@ -0,0 +1,51 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "S2_old_validation"
|
||||||
|
body_frame:
|
||||||
|
name: "rear_axle_center"
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "S2_old_smoke"
|
||||||
|
installed_at: "unknown"
|
||||||
|
notes: "Smoke-test on old S2 host-time data only. Not for delivery."
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "HI13_old_S2"
|
||||||
|
raw_frame:
|
||||||
|
axes: "as exported HI91"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "frontlidar"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "Cartesian metres from points_raw spherical conversion"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "host_utc_receive_of_serial_chunk"
|
||||||
|
lidar_timestamp_source: "unix_time_ns_from_dlog_export"
|
||||||
|
lidar_frame_time_definition: "frame midpoint approx from unix_time_ns"
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_m: null
|
||||||
|
rotation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_deg: null
|
||||||
@@ -0,0 +1,51 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "example_vehicle"
|
||||||
|
body_frame:
|
||||||
|
name: "base_link"
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "example_install"
|
||||||
|
installed_at: "unknown"
|
||||||
|
notes: "V1 example config. Mount translations may stay null."
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "unknown_imu"
|
||||||
|
raw_frame:
|
||||||
|
axes: "declare after black-box tests: e.g. out_x=forward, out_y=left, out_z=up"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "unknown_lidar"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "device_or_file_clock_seconds"
|
||||||
|
lidar_timestamp_source: "frame_midpoint_seconds"
|
||||||
|
lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint"
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_m: null
|
||||||
|
rotation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_deg: null
|
||||||
@@ -0,0 +1,194 @@
|
|||||||
|
# LiDAR–IMU 外参标定说明
|
||||||
|
|
||||||
|
本文说明本仓库采用的标定目标、约定、流水线与实现状态。
|
||||||
|
代码位于本仓库的 `imu_lidar/` 包。场地采集细则可另见项目侧的采集需求文档。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 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`，不当交付 |
|
||||||
|
| 设备时间新车数据正式验收 | **待做** |
|
||||||
|
| 原生存储一键导出为中间格式 | **待完善** |
|
||||||
|
|
||||||
|
细项见 [`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) |
|
||||||
@@ -0,0 +1,69 @@
|
|||||||
|
# V1 标准中间数据格式
|
||||||
|
|
||||||
|
第一版流水线**不直接读** dlog / rscap。请先把数据整理成下列格式。
|
||||||
|
|
||||||
|
## IMU
|
||||||
|
|
||||||
|
文件：`imu.csv` 或 `imu.npz`
|
||||||
|
|
||||||
|
### CSV
|
||||||
|
|
||||||
|
```text
|
||||||
|
t,gx,gy,gz,ax,ay,az
|
||||||
|
0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81
|
||||||
|
...
|
||||||
|
```
|
||||||
|
|
||||||
|
| 列 | 含义 | 单位 |
|
||||||
|
|---|---|---|
|
||||||
|
| t | IMU 时钟时间 | s |
|
||||||
|
| gx,gy,gz | 角速度 | rad/s |
|
||||||
|
| ax,ay,az | 比力/加速度 | m/s² |
|
||||||
|
|
||||||
|
### 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
|
||||||
|
0,frames/frame_00000.npz,10.000,10.100
|
||||||
|
1,frames/frame_00001.npz,10.100,10.200
|
||||||
|
```
|
||||||
|
|
||||||
|
也兼容旧列名 `file`（NumPy 读取时可能变成 `file_`）。
|
||||||
|
|
||||||
|
### 每帧 NPZ
|
||||||
|
|
||||||
|
- `points`: `float64/float32`，形状 `(N, 3)`，LiDAR 直角坐标系，单位米
|
||||||
|
|
||||||
|
## 时间不同步能不能用？
|
||||||
|
|
||||||
|
可以，前提是：
|
||||||
|
|
||||||
|
1. 两边都覆盖同一段**有角速度激励**的物理运动（尤其是转弯）；
|
||||||
|
2. 偏置近似为**常数**（短会话）；
|
||||||
|
3. `--time-offset-search-s` 足够覆盖可能的偏移（默认 ±1 s，可加大）。
|
||||||
|
|
||||||
|
若两段数据完全不是同一趟行驶，或只有静止 IMU、没有重叠运动，则无法估 δt，标定会被 `blocked`。
|
||||||
|
|
||||||
|
## 最小可用会话
|
||||||
|
|
||||||
|
- IMU：建议含静止段 + 运动段，采样率稳定
|
||||||
|
- LiDAR：建议 ≥ 20 帧，场景有墙/柱等结构，含转弯
|
||||||
@@ -0,0 +1,156 @@
|
|||||||
|
# 纯 LiDAR–IMU：标定流程与采集清单
|
||||||
|
|
||||||
|
仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐流程。
|
||||||
|
目标外参：`p_IMU = T_IMU_lidar · p_lidar`（`T_A_B` 表示把 B 系点变到 A 系）。
|
||||||
|
|
||||||
|
算法实现与命令见根目录 `[README.md](../README.md)`；改动史见 `[imu_lidar/CHANGELOG.md](../imu_lidar/CHANGELOG.md)`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 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 → 验证后交付
|
||||||
|
```
|
||||||
|
|
||||||
@@ -0,0 +1,149 @@
|
|||||||
|
# `imu_lidar` 改动记录
|
||||||
|
|
||||||
|
本文件专门记录 `imu_lidar` 目录内的实现改动。
|
||||||
|
每条包含：**时间戳**、**改动内容**（以「原本怎么做 → 改成怎么做」书写）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-01 11:40 (UTC+8)
|
||||||
|
|
||||||
|
### 文档：现状一览补充「合格数据」定义
|
||||||
|
|
||||||
|
- **原本**：§0 只写「合格数据拿到后」怎么跑，未写清何为合格。
|
||||||
|
- **改成**：根 [`README.md`](../README.md) §0 增加「什么叫合格数据」表（时间戳 / 会话 / 场景 / 格式 / 反例）及拿到后的模式与预期。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-01 11:30 (UTC+8)
|
||||||
|
|
||||||
|
### 文档：现状一览 + 去掉「方案」二分表述
|
||||||
|
|
||||||
|
- **原本**：对外说明仍偶发「方案二」等旧称呼；根 README 缺少一眼可读的阶段 / 合成 vs 旧车 / 合格数据预期；烟测配置与对比脚本文件名带 `scheme2`。
|
||||||
|
- **改成**：
|
||||||
|
- 根 [`README.md`](../README.md) 增加 §0「现状一览」；明确仓库只有一条连续运动标定路径。
|
||||||
|
- [`tests/README.md`](../tests/README.md)、[`docs/IMU-LiDAR标定.md`](../docs/IMU-LiDAR标定.md)、本目录说明同步边界与阶段。
|
||||||
|
- `config/s2_old_smoke.yaml`、`tools/compare_s2_runs.py` 替换旧 `*scheme2*` 命名。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-31 18:10 (UTC+8)
|
||||||
|
|
||||||
|
### 配准可视化工具 + tests 说明（含 S2 线下记录）
|
||||||
|
|
||||||
|
- **原本**：无类似 RTK 仓库的运动对叠点 3D 查看；`tests/` 未说明合成 pytest 与 S2 旧数据线下试验的区别与结果。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/visualize_pair_3d.py` / `view_pair.ps1`（键 1–4：原始 / IMU(X=I) / 雷达 B / `X⁻¹AX`；可 `--save-png`）。
|
||||||
|
- 新增 [`tests/README.md`](../tests/README.md)：自动化用例表 + S2 主机时间数据做了什么、结果为何 `blocked`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-31 17:20 (UTC+8)
|
||||||
|
|
||||||
|
### 文档同步 + 合成数据一键复现
|
||||||
|
|
||||||
|
- **原本**：`docs/标定流程与采集清单.md` 仍偏旧版「待写代码 / 因子图设想」；根 README 缺少清晰的一键复现入口与输入输出总表。
|
||||||
|
- **改成**：
|
||||||
|
- 采集清单与现行流水线对齐（完整预积分、δt↔R 交替、可观时再估平移）。
|
||||||
|
- 新增 `tools/reproduce_synthetic.py` / `.ps1`、`tools/show_calibration_report.py`；合成生成写入 `meta.json`；根 README 增加「系统输入输出 + 一键复现」。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-31 16:30 (UTC+8)
|
||||||
|
|
||||||
|
### 文档：移除已删除的静站路径表述，对外 README 重写
|
||||||
|
|
||||||
|
- **原本**：根 README / `docs` / 包说明仍对照已删除的静站路径与内部阶段黑话；`pyproject` 仍声明已删除的 `static_station` 包。
|
||||||
|
- **改成**：
|
||||||
|
- 删除旧静站文档；采集清单定为 [`docs/标定流程与采集清单.md`](../docs/标定流程与采集清单.md)。
|
||||||
|
- 根 [`README.md`](../README.md)、[`docs/IMU-LiDAR标定.md`](../docs/IMU-LiDAR标定.md)、本目录说明改为对外可读，只保留连续运动标定路径。
|
||||||
|
- `pyproject.toml` 仅保留 `imu_lidar` / `tools`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-31 14:00 (UTC+8)
|
||||||
|
|
||||||
|
### Phase-C：完整 IMU 预积分 + 重力/速度/动态零偏（full_se3）
|
||||||
|
|
||||||
|
- **原本**：
|
||||||
|
- 运动对仅陀螺旋转预积分（`ΔR/Σ/J_bg`）；`t_A` 为空。
|
||||||
|
- 联合精修只估常值陀螺零偏修正；SE(3) 平移用经典手眼式 `(R_A-I)t ≈ R_X t_B`，无重力/速度/`b_a`。
|
||||||
|
- **改成**：
|
||||||
|
- `imu_preintegration.preintegrate_imu`：中值法积分 `ΔR/Δv/Δp`，传播 15 维误差态后输出 9×9 `Σ`（含 bias RW 过程噪声）与 9×3 `J_bg/J_ba`；保留 `preintegrate_gyro`。
|
||||||
|
- `motion_pairs` 始终调用完整预积分，写入 `delta_v/delta_p/cov9/J_bg9/J_ba`，`t_A_m=Δp`。
|
||||||
|
- `joint_optimizer`：`rotation_only` 仍 Phase-A；`full_se3` 可观时 Phase-C 联合估 `R_X,t_X,g,v_k,b_g,k,b_a,k`（关键帧 RW 先验）。
|
||||||
|
- `pipeline` 用静止加速度推重力初值；`summary.joint` 增加 `gravity_m_s2` / `accel_bias_m_s2`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-31 11:20 (UTC+8)
|
||||||
|
|
||||||
|
### 文档维护约定 + README 与现行实现对齐
|
||||||
|
|
||||||
|
- **原本**：根 README 与已删除的静站目录说明仍按「双路径并行」表述；部分模块说明未写明有符号 δt；改代码时 README 更新不完整。
|
||||||
|
- **改成**：
|
||||||
|
- 对外说明统一为**唯一连续运动标定路径**；流水线描述对齐有符号 δt 与联合精修。
|
||||||
|
- 根 README 增加「文档维护」表：每次改代码必须同步涉及的 README / 本 CHANGELOG。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-31 09:40 (UTC+8)
|
||||||
|
|
||||||
|
### 流水线：手眼未过门时仍尝试有符号 δt 精修
|
||||||
|
|
||||||
|
- **原本**：`rotation_handeye.ok=false`（如 RMS>5°）时立即 `blocked` 返回，阶段 A 的有符号 δt 精修根本不会执行。
|
||||||
|
- **改成**：只要可用运动对数 ≥3，即使用当前候选 `R` 做最多 2 轮有符号 δt 精修并重建运动对；精修后再按手眼门控决定是否 `blocked`。保证阶段 A 在困难数据上也能完整参与。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-31 09:20 (UTC+8)
|
||||||
|
|
||||||
|
### 阶段 A：标准旋转预积分因子 + 精确时间边界 + 有符号 δt 精修
|
||||||
|
|
||||||
|
- **原本**：
|
||||||
|
- 预积分只输出 `ΔR` 与启发式标量 weight/`σ`，区间端点用邻近 IMU 样本，无 `Σ`、无 `J_bg`。
|
||||||
|
- δt 仅靠角速度模长互相关粗估；手眼得到 `R` 后不再回头精修时间。
|
||||||
|
- 联合精修对零偏多用重积分或 `Exp(-δbΔt)` 近似，残差未按协方差白化，也无 `δb` 先验。
|
||||||
|
- **改成**：
|
||||||
|
- `imu_preintegration.preintegrate_gyro`：区间端点 **线性插值** 到精确 `t0/t1`；离散中值更新同时传播 **`cov(Σ)`** 与 **`J_bg`**（`ΔR(b+δb)≈ΔR Exp(J_bg δb)`）；weight 由 `trace(Σ)` + 激励/时长构造。
|
||||||
|
- `motion_pairs` metadata 增加 `cov`、`J_bg`，modeling 标记为 `gyro_preintegration_factor_phase_a`。
|
||||||
|
- `time_offset.refine_time_offset_signed`：用当前 `R_IMU_lidar` 把 LiDAR 角速度变到 IMU 系，在粗 δt 邻域做 **三轴有符号 MSE 精修**；仅当 MSE 下降且 **模长相关不劣化** 时才接受，避免 ICP 噪声带偏；`pipeline` 在手眼后与构对交替最多 2 轮。
|
||||||
|
- `joint_optimizer`：残差按 `Σ` **信息白化**；零偏用 `J_bg` 一阶修正；增加弱 `δb` 先验。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-07-30 17:50 (UTC+8)
|
||||||
|
|
||||||
|
### 第 1 步：帧间 IMU 轻量加强（加权预积分手眼）
|
||||||
|
|
||||||
|
- **原本**：`motion_pairs` 用 `integrate_gyro_rotation` 直接得到 `R_A`，各运动对等权进入 `rotation_handeye`；手眼残差不区分长短间隔与激励强弱。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `imu_preintegration.py`：对 `[t_i, t_j]` 做中值陀螺预积分，估计 `σ` 与 **pair weight**（偏短间隔、有角速度、低不确定度）。
|
||||||
|
- `motion_pairs` 改为调用 `preintegrate_gyro`，在 `metadata` 写入 `weight/duration_s/mean_gyro_norm/preint_sigma_rad/t_*_imu_s`，并增加 A/B 转角粗一致性过滤。
|
||||||
|
- `rotation_handeye` 改为 **√weight 加权** 的 Tsai 初值与 Huber 非线性精修；报告仍给未加权 RMS/中位数便于解读。
|
||||||
|
|
||||||
|
### 第 2 步：预积分残差联合精修（外参 + 陀螺零偏）
|
||||||
|
|
||||||
|
- **原本**：`joint_optimizer` 在手眼 `R_X` 基础上，仅在可观时用离散手眼平移式尝试 SE(3)；旋转侧不再用 IMU 过程模型，也不联合估零偏。
|
||||||
|
- **改成**：
|
||||||
|
- `joint_optimizer.solve_joint_extrinsic` 增加预积分旋转残差：`log(ΔRᵀ · R_X R_B R_Xᵀ)`，按 weight 加权。
|
||||||
|
- 联合变量增加陀螺零偏修正 `δb`：有 `imu` 时按区间 **重预积分**；否则用一阶修正 `ΔR(b+δb)≈ΔR Exp(-δbΔt)`。
|
||||||
|
- `pipeline` 把 `imu`、静止零偏、`δt` 传入 joint；summary 增加 `gyro_bias_rad_s`。
|
||||||
|
- 平移仍受可观性门控；`rotation_only` 时不交付平移。
|
||||||
|
|
||||||
|
### 文档
|
||||||
|
|
||||||
|
- **原本**：`imu_lidar/README.md` 仅模块列表，无逐次改动史。
|
||||||
|
- **改成**：新增本文件 `CHANGELOG.md`；模块说明中补充 `imu_preintegration.py` 与建模步骤描述。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 模板（以后追加用）
|
||||||
|
|
||||||
|
```markdown
|
||||||
|
## YYYY-MM-DD HH:MM (UTC+8)
|
||||||
|
|
||||||
|
### 标题
|
||||||
|
|
||||||
|
- **原本**：...
|
||||||
|
- **改成**：...
|
||||||
|
```
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""LiDAR–IMU calibration package (V1 runnable pipeline)."""
|
||||||
|
|
||||||
|
from .contracts import CalibrationMode, CalibrationStatus, TransformConvention
|
||||||
|
|
||||||
|
__all__ = ["CalibrationMode", "CalibrationStatus", "TransformConvention"]
|
||||||
@@ -0,0 +1,90 @@
|
|||||||
|
"""Command-line entry point for LiDAR–IMU calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from .contracts import CalibrationMode, CalibrationRequest, CalibrationStatus, SessionInput
|
||||||
|
from .pipeline import describe_pipeline, run_calibration
|
||||||
|
|
||||||
|
|
||||||
|
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 标定流水线")
|
||||||
|
run.add_argument("--session-id", default="session0")
|
||||||
|
run.add_argument("--imu", required=True, help="IMU CSV/NPZ 路径")
|
||||||
|
run.add_argument("--lidar", required=True, help="LiDAR 会话目录（含 frames_index.csv）")
|
||||||
|
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("--min-pair-rotation-deg", type=float, default=3.0)
|
||||||
|
run.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
||||||
|
return parser
|
||||||
|
|
||||||
|
|
||||||
|
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 == "run":
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=Path(args.vehicle_config),
|
||||||
|
sessions=(
|
||||||
|
SessionInput(
|
||||||
|
session_id=args.session_id,
|
||||||
|
imu_source=Path(args.imu),
|
||||||
|
lidar_source=Path(args.lidar),
|
||||||
|
),
|
||||||
|
),
|
||||||
|
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,
|
||||||
|
time_offset_search_s=args.time_offset_search_s,
|
||||||
|
)
|
||||||
|
result = run_calibration(request)
|
||||||
|
print(f"status: {result.status.value}")
|
||||||
|
print(f"message: {result.message}")
|
||||||
|
if result.time_offset_s is not None:
|
||||||
|
print(f"time_offset_s (t_imu = t_lidar + dt): {result.time_offset_s:.6f}")
|
||||||
|
if result.T_IMU_lidar is not None:
|
||||||
|
print("T_IMU_lidar:")
|
||||||
|
print(result.T_IMU_lidar)
|
||||||
|
print(f"report directory: {args.output}")
|
||||||
|
return 0 if result.status != CalibrationStatus.BLOCKED else 2
|
||||||
|
|
||||||
|
parser.error(f"unknown command {args.command}")
|
||||||
|
return 2
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,117 @@
|
|||||||
|
"""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"
|
||||||
|
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
|
||||||
|
|
||||||
|
|
||||||
|
@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
|
||||||
|
time_offset_search_s: float = 1.0
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class CalibrationResult:
|
||||||
|
"""Result envelope written by finalize after pipeline gates."""
|
||||||
|
|
||||||
|
status: CalibrationStatus = CalibrationStatus.NOT_RUN
|
||||||
|
message: str = "Calibration has not been executed."
|
||||||
|
details: dict[str, Any] = field(default_factory=dict)
|
||||||
|
T_IMU_lidar: np.ndarray | None = None
|
||||||
|
time_offset_s: float | None = None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuSeries:
|
||||||
|
"""Normalized IMU samples.
|
||||||
|
|
||||||
|
``t_s`` is the native IMU clock in seconds (need not match LiDAR epoch).
|
||||||
|
Gyro must be rad/s; accelerometer must be m/s^2.
|
||||||
|
"""
|
||||||
|
|
||||||
|
t_s: np.ndarray
|
||||||
|
gyro_rad_s: np.ndarray
|
||||||
|
acc_m_s2: np.ndarray
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
object.__setattr__(self, "t_s", np.asarray(self.t_s, dtype=float).reshape(-1))
|
||||||
|
object.__setattr__(self, "gyro_rad_s", np.asarray(self.gyro_rad_s, dtype=float).reshape(-1, 3))
|
||||||
|
object.__setattr__(self, "acc_m_s2", np.asarray(self.acc_m_s2, dtype=float).reshape(-1, 3))
|
||||||
|
n = self.t_s.size
|
||||||
|
if self.gyro_rad_s.shape != (n, 3) or self.acc_m_s2.shape != (n, 3):
|
||||||
|
raise ValueError("IMU arrays must share the same length and have shape (N, 3)")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class LidarFrame:
|
||||||
|
"""One LiDAR sweep in Cartesian sensor coordinates."""
|
||||||
|
|
||||||
|
frame_id: str
|
||||||
|
t_start_s: float
|
||||||
|
t_end_s: float
|
||||||
|
points_xyz: np.ndarray
|
||||||
|
path: Path | None = None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def t_mid_s(self) -> float:
|
||||||
|
return 0.5 * (self.t_start_s + self.t_end_s)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MotionPair:
|
||||||
|
"""One relative-motion observation between keyframes i and j."""
|
||||||
|
|
||||||
|
session_id: str
|
||||||
|
i: int
|
||||||
|
j: int
|
||||||
|
t_i_s: float
|
||||||
|
t_j_s: float
|
||||||
|
R_A: np.ndarray
|
||||||
|
R_B: np.ndarray
|
||||||
|
t_A_m: np.ndarray | None = None
|
||||||
|
t_B_m: np.ndarray | None = None
|
||||||
|
fitness: float = 0.0
|
||||||
|
metadata: dict[str, Any] = field(default_factory=dict)
|
||||||
@@ -0,0 +1,76 @@
|
|||||||
|
"""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,
|
||||||
|
) -> 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",
|
||||||
|
)
|
||||||
|
(output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
|
||||||
|
return result
|
||||||
@@ -0,0 +1,207 @@
|
|||||||
|
"""SE(3)/SO(3) utilities for LiDAR–IMU calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
def skew(vector: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return the skew-symmetric matrix such that ``skew(v) @ w == v x w``."""
|
||||||
|
|
||||||
|
x, y, z = np.asarray(vector, dtype=float).reshape(3)
|
||||||
|
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def so3_exp(rotation_vector: np.ndarray) -> np.ndarray:
|
||||||
|
"""Map a rotation vector in radians onto SO(3)."""
|
||||||
|
|
||||||
|
vector = np.asarray(rotation_vector, dtype=float).reshape(3)
|
||||||
|
angle = float(np.linalg.norm(vector))
|
||||||
|
if angle < 1e-12:
|
||||||
|
return np.eye(3) + skew(vector)
|
||||||
|
axis_cross = skew(vector / angle)
|
||||||
|
return np.eye(3) + math.sin(angle) * axis_cross + (1.0 - math.cos(angle)) * axis_cross @ axis_cross
|
||||||
|
|
||||||
|
|
||||||
|
def so3_log(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Map an SO(3) matrix to a rotation vector in radians."""
|
||||||
|
|
||||||
|
rotation = np.asarray(rotation, dtype=float).reshape(3, 3)
|
||||||
|
cos_angle = float(np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0))
|
||||||
|
angle = math.acos(cos_angle)
|
||||||
|
if angle < 1e-12:
|
||||||
|
return 0.5 * np.array(
|
||||||
|
[
|
||||||
|
rotation[2, 1] - rotation[1, 2],
|
||||||
|
rotation[0, 2] - rotation[2, 0],
|
||||||
|
rotation[1, 0] - rotation[0, 1],
|
||||||
|
],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
if abs(angle - math.pi) < 1e-6:
|
||||||
|
# Near 180°: use eigenvector of the +1 eigenvalue.
|
||||||
|
eigvals, eigvecs = np.linalg.eigh(0.5 * (rotation + rotation.T))
|
||||||
|
axis = eigvecs[:, int(np.argmax(eigvals))]
|
||||||
|
return axis * angle
|
||||||
|
return (
|
||||||
|
0.5
|
||||||
|
* angle
|
||||||
|
/ math.sin(angle)
|
||||||
|
* np.array(
|
||||||
|
[
|
||||||
|
rotation[2, 1] - rotation[1, 2],
|
||||||
|
rotation[0, 2] - rotation[2, 0],
|
||||||
|
rotation[1, 0] - rotation[0, 1],
|
||||||
|
],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def rotation_angle_deg(rotation: np.ndarray) -> float:
|
||||||
|
"""Return the rotation angle in degrees."""
|
||||||
|
|
||||||
|
return float(np.degrees(np.linalg.norm(so3_log(rotation))))
|
||||||
|
|
||||||
|
|
||||||
|
def inverse_transform(transform: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return the inverse of a rigid 4x4 transform."""
|
||||||
|
|
||||||
|
transform = np.asarray(transform, dtype=float)
|
||||||
|
if transform.shape != (4, 4):
|
||||||
|
raise ValueError("a rigid transform must have shape (4, 4)")
|
||||||
|
result = np.eye(4)
|
||||||
|
result[:3, :3] = transform[:3, :3].T
|
||||||
|
result[:3, 3] = -result[:3, :3] @ transform[:3, 3]
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def make_transform(translation_m: np.ndarray, rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Build ``T_A_B`` from its translation and rotation components."""
|
||||||
|
|
||||||
|
translation_m = np.asarray(translation_m, dtype=float).reshape(3)
|
||||||
|
rotation = np.asarray(rotation, dtype=float)
|
||||||
|
if rotation.shape != (3, 3):
|
||||||
|
raise ValueError("a rotation matrix must have shape (3, 3)")
|
||||||
|
result = np.eye(4)
|
||||||
|
result[:3, :3] = rotation
|
||||||
|
result[:3, 3] = translation_m
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def transform_points(points: np.ndarray, transform: np.ndarray) -> np.ndarray:
|
||||||
|
"""Apply ``T_A_B`` to an ``(N, 3)`` point array expressed in frame B."""
|
||||||
|
|
||||||
|
points = np.asarray(points, dtype=float)
|
||||||
|
if points.ndim != 2 or points.shape[1] != 3:
|
||||||
|
raise ValueError("points must have shape (N, 3)")
|
||||||
|
return points @ transform[:3, :3].T + transform[:3, 3]
|
||||||
|
|
||||||
|
|
||||||
|
def orthonormalize_rotation(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Project a near-rotation matrix onto SO(3)."""
|
||||||
|
|
||||||
|
u, _, vt = np.linalg.svd(np.asarray(rotation, dtype=float).reshape(3, 3))
|
||||||
|
result = u @ vt
|
||||||
|
if np.linalg.det(result) < 0:
|
||||||
|
u[:, -1] *= -1
|
||||||
|
result = u @ vt
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def integrate_gyro_rotation(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
bias_rad_s: np.ndarray | None = None,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Integrate gyroscope samples on ``[t0, t1]`` and return ``R(t0<-t1)`` wait.
|
||||||
|
|
||||||
|
Returns ``R_i_j`` that maps vectors from the IMU frame at ``t1`` into the
|
||||||
|
IMU frame at ``t0`` using right-invariant discrete integration:
|
||||||
|
|
||||||
|
R <- R @ Exp(omega * dt)
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
if times_s.size < 2:
|
||||||
|
return np.eye(3)
|
||||||
|
bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
if t1 < t0:
|
||||||
|
raise ValueError("t1 must be >= t0")
|
||||||
|
|
||||||
|
# Include one sample before t0 and after t1 when possible for interpolation.
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return np.eye(3)
|
||||||
|
|
||||||
|
rotation = np.eye(3)
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
omega = 0.5 * (gyro_rad_s[index] + gyro_rad_s[index + 1]) - bias
|
||||||
|
rotation = rotation @ so3_exp(omega * dt)
|
||||||
|
return orthonormalize_rotation(rotation)
|
||||||
|
|
||||||
|
|
||||||
|
def rotation_matrix_to_quaternion_xyzw(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Convert SO(3) to quaternion ``[x, y, z, w]``."""
|
||||||
|
|
||||||
|
rotation = orthonormalize_rotation(rotation)
|
||||||
|
trace = float(np.trace(rotation))
|
||||||
|
if trace > 0:
|
||||||
|
s = math.sqrt(trace + 1.0) * 2.0
|
||||||
|
w = 0.25 * s
|
||||||
|
x = (rotation[2, 1] - rotation[1, 2]) / s
|
||||||
|
y = (rotation[0, 2] - rotation[2, 0]) / s
|
||||||
|
z = (rotation[1, 0] - rotation[0, 1]) / s
|
||||||
|
elif rotation[0, 0] > rotation[1, 1] and rotation[0, 0] > rotation[2, 2]:
|
||||||
|
s = math.sqrt(1.0 + rotation[0, 0] - rotation[1, 1] - rotation[2, 2]) * 2.0
|
||||||
|
w = (rotation[2, 1] - rotation[1, 2]) / s
|
||||||
|
x = 0.25 * s
|
||||||
|
y = (rotation[0, 1] + rotation[1, 0]) / s
|
||||||
|
z = (rotation[0, 2] + rotation[2, 0]) / s
|
||||||
|
elif rotation[1, 1] > rotation[2, 2]:
|
||||||
|
s = math.sqrt(1.0 + rotation[1, 1] - rotation[0, 0] - rotation[2, 2]) * 2.0
|
||||||
|
w = (rotation[0, 2] - rotation[2, 0]) / s
|
||||||
|
x = (rotation[0, 1] + rotation[1, 0]) / s
|
||||||
|
y = 0.25 * s
|
||||||
|
z = (rotation[1, 2] + rotation[2, 1]) / s
|
||||||
|
else:
|
||||||
|
s = math.sqrt(1.0 + rotation[2, 2] - rotation[0, 0] - rotation[1, 1]) * 2.0
|
||||||
|
w = (rotation[1, 0] - rotation[0, 1]) / s
|
||||||
|
x = (rotation[0, 2] + rotation[2, 0]) / s
|
||||||
|
y = (rotation[1, 2] + rotation[2, 1]) / s
|
||||||
|
z = 0.25 * s
|
||||||
|
return np.array([x, y, z, w], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def rpy_deg_xyz(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
"""Intrinsic XYZ Euler angles in degrees from a rotation matrix."""
|
||||||
|
|
||||||
|
rotation = orthonormalize_rotation(rotation)
|
||||||
|
sy = math.sqrt(rotation[0, 0] ** 2 + rotation[1, 0] ** 2)
|
||||||
|
if sy > 1e-8:
|
||||||
|
roll = math.atan2(rotation[2, 1], rotation[2, 2])
|
||||||
|
pitch = math.atan2(-rotation[2, 0], sy)
|
||||||
|
yaw = math.atan2(rotation[1, 0], rotation[0, 0])
|
||||||
|
else:
|
||||||
|
roll = math.atan2(-rotation[1, 2], rotation[1, 1])
|
||||||
|
pitch = math.atan2(-rotation[2, 0], sy)
|
||||||
|
yaw = 0.0
|
||||||
|
return np.degrees(np.array([roll, pitch, yaw], dtype=float))
|
||||||
@@ -0,0 +1,86 @@
|
|||||||
|
"""IMU unit, axis, bias, and saturation audit."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries
|
||||||
|
|
||||||
|
G = 9.80665
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuAuditReport:
|
||||||
|
ok: bool
|
||||||
|
gyro_bias_rad_s: np.ndarray
|
||||||
|
static_acc_mean_m_s2: np.ndarray
|
||||||
|
static_acc_norm_m_s2: float
|
||||||
|
suggested_up_axis: int
|
||||||
|
suggested_up_sign: float
|
||||||
|
static_ratio: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _static_mask(gyro: np.ndarray, acc: np.ndarray) -> np.ndarray:
|
||||||
|
gyro_norm = np.linalg.norm(gyro, axis=1)
|
||||||
|
acc_norm = np.linalg.norm(acc, axis=1)
|
||||||
|
gyro_thr = max(0.02, float(np.percentile(gyro_norm, 20)) * 1.5)
|
||||||
|
acc_thr_low = 0.7 * G
|
||||||
|
acc_thr_high = 1.3 * G
|
||||||
|
return (gyro_norm < gyro_thr) & (acc_norm > acc_thr_low) & (acc_norm < acc_thr_high)
|
||||||
|
|
||||||
|
|
||||||
|
def audit_imu(imu: ImuSeries) -> ImuAuditReport:
|
||||||
|
"""Audit normalized IMU samples and estimate a static gyro bias."""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
mask = _static_mask(imu.gyro_rad_s, imu.acc_m_s2)
|
||||||
|
static_ratio = float(np.mean(mask)) if mask.size else 0.0
|
||||||
|
if static_ratio < 0.02:
|
||||||
|
# Fall back to lowest-gyro percentile window.
|
||||||
|
gyro_norm = np.linalg.norm(imu.gyro_rad_s, axis=1)
|
||||||
|
cutoff = float(np.percentile(gyro_norm, 10))
|
||||||
|
mask = gyro_norm <= cutoff
|
||||||
|
notes.append("few gravity-consistent static samples; using lowest-gyro percentile")
|
||||||
|
static_ratio = float(np.mean(mask))
|
||||||
|
|
||||||
|
if not np.any(mask):
|
||||||
|
notes.append("no static samples found")
|
||||||
|
bias = np.zeros(3)
|
||||||
|
acc_mean = np.zeros(3)
|
||||||
|
acc_norm = 0.0
|
||||||
|
up_axis = 2
|
||||||
|
up_sign = 1.0
|
||||||
|
ok = False
|
||||||
|
else:
|
||||||
|
bias = np.mean(imu.gyro_rad_s[mask], axis=0)
|
||||||
|
acc_mean = np.mean(imu.acc_m_s2[mask], axis=0)
|
||||||
|
acc_norm = float(np.linalg.norm(acc_mean))
|
||||||
|
up_axis = int(np.argmax(np.abs(acc_mean)))
|
||||||
|
up_sign = float(np.sign(acc_mean[up_axis]) or 1.0)
|
||||||
|
if abs(acc_norm - G) > 2.5:
|
||||||
|
notes.append(
|
||||||
|
f"static |acc|={acc_norm:.3f} differs from g={G}; check units (expect m/s^2)"
|
||||||
|
)
|
||||||
|
gyro_peak = float(np.max(np.linalg.norm(imu.gyro_rad_s, axis=1)))
|
||||||
|
if gyro_peak > 20.0:
|
||||||
|
notes.append(
|
||||||
|
f"peak |gyro|={gyro_peak:.1f} rad/s looks extreme; check whether data is deg/s"
|
||||||
|
)
|
||||||
|
ok = abs(acc_norm - G) < 3.5 or static_ratio > 0.05
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"suggested up axis index={up_axis} sign={up_sign:+.0f} (0=x,1=y,2=z)"
|
||||||
|
)
|
||||||
|
return ImuAuditReport(
|
||||||
|
ok=ok,
|
||||||
|
gyro_bias_rad_s=np.asarray(bias, dtype=float),
|
||||||
|
static_acc_mean_m_s2=np.asarray(acc_mean, dtype=float),
|
||||||
|
static_acc_norm_m_s2=float(acc_norm),
|
||||||
|
suggested_up_axis=up_axis,
|
||||||
|
suggested_up_sign=up_sign,
|
||||||
|
static_ratio=static_ratio,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,72 @@
|
|||||||
|
"""IMU adapters for the V1 standard intermediate format.
|
||||||
|
|
||||||
|
Accepted inputs
|
||||||
|
---------------
|
||||||
|
1. CSV with header:
|
||||||
|
t,gx,gy,gz,ax,ay,az
|
||||||
|
- ``t`` in seconds on the IMU clock
|
||||||
|
- gyro in rad/s
|
||||||
|
- accel in m/s^2
|
||||||
|
|
||||||
|
2. NPZ with arrays:
|
||||||
|
t, gyro, acc
|
||||||
|
shapes: (N,), (N,3), (N,3)
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries
|
||||||
|
|
||||||
|
|
||||||
|
def load_imu_samples(path: Path | str) -> ImuSeries:
|
||||||
|
"""Load normalized IMU samples from CSV or NPZ."""
|
||||||
|
|
||||||
|
source = Path(path)
|
||||||
|
if not source.exists():
|
||||||
|
raise FileNotFoundError(source)
|
||||||
|
if source.suffix.lower() == ".csv":
|
||||||
|
return _load_imu_csv(source)
|
||||||
|
if source.suffix.lower() == ".npz":
|
||||||
|
return _load_imu_npz(source)
|
||||||
|
raise ValueError(f"unsupported IMU format '{source.suffix}' (use .csv or .npz)")
|
||||||
|
|
||||||
|
|
||||||
|
def _load_imu_csv(path: Path) -> ImuSeries:
|
||||||
|
data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
|
||||||
|
if data.ndim == 0:
|
||||||
|
data = np.array([data])
|
||||||
|
names = set(data.dtype.names or ())
|
||||||
|
required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
|
||||||
|
if not required.issubset(names):
|
||||||
|
raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
|
||||||
|
t = np.asarray(data["t"], dtype=float).reshape(-1)
|
||||||
|
gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
|
||||||
|
acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
|
||||||
|
order = np.argsort(t)
|
||||||
|
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||||
|
|
||||||
|
|
||||||
|
def _load_imu_npz(path: Path) -> ImuSeries:
|
||||||
|
with np.load(path) as payload:
|
||||||
|
keys = set(payload.files)
|
||||||
|
if not {"t", "gyro", "acc"}.issubset(keys):
|
||||||
|
raise ValueError(f"IMU NPZ must contain t, gyro, acc; got {sorted(keys)}")
|
||||||
|
t = np.asarray(payload["t"], dtype=float).reshape(-1)
|
||||||
|
gyro = np.asarray(payload["gyro"], dtype=float).reshape(-1, 3)
|
||||||
|
acc = np.asarray(payload["acc"], dtype=float).reshape(-1, 3)
|
||||||
|
order = np.argsort(t)
|
||||||
|
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||||
|
|
||||||
|
|
||||||
|
def save_imu_csv(path: Path | str, imu: ImuSeries) -> None:
|
||||||
|
"""Write IMU samples to the standard CSV format."""
|
||||||
|
|
||||||
|
destination = Path(path)
|
||||||
|
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
array = np.column_stack([imu.t_s, imu.gyro_rad_s, imu.acc_m_s2])
|
||||||
|
header = "t,gx,gy,gz,ax,ay,az"
|
||||||
|
np.savetxt(destination, array, delimiter=",", header=header, comments="")
|
||||||
@@ -0,0 +1,452 @@
|
|||||||
|
"""Frame-to-frame IMU preintegration (Phase-A rotation + Phase-C full factor).
|
||||||
|
|
||||||
|
Phase-A: ``ΔR``, 3×3 ``Σ``, ``J_bg``.
|
||||||
|
Phase-C: ``ΔR/Δv/Δp``, 9×9 ``Σ`` (with bias RW process noise), ``J_bg``/``J_ba``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .geometry import orthonormalize_rotation, so3_exp, so3_log, skew
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class GyroPreintegration:
|
||||||
|
"""Rotation-only preintegration on ``[t0, t1]`` (IMU clock)."""
|
||||||
|
|
||||||
|
delta_R: np.ndarray
|
||||||
|
duration_s: float
|
||||||
|
mean_gyro_norm: float
|
||||||
|
sigma_rad: float
|
||||||
|
weight: float
|
||||||
|
bias_rad_s: np.ndarray
|
||||||
|
cov: np.ndarray
|
||||||
|
J_bg: np.ndarray
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuPreintegration:
|
||||||
|
"""Full IMU preintegration on ``[t0, t1]`` (IMU clock).
|
||||||
|
|
||||||
|
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``.
|
||||||
|
``delta_v`` / ``delta_p`` are body-frame increments (no gravity).
|
||||||
|
Error-state order in ``cov`` / Jacobians: ``[δθ, δv, δp]`` (9).
|
||||||
|
``J_bg`` / ``J_ba`` are 9×3: first-order correction w.r.t. constant bias deltas.
|
||||||
|
"""
|
||||||
|
|
||||||
|
delta_R: np.ndarray
|
||||||
|
delta_v: np.ndarray
|
||||||
|
delta_p: np.ndarray
|
||||||
|
duration_s: float
|
||||||
|
mean_gyro_norm: float
|
||||||
|
sigma_rad: float
|
||||||
|
weight: float
|
||||||
|
gyro_bias_rad_s: np.ndarray
|
||||||
|
acc_bias_m_s2: np.ndarray
|
||||||
|
cov: np.ndarray
|
||||||
|
J_bg: np.ndarray
|
||||||
|
J_ba: np.ndarray
|
||||||
|
|
||||||
|
|
||||||
|
def _right_jacobian(phi: np.ndarray) -> np.ndarray:
|
||||||
|
"""SO(3) right Jacobian ``Jr(φ)`` with ``Exp(φ+δ)≈Exp(φ)Exp(Jr δ)``."""
|
||||||
|
|
||||||
|
phi = np.asarray(phi, dtype=float).reshape(3)
|
||||||
|
angle = float(np.linalg.norm(phi))
|
||||||
|
if angle < 1e-8:
|
||||||
|
return np.eye(3) - 0.5 * skew(phi)
|
||||||
|
axis = phi / angle
|
||||||
|
s = skew(axis)
|
||||||
|
return (
|
||||||
|
np.eye(3)
|
||||||
|
- ((1.0 - np.cos(angle)) / angle) * s
|
||||||
|
+ ((angle - np.sin(angle)) / angle) * (s @ s)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _interp_vec(times_s: np.ndarray, values: np.ndarray, t: float) -> np.ndarray:
|
||||||
|
"""Linear interpolate a 3-vector series at an exact time."""
|
||||||
|
|
||||||
|
return np.array(
|
||||||
|
[float(np.interp(t, times_s, values[:, axis])) for axis in range(3)],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _interp_gyro(times_s: np.ndarray, gyro_rad_s: np.ndarray, t: float) -> np.ndarray:
|
||||||
|
"""Linear interpolate gyro at an exact time."""
|
||||||
|
|
||||||
|
return _interp_vec(times_s, gyro_rad_s, t)
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_weight(duration_s: float, mean_gyro_norm: float, cov_trace: float) -> float:
|
||||||
|
"""Larger weight for short, excited, low-covariance intervals."""
|
||||||
|
|
||||||
|
duration_term = 1.0 / max(duration_s, 0.05)
|
||||||
|
excite_term = min(max(mean_gyro_norm, 1e-3), 1.0)
|
||||||
|
avg_var = max(cov_trace / 3.0, 1e-8)
|
||||||
|
return float(duration_term * excite_term / avg_var)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegrate_gyro(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
bias_rad_s: np.ndarray | None = None,
|
||||||
|
*,
|
||||||
|
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||||
|
) -> GyroPreintegration:
|
||||||
|
"""Discrete mid-point gyro preintegration with exact endpoints.
|
||||||
|
|
||||||
|
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``
|
||||||
|
via right-invariant updates ``ΔR ← ΔR Exp((ω-b) dt)``.
|
||||||
|
|
||||||
|
Also returns:
|
||||||
|
- ``cov``: 3×3 covariance of the right tangent noise on ``ΔR``
|
||||||
|
- ``J_bg``: ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
|
||||||
|
empty = GyroPreintegration(
|
||||||
|
delta_R=np.eye(3),
|
||||||
|
duration_s=0.0,
|
||||||
|
mean_gyro_norm=0.0,
|
||||||
|
sigma_rad=1e3,
|
||||||
|
weight=1e-6,
|
||||||
|
bias_rad_s=bias.copy(),
|
||||||
|
cov=np.eye(3) * 1e6,
|
||||||
|
J_bg=np.zeros((3, 3)),
|
||||||
|
)
|
||||||
|
if times_s.size < 2 or duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||||
|
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
if duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
delta_r = np.eye(3)
|
||||||
|
j_bg = np.zeros((3, 3))
|
||||||
|
cov = np.zeros((3, 3))
|
||||||
|
sigma2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||||
|
gyro_norms: list[float] = []
|
||||||
|
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Exact endpoint gyro via linear interpolation inside the sample interval.
|
||||||
|
g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
|
||||||
|
g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
|
||||||
|
omega = 0.5 * (g_a + g_b) - bias
|
||||||
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|
||||||
|
theta = omega * dt
|
||||||
|
jr = _right_jacobian(theta)
|
||||||
|
a_mat = so3_exp(-theta)
|
||||||
|
j_bg = a_mat @ j_bg - jr * dt
|
||||||
|
cov = a_mat @ cov @ a_mat.T + jr @ (sigma2 * dt * np.eye(3)) @ jr.T
|
||||||
|
delta_r = delta_r @ so3_exp(theta)
|
||||||
|
|
||||||
|
delta_r = orthonormalize_rotation(delta_r)
|
||||||
|
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||||
|
cov = 0.5 * (cov + cov.T)
|
||||||
|
cov = cov + np.eye(3) * 1e-12
|
||||||
|
if mean_gyro_norm < 0.02:
|
||||||
|
cov = cov * 4.0
|
||||||
|
cov_trace = float(np.trace(cov))
|
||||||
|
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||||
|
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||||
|
return GyroPreintegration(
|
||||||
|
delta_R=delta_r,
|
||||||
|
duration_s=duration,
|
||||||
|
mean_gyro_norm=mean_gyro_norm,
|
||||||
|
sigma_rad=sigma_rad,
|
||||||
|
weight=weight,
|
||||||
|
bias_rad_s=bias.copy(),
|
||||||
|
cov=cov,
|
||||||
|
J_bg=np.asarray(j_bg, dtype=float),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegrate_imu(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
acc_m_s2: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
acc_bias_m_s2: np.ndarray | None = None,
|
||||||
|
*,
|
||||||
|
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||||
|
sigma_a_m_s2_sqrt_hz: float = 2.0e-2,
|
||||||
|
sigma_bg_rw_rad_s_sqrt_hz: float = 1.0e-5,
|
||||||
|
sigma_ba_rw_m_s2_sqrt_hz: float = 1.0e-3,
|
||||||
|
) -> ImuPreintegration:
|
||||||
|
"""Mid-point IMU preintegration with exact endpoints and bias-RW noise.
|
||||||
|
|
||||||
|
Discrete updates (right-invariant)::
|
||||||
|
|
||||||
|
ΔR ← ΔR Exp((ω-bg) dt)
|
||||||
|
Δv ← Δv + ΔR (a-ba) dt
|
||||||
|
Δp ← Δp + Δv_old dt + 0.5 ΔR (a-ba) dt²
|
||||||
|
|
||||||
|
Propagates a 15-DoF error state ``[δθ, δv, δp, δbg, δba]`` then returns the
|
||||||
|
top-left 9×9 covariance (bias RW already folded in) and 9×3 Jacobians.
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
acc_m_s2 = np.asarray(acc_m_s2, dtype=float).reshape(-1, 3)
|
||||||
|
bg = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
ba = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
empty = ImuPreintegration(
|
||||||
|
delta_R=np.eye(3),
|
||||||
|
delta_v=np.zeros(3),
|
||||||
|
delta_p=np.zeros(3),
|
||||||
|
duration_s=0.0,
|
||||||
|
mean_gyro_norm=0.0,
|
||||||
|
sigma_rad=1e3,
|
||||||
|
weight=1e-6,
|
||||||
|
gyro_bias_rad_s=bg.copy(),
|
||||||
|
acc_bias_m_s2=ba.copy(),
|
||||||
|
cov=np.eye(9) * 1e6,
|
||||||
|
J_bg=np.zeros((9, 3)),
|
||||||
|
J_ba=np.zeros((9, 3)),
|
||||||
|
)
|
||||||
|
if times_s.size < 2 or acc_m_s2.shape != gyro_rad_s.shape:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||||
|
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
if duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
delta_r = np.eye(3)
|
||||||
|
delta_v = np.zeros(3)
|
||||||
|
delta_p = np.zeros(3)
|
||||||
|
# Jacobians of [δθ, δv, δp] w.r.t. constant bias (accumulated analytically).
|
||||||
|
j_bg = np.zeros((9, 3))
|
||||||
|
j_ba = np.zeros((9, 3))
|
||||||
|
# 15×15 covariance: [θ, v, p, bg, ba]
|
||||||
|
cov15 = np.zeros((15, 15))
|
||||||
|
sg2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||||
|
sa2 = float(sigma_a_m_s2_sqrt_hz) ** 2
|
||||||
|
sbg2 = float(sigma_bg_rw_rad_s_sqrt_hz) ** 2
|
||||||
|
sba2 = float(sigma_ba_rw_m_s2_sqrt_hz) ** 2
|
||||||
|
gyro_norms: list[float] = []
|
||||||
|
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
g_a = _interp_vec(times_s, gyro_rad_s, seg0)
|
||||||
|
g_b = _interp_vec(times_s, gyro_rad_s, seg1)
|
||||||
|
a_a = _interp_vec(times_s, acc_m_s2, seg0)
|
||||||
|
a_b = _interp_vec(times_s, acc_m_s2, seg1)
|
||||||
|
omega = 0.5 * (g_a + g_b) - bg
|
||||||
|
acc = 0.5 * (a_a + a_b) - ba
|
||||||
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|
||||||
|
theta = omega * dt
|
||||||
|
jr = _right_jacobian(theta)
|
||||||
|
r_dt = so3_exp(theta)
|
||||||
|
r_mid = delta_r # rotate body accel into i0 frame before update
|
||||||
|
|
||||||
|
# Bias Jacobians (Forster-style first-order recursion).
|
||||||
|
j_r_bg = j_bg[0:3]
|
||||||
|
j_v_bg = j_bg[3:6]
|
||||||
|
j_p_bg = j_bg[6:9]
|
||||||
|
j_r_ba = j_ba[0:3]
|
||||||
|
j_v_ba = j_ba[3:6]
|
||||||
|
j_p_ba = j_ba[6:9]
|
||||||
|
|
||||||
|
acc_skew = skew(acc)
|
||||||
|
j_p_bg_new = j_p_bg + j_v_bg * dt - 0.5 * r_mid @ acc_skew @ j_r_bg * (dt**2)
|
||||||
|
j_v_bg_new = j_v_bg - r_mid @ acc_skew @ j_r_bg * dt
|
||||||
|
j_r_bg_new = r_dt.T @ j_r_bg - jr * dt
|
||||||
|
|
||||||
|
j_p_ba_new = j_p_ba + j_v_ba * dt - 0.5 * r_mid * (dt**2)
|
||||||
|
j_v_ba_new = j_v_ba - r_mid * dt
|
||||||
|
j_r_ba_new = r_dt.T @ j_r_ba
|
||||||
|
|
||||||
|
j_bg = np.vstack([j_r_bg_new, j_v_bg_new, j_p_bg_new])
|
||||||
|
j_ba = np.vstack([j_r_ba_new, j_v_ba_new, j_p_ba_new])
|
||||||
|
|
||||||
|
# Nominal state update (use pre-update Δv in position).
|
||||||
|
delta_p = delta_p + delta_v * dt + 0.5 * r_mid @ acc * (dt**2)
|
||||||
|
delta_v = delta_v + r_mid @ acc * dt
|
||||||
|
delta_r = orthonormalize_rotation(delta_r @ r_dt)
|
||||||
|
|
||||||
|
# Linearized error-state transition (15×15).
|
||||||
|
f = np.eye(15)
|
||||||
|
a_mat = so3_exp(-theta)
|
||||||
|
f[0:3, 0:3] = a_mat
|
||||||
|
f[0:3, 9:12] = -jr * dt
|
||||||
|
f[3:6, 0:3] = -r_mid @ acc_skew * dt
|
||||||
|
f[3:6, 12:15] = -r_mid * dt
|
||||||
|
f[6:9, 0:3] = -0.5 * r_mid @ acc_skew * (dt**2)
|
||||||
|
f[6:9, 3:6] = np.eye(3) * dt
|
||||||
|
f[6:9, 12:15] = -0.5 * r_mid * (dt**2)
|
||||||
|
|
||||||
|
# Noise: continuous densities σ²; Var(∫n dt)=σ² dt. Columns: n_g, n_a, n_bg, n_ba.
|
||||||
|
g_mat = np.zeros((15, 12))
|
||||||
|
g_mat[0:3, 0:3] = jr
|
||||||
|
g_mat[3:6, 3:6] = r_mid
|
||||||
|
g_mat[6:9, 3:6] = 0.5 * r_mid * dt
|
||||||
|
g_mat[9:12, 6:9] = np.eye(3)
|
||||||
|
g_mat[12:15, 9:12] = np.eye(3)
|
||||||
|
q = np.zeros((12, 12))
|
||||||
|
q[0:3, 0:3] = sg2 * dt * np.eye(3)
|
||||||
|
q[3:6, 3:6] = sa2 * dt * np.eye(3)
|
||||||
|
q[6:9, 6:9] = sbg2 * dt * np.eye(3)
|
||||||
|
q[9:12, 9:12] = sba2 * dt * np.eye(3)
|
||||||
|
cov15 = f @ cov15 @ f.T + g_mat @ q @ g_mat.T
|
||||||
|
|
||||||
|
delta_r = orthonormalize_rotation(delta_r)
|
||||||
|
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||||
|
cov9 = cov15[0:9, 0:9]
|
||||||
|
cov9 = 0.5 * (cov9 + cov9.T) + np.eye(9) * 1e-12
|
||||||
|
if mean_gyro_norm < 0.02:
|
||||||
|
cov9 = cov9.copy()
|
||||||
|
cov9[0:3, 0:3] = cov9[0:3, 0:3] * 4.0
|
||||||
|
cov_trace = float(np.trace(cov9[0:3, 0:3]))
|
||||||
|
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||||
|
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||||
|
return ImuPreintegration(
|
||||||
|
delta_R=delta_r,
|
||||||
|
delta_v=np.asarray(delta_v, dtype=float),
|
||||||
|
delta_p=np.asarray(delta_p, dtype=float),
|
||||||
|
duration_s=duration,
|
||||||
|
mean_gyro_norm=mean_gyro_norm,
|
||||||
|
sigma_rad=sigma_rad,
|
||||||
|
weight=weight,
|
||||||
|
gyro_bias_rad_s=bg.copy(),
|
||||||
|
acc_bias_m_s2=ba.copy(),
|
||||||
|
cov=np.asarray(cov9, dtype=float),
|
||||||
|
J_bg=np.asarray(j_bg, dtype=float),
|
||||||
|
J_ba=np.asarray(j_ba, dtype=float),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def apply_bias_correction_imu(
|
||||||
|
preint: ImuPreintegration,
|
||||||
|
delta_gyro_bias: np.ndarray | None = None,
|
||||||
|
delta_acc_bias: np.ndarray | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""First-order bias correction of ``ΔR/Δv/Δp``.
|
||||||
|
|
||||||
|
Returns ``(delta_R, delta_v, delta_p)``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
dbg = np.zeros(3) if delta_gyro_bias is None else np.asarray(delta_gyro_bias, dtype=float).reshape(3)
|
||||||
|
dba = np.zeros(3) if delta_acc_bias is None else np.asarray(delta_acc_bias, dtype=float).reshape(3)
|
||||||
|
j_bg = np.asarray(preint.J_bg, dtype=float).reshape(9, 3)
|
||||||
|
j_ba = np.asarray(preint.J_ba, dtype=float).reshape(9, 3)
|
||||||
|
delta_r = orthonormalize_rotation(preint.delta_R @ so3_exp(j_bg[0:3] @ dbg))
|
||||||
|
delta_v = preint.delta_v + j_bg[3:6] @ dbg + j_ba[3:6] @ dba
|
||||||
|
delta_p = preint.delta_p + j_bg[6:9] @ dbg + j_ba[6:9] @ dba
|
||||||
|
return delta_r, np.asarray(delta_v, dtype=float), np.asarray(delta_p, dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def relative_rotation_from_lidar(R_X: np.ndarray, R_B: np.ndarray) -> np.ndarray:
|
||||||
|
"""Map LiDAR relative rotation into IMU frame: ``R_X R_B R_X^T``."""
|
||||||
|
|
||||||
|
r_x = orthonormalize_rotation(R_X)
|
||||||
|
r_b = orthonormalize_rotation(R_B)
|
||||||
|
return orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegration_rotation_residual(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
R_X: np.ndarray,
|
||||||
|
R_B: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""``log( delta_R^T * R_X R_B R_X^T )`` in so(3)."""
|
||||||
|
|
||||||
|
predicted = relative_rotation_from_lidar(R_X, R_B)
|
||||||
|
return so3_log(delta_R.T @ predicted)
|
||||||
|
|
||||||
|
|
||||||
|
def apply_bias_jacobian_correction(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
J_bg: np.ndarray,
|
||||||
|
delta_bias_rad_s: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""First-order update ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``."""
|
||||||
|
|
||||||
|
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
j_bg = np.asarray(J_bg, dtype=float).reshape(3, 3)
|
||||||
|
return orthonormalize_rotation(delta_R @ so3_exp(j_bg @ db))
|
||||||
|
|
||||||
|
|
||||||
|
def apply_constant_bias_correction(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
duration_s: float,
|
||||||
|
delta_bias_rad_s: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Legacy first-order correction when ``J_bg`` is unavailable.
|
||||||
|
|
||||||
|
``ΔR(b+δb) ≈ ΔR Exp(-δb Δt)`` (identity Jacobian approximation).
|
||||||
|
"""
|
||||||
|
|
||||||
|
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
return orthonormalize_rotation(delta_R @ so3_exp(-db * float(duration_s)))
|
||||||
|
|
||||||
|
|
||||||
|
def residual_whiten_matrix(cov: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return ``W`` such that ``W @ e`` is approximately information-whitened.
|
||||||
|
|
||||||
|
Accepts square ``n×n`` covariances (3×3 rotation or 9×9 full IMU).
|
||||||
|
"""
|
||||||
|
|
||||||
|
matrix = np.asarray(cov, dtype=float)
|
||||||
|
if matrix.ndim != 2 or matrix.shape[0] != matrix.shape[1]:
|
||||||
|
raise ValueError("cov must be square")
|
||||||
|
n = matrix.shape[0]
|
||||||
|
matrix = 0.5 * (matrix + matrix.T) + np.eye(n) * 1e-10
|
||||||
|
try:
|
||||||
|
info = np.linalg.inv(matrix)
|
||||||
|
return np.linalg.cholesky(info).T
|
||||||
|
except np.linalg.LinAlgError:
|
||||||
|
scale = 1.0 / max(float(np.sqrt(np.trace(matrix) / n)), 1e-6)
|
||||||
|
return np.eye(n) * scale
|
||||||
@@ -0,0 +1,473 @@
|
|||||||
|
"""Joint extrinsic refinement: Phase-A rotation factors + Phase-C SE(3) IMU factors."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
G_NORM = 9.80665
|
||||||
|
|
||||||
|
|
||||||
|
@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
|
||||||
|
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 in the first-keyframe nav frame using LiDAR+extrinsic."""
|
||||||
|
|
||||||
|
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:
|
||||||
|
rotations[k + 1] = rotations[k]
|
||||||
|
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)
|
||||||
|
# Ensure list indexed by id_to_idx
|
||||||
|
del id_to_idx
|
||||||
|
return rotations
|
||||||
|
|
||||||
|
|
||||||
|
def _solve_phase_c_se3(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
gyro_bias0: np.ndarray,
|
||||||
|
gravity_init: np.ndarray,
|
||||||
|
sigma_bg_rw: float = 1.0e-5,
|
||||||
|
sigma_ba_rw: float = 1.0e-3,
|
||||||
|
) -> 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")
|
||||||
|
return r_x, np.zeros(3), gravity_init, gyro_bias0, np.zeros(3), 1e9, 1e9, notes
|
||||||
|
|
||||||
|
# Unique keyframes sorted by IMU time.
|
||||||
|
stamp: dict[int, float] = {}
|
||||||
|
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))
|
||||||
|
keyframe_ids = sorted(stamp.keys(), key=lambda kid: stamp[kid])
|
||||||
|
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 id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
|
||||||
|
consecutive_pairs[(pair.i, pair.j)] = pair
|
||||||
|
|
||||||
|
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_bias0, 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)
|
||||||
|
# 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] = bg0
|
||||||
|
|
||||||
|
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.
|
||||||
|
for k in range(k_count - 1):
|
||||||
|
dt = max(stamp[keyframe_ids[k + 1]] - stamp[keyframe_ids[k]], 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 biases and translation magnitude.
|
||||||
|
out.append(50.0 * (bgs[0] - bg0))
|
||||||
|
out.append(20.0 * bas[0])
|
||||||
|
out.append(0.2 * t_opt) # soft |t| prior ~ meters
|
||||||
|
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 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,
|
||||||
|
gravity_init_m_s2: np.ndarray | None = None,
|
||||||
|
bias_prior_sigma_rad_s: float = 0.02,
|
||||||
|
enable_phase_c: bool | 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)
|
||||||
|
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, weight, whiten in zip(usable, weights, 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(np.sqrt(weight) * (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=np.deg2rad(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 (Σ-whitened + J_bg): "
|
||||||
|
f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
|
||||||
|
f"weighted 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)
|
||||||
|
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 (
|
||||||
|
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_bias0=bias_out,
|
||||||
|
gravity_init=gravity_init,
|
||||||
|
)
|
||||||
|
notes.extend(c_notes)
|
||||||
|
translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
|
||||||
|
if not translation_accepted:
|
||||||
|
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))
|
||||||
|
return np.concatenate(residuals)
|
||||||
|
|
||||||
|
opt_t = least_squares(residual_se3, np.zeros(6), 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)
|
||||||
|
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=0.0 if not translation_accepted else 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),
|
||||||
|
)
|
||||||
@@ -0,0 +1,49 @@
|
|||||||
|
"""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,
|
||||||
|
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:
|
||||||
|
continue
|
||||||
|
if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
|
||||||
|
selected.append(index)
|
||||||
|
last = index
|
||||||
|
|
||||||
|
if selected[-1] != len(frames) - 1 and len(frames) > 1:
|
||||||
|
selected.append(len(frames) - 1)
|
||||||
|
|
||||||
|
unique = tuple(dict.fromkeys(selected))
|
||||||
|
return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))
|
||||||
@@ -0,0 +1,59 @@
|
|||||||
|
"""Coarse LiDAR deskew using a constant body rate over the sweep."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
from .geometry import so3_exp
|
||||||
|
from .time_offset import lidar_time_to_imu_time
|
||||||
|
|
||||||
|
|
||||||
|
def deskew_lidar_frames(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
imu: ImuSeries,
|
||||||
|
*,
|
||||||
|
delta_t_s: float,
|
||||||
|
R_IMU_lidar: np.ndarray | None = None,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
) -> list[LidarFrame]:
|
||||||
|
"""Return deskewed copies when extrinsic is known; otherwise return originals."""
|
||||||
|
|
||||||
|
if R_IMU_lidar is None:
|
||||||
|
return frames
|
||||||
|
|
||||||
|
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
r_li = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3).T
|
||||||
|
output: list[LidarFrame] = []
|
||||||
|
|
||||||
|
for frame in frames:
|
||||||
|
n = frame.points_xyz.shape[0]
|
||||||
|
if n < 10:
|
||||||
|
output.append(frame)
|
||||||
|
continue
|
||||||
|
t_mid_imu = lidar_time_to_imu_time(frame.t_mid_s, delta_t_s)
|
||||||
|
index = int(np.clip(np.searchsorted(imu.t_s, t_mid_imu), 1, imu.t_s.size - 1))
|
||||||
|
omega_lidar = r_li @ (imu.gyro_rad_s[index] - bias)
|
||||||
|
duration = max(frame.t_end_s - frame.t_start_s, 1e-3)
|
||||||
|
rel = np.linspace(-0.5, 0.5, n) * duration
|
||||||
|
deskewed = np.empty_like(frame.points_xyz)
|
||||||
|
# Piecewise-constant rotation over a few time bins.
|
||||||
|
bins = 12
|
||||||
|
edges = np.linspace(-0.5 * duration, 0.5 * duration, bins + 1)
|
||||||
|
for b in range(bins):
|
||||||
|
mask = (rel >= edges[b]) & (rel <= edges[b + 1] if b == bins - 1 else rel < edges[b + 1])
|
||||||
|
if not np.any(mask):
|
||||||
|
continue
|
||||||
|
tau = 0.5 * (edges[b] + edges[b + 1])
|
||||||
|
rot = so3_exp(omega_lidar * float(tau))
|
||||||
|
deskewed[mask] = frame.points_xyz[mask] @ rot.T
|
||||||
|
output.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=frame.frame_id,
|
||||||
|
t_start_s=frame.t_start_s,
|
||||||
|
t_end_s=frame.t_end_s,
|
||||||
|
points_xyz=deskewed,
|
||||||
|
path=frame.path,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return output
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
"""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 load_lidar_frames(path: Path | str) -> list[LidarFrame]:
|
||||||
|
"""Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
|
||||||
|
|
||||||
|
root = Path(path)
|
||||||
|
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)}"
|
||||||
|
)
|
||||||
|
|
||||||
|
frames: list[LidarFrame] = []
|
||||||
|
for row in rows:
|
||||||
|
frame_id = str(row["frame_id"])
|
||||||
|
rel = str(row[file_key])
|
||||||
|
npz_path = root / rel
|
||||||
|
with np.load(npz_path) as payload:
|
||||||
|
if "points" not in payload.files:
|
||||||
|
raise ValueError(f"{npz_path} must contain array 'points'")
|
||||||
|
points = np.asarray(payload["points"], dtype=float)
|
||||||
|
if points.ndim != 2 or points.shape[1] < 3:
|
||||||
|
raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
|
||||||
|
frames.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=frame_id,
|
||||||
|
t_start_s=float(row["t_start"]),
|
||||||
|
t_end_s=float(row["t_end"]),
|
||||||
|
points_xyz=points[:, :3],
|
||||||
|
path=npz_path,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
frames.sort(key=lambda frame: frame.t_mid_s)
|
||||||
|
return frames
|
||||||
|
|
||||||
|
|
||||||
|
def save_lidar_session(
|
||||||
|
root: Path | str,
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
points_dirname: str = "frames",
|
||||||
|
) -> None:
|
||||||
|
"""Write a LiDAR session directory in the standard intermediate format."""
|
||||||
|
|
||||||
|
destination = Path(root)
|
||||||
|
frames_dir = destination / points_dirname
|
||||||
|
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||||
|
index_rows: list[str] = ["frame_id,filename,t_start,t_end"]
|
||||||
|
for index, frame in enumerate(frames):
|
||||||
|
relative = f"{points_dirname}/frame_{index:05d}.npz"
|
||||||
|
np.savez_compressed(destination / relative, points=np.asarray(frame.points_xyz, dtype=float))
|
||||||
|
index_rows.append(f"{frame.frame_id},{relative},{frame.t_start_s:.9f},{frame.t_end_s:.9f}")
|
||||||
|
(destination / "frames_index.csv").write_text("\n".join(index_rows) + "\n", encoding="utf-8")
|
||||||
@@ -0,0 +1,136 @@
|
|||||||
|
"""Build IMU/LiDAR relative-motion pairs for hand-eye calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
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,
|
||||||
|
max_index_span: int = 4,
|
||||||
|
) -> 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] = []
|
||||||
|
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",))
|
||||||
|
|
||||||
|
for span in range(1, max_index_span + 1):
|
||||||
|
for start in range(0, n - span):
|
||||||
|
i = start
|
||||||
|
j = start + span
|
||||||
|
frame_i = keyframes[i]
|
||||||
|
frame_j = keyframes[j]
|
||||||
|
reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
|
||||||
|
if not reg.ok:
|
||||||
|
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
|
||||||
|
|
||||||
|
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,
|
||||||
|
"modeling": "imu_preintegration_factor_phase_c",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)"
|
||||||
|
)
|
||||||
|
return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes))
|
||||||
|
|
||||||
|
|
||||||
|
def pairs_to_transforms(pairs: tuple[MotionPair, ...]) -> tuple[list[np.ndarray], list[np.ndarray]]:
|
||||||
|
"""Helper returning SE(3) lists when translations are present."""
|
||||||
|
|
||||||
|
a_list: list[np.ndarray] = []
|
||||||
|
b_list: list[np.ndarray] = []
|
||||||
|
for pair in pairs:
|
||||||
|
if pair.t_B_m is None:
|
||||||
|
continue
|
||||||
|
t_a = np.zeros(3) if pair.t_A_m is None else pair.t_A_m
|
||||||
|
a_list.append(make_transform(t_a, pair.R_A))
|
||||||
|
b_list.append(make_transform(pair.t_B_m, pair.R_B))
|
||||||
|
return a_list, b_list
|
||||||
@@ -0,0 +1,104 @@
|
|||||||
|
"""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",))
|
||||||
|
|
||||||
|
# Normalize columns.
|
||||||
|
col_norm = np.linalg.norm(j_r, axis=0) + 1e-12
|
||||||
|
j_r_n = j_r / col_norm
|
||||||
|
singular = np.linalg.svd(j_r_n, compute_uv=False)
|
||||||
|
cond_r = float(singular[0] / max(singular[-1], 1e-12))
|
||||||
|
rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3
|
||||||
|
|
||||||
|
# Translation observability proxy: diversity of rotation axes and presence of translation in B.
|
||||||
|
axes = []
|
||||||
|
translations = []
|
||||||
|
for pair in usable:
|
||||||
|
axis = so3_log(pair.R_B)
|
||||||
|
n = np.linalg.norm(axis)
|
||||||
|
if n > 1e-8:
|
||||||
|
axes.append(axis / n)
|
||||||
|
if pair.t_B_m is not None:
|
||||||
|
translations.append(pair.t_B_m)
|
||||||
|
axis_rank = 0
|
||||||
|
if axes:
|
||||||
|
axis_mat = np.asarray(axes, dtype=float)
|
||||||
|
axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
|
||||||
|
trans_span = 0.0
|
||||||
|
if translations:
|
||||||
|
tmat = np.asarray(translations, dtype=float)
|
||||||
|
trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
|
||||||
|
# For planar yaw-mostly motion, translation z is typically weak.
|
||||||
|
translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
|
||||||
|
cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))
|
||||||
|
|
||||||
|
if not rotation_ok:
|
||||||
|
notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
|
||||||
|
else:
|
||||||
|
notes.append(f"rotation condition {cond_r:.1f}")
|
||||||
|
if not translation_ok:
|
||||||
|
notes.append(
|
||||||
|
f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); "
|
||||||
|
"V1 will reject full SE3 without strong priors"
|
||||||
|
)
|
||||||
|
return ObservabilityReport(
|
||||||
|
rotation_observable=rotation_ok,
|
||||||
|
translation_observable=translation_ok,
|
||||||
|
condition_rotation=cond_r,
|
||||||
|
condition_translation=cond_t,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,347 @@
|
|||||||
|
"""Executable LiDAR–IMU calibration pipeline (V1)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import asdict, dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import (
|
||||||
|
CalibrationMode,
|
||||||
|
CalibrationRequest,
|
||||||
|
CalibrationResult,
|
||||||
|
CalibrationStatus,
|
||||||
|
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 .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
|
||||||
|
|
||||||
|
|
||||||
|
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，并用 R 做有符号三轴精修"),
|
||||||
|
PipelineStage("lidar_motion", "关键帧、可选去畸变与 LiDAR 相对运动"),
|
||||||
|
PipelineStage("motion_pairs", "IMU 预积分与雷达配准，构造相对运动对"),
|
||||||
|
PipelineStage("rotation_handeye", "加权求解旋转外参"),
|
||||||
|
PipelineStage("joint_optimizer", "联合精修；完整模式下可估计平移"),
|
||||||
|
PipelineStage("finalize", "写出结果与质量报告"),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
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,
|
||||||
|
):
|
||||||
|
keyframes = build_keyframes(
|
||||||
|
working_frames,
|
||||||
|
min_translation_m=request.min_pair_translation_m,
|
||||||
|
min_rotation_deg=request.min_pair_rotation_deg,
|
||||||
|
)
|
||||||
|
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,
|
||||||
|
)
|
||||||
|
handeye = solve_rotation_handeye(pair_set.pairs)
|
||||||
|
return keyframes, pair_set, handeye
|
||||||
|
|
||||||
|
|
||||||
|
def _session_details(
|
||||||
|
session: SessionInput,
|
||||||
|
request: CalibrationRequest,
|
||||||
|
vehicle_config: dict[str, Any] | None,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
imu = load_imu_samples(session.imu_source)
|
||||||
|
frames = load_lidar_frames(session.lidar_source)
|
||||||
|
|
||||||
|
ts = audit_timestamps(imu, frames)
|
||||||
|
if not ts.ok:
|
||||||
|
return {"ok": False, "stage": "timestamp_audit", "report": asdict(ts)}
|
||||||
|
|
||||||
|
imu_report = audit_imu(imu)
|
||||||
|
if not imu_report.ok:
|
||||||
|
return {"ok": False, "stage": "imu_audit", "report": asdict(imu_report)}
|
||||||
|
|
||||||
|
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:
|
||||||
|
return {"ok": False, "stage": "time_offset", "report": asdict(offset)}
|
||||||
|
|
||||||
|
working_frames = frames
|
||||||
|
r_x = np.eye(3)
|
||||||
|
handeye = None
|
||||||
|
pair_set = None
|
||||||
|
keyframes = None
|
||||||
|
pairs_notes: list[str] = []
|
||||||
|
pair_count = 0
|
||||||
|
time_offset_notes = list(offset.notes)
|
||||||
|
|
||||||
|
for iteration in range(max(1, request.max_iterations)):
|
||||||
|
if iteration > 0:
|
||||||
|
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,
|
||||||
|
)
|
||||||
|
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,
|
||||||
|
)
|
||||||
|
pairs_notes = list(pair_set.notes)
|
||||||
|
pair_count = len(pair_set.pairs)
|
||||||
|
if handeye.pair_count < 3:
|
||||||
|
return {
|
||||||
|
"ok": False,
|
||||||
|
"stage": "rotation_handeye",
|
||||||
|
"iteration": iteration,
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"imu_audit": asdict(imu_report),
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"keyframes": len(keyframes.indices),
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": asdict(handeye),
|
||||||
|
}
|
||||||
|
# Use candidate R even if RMS gate failed, so signed δt refine can still run.
|
||||||
|
r_x = handeye.R_IMU_lidar
|
||||||
|
|
||||||
|
# Phase-A: alternate signed δt refine with current R (up to 2 rounds).
|
||||||
|
for _ in range(2):
|
||||||
|
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)),
|
||||||
|
)
|
||||||
|
delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
|
||||||
|
offset = _merge_time_offset(offset, refined)
|
||||||
|
time_offset_notes = list(offset.notes)
|
||||||
|
if delta_shift < 1e-3:
|
||||||
|
break
|
||||||
|
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,
|
||||||
|
)
|
||||||
|
pairs_notes = list(pair_set.notes)
|
||||||
|
pair_count = len(pair_set.pairs)
|
||||||
|
if handeye.pair_count < 3:
|
||||||
|
return {
|
||||||
|
"ok": False,
|
||||||
|
"stage": "rotation_handeye",
|
||||||
|
"iteration": iteration,
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"imu_audit": asdict(imu_report),
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"keyframes": len(keyframes.indices),
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": asdict(handeye),
|
||||||
|
}
|
||||||
|
r_x = handeye.R_IMU_lidar
|
||||||
|
|
||||||
|
if not handeye.ok:
|
||||||
|
return {
|
||||||
|
"ok": False,
|
||||||
|
"stage": "rotation_handeye",
|
||||||
|
"iteration": iteration,
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"imu_audit": asdict(imu_report),
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"keyframes": len(keyframes.indices),
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": asdict(handeye),
|
||||||
|
}
|
||||||
|
|
||||||
|
assert handeye is not None and pair_set is not None and keyframes is not None
|
||||||
|
force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
|
||||||
|
# Specific force opposing measured specific force ≈ −g in the static IMU frame.
|
||||||
|
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])
|
||||||
|
joint = solve_joint_extrinsic(
|
||||||
|
pair_set.pairs,
|
||||||
|
r_x,
|
||||||
|
force_rotation_only=force_rotation_only,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
gravity_init_m_s2=gravity_init,
|
||||||
|
enable_phase_c=not force_rotation_only,
|
||||||
|
)
|
||||||
|
|
||||||
|
offset_payload = asdict(offset)
|
||||||
|
|
||||||
|
return {
|
||||||
|
"ok": True,
|
||||||
|
"session_id": session.session_id,
|
||||||
|
"vehicle_config_loaded": vehicle_config is not None,
|
||||||
|
"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": offset_payload,
|
||||||
|
"keyframes": len(keyframes.indices),
|
||||||
|
"pair_count": pair_count,
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": {
|
||||||
|
"residual_rms_deg": handeye.residual_rms_deg,
|
||||||
|
"residual_median_deg": handeye.residual_median_deg,
|
||||||
|
"pair_count": handeye.pair_count,
|
||||||
|
"ok": handeye.ok,
|
||||||
|
"notes": handeye.notes,
|
||||||
|
"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(),
|
||||||
|
"gyro_bias_rad_s": None
|
||||||
|
if joint.gyro_bias_rad_s is None
|
||||||
|
else np.asarray(joint.gyro_bias_rad_s, 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(),
|
||||||
|
},
|
||||||
|
"T_IMU_lidar": joint.T_IMU_lidar,
|
||||||
|
"time_offset_s": offset.delta_t_s,
|
||||||
|
"translation_accepted": joint.translation_accepted,
|
||||||
|
"rotation_ok": handeye.ok and joint.observability.rotation_observable,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def run_calibration(request: CalibrationRequest) -> CalibrationResult:
|
||||||
|
"""Run the V1 calibration pipeline for one or more sessions."""
|
||||||
|
|
||||||
|
if not request.sessions:
|
||||||
|
return finalize_result(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message="no sessions provided",
|
||||||
|
details={},
|
||||||
|
output_directory=request.output_directory,
|
||||||
|
)
|
||||||
|
|
||||||
|
vehicle_config = None
|
||||||
|
if request.vehicle_config is not None:
|
||||||
|
try:
|
||||||
|
vehicle_config = load_vehicle_config(request.vehicle_config)
|
||||||
|
except Exception as exc: # noqa: BLE001 - surface config problems as blocked
|
||||||
|
return finalize_result(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=f"vehicle config failed: {exc}",
|
||||||
|
details={},
|
||||||
|
output_directory=request.output_directory,
|
||||||
|
)
|
||||||
|
|
||||||
|
session_results = []
|
||||||
|
for session in request.sessions:
|
||||||
|
session_results.append(_session_details(session, request, vehicle_config))
|
||||||
|
|
||||||
|
primary = session_results[0]
|
||||||
|
if not primary.get("ok"):
|
||||||
|
return finalize_result(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=f"blocked at stage {primary.get('stage')}",
|
||||||
|
details={"sessions": session_results},
|
||||||
|
output_directory=request.output_directory,
|
||||||
|
)
|
||||||
|
|
||||||
|
T = np.asarray(primary["T_IMU_lidar"], dtype=float)
|
||||||
|
delta_t = float(primary["time_offset_s"])
|
||||||
|
if request.requested_mode == CalibrationMode.FULL_SE3:
|
||||||
|
if primary.get("translation_accepted"):
|
||||||
|
status = CalibrationStatus.FULL_SE3_ACCEPTED
|
||||||
|
message = "full SE3 accepted"
|
||||||
|
else:
|
||||||
|
status = CalibrationStatus.FULL_SE3_REJECTED
|
||||||
|
message = "rotation accepted; translation rejected by observability/residual gates"
|
||||||
|
else:
|
||||||
|
status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
|
||||||
|
message = "rotation-only calibration accepted"
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
|
||||||
|
return finalize_result(
|
||||||
|
status=status,
|
||||||
|
message=message,
|
||||||
|
details={"sessions": [_public_session(s) for s in session_results]},
|
||||||
|
T_IMU_lidar=T,
|
||||||
|
time_offset_s=delta_t,
|
||||||
|
output_directory=request.output_directory,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
|
||||||
|
payload = dict(session_result)
|
||||||
|
payload.pop("T_IMU_lidar", None)
|
||||||
|
return payload
|
||||||
@@ -0,0 +1,159 @@
|
|||||||
|
"""LiDAR relative-motion registration.
|
||||||
|
|
||||||
|
Uses Open3D Generalized ICP when available; otherwise a NumPy point-to-point ICP.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import LidarFrame
|
||||||
|
from .geometry import make_transform, orthonormalize_rotation, rotation_angle_deg, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RegistrationResult:
|
||||||
|
transform: np.ndarray
|
||||||
|
fitness: float
|
||||||
|
rotation_deg: float
|
||||||
|
translation_m: float
|
||||||
|
backend: str
|
||||||
|
ok: bool
|
||||||
|
|
||||||
|
|
||||||
|
def _voxel_downsample(points: np.ndarray, voxel: float) -> np.ndarray:
|
||||||
|
if points.shape[0] == 0:
|
||||||
|
return points
|
||||||
|
quantized = np.floor(points / voxel).astype(np.int64)
|
||||||
|
_, unique_indices = np.unique(quantized, axis=0, return_index=True)
|
||||||
|
return points[np.sort(unique_indices)]
|
||||||
|
|
||||||
|
|
||||||
|
def _numpy_icp(
|
||||||
|
source: np.ndarray,
|
||||||
|
target: np.ndarray,
|
||||||
|
*,
|
||||||
|
max_iterations: int = 30,
|
||||||
|
max_correspondence: float = 1.0,
|
||||||
|
) -> RegistrationResult:
|
||||||
|
src = _voxel_downsample(source, 0.2)
|
||||||
|
tgt = _voxel_downsample(target, 0.2)
|
||||||
|
if src.shape[0] < 50 or tgt.shape[0] < 50:
|
||||||
|
return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "numpy_icp", False)
|
||||||
|
|
||||||
|
# Subsample for speed.
|
||||||
|
rng = np.random.default_rng(0)
|
||||||
|
if src.shape[0] > 4000:
|
||||||
|
src = src[rng.choice(src.shape[0], 4000, replace=False)]
|
||||||
|
if tgt.shape[0] > 8000:
|
||||||
|
tgt = tgt[rng.choice(tgt.shape[0], 8000, replace=False)]
|
||||||
|
|
||||||
|
r = np.eye(3)
|
||||||
|
t = np.zeros(3)
|
||||||
|
last_error = 1e9
|
||||||
|
inlier_ratio = 0.0
|
||||||
|
for _ in range(max_iterations):
|
||||||
|
transformed = src @ r.T + t
|
||||||
|
# Nearest neighbour in target via brute force on chunks.
|
||||||
|
diff = transformed[:, None, :] - tgt[None, :, :]
|
||||||
|
dist2 = np.sum(diff * diff, axis=2)
|
||||||
|
nn = np.argmin(dist2, axis=1)
|
||||||
|
dist = np.sqrt(dist2[np.arange(src.shape[0]), nn])
|
||||||
|
mask = dist < max_correspondence
|
||||||
|
inlier_ratio = float(np.mean(mask))
|
||||||
|
if np.count_nonzero(mask) < 30:
|
||||||
|
break
|
||||||
|
p = transformed[mask]
|
||||||
|
q = tgt[nn[mask]]
|
||||||
|
mu_p = p.mean(axis=0)
|
||||||
|
mu_q = q.mean(axis=0)
|
||||||
|
h = (p - mu_p).T @ (q - mu_q)
|
||||||
|
u, _, vt = np.linalg.svd(h)
|
||||||
|
r_delta = vt.T @ u.T
|
||||||
|
if np.linalg.det(r_delta) < 0:
|
||||||
|
vt[-1, :] *= -1
|
||||||
|
r_delta = vt.T @ u.T
|
||||||
|
t_delta = mu_q - r_delta @ mu_p
|
||||||
|
# Update global transform: x' = r_delta (r x + t) + t_delta
|
||||||
|
r = orthonormalize_rotation(r_delta @ r)
|
||||||
|
t = r_delta @ t + t_delta
|
||||||
|
mean_err = float(np.mean(dist[mask]))
|
||||||
|
if abs(last_error - mean_err) < 1e-4:
|
||||||
|
break
|
||||||
|
last_error = mean_err
|
||||||
|
|
||||||
|
transform = make_transform(t, r)
|
||||||
|
return RegistrationResult(
|
||||||
|
transform=transform,
|
||||||
|
fitness=inlier_ratio,
|
||||||
|
rotation_deg=rotation_angle_deg(r),
|
||||||
|
translation_m=float(np.linalg.norm(t)),
|
||||||
|
backend="numpy_icp",
|
||||||
|
ok=inlier_ratio > 0.15,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _open3d_gicp(source: np.ndarray, target: np.ndarray) -> RegistrationResult | None:
|
||||||
|
try:
|
||||||
|
import open3d as o3d
|
||||||
|
except ImportError:
|
||||||
|
return None
|
||||||
|
|
||||||
|
src = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(source))
|
||||||
|
tgt = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(target))
|
||||||
|
src = src.voxel_down_sample(0.2)
|
||||||
|
tgt = tgt.voxel_down_sample(0.2)
|
||||||
|
if len(src.points) < 50 or len(tgt.points) < 50:
|
||||||
|
return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "open3d_gicp", False)
|
||||||
|
src.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
|
||||||
|
tgt.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
|
||||||
|
result = o3d.pipelines.registration.registration_generalized_icp(
|
||||||
|
src,
|
||||||
|
tgt,
|
||||||
|
1.0,
|
||||||
|
np.eye(4),
|
||||||
|
o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),
|
||||||
|
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50),
|
||||||
|
)
|
||||||
|
transform = np.asarray(result.transformation, dtype=float)
|
||||||
|
return RegistrationResult(
|
||||||
|
transform=transform,
|
||||||
|
fitness=float(result.fitness),
|
||||||
|
rotation_deg=rotation_angle_deg(transform[:3, :3]),
|
||||||
|
translation_m=float(np.linalg.norm(transform[:3, 3])),
|
||||||
|
backend="open3d_gicp",
|
||||||
|
ok=float(result.fitness) > 0.15,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def register_lidar_pair(source_points: np.ndarray, target_points: np.ndarray) -> RegistrationResult:
|
||||||
|
"""Register source -> target and return ``T_target_source``."""
|
||||||
|
|
||||||
|
source = np.asarray(source_points, dtype=float).reshape(-1, 3)
|
||||||
|
target = np.asarray(target_points, dtype=float).reshape(-1, 3)
|
||||||
|
open3d_result = _open3d_gicp(source, target)
|
||||||
|
if open3d_result is not None:
|
||||||
|
return open3d_result
|
||||||
|
return _numpy_icp(source, target)
|
||||||
|
|
||||||
|
|
||||||
|
def estimate_frame_rotations(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
stride: int = 1,
|
||||||
|
) -> tuple[list[np.ndarray], list[tuple[float, float]]]:
|
||||||
|
"""Estimate consecutive (or strided) LiDAR relative rotations for time sync."""
|
||||||
|
|
||||||
|
rotations: list[np.ndarray] = []
|
||||||
|
pair_times: list[tuple[float, float]] = []
|
||||||
|
for index in range(0, len(frames) - stride, max(stride, 1)):
|
||||||
|
a = frames[index]
|
||||||
|
b = frames[index + stride]
|
||||||
|
result = register_lidar_pair(b.points_xyz, a.points_xyz)
|
||||||
|
if not result.ok:
|
||||||
|
continue
|
||||||
|
rotations.append(result.transform[:3, :3])
|
||||||
|
pair_times.append((a.t_mid_s, b.t_mid_s))
|
||||||
|
return rotations, pair_times
|
||||||
@@ -0,0 +1,113 @@
|
|||||||
|
"""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
|
||||||
|
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]) -> np.ndarray:
|
||||||
|
"""Closed-form rotation hand-eye initial guess (Tsai-style linear solve)."""
|
||||||
|
|
||||||
|
rows: list[np.ndarray] = []
|
||||||
|
rhs: list[np.ndarray] = []
|
||||||
|
for pair in pairs:
|
||||||
|
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(_pair_weight(pair))
|
||||||
|
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 solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> RotationHandeyeResult:
|
||||||
|
"""Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement."""
|
||||||
|
|
||||||
|
usable = [pair for pair in pairs if rotation_angle_deg(pair.R_A) > 1.0 and rotation_angle_deg(pair.R_B) > 1.0]
|
||||||
|
notes: list[str] = []
|
||||||
|
if len(usable) < 3:
|
||||||
|
return RotationHandeyeResult(
|
||||||
|
R_IMU_lidar=np.eye(3),
|
||||||
|
residual_rms_deg=1e9,
|
||||||
|
residual_median_deg=1e9,
|
||||||
|
pair_count=len(usable),
|
||||||
|
ok=False,
|
||||||
|
notes=("need at least 3 motion pairs with meaningful rotation",),
|
||||||
|
)
|
||||||
|
|
||||||
|
r0 = _tsai_rotation_initial(usable)
|
||||||
|
weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
|
||||||
|
notes.append(
|
||||||
|
f"weighted hand-eye: weight median={float(np.median(weights)):.3g}, "
|
||||||
|
f"min={float(np.min(weights)):.3g}, 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))
|
||||||
|
|
||||||
|
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)
|
||||||
|
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))
|
||||||
|
notes.append(f"optimized over {len(usable)} pairs")
|
||||||
|
ok = rms < 5.0 and len(usable) >= 3
|
||||||
|
if not ok:
|
||||||
|
notes.append("rotation residual RMS too high or too few pairs")
|
||||||
|
return RotationHandeyeResult(
|
||||||
|
R_IMU_lidar=r_x,
|
||||||
|
residual_rms_deg=rms,
|
||||||
|
residual_median_deg=med,
|
||||||
|
pair_count=len(usable),
|
||||||
|
ok=ok,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,303 @@
|
|||||||
|
"""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:
|
||||||
|
best_delta = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
|
||||||
|
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
|
||||||
|
|
||||||
|
stride = max(1, len(frames) // 20)
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
|
||||||
|
if len(rotations) < 4:
|
||||||
|
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")
|
||||||
|
ok = peak > 0.15
|
||||||
|
if 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,
|
||||||
|
) -> 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"
|
||||||
|
)
|
||||||
|
improved = (
|
||||||
|
np.isfinite(best_cost)
|
||||||
|
and best_cost < coarse_cost * 0.999
|
||||||
|
# Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
|
||||||
|
and mag_at_best + 1e-4 >= mag_at_coarse
|
||||||
|
)
|
||||||
|
if not improved:
|
||||||
|
notes.append("signed refine rejected by MSE/mag-consistency; keeping previous delta_t")
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=float(delta_t_s),
|
||||||
|
correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=best_delta,
|
||||||
|
correlation_peak=mag_at_best,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
"""Timestamp audit for IMU and LiDAR streams."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class TimestampAuditReport:
|
||||||
|
monotonic: bool
|
||||||
|
epoch_count: int
|
||||||
|
imu_rate_hz: float
|
||||||
|
lidar_rate_hz: float
|
||||||
|
imu_duration_s: float
|
||||||
|
lidar_duration_s: float
|
||||||
|
max_imu_gap_s: float
|
||||||
|
max_lidar_gap_s: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
ok: bool = True
|
||||||
|
|
||||||
|
|
||||||
|
def _rate_and_gaps(times: np.ndarray) -> tuple[float, float]:
|
||||||
|
if times.size < 2:
|
||||||
|
return 0.0, 0.0
|
||||||
|
dt = np.diff(times)
|
||||||
|
positive = dt[dt > 0]
|
||||||
|
if positive.size == 0:
|
||||||
|
return 0.0, float("inf")
|
||||||
|
rate = float(1.0 / np.median(positive))
|
||||||
|
return rate, float(np.max(dt))
|
||||||
|
|
||||||
|
|
||||||
|
def audit_timestamps(imu: ImuSeries, frames: list[LidarFrame]) -> TimestampAuditReport:
|
||||||
|
"""Audit native timestamps without assuming the two clocks share an epoch."""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
imu_t = imu.t_s
|
||||||
|
lidar_t = np.asarray([frame.t_mid_s for frame in frames], dtype=float)
|
||||||
|
|
||||||
|
imu_mono = bool(np.all(np.diff(imu_t) >= 0)) if imu_t.size > 1 else False
|
||||||
|
lidar_mono = bool(np.all(np.diff(lidar_t) >= 0)) if lidar_t.size > 1 else False
|
||||||
|
if not imu_mono:
|
||||||
|
notes.append("IMU timestamps are not monotonic")
|
||||||
|
if not lidar_mono:
|
||||||
|
notes.append("LiDAR timestamps are not monotonic")
|
||||||
|
|
||||||
|
imu_rate, imu_gap = _rate_and_gaps(imu_t)
|
||||||
|
lidar_rate, lidar_gap = _rate_and_gaps(lidar_t)
|
||||||
|
if imu_t.size < 50:
|
||||||
|
notes.append(f"IMU sample count is low ({imu_t.size})")
|
||||||
|
if len(frames) < 5:
|
||||||
|
notes.append(f"LiDAR frame count is low ({len(frames)})")
|
||||||
|
if imu_gap > 0.05:
|
||||||
|
notes.append(f"large IMU gap detected: {imu_gap:.3f}s")
|
||||||
|
if lidar_gap > 1.0:
|
||||||
|
notes.append(f"large LiDAR gap detected: {lidar_gap:.3f}s")
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
"IMU and LiDAR clocks are treated as independent; constant offset is estimated later."
|
||||||
|
)
|
||||||
|
|
||||||
|
ok = imu_mono and lidar_mono and imu_t.size >= 50 and len(frames) >= 5
|
||||||
|
return TimestampAuditReport(
|
||||||
|
monotonic=imu_mono and lidar_mono,
|
||||||
|
epoch_count=2,
|
||||||
|
imu_rate_hz=imu_rate,
|
||||||
|
lidar_rate_hz=lidar_rate,
|
||||||
|
imu_duration_s=float(imu_t[-1] - imu_t[0]) if imu_t.size else 0.0,
|
||||||
|
lidar_duration_s=float(lidar_t[-1] - lidar_t[0]) if lidar_t.size else 0.0,
|
||||||
|
max_imu_gap_s=imu_gap,
|
||||||
|
max_lidar_gap_s=lidar_gap,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=ok,
|
||||||
|
)
|
||||||
@@ -0,0 +1,83 @@
|
|||||||
|
"""Vehicle-installation configuration loading and light validation."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Mapping
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
REQUIRED_TOP_LEVEL_KEYS = frozenset({"schema_version", "vehicle", "installation", "sensors", "time"})
|
||||||
|
|
||||||
|
|
||||||
|
def validate_config_shape(config: Mapping[str, object]) -> list[str]:
|
||||||
|
"""Return missing top-level keys without inventing default values."""
|
||||||
|
|
||||||
|
return sorted(REQUIRED_TOP_LEVEL_KEYS.difference(config))
|
||||||
|
|
||||||
|
|
||||||
|
def validate_config_semantics(config: Mapping[str, Any]) -> list[str]:
|
||||||
|
"""Return semantic issues that block calibration interpretation."""
|
||||||
|
|
||||||
|
issues: list[str] = []
|
||||||
|
sensors = config.get("sensors")
|
||||||
|
if not isinstance(sensors, Mapping):
|
||||||
|
return ["sensors must be a mapping"]
|
||||||
|
imu = sensors.get("imu")
|
||||||
|
lidar = sensors.get("lidar")
|
||||||
|
if not isinstance(imu, Mapping):
|
||||||
|
issues.append("sensors.imu missing")
|
||||||
|
else:
|
||||||
|
axes = ((imu.get("raw_frame") or {}) if isinstance(imu.get("raw_frame"), Mapping) else {}).get("axes")
|
||||||
|
if not axes:
|
||||||
|
issues.append("sensors.imu.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||||
|
if not isinstance(lidar, Mapping):
|
||||||
|
issues.append("sensors.lidar missing")
|
||||||
|
else:
|
||||||
|
axes = ((lidar.get("raw_frame") or {}) if isinstance(lidar.get("raw_frame"), Mapping) else {}).get("axes")
|
||||||
|
if not axes:
|
||||||
|
issues.append("sensors.lidar.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||||
|
|
||||||
|
time_cfg = config.get("time")
|
||||||
|
if not isinstance(time_cfg, Mapping):
|
||||||
|
issues.append("time missing")
|
||||||
|
else:
|
||||||
|
for key in ("imu_timestamp_source", "lidar_timestamp_source", "lidar_frame_time_definition"):
|
||||||
|
if not time_cfg.get(key):
|
||||||
|
issues.append(f"time.{key} is empty")
|
||||||
|
return issues
|
||||||
|
|
||||||
|
|
||||||
|
def load_vehicle_config(path: str | Path) -> dict[str, Any]:
|
||||||
|
"""Load and lightly validate a YAML vehicle configuration."""
|
||||||
|
|
||||||
|
try:
|
||||||
|
import yaml
|
||||||
|
except ImportError as exc: # pragma: no cover
|
||||||
|
raise ImportError("PyYAML is required to load vehicle configuration files") from exc
|
||||||
|
|
||||||
|
config_path = Path(path)
|
||||||
|
with config_path.open("r", encoding="utf-8") as handle:
|
||||||
|
loaded = yaml.safe_load(handle)
|
||||||
|
if not isinstance(loaded, dict):
|
||||||
|
raise ValueError(f"vehicle config must be a mapping: {config_path}")
|
||||||
|
|
||||||
|
missing = validate_config_shape(loaded)
|
||||||
|
if missing:
|
||||||
|
raise ValueError(f"vehicle config missing keys {missing}: {config_path}")
|
||||||
|
|
||||||
|
semantic = validate_config_semantics(loaded)
|
||||||
|
if semantic:
|
||||||
|
raise ValueError("vehicle config semantic issues:\n- " + "\n- ".join(semantic))
|
||||||
|
return loaded
|
||||||
|
|
||||||
|
|
||||||
|
def prior_enabled(config: Mapping[str, Any], name: str) -> bool:
|
||||||
|
"""Return whether an optional prior is enabled."""
|
||||||
|
|
||||||
|
init = config.get("initialization")
|
||||||
|
if not isinstance(init, Mapping):
|
||||||
|
return False
|
||||||
|
prior = init.get(name)
|
||||||
|
if not isinstance(prior, Mapping):
|
||||||
|
return False
|
||||||
|
return bool(prior.get("enabled", False))
|
||||||
@@ -0,0 +1,86 @@
|
|||||||
|
# `imu_lidar` 模块说明
|
||||||
|
|
||||||
|
本包实现 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` | 构造运动对；手眼使用其中的旋转 |
|
||||||
|
| 7 | `rotation_handeye.py` | 加权旋转手眼 |
|
||||||
|
| 8 | `observability.py` | 旋转 / 平移可观性检查 |
|
||||||
|
| 8 | `joint_optimizer.py` | 联合精修；完整模式下可估计平移、重力、速度与时变零偏 |
|
||||||
|
| 9 | `finalize.py` | 写出结果 JSON |
|
||||||
|
| — | `pipeline.py` | 编排全流程 |
|
||||||
|
| — | `cli.py` | 命令行入口 |
|
||||||
|
| — | `CHANGELOG.md` | 改动记录 |
|
||||||
|
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 运行模式要点
|
||||||
|
|
||||||
|
- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。
|
||||||
|
- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。
|
||||||
|
- `--mode full_se3`：在可观时再估计重力、关键帧速度、时变零偏与平移；结果写入 `summary.json` 的 joint 字段。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 输入格式
|
||||||
|
|
||||||
|
```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) 米
|
||||||
|
```
|
||||||
|
|
||||||
|
原始录制格式不在本包内解析，需先导出为上述中间格式。详见 `[docs/V1_数据格式.md](../docs/V1_数据格式.md)`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 当前能力
|
||||||
|
|
||||||
|
- 本包是仓库**唯一**标定路径：质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →（可选）完整六自由度 → 报告
|
||||||
|
- 点云去畸变：可选
|
||||||
|
- 阶段与试验边界见根目录 [`README.md`](../README.md) §0
|
||||||
|
- 改动史：[`CHANGELOG.md`](CHANGELOG.md)
|
||||||
|
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
[project]
|
||||||
|
name = "lidar-imu-calibration"
|
||||||
|
version = "0.3.0"
|
||||||
|
description = "LiDAR–IMU extrinsic calibration from continuous-motion keyframes (imu_lidar)"
|
||||||
|
requires-python = ">=3.10"
|
||||||
|
dependencies = [
|
||||||
|
"numpy>=1.26",
|
||||||
|
"scipy>=1.11",
|
||||||
|
"pyyaml>=6.0",
|
||||||
|
]
|
||||||
|
|
||||||
|
[project.optional-dependencies]
|
||||||
|
open3d = ["open3d>=0.17"]
|
||||||
|
dev = ["pytest>=7.4"]
|
||||||
|
|
||||||
|
[project.scripts]
|
||||||
|
lidar-imu-calibration = "imu_lidar.cli:main"
|
||||||
|
|
||||||
|
[build-system]
|
||||||
|
requires = ["setuptools>=68", "wheel"]
|
||||||
|
build-backend = "setuptools.build_meta"
|
||||||
|
|
||||||
|
[tool.setuptools]
|
||||||
|
packages = ["imu_lidar", "tools"]
|
||||||
|
|
||||||
|
[tool.pytest.ini_options]
|
||||||
|
testpaths = ["tests"]
|
||||||
|
pythonpath = ["."]
|
||||||
+127
@@ -0,0 +1,127 @@
|
|||||||
|
# 测试说明
|
||||||
|
|
||||||
|
本目录说明两类试验的**边界**，避免把「合成通过」或「旧车 blocked」误读成实车外参已交付 / 算法不可用。
|
||||||
|
|
||||||
|
总览见根目录 [`README.md`](../README.md) §0。
|
||||||
|
|
||||||
|
| 试验 | 是否默认 pytest | 在证明什么 | 不在证明什么 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| **合成数据** | 是 | 算法链路正确、能收回已知 yaw / δt | 实车安装精度、平移可交付 |
|
||||||
|
| **旧车 S2** | 否（线下手工） | 主机时间旧数据上流水线能跑完；质量门会拒绝坏结果 | 外参真值；新车可用性 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 自动化测试（合成数据 / `pytest`）
|
||||||
|
|
||||||
|
入口：`tests/test_v1_pipeline.py`
|
||||||
|
命令：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
cd <仓库根目录>
|
||||||
|
python -m pytest -q
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
| 测试 | 输入 | 在测什么 | 期望结果 |
|
||||||
|
| ------------------------------------------------------------- | ------------ | ------------------- | ------------------------------------------ |
|
||||||
|
| `test_rotation_handeye_recovers_yaw` | 合成运动对（无点云） | 旋转手眼能否收回已知 yaw | 旋转误差 < 1° |
|
||||||
|
| `test_time_offset_on_synthetic` | 合成会话（故意加 δt） | 模长相关粗估时间偏置 | \|δt 误差\| < 0.05 s |
|
||||||
|
| `test_signed_time_offset_refine_improves_or_keeps` | 同上 + 真值 R | 有符号三轴 δt 精修 | 不比粗估明显更差 |
|
||||||
|
| `test_preintegration_bias_jacobian_matches_finite_difference` | 随机陀螺序列 | 旋转预积分 `J_bg` | 与有限差分一致（松阈值） |
|
||||||
|
| `test_imu_preintegration_recovers_constant_accel_translation` | 常值加速度 | 完整预积分 Δv/Δp | 接近解析值 |
|
||||||
|
| `test_imu_preintegration_bias_jacobian_finite_difference` | 随机 IMU | `J_bg`/`J_ba` 一阶修正 | 与重积分接近 |
|
||||||
|
| `test_synthetic_pipeline_rotation_and_time_offset` | 端到端合成会话 | `rotation_only` 全流程 | `rotation_only_accepted`；δt 准；手眼 RMS < 5° |
|
||||||
|
| `test_synthetic_pipeline_full_se3_smoke` | 同上 | `full_se3` 不崩溃 | 状态为 accepted / rejected / rotation_only 之一 |
|
||||||
|
|
||||||
|
|
||||||
|
合成数据由 `tools/generate_synthetic_session.py` 生成（墙面点云 + 已知外参 yaw 与 δt）。
|
||||||
|
一键复现见根目录 README：`tools/reproduce_synthetic.py`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2. 旧车 S2 线下试验（不在默认 pytest 里）
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 用了什么数据
|
||||||
|
|
||||||
|
|
||||||
|
| 项 | 内容 |
|
||||||
|
| ------- | -------------------------------------------------------------------- |
|
||||||
|
| 车辆 / 批次 | 旧 **S2** 验证集（主机时间时代的录制） |
|
||||||
|
| 典型路径 | `D:\IMU_calibration\work\S2_scheme1_validation\`（历史目录名；含 `imu.csv` + 雷达会话） |
|
||||||
|
| IMU 时间 | **主机 UTC 接收时间**（串口块到达时刻），不是 IMU 设备时间 |
|
||||||
|
| 雷达时间 | dlog 导出的主机侧 `unix_time_ns`，不是 MSOP 设备时间 |
|
||||||
|
| 帧率特征 | 雷达约 **1 Hz** 量级，关键帧间隔偏长 |
|
||||||
|
| 配置烟测 | `config/s2_old_smoke.yaml`（仅声明为旧数据烟测，不当交付） |
|
||||||
|
|
||||||
|
|
||||||
|
这些数据**只能用来验证流水线能否跑通**，不能当作新车外参真值来源。
|
||||||
|
`blocked` 是质量门的**预期结果**，不是「算法突然坏了」。
|
||||||
|
|
||||||
|
### 做了什么测试
|
||||||
|
|
||||||
|
对同一批 S2 中间格式多次跑 `cli run`，例如：
|
||||||
|
|
||||||
|
- 预积分加强后的输出目录（本机历史名如 `out_scheme2_preint`）
|
||||||
|
- 有符号 δt / 联合精修后的输出（本机历史名如 `out_scheme2_phaseA`）
|
||||||
|
- 用 `tools/compare_s2_runs.py` 对比两次 `summary.json`
|
||||||
|
|
||||||
|
命令形态（路径按本机实际修改；雷达会话目录若仍叫 `scheme2_session` 为历史命名）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m imu_lidar.cli run `
|
||||||
|
--vehicle-config config\s2_old_smoke.yaml `
|
||||||
|
--imu D:\IMU_calibration\work\S2_scheme1_validation\imu.csv `
|
||||||
|
--lidar D:\IMU_calibration\work\S2_scheme1_validation\scheme2_session `
|
||||||
|
--output path\to\out_s2 `
|
||||||
|
--mode rotation_only `
|
||||||
|
--time-offset-search-s 2.0
|
||||||
|
|
||||||
|
python tools\compare_s2_runs.py path\to\out_old\summary.json path\to\out_new\summary.json
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 得到什么结果（记录摘要）
|
||||||
|
|
||||||
|
|
||||||
|
| 指标 | 预积分加强一轮 | 有符号 δt / 精修一轮 |
|
||||||
|
| -------- | ------------- | ------------------ |
|
||||||
|
| `status` | `blocked`（预期） | `blocked`（预期） |
|
||||||
|
| 手眼 RMS | 约 **15.0°** | 约 **14.5°** |
|
||||||
|
| 手眼中位数 | — | 约 **6.8°** |
|
||||||
|
| δt | 约 **−2.0 s** | 约 **−1.75 s**（有修正） |
|
||||||
|
| 相关峰 | 很弱（约 0.18） | 仍弱（约 0.13） |
|
||||||
|
| 结论 | 链路可跑 | 残差略降，但 **不当交付外参** |
|
||||||
|
|
||||||
|
|
||||||
|
原因归纳：
|
||||||
|
|
||||||
|
1. 时间戳是**主机时间**，相关峰弱，δt / yaw / 零偏互相耦合；
|
||||||
|
2. 雷达约 1 Hz，运动对间隔长，IMU 侧更易漂；
|
||||||
|
3. 质量门主动 `blocked`，避免把坏结果当成安装参数。
|
||||||
|
|
||||||
|
**正式标定**必须改用设备时间（IMU `device_timestamp`、雷达 MSOP 设备时）重新采集后再跑。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 3. 配准结果怎么目视检查
|
||||||
|
|
||||||
|
标定跑完后（合成或实车）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar examples\synthetic_session\lidar `
|
||||||
|
--imu examples\synthetic_session\imu.csv `
|
||||||
|
--summary examples\synthetic_session\out\summary.json `
|
||||||
|
--pair-index 0 `
|
||||||
|
--save-png examples\synthetic_session\out\pair0_overlay.png
|
||||||
|
```
|
||||||
|
|
||||||
|
交互窗口快捷键：`1`–`4` 切换叠点模式；`N`/`]` 下一运动对，`P`/`[` 上一运动对。
|
||||||
|
无显示器时加 `--no-gui --save-png ...` 只出俯视图 PNG。
|
||||||
@@ -0,0 +1,206 @@
|
|||||||
|
"""Automated tests for imu_lidar (synthetic data).
|
||||||
|
|
||||||
|
See ``tests/README.md`` for:
|
||||||
|
- what each pytest covers;
|
||||||
|
- offline S2 host-time experiments (not run in default pytest) and recorded outcomes.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.contracts import CalibrationMode, CalibrationRequest, MotionPair, SessionInput
|
||||||
|
from imu_lidar.geometry import so3_exp
|
||||||
|
from imu_lidar.pipeline import run_calibration
|
||||||
|
from imu_lidar.rotation_handeye import solve_rotation_handeye
|
||||||
|
from imu_lidar.time_offset import estimate_time_offset
|
||||||
|
from imu_lidar.imu_io import load_imu_samples
|
||||||
|
from imu_lidar.lidar_io import load_lidar_frames
|
||||||
|
from tools.generate_synthetic_session import generate_synthetic_session
|
||||||
|
|
||||||
|
|
||||||
|
def test_rotation_handeye_recovers_yaw():
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([1.0, -2.0, 30.0])))
|
||||||
|
pairs = []
|
||||||
|
rng = np.random.default_rng(1)
|
||||||
|
for _ in range(20):
|
||||||
|
axis = rng.normal(size=3)
|
||||||
|
axis /= np.linalg.norm(axis)
|
||||||
|
angle = np.deg2rad(rng.uniform(8.0, 35.0))
|
||||||
|
r_b = so3_exp(axis * angle)
|
||||||
|
r_a = r_true @ r_b @ r_true.T
|
||||||
|
pairs.append(
|
||||||
|
MotionPair(
|
||||||
|
session_id="s",
|
||||||
|
i=0,
|
||||||
|
j=1,
|
||||||
|
t_i_s=0.0,
|
||||||
|
t_j_s=1.0,
|
||||||
|
R_A=r_a,
|
||||||
|
R_B=r_b,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
result = solve_rotation_handeye(pairs)
|
||||||
|
assert result.ok
|
||||||
|
err = np.linalg.norm(_log(r_true.T @ result.R_IMU_lidar))
|
||||||
|
assert np.degrees(err) < 1.0
|
||||||
|
|
||||||
|
|
||||||
|
def _log(rotation: np.ndarray) -> np.ndarray:
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
return so3_log(rotation)
|
||||||
|
|
||||||
|
|
||||||
|
def test_synthetic_pipeline_rotation_and_time_offset(tmp_path: Path):
|
||||||
|
meta = generate_synthetic_session(tmp_path, delta_t_s=0.17, yaw_extrinsic_deg=25.0)
|
||||||
|
config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
|
||||||
|
out = tmp_path / "out"
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=config,
|
||||||
|
sessions=(
|
||||||
|
SessionInput(
|
||||||
|
session_id="synth",
|
||||||
|
imu_source=tmp_path / "imu.csv",
|
||||||
|
lidar_source=tmp_path / "lidar",
|
||||||
|
),
|
||||||
|
),
|
||||||
|
requested_mode=CalibrationMode.ROTATION_ONLY,
|
||||||
|
output_directory=out,
|
||||||
|
max_iterations=1,
|
||||||
|
time_offset_search_s=0.5,
|
||||||
|
min_pair_rotation_deg=2.0,
|
||||||
|
min_pair_translation_m=0.05,
|
||||||
|
)
|
||||||
|
result = run_calibration(request)
|
||||||
|
assert result.status.value == "rotation_only_accepted"
|
||||||
|
assert result.time_offset_s is not None
|
||||||
|
assert abs(result.time_offset_s - meta["delta_t_s"]) < 0.05
|
||||||
|
assert result.T_IMU_lidar is not None
|
||||||
|
# End-to-end uses approximate ICP; allow moderate absolute error but require consistency.
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
|
||||||
|
err_deg = np.degrees(np.linalg.norm(_log(r_true.T @ result.T_IMU_lidar[:3, :3])))
|
||||||
|
assert err_deg < 15.0
|
||||||
|
session0 = result.details["sessions"][0]
|
||||||
|
assert session0["handeye"]["residual_rms_deg"] < 5.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_time_offset_on_synthetic(tmp_path: Path):
|
||||||
|
meta = generate_synthetic_session(tmp_path, delta_t_s=0.21, yaw_extrinsic_deg=15.0)
|
||||||
|
imu = load_imu_samples(tmp_path / "imu.csv")
|
||||||
|
frames = load_lidar_frames(tmp_path / "lidar")
|
||||||
|
offset = estimate_time_offset(imu, frames, search_s=0.5)
|
||||||
|
assert offset.ok
|
||||||
|
assert abs(offset.delta_t_s - meta["delta_t_s"]) < 0.05
|
||||||
|
|
||||||
|
|
||||||
|
def test_preintegration_bias_jacobian_matches_finite_difference():
|
||||||
|
from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
rng = np.random.default_rng(0)
|
||||||
|
t = np.linspace(0.0, 1.0, 200)
|
||||||
|
gyro = rng.normal(scale=0.2, size=(t.size, 3))
|
||||||
|
bias0 = np.array([0.01, -0.02, 0.005])
|
||||||
|
preint = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0)
|
||||||
|
db = np.array([1e-3, -2e-3, 5e-4])
|
||||||
|
approx = apply_bias_jacobian_correction(preint.delta_R, preint.J_bg, db)
|
||||||
|
exact = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0 + db).delta_R
|
||||||
|
err = np.linalg.norm(so3_log(approx.T @ exact))
|
||||||
|
assert err < 2e-3
|
||||||
|
|
||||||
|
|
||||||
|
def test_imu_preintegration_recovers_constant_accel_translation():
|
||||||
|
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
# Constant body accel (no gravity in preint body increments), zero gyro.
|
||||||
|
dt = 0.01
|
||||||
|
t = np.arange(0.0, 1.0 + 1e-9, dt)
|
||||||
|
gyro = np.zeros((t.size, 3))
|
||||||
|
acc = np.tile(np.array([0.5, -0.2, 0.1]), (t.size, 1))
|
||||||
|
preint = preintegrate_imu(t, gyro, acc, 0.0, 1.0, np.zeros(3), np.zeros(3))
|
||||||
|
assert np.linalg.norm(so3_log(preint.delta_R)) < 1e-9
|
||||||
|
# Δv ≈ a Δt, Δp ≈ 0.5 a Δt²
|
||||||
|
assert np.linalg.norm(preint.delta_v - acc[0] * 1.0) < 5e-3
|
||||||
|
assert np.linalg.norm(preint.delta_p - 0.5 * acc[0] * 1.0) < 1e-2
|
||||||
|
assert preint.cov.shape == (9, 9)
|
||||||
|
assert preint.J_bg.shape == (9, 3) and preint.J_ba.shape == (9, 3)
|
||||||
|
|
||||||
|
|
||||||
|
def test_imu_preintegration_bias_jacobian_finite_difference():
|
||||||
|
from imu_lidar.imu_preintegration import apply_bias_correction_imu, preintegrate_imu
|
||||||
|
|
||||||
|
rng = np.random.default_rng(2)
|
||||||
|
t = np.linspace(0.0, 0.8, 160)
|
||||||
|
gyro = rng.normal(scale=0.15, size=(t.size, 3))
|
||||||
|
acc = rng.normal(scale=0.5, size=(t.size, 3)) + np.array([0.0, 0.0, 9.8])
|
||||||
|
bg0 = np.array([0.01, -0.01, 0.0])
|
||||||
|
ba0 = np.array([0.02, 0.0, -0.01])
|
||||||
|
base = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0, ba0)
|
||||||
|
dbg = np.array([5e-4, -3e-4, 2e-4])
|
||||||
|
dba = np.array([1e-3, -5e-4, 0.0])
|
||||||
|
r_a, v_a, p_a = apply_bias_correction_imu(base, dbg, dba)
|
||||||
|
exact = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0 + dbg, ba0 + dba)
|
||||||
|
from imu_lidar.geometry import so3_log
|
||||||
|
|
||||||
|
assert np.linalg.norm(so3_log(r_a.T @ exact.delta_R)) < 5e-3
|
||||||
|
assert np.linalg.norm(v_a - exact.delta_v) < 3e-2
|
||||||
|
assert np.linalg.norm(p_a - exact.delta_p) < 2e-2
|
||||||
|
|
||||||
|
|
||||||
|
def test_synthetic_pipeline_full_se3_smoke(tmp_path: Path):
|
||||||
|
generate_synthetic_session(tmp_path, delta_t_s=0.12, yaw_extrinsic_deg=18.0)
|
||||||
|
config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
|
||||||
|
out = tmp_path / "out_se3"
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=config,
|
||||||
|
sessions=(
|
||||||
|
SessionInput(
|
||||||
|
session_id="synth",
|
||||||
|
imu_source=tmp_path / "imu.csv",
|
||||||
|
lidar_source=tmp_path / "lidar",
|
||||||
|
),
|
||||||
|
),
|
||||||
|
requested_mode=CalibrationMode.FULL_SE3,
|
||||||
|
output_directory=out,
|
||||||
|
max_iterations=1,
|
||||||
|
time_offset_search_s=0.5,
|
||||||
|
min_pair_rotation_deg=2.0,
|
||||||
|
min_pair_translation_m=0.05,
|
||||||
|
)
|
||||||
|
result = run_calibration(request)
|
||||||
|
assert result.status.value in {
|
||||||
|
"full_se3_accepted",
|
||||||
|
"full_se3_rejected_due_to_observability",
|
||||||
|
"rotation_only_accepted",
|
||||||
|
}
|
||||||
|
assert result.T_IMU_lidar is not None
|
||||||
|
session0 = result.details["sessions"][0]
|
||||||
|
assert "delta_v" in session0.get("pair_notes", []) or session0.get("pair_count", 0) >= 0
|
||||||
|
# Phase-C fields appear when joint ran successfully on pairs.
|
||||||
|
if session0.get("ok"):
|
||||||
|
assert "gyro_bias_rad_s" in session0["joint"]
|
||||||
|
|
||||||
|
|
||||||
|
def test_signed_time_offset_refine_improves_or_keeps(tmp_path: Path):
|
||||||
|
from imu_lidar.geometry import so3_exp
|
||||||
|
from imu_lidar.time_offset import refine_time_offset_signed
|
||||||
|
|
||||||
|
meta = generate_synthetic_session(tmp_path, delta_t_s=0.18, yaw_extrinsic_deg=20.0)
|
||||||
|
imu = load_imu_samples(tmp_path / "imu.csv")
|
||||||
|
frames = load_lidar_frames(tmp_path / "lidar")
|
||||||
|
coarse = estimate_time_offset(imu, frames, search_s=0.5)
|
||||||
|
r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
|
||||||
|
refined = refine_time_offset_signed(
|
||||||
|
imu,
|
||||||
|
frames,
|
||||||
|
delta_t_s=coarse.delta_t_s,
|
||||||
|
R_IMU_lidar=r_true,
|
||||||
|
search_s=0.08,
|
||||||
|
)
|
||||||
|
assert refined.ok
|
||||||
|
# Must not drift farther from truth than the coarse estimate by a large margin.
|
||||||
|
assert abs(refined.delta_t_s - meta["delta_t_s"]) <= abs(coarse.delta_t_s - meta["delta_t_s"]) + 0.01
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
# Tools package for local scripts and tests.
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Compare two S2 offline summary.json runs (real artifacts only)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.geometry import rpy_deg_xyz, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
def dig(path: Path):
|
||||||
|
data = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
session = data["details"]["sessions"][0]
|
||||||
|
return data, session, session.get("handeye", {}), session.get("time_offset", {})
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
old_path = Path(sys.argv[1])
|
||||||
|
new_path = Path(sys.argv[2])
|
||||||
|
old, so, heo, too = dig(old_path)
|
||||||
|
new, sn, hen, ton = dig(new_path)
|
||||||
|
|
||||||
|
print("==== COMPARISON (real summary.json artifacts) ====")
|
||||||
|
print(f"{'metric':28s} {'run_a':28s} {'run_b':28s}")
|
||||||
|
rows = [
|
||||||
|
("status", old.get("status"), new.get("status")),
|
||||||
|
("stage", so.get("stage"), sn.get("stage")),
|
||||||
|
("delta_t_s", f"{too.get('delta_t_s'):.6f}", f"{ton.get('delta_t_s'):.6f}"),
|
||||||
|
(
|
||||||
|
"corr/mag_peak",
|
||||||
|
f"{too.get('correlation_peak'):.6f}",
|
||||||
|
f"{ton.get('correlation_peak'):.6f}",
|
||||||
|
),
|
||||||
|
("keyframes", so.get("keyframes"), sn.get("keyframes")),
|
||||||
|
("handeye_pairs", heo.get("pair_count"), hen.get("pair_count")),
|
||||||
|
("handeye_ok", heo.get("ok"), hen.get("ok")),
|
||||||
|
("rms_deg", f"{heo.get('residual_rms_deg'):.4f}", f"{hen.get('residual_rms_deg'):.4f}"),
|
||||||
|
(
|
||||||
|
"median_deg",
|
||||||
|
f"{heo.get('residual_median_deg'):.4f}",
|
||||||
|
f"{hen.get('residual_median_deg'):.4f}",
|
||||||
|
),
|
||||||
|
]
|
||||||
|
for key, a, b in rows:
|
||||||
|
print(f"{key:28s} {str(a):28s} {str(b):28s}")
|
||||||
|
|
||||||
|
print("run_a pair_notes:", so.get("pair_notes"))
|
||||||
|
print("run_b pair_notes:", sn.get("pair_notes"))
|
||||||
|
print("run_a time notes:", too.get("notes"))
|
||||||
|
print("run_b time notes:", ton.get("notes"))
|
||||||
|
print("run_a handeye notes:", heo.get("notes"))
|
||||||
|
print("run_b handeye notes:", hen.get("notes"))
|
||||||
|
|
||||||
|
r_old = np.asarray(heo["R_IMU_lidar"], dtype=float)
|
||||||
|
r_new = np.asarray(hen["R_IMU_lidar"], dtype=float)
|
||||||
|
print("R relative change deg:", float(np.degrees(np.linalg.norm(so3_log(r_old.T @ r_new)))))
|
||||||
|
print("RPY run_a deg:", rpy_deg_xyz(r_old))
|
||||||
|
print("RPY run_b deg:", rpy_deg_xyz(r_new))
|
||||||
|
print("delta rms (b-a):", hen.get("residual_rms_deg") - heo.get("residual_rms_deg"))
|
||||||
|
print(
|
||||||
|
"delta median (b-a):",
|
||||||
|
hen.get("residual_median_deg") - heo.get("residual_median_deg"),
|
||||||
|
)
|
||||||
|
print("artifacts:")
|
||||||
|
print(" run_a:", old_path)
|
||||||
|
print(" run_b:", new_path)
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,114 @@
|
|||||||
|
"""Generate a tiny synthetic session for V1 smoke tests."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.contracts import ImuSeries, LidarFrame
|
||||||
|
from imu_lidar.geometry import so3_exp
|
||||||
|
from imu_lidar.imu_io import save_imu_csv
|
||||||
|
from imu_lidar.lidar_io import save_lidar_session
|
||||||
|
|
||||||
|
|
||||||
|
def _wall_cloud(rng: np.random.Generator, n: int = 800) -> np.ndarray:
|
||||||
|
yz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
|
||||||
|
wall_x = np.column_stack([np.full(n // 3, 8.0), yz[:, 0], yz[:, 1]])
|
||||||
|
xz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
|
||||||
|
wall_y = np.column_stack([xz[:, 0], np.full(n // 3, 6.0), xz[:, 1]])
|
||||||
|
xy = rng.uniform([-5, -5], [5, 5], size=(n - 2 * (n // 3), 2))
|
||||||
|
ground = np.column_stack([xy[:, 0], xy[:, 1], np.full(xy.shape[0], -1.0)])
|
||||||
|
return np.vstack([wall_x, wall_y, ground])
|
||||||
|
|
||||||
|
|
||||||
|
def generate_synthetic_session(
|
||||||
|
output_root: Path,
|
||||||
|
*,
|
||||||
|
delta_t_s: float = 0.17,
|
||||||
|
yaw_extrinsic_deg: float = 25.0,
|
||||||
|
seed: int = 0,
|
||||||
|
) -> dict[str, float]:
|
||||||
|
"""Write IMU CSV + LiDAR frames with known extrinsic rotation and time offset."""
|
||||||
|
|
||||||
|
rng = np.random.default_rng(seed)
|
||||||
|
output_root = Path(output_root)
|
||||||
|
output_root.mkdir(parents=True, exist_ok=True)
|
||||||
|
|
||||||
|
r_x = so3_exp(np.deg2rad(np.array([2.0, -1.5, yaw_extrinsic_deg])))
|
||||||
|
map_points = _wall_cloud(rng)
|
||||||
|
|
||||||
|
lidar_hz = 10.0
|
||||||
|
duration = 8.0
|
||||||
|
lidar_times = np.arange(0.0, duration, 1.0 / lidar_hz)
|
||||||
|
# Non-yaw excitation is required for unique SO(3) hand-eye observability.
|
||||||
|
yaw = 0.5 * np.sin(0.8 * lidar_times) + 0.12 * lidar_times
|
||||||
|
pitch = 0.18 * np.sin(1.3 * lidar_times + 0.4)
|
||||||
|
roll = 0.12 * np.sin(1.7 * lidar_times + 1.0)
|
||||||
|
yaw_rate = np.gradient(yaw, lidar_times)
|
||||||
|
pitch_rate = np.gradient(pitch, lidar_times)
|
||||||
|
roll_rate = np.gradient(roll, lidar_times)
|
||||||
|
|
||||||
|
frames: list[LidarFrame] = []
|
||||||
|
for index, (t, yaw_i, pitch_i, roll_i) in enumerate(zip(lidar_times, yaw, pitch, roll)):
|
||||||
|
r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
|
||||||
|
t_wl = np.array([0.4 * t, 0.05 * np.sin(0.5 * t), 0.0])
|
||||||
|
points = (map_points - t_wl) @ r_wl
|
||||||
|
points = points + rng.normal(0.0, 0.01, size=points.shape)
|
||||||
|
frames.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=str(index),
|
||||||
|
t_start_s=float(t),
|
||||||
|
t_end_s=float(t + 0.08),
|
||||||
|
points_xyz=points.astype(float),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
save_lidar_session(output_root / "lidar", frames)
|
||||||
|
|
||||||
|
imu_hz = 100.0
|
||||||
|
t_lidar_grid = np.arange(0.0, duration, 1.0 / imu_hz)
|
||||||
|
omega_lidar = np.column_stack(
|
||||||
|
[
|
||||||
|
np.interp(t_lidar_grid, lidar_times, roll_rate),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, pitch_rate),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, yaw_rate),
|
||||||
|
]
|
||||||
|
)
|
||||||
|
omega_imu = omega_lidar @ r_x.T
|
||||||
|
|
||||||
|
g_world = np.array([0.0, 0.0, 9.80665])
|
||||||
|
acc_rows = []
|
||||||
|
for yaw_i, pitch_i, roll_i in zip(
|
||||||
|
np.interp(t_lidar_grid, lidar_times, yaw),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, pitch),
|
||||||
|
np.interp(t_lidar_grid, lidar_times, roll),
|
||||||
|
):
|
||||||
|
r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
|
||||||
|
g_in_lidar = r_wl.T @ g_world
|
||||||
|
acc_rows.append(r_x @ g_in_lidar)
|
||||||
|
acc = np.asarray(acc_rows, dtype=float)
|
||||||
|
|
||||||
|
static_t = np.arange(-1.0, 0.0, 1.0 / imu_hz)
|
||||||
|
static_gyro = np.zeros((static_t.size, 3))
|
||||||
|
static_acc = np.tile(r_x @ g_world, (static_t.size, 1))
|
||||||
|
|
||||||
|
t_imu = np.concatenate([static_t + delta_t_s, t_lidar_grid + delta_t_s])
|
||||||
|
gyro = np.vstack([static_gyro, omega_imu]) + rng.normal(0.0, 0.001, size=(t_imu.size, 3))
|
||||||
|
acc_all = np.vstack([static_acc, acc]) + rng.normal(0.0, 0.01, size=(t_imu.size, 3))
|
||||||
|
imu = ImuSeries(t_s=t_imu, gyro_rad_s=gyro, acc_m_s2=acc_all)
|
||||||
|
save_imu_csv(output_root / "imu.csv", imu)
|
||||||
|
|
||||||
|
return {
|
||||||
|
"delta_t_s": float(delta_t_s),
|
||||||
|
"yaw_extrinsic_deg": float(yaw_extrinsic_deg),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
import json
|
||||||
|
|
||||||
|
out = Path("examples/synthetic_session")
|
||||||
|
meta = generate_synthetic_session(out)
|
||||||
|
(out / "meta.json").write_text(json.dumps(meta, indent=2), encoding="utf-8")
|
||||||
|
print(f"wrote {out}")
|
||||||
|
print(meta)
|
||||||
@@ -0,0 +1,23 @@
|
|||||||
|
# 一键复现合成标定（Windows）
|
||||||
|
# 用法：在仓库根目录执行
|
||||||
|
# powershell -File tools\reproduce_synthetic.ps1
|
||||||
|
# powershell -File tools\reproduce_synthetic.ps1 -SkipPytest
|
||||||
|
# powershell -File tools\reproduce_synthetic.ps1 -Mode full_se3
|
||||||
|
|
||||||
|
param(
|
||||||
|
[ValidateSet("rotation_only", "full_se3")]
|
||||||
|
[string]$Mode = "rotation_only",
|
||||||
|
[switch]$SkipPytest
|
||||||
|
)
|
||||||
|
|
||||||
|
$ErrorActionPreference = "Stop"
|
||||||
|
$Root = Split-Path -Parent $PSScriptRoot
|
||||||
|
Set-Location $Root
|
||||||
|
|
||||||
|
$args = @("tools\reproduce_synthetic.py", "--mode", $Mode)
|
||||||
|
if ($SkipPytest) {
|
||||||
|
$args += "--skip-pytest"
|
||||||
|
}
|
||||||
|
|
||||||
|
python @args
|
||||||
|
exit $LASTEXITCODE
|
||||||
@@ -0,0 +1,109 @@
|
|||||||
|
"""One-click synthetic reproduce: generate → calibrate → report → pytest."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import subprocess
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
||||||
|
def _repo_root() -> Path:
|
||||||
|
return Path(__file__).resolve().parents[1]
|
||||||
|
|
||||||
|
|
||||||
|
def _run(cmd: list[str], cwd: Path) -> None:
|
||||||
|
print("+", " ".join(cmd), flush=True)
|
||||||
|
completed = subprocess.run(cmd, cwd=str(cwd), check=False)
|
||||||
|
if completed.returncode != 0:
|
||||||
|
raise SystemExit(completed.returncode)
|
||||||
|
|
||||||
|
|
||||||
|
def main(argv: list[str] | None = None) -> int:
|
||||||
|
parser = argparse.ArgumentParser(
|
||||||
|
description="Generate synthetic LiDAR–IMU data, run calibration, show report, run tests."
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--mode",
|
||||||
|
choices=["rotation_only", "full_se3"],
|
||||||
|
default="rotation_only",
|
||||||
|
)
|
||||||
|
parser.add_argument("--skip-pytest", action="store_true")
|
||||||
|
args = parser.parse_args(argv)
|
||||||
|
|
||||||
|
root = _repo_root()
|
||||||
|
session = root / "examples" / "synthetic_session"
|
||||||
|
imu = session / "imu.csv"
|
||||||
|
lidar = session / "lidar"
|
||||||
|
calib_out = session / "out"
|
||||||
|
config = root / "config" / "vehicle_installation.template.yaml"
|
||||||
|
|
||||||
|
print("=== 1/4 generate synthetic session ===", flush=True)
|
||||||
|
_run([sys.executable, str(root / "tools" / "generate_synthetic_session.py")], cwd=root)
|
||||||
|
|
||||||
|
print("=== 2/4 run calibration ===", flush=True)
|
||||||
|
_run(
|
||||||
|
[
|
||||||
|
sys.executable,
|
||||||
|
"-m",
|
||||||
|
"imu_lidar.cli",
|
||||||
|
"run",
|
||||||
|
"--vehicle-config",
|
||||||
|
str(config),
|
||||||
|
"--imu",
|
||||||
|
str(imu),
|
||||||
|
"--lidar",
|
||||||
|
str(lidar),
|
||||||
|
"--output",
|
||||||
|
str(calib_out),
|
||||||
|
"--mode",
|
||||||
|
args.mode,
|
||||||
|
"--time-offset-search-s",
|
||||||
|
"0.5",
|
||||||
|
"--min-pair-rotation-deg",
|
||||||
|
"2.0",
|
||||||
|
"--min-pair-translation-m",
|
||||||
|
"0.05",
|
||||||
|
"--max-iterations",
|
||||||
|
"1",
|
||||||
|
],
|
||||||
|
cwd=root,
|
||||||
|
)
|
||||||
|
|
||||||
|
print("=== 3/4 show report ===", flush=True)
|
||||||
|
_run(
|
||||||
|
[
|
||||||
|
sys.executable,
|
||||||
|
str(root / "tools" / "show_calibration_report.py"),
|
||||||
|
"--summary",
|
||||||
|
str(calib_out / "summary.json"),
|
||||||
|
"--truth-meta",
|
||||||
|
str(session / "meta.json"),
|
||||||
|
"--plot",
|
||||||
|
str(calib_out / "report_preview.png"),
|
||||||
|
],
|
||||||
|
cwd=root,
|
||||||
|
)
|
||||||
|
|
||||||
|
if args.skip_pytest:
|
||||||
|
print("=== 4/4 pytest skipped ===", flush=True)
|
||||||
|
else:
|
||||||
|
print("=== 4/4 pytest ===", flush=True)
|
||||||
|
_run([sys.executable, "-m", "pytest", "-q"], cwd=root)
|
||||||
|
|
||||||
|
print("\nDone.")
|
||||||
|
print(" INPUT")
|
||||||
|
print(f" IMU CSV : {imu}")
|
||||||
|
print(f" LiDAR dir : {lidar}")
|
||||||
|
print(f" vehicle YAML: {config}")
|
||||||
|
print(" OUTPUT")
|
||||||
|
print(f" directory : {calib_out}")
|
||||||
|
print(f" T : {calib_out / 'T_IMU_lidar.json'}")
|
||||||
|
print(f" δt : {calib_out / 'time_offset.json'}")
|
||||||
|
print(f" summary : {calib_out / 'summary.json'}")
|
||||||
|
print(f" preview PNG : {calib_out / 'report_preview.png'}")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,133 @@
|
|||||||
|
"""Pretty-print calibration summary.json and optionally write a small preview figure."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
||||||
|
def _load(path: Path) -> dict:
|
||||||
|
return json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
|
||||||
|
|
||||||
|
def _session0(summary: dict) -> dict:
|
||||||
|
sessions = summary.get("details", {}).get("sessions", [])
|
||||||
|
return sessions[0] if sessions else {}
|
||||||
|
|
||||||
|
|
||||||
|
def print_report(summary: dict, truth: dict | None = None) -> None:
|
||||||
|
print("---------- calibration report ----------")
|
||||||
|
print(f"status : {summary.get('status')}")
|
||||||
|
print(f"message: {summary.get('message')}")
|
||||||
|
dt = summary.get("time_offset_s")
|
||||||
|
if dt is not None:
|
||||||
|
print(f"δt : {dt:.6f} s (t_imu = t_lidar + δt)")
|
||||||
|
if truth and "delta_t_s" in truth:
|
||||||
|
print(f" truth={truth['delta_t_s']:.6f} s err={dt - truth['delta_t_s']:+.6f} s")
|
||||||
|
|
||||||
|
t_block = summary.get("T_IMU_lidar")
|
||||||
|
if isinstance(t_block, dict):
|
||||||
|
rpy = t_block.get("rpy_deg_xyz")
|
||||||
|
trans = t_block.get("translation_m")
|
||||||
|
if rpy is not None:
|
||||||
|
print(f"RPY xyz: [{rpy[0]:.3f}, {rpy[1]:.3f}, {rpy[2]:.3f}] deg")
|
||||||
|
if truth and "yaw_extrinsic_deg" in truth:
|
||||||
|
print(f" yaw truth≈{truth['yaw_extrinsic_deg']:.3f} deg")
|
||||||
|
if trans is not None:
|
||||||
|
print(f"t : [{trans[0]:.4f}, {trans[1]:.4f}, {trans[2]:.4f}] m")
|
||||||
|
|
||||||
|
session = _session0(summary)
|
||||||
|
handeye = session.get("handeye") or {}
|
||||||
|
joint = session.get("joint") or {}
|
||||||
|
if handeye:
|
||||||
|
print(
|
||||||
|
"handeye: "
|
||||||
|
f"pairs={handeye.get('pair_count')} "
|
||||||
|
f"rms={handeye.get('residual_rms_deg')}° "
|
||||||
|
f"median={handeye.get('residual_median_deg')}° "
|
||||||
|
f"ok={handeye.get('ok')}"
|
||||||
|
)
|
||||||
|
if joint:
|
||||||
|
obs = joint.get("observability") or {}
|
||||||
|
print(
|
||||||
|
"joint : "
|
||||||
|
f"rot_rms={joint.get('residual_rms_rot_deg')}° "
|
||||||
|
f"trans_rms={joint.get('residual_rms_trans_m')} m "
|
||||||
|
f"trans_accepted={joint.get('translation_accepted')}"
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
"observ : "
|
||||||
|
f"rotation={obs.get('rotation_observable')} "
|
||||||
|
f"translation={obs.get('translation_observable')} "
|
||||||
|
f"cond_R={obs.get('condition_rotation')}"
|
||||||
|
)
|
||||||
|
if joint.get("gravity_m_s2") is not None:
|
||||||
|
print(f"gravity: {joint.get('gravity_m_s2')}")
|
||||||
|
if joint.get("gyro_bias_rad_s") is not None:
|
||||||
|
print(f"b_g : {joint.get('gyro_bias_rad_s')}")
|
||||||
|
print("----------------------------------------")
|
||||||
|
|
||||||
|
|
||||||
|
def maybe_plot(summary: dict, plot_path: Path, truth: dict | None = None) -> None:
|
||||||
|
try:
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
except ImportError:
|
||||||
|
print("(matplotlib not installed; skip plot)")
|
||||||
|
return
|
||||||
|
|
||||||
|
session = _session0(summary)
|
||||||
|
handeye = session.get("handeye") or {}
|
||||||
|
joint = session.get("joint") or {}
|
||||||
|
labels = []
|
||||||
|
values = []
|
||||||
|
if handeye.get("residual_rms_deg") is not None:
|
||||||
|
labels.append("handeye\nRMS (°)")
|
||||||
|
values.append(float(handeye["residual_rms_deg"]))
|
||||||
|
if joint.get("residual_rms_rot_deg") is not None:
|
||||||
|
labels.append("joint rot\nRMS (°)")
|
||||||
|
values.append(float(joint["residual_rms_rot_deg"]))
|
||||||
|
if joint.get("residual_rms_trans_m") is not None:
|
||||||
|
labels.append("joint trans\nRMS (m)")
|
||||||
|
values.append(float(joint["residual_rms_trans_m"]))
|
||||||
|
dt = summary.get("time_offset_s")
|
||||||
|
if dt is not None:
|
||||||
|
labels.append("|δt| (s)")
|
||||||
|
values.append(abs(float(dt)))
|
||||||
|
if truth and "delta_t_s" in truth:
|
||||||
|
labels.append("|δt err| (s)")
|
||||||
|
values.append(abs(float(dt) - float(truth["delta_t_s"])))
|
||||||
|
|
||||||
|
if not labels:
|
||||||
|
print("(no numeric fields to plot)")
|
||||||
|
return
|
||||||
|
|
||||||
|
fig, ax = plt.subplots(figsize=(7.5, 3.8))
|
||||||
|
ax.bar(labels, values, color="#3b6ea5")
|
||||||
|
ax.set_title(f"Calibration preview — {summary.get('status')}")
|
||||||
|
ax.set_ylabel("value")
|
||||||
|
ax.grid(axis="y", alpha=0.3)
|
||||||
|
fig.tight_layout()
|
||||||
|
plot_path.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
fig.savefig(plot_path, dpi=120)
|
||||||
|
plt.close(fig)
|
||||||
|
print(f"wrote plot: {plot_path}")
|
||||||
|
|
||||||
|
|
||||||
|
def main(argv: list[str] | None = None) -> int:
|
||||||
|
parser = argparse.ArgumentParser(description="Show LiDAR–IMU calibration summary")
|
||||||
|
parser.add_argument("--summary", type=Path, required=True, help="Path to summary.json")
|
||||||
|
parser.add_argument("--truth-meta", type=Path, default=None, help="Optional synthetic meta.json")
|
||||||
|
parser.add_argument("--plot", type=Path, default=None, help="Optional PNG path for a bar chart")
|
||||||
|
args = parser.parse_args(argv)
|
||||||
|
|
||||||
|
summary = _load(args.summary)
|
||||||
|
truth = _load(args.truth_meta) if args.truth_meta and args.truth_meta.exists() else None
|
||||||
|
print_report(summary, truth)
|
||||||
|
if args.plot is not None:
|
||||||
|
maybe_plot(summary, args.plot, truth)
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,27 @@
|
|||||||
|
param(
|
||||||
|
[Parameter(Mandatory = $true)][string]$Lidar,
|
||||||
|
[Parameter(Mandatory = $true)][string]$Imu,
|
||||||
|
[Parameter(Mandatory = $true)][string]$Summary,
|
||||||
|
[int]$PairIndex = 0,
|
||||||
|
[double]$Voxel = 0.12,
|
||||||
|
[string]$SavePng = ""
|
||||||
|
)
|
||||||
|
|
||||||
|
$ErrorActionPreference = "Stop"
|
||||||
|
$Root = Split-Path -Parent $PSScriptRoot
|
||||||
|
Set-Location $Root
|
||||||
|
|
||||||
|
$args = @(
|
||||||
|
"tools\visualize_pair_3d.py",
|
||||||
|
"--lidar", $Lidar,
|
||||||
|
"--imu", $Imu,
|
||||||
|
"--summary", $Summary,
|
||||||
|
"--pair-index", "$PairIndex",
|
||||||
|
"--voxel", "$Voxel"
|
||||||
|
)
|
||||||
|
if ($SavePng -ne "") {
|
||||||
|
$args += @("--save-png", $SavePng)
|
||||||
|
}
|
||||||
|
|
||||||
|
python @args
|
||||||
|
if ($LASTEXITCODE -ne 0) { throw "visualize_pair_3d failed: $LASTEXITCODE" }
|
||||||
@@ -0,0 +1,452 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Interactive 3D inspection of LiDAR–IMU motion-pair registration.
|
||||||
|
|
||||||
|
Similar to the RTK–LiDAR ``visualize_pair_3d`` viewer, but A comes from IMU
|
||||||
|
preintegration and X is ``T_IMU_lidar``.
|
||||||
|
|
||||||
|
Modes (keyboard):
|
||||||
|
1 raw source (no transform)
|
||||||
|
2 IMU prediction with X=I (B_pred = A)
|
||||||
|
3 LiDAR registration B (reference)
|
||||||
|
4 calibrated prediction B_pred = X^{-1} A X
|
||||||
|
N / ] next motion pair
|
||||||
|
P / [ previous motion pair
|
||||||
|
Q / Esc exit
|
||||||
|
|
||||||
|
Blue = target keyframe i; orange = source keyframe j after the selected transform.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from imu_lidar.geometry import (
|
||||||
|
inverse_transform,
|
||||||
|
make_transform,
|
||||||
|
rotation_angle_deg,
|
||||||
|
rpy_deg_xyz,
|
||||||
|
transform_points,
|
||||||
|
)
|
||||||
|
from imu_lidar.imu_io import load_imu_samples
|
||||||
|
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||||
|
from imu_lidar.keyframes import build_keyframes
|
||||||
|
from imu_lidar.lidar_io import load_lidar_frames
|
||||||
|
from imu_lidar.motion_pairs import build_motion_pairs
|
||||||
|
from imu_lidar.registration import register_lidar_pair
|
||||||
|
from imu_lidar.time_offset import lidar_time_to_imu_time
|
||||||
|
|
||||||
|
|
||||||
|
COLORS = {
|
||||||
|
"target": [0.10, 0.65, 1.00],
|
||||||
|
"source": [1.00, 0.35, 0.05],
|
||||||
|
}
|
||||||
|
|
||||||
|
MODE_NAMES = (
|
||||||
|
"1 raw",
|
||||||
|
"2 IMU (X=I)",
|
||||||
|
"3 LiDAR B",
|
||||||
|
"4 calibrated X^-1 A X",
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray]:
|
||||||
|
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||||
|
t_block = summary.get("T_IMU_lidar")
|
||||||
|
if t_block is None:
|
||||||
|
matrix = summary.get("matrix")
|
||||||
|
if matrix is not None:
|
||||||
|
return np.asarray(matrix, dtype=float), 0.0, np.zeros(3)
|
||||||
|
raise ValueError(f"no T_IMU_lidar in {summary_path}")
|
||||||
|
t_mat = np.asarray(t_block["matrix"], dtype=float)
|
||||||
|
dt = float(summary.get("time_offset_s") or 0.0)
|
||||||
|
session = (summary.get("details") or {}).get("sessions", [{}])[0]
|
||||||
|
bias = np.asarray(
|
||||||
|
(session.get("imu_audit") or {}).get("gyro_bias_rad_s")
|
||||||
|
or (session.get("joint") or {}).get("gyro_bias_rad_s")
|
||||||
|
or [0.0, 0.0, 0.0],
|
||||||
|
dtype=float,
|
||||||
|
).reshape(3)
|
||||||
|
return t_mat, dt, bias
|
||||||
|
|
||||||
|
|
||||||
|
def _delta_components(reference: np.ndarray, candidate: np.ndarray) -> dict:
|
||||||
|
delta = inverse_transform(reference) @ candidate
|
||||||
|
translation = np.asarray(delta[:3, 3], dtype=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_xyz(delta[:3, :3]).tolist(),
|
||||||
|
"rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _print_delta(name: str, reference: np.ndarray, candidate: np.ndarray) -> dict:
|
||||||
|
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 _cloud(o3d, points: np.ndarray, color, voxel: float):
|
||||||
|
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, points: np.ndarray, color, voxel: float, o3d) -> None:
|
||||||
|
tmp = _cloud(o3d, points, color, voxel)
|
||||||
|
cloud.points = tmp.points
|
||||||
|
cloud.colors = tmp.colors
|
||||||
|
|
||||||
|
|
||||||
|
def _build_pair_list(
|
||||||
|
*,
|
||||||
|
lidar_dir: Path,
|
||||||
|
imu_path: Path,
|
||||||
|
delta_t_s: float,
|
||||||
|
gyro_bias: np.ndarray,
|
||||||
|
min_rotation_deg: float,
|
||||||
|
min_translation_m: float,
|
||||||
|
):
|
||||||
|
frames = load_lidar_frames(lidar_dir)
|
||||||
|
imu = load_imu_samples(imu_path)
|
||||||
|
keyframes = build_keyframes(
|
||||||
|
frames,
|
||||||
|
min_translation_m=min_translation_m,
|
||||||
|
min_rotation_deg=min_rotation_deg,
|
||||||
|
)
|
||||||
|
pair_set = build_motion_pairs(
|
||||||
|
session_id="viz",
|
||||||
|
keyframes=list(keyframes.frames),
|
||||||
|
keyframe_indices=keyframes.indices,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=delta_t_s,
|
||||||
|
gyro_bias_rad_s=gyro_bias,
|
||||||
|
min_rotation_deg=min_rotation_deg,
|
||||||
|
min_translation_m=min_translation_m,
|
||||||
|
)
|
||||||
|
return frames, imu, keyframes, pair_set
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_from_indices(
|
||||||
|
frames,
|
||||||
|
imu,
|
||||||
|
*,
|
||||||
|
i: int,
|
||||||
|
j: int,
|
||||||
|
delta_t_s: float,
|
||||||
|
gyro_bias: np.ndarray,
|
||||||
|
):
|
||||||
|
frame_i = frames[i]
|
||||||
|
frame_j = frames[j]
|
||||||
|
reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
|
||||||
|
t_i = lidar_time_to_imu_time(frame_i.t_mid_s, delta_t_s)
|
||||||
|
t_j = lidar_time_to_imu_time(frame_j.t_mid_s, delta_t_s)
|
||||||
|
preint = preintegrate_imu(
|
||||||
|
imu.t_s,
|
||||||
|
imu.gyro_rad_s,
|
||||||
|
imu.acc_m_s2,
|
||||||
|
t_i,
|
||||||
|
t_j,
|
||||||
|
gyro_bias,
|
||||||
|
np.zeros(3),
|
||||||
|
)
|
||||||
|
a = make_transform(preint.delta_p, preint.delta_R)
|
||||||
|
return frame_i, frame_j, a, reg.transform
|
||||||
|
|
||||||
|
|
||||||
|
def _transforms_for_pair(x: np.ndarray, a_ij: np.ndarray, b_gicp: np.ndarray) -> dict[str, np.ndarray]:
|
||||||
|
return {
|
||||||
|
MODE_NAMES[0]: np.eye(4),
|
||||||
|
MODE_NAMES[1]: a_ij.copy(),
|
||||||
|
MODE_NAMES[2]: b_gicp.copy(),
|
||||||
|
MODE_NAMES[3]: inverse_transform(x) @ a_ij @ x,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray):
|
||||||
|
pair = pairs[pair_index]
|
||||||
|
frame_i = frames[pair.i]
|
||||||
|
frame_j = frames[pair.j]
|
||||||
|
a_ij = make_transform(
|
||||||
|
pair.t_A_m if pair.t_A_m is not None else np.zeros(3),
|
||||||
|
pair.R_A,
|
||||||
|
)
|
||||||
|
b_gicp = make_transform(
|
||||||
|
pair.t_B_m if pair.t_B_m is not None else np.zeros(3),
|
||||||
|
pair.R_B,
|
||||||
|
)
|
||||||
|
transforms = _transforms_for_pair(x, a_ij, b_gicp)
|
||||||
|
label = (
|
||||||
|
f"pair {pair_index + 1}/{len(pairs)} "
|
||||||
|
f"frames {pair.i} <- {pair.j} "
|
||||||
|
f"rotB={rotation_angle_deg(pair.R_B):.2f} deg "
|
||||||
|
f"|tB|={0.0 if pair.t_B_m is None else float(np.linalg.norm(pair.t_B_m)):.3f} m"
|
||||||
|
)
|
||||||
|
return frame_i, frame_j, a_ij, b_gicp, transforms, label
|
||||||
|
|
||||||
|
|
||||||
|
def _print_pair_header(label: str, b_gicp: np.ndarray, transforms: dict[str, np.ndarray]) -> None:
|
||||||
|
print("-" * 72)
|
||||||
|
print(label)
|
||||||
|
print("blue=target i | orange=source j")
|
||||||
|
print("1-4: overlay mode | N/]: next pair | P/[: prev pair | Q/Esc: exit")
|
||||||
|
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.")
|
||||||
|
|
||||||
|
|
||||||
|
def _save_topdown_png(
|
||||||
|
path: Path,
|
||||||
|
target: np.ndarray,
|
||||||
|
source: np.ndarray,
|
||||||
|
transforms: dict[str, np.ndarray],
|
||||||
|
) -> None:
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
def downsample(points: np.ndarray) -> np.ndarray:
|
||||||
|
if points.shape[0] <= 8000:
|
||||||
|
return points
|
||||||
|
idx = np.linspace(0, points.shape[0] - 1, 8000).astype(int)
|
||||||
|
return points[idx]
|
||||||
|
|
||||||
|
names = list(transforms.keys())
|
||||||
|
fig, axes = plt.subplots(2, 2, figsize=(10, 10), sharex=True, sharey=True)
|
||||||
|
tgt = downsample(target)
|
||||||
|
for ax, name in zip(axes.ravel(), names):
|
||||||
|
src = downsample(transform_points(source, transforms[name]))
|
||||||
|
ax.scatter(tgt[:, 0], tgt[:, 1], s=1, c="tab:blue", alpha=0.35, label="target i")
|
||||||
|
ax.scatter(src[:, 0], src[:, 1], s=1, c="tab:orange", alpha=0.35, label="source j")
|
||||||
|
ax.set_title(name)
|
||||||
|
ax.set_aspect("equal", adjustable="box")
|
||||||
|
ax.grid(alpha=0.3)
|
||||||
|
axes[0, 0].legend(loc="upper right", markerscale=4)
|
||||||
|
fig.suptitle("LiDAR–IMU pair overlay (XY top-down)")
|
||||||
|
fig.tight_layout()
|
||||||
|
path.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
fig.savefig(path, dpi=140)
|
||||||
|
plt.close(fig)
|
||||||
|
print(f"wrote {path}")
|
||||||
|
|
||||||
|
|
||||||
|
def _run_gui(
|
||||||
|
*,
|
||||||
|
frames,
|
||||||
|
pairs,
|
||||||
|
x: np.ndarray,
|
||||||
|
start_index: int,
|
||||||
|
voxel: float,
|
||||||
|
fixed_single_pair: tuple | None,
|
||||||
|
) -> None:
|
||||||
|
import open3d as o3d
|
||||||
|
|
||||||
|
if fixed_single_pair is not None:
|
||||||
|
frame_i, frame_j, a_ij, b_gicp = fixed_single_pair
|
||||||
|
transforms = _transforms_for_pair(x, a_ij, b_gicp)
|
||||||
|
label = f"fixed frames (no pair switching)"
|
||||||
|
pair_index = 0
|
||||||
|
n_pairs = 1
|
||||||
|
use_list = False
|
||||||
|
else:
|
||||||
|
pair_index = int(np.clip(start_index, 0, len(pairs) - 1))
|
||||||
|
n_pairs = len(pairs)
|
||||||
|
use_list = True
|
||||||
|
frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
|
||||||
|
frames, pairs, pair_index, x
|
||||||
|
)
|
||||||
|
|
||||||
|
viewer = o3d.visualization.VisualizerWithKeyCallback()
|
||||||
|
viewer.create_window("LiDAR–IMU registration inspection", 1400, 900)
|
||||||
|
target_cloud = _cloud(o3d, frame_i.points_xyz, COLORS["target"], voxel)
|
||||||
|
source_cloud = _cloud(o3d, frame_j.points_xyz, COLORS["source"], 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,
|
||||||
|
"frame_i": frame_i,
|
||||||
|
"frame_j": frame_j,
|
||||||
|
}
|
||||||
|
|
||||||
|
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:
|
||||||
|
if not use_list:
|
||||||
|
print("pair switching disabled in --frame-i/--frame-j mode")
|
||||||
|
return
|
||||||
|
new_index = int(new_index) % n_pairs
|
||||||
|
frame_i, frame_j, _a, b_gicp, transforms, label = _resolve_pair(
|
||||||
|
frames, pairs, new_index, x
|
||||||
|
)
|
||||||
|
state["pair_index"] = new_index
|
||||||
|
state["transforms"] = transforms
|
||||||
|
state["b_gicp"] = b_gicp
|
||||||
|
state["frame_i"] = frame_i
|
||||||
|
state["frame_j"] = frame_j
|
||||||
|
state["current"] = np.eye(4)
|
||||||
|
_set_cloud_points(target_cloud, frame_i.points_xyz, COLORS["target"], voxel, o3d)
|
||||||
|
_set_cloud_points(source_cloud, frame_j.points_xyz, COLORS["source"], voxel, o3d)
|
||||||
|
vis.update_geometry(target_cloud)
|
||||||
|
vis.update_geometry(source_cloud)
|
||||||
|
_print_pair_header(label, b_gicp, transforms)
|
||||||
|
apply_mode(vis, state["mode_name"], announce=True)
|
||||||
|
|
||||||
|
def make_mode_cb(mode_name: str):
|
||||||
|
def callback(vis):
|
||||||
|
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)
|
||||||
|
for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4")), MODE_NAMES):
|
||||||
|
viewer.register_key_callback(key, make_mode_cb(name))
|
||||||
|
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()
|
||||||
|
|
||||||
|
|
||||||
|
def main(argv: list[str] | None = None) -> int:
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument("--lidar", required=True, type=Path, help="LiDAR session directory")
|
||||||
|
parser.add_argument("--imu", required=True, type=Path, help="IMU CSV")
|
||||||
|
parser.add_argument(
|
||||||
|
"--summary",
|
||||||
|
required=True,
|
||||||
|
type=Path,
|
||||||
|
help="summary.json (or T_IMU_lidar.json) from a calibration run",
|
||||||
|
)
|
||||||
|
parser.add_argument("--pair-index", type=int, default=0, help="Starting motion-pair index")
|
||||||
|
parser.add_argument("--frame-i", type=int, default=None, help="Optional explicit frame index i")
|
||||||
|
parser.add_argument("--frame-j", type=int, default=None, help="Optional explicit frame index j")
|
||||||
|
parser.add_argument("--voxel", type=float, default=0.12)
|
||||||
|
parser.add_argument("--min-pair-rotation-deg", type=float, default=3.0)
|
||||||
|
parser.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
||||||
|
parser.add_argument(
|
||||||
|
"--save-png",
|
||||||
|
type=Path,
|
||||||
|
default=None,
|
||||||
|
help="Write a 2x2 XY top-down comparison PNG for the starting pair",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--no-gui",
|
||||||
|
action="store_true",
|
||||||
|
help="Skip Open3D window (use with --save-png)",
|
||||||
|
)
|
||||||
|
args = parser.parse_args(argv)
|
||||||
|
|
||||||
|
x, delta_t_s, gyro_bias = _load_extrinsic(args.summary)
|
||||||
|
frames, imu, keyframes, pair_set = _build_pair_list(
|
||||||
|
lidar_dir=args.lidar,
|
||||||
|
imu_path=args.imu,
|
||||||
|
delta_t_s=delta_t_s,
|
||||||
|
gyro_bias=gyro_bias,
|
||||||
|
min_rotation_deg=args.min_pair_rotation_deg,
|
||||||
|
min_translation_m=args.min_pair_translation_m,
|
||||||
|
)
|
||||||
|
|
||||||
|
fixed_single_pair = None
|
||||||
|
if args.frame_i is not None and args.frame_j is not None:
|
||||||
|
frame_i, frame_j, a_ij, b_gicp = _pair_from_indices(
|
||||||
|
frames,
|
||||||
|
imu,
|
||||||
|
i=args.frame_i,
|
||||||
|
j=args.frame_j,
|
||||||
|
delta_t_s=delta_t_s,
|
||||||
|
gyro_bias=gyro_bias,
|
||||||
|
)
|
||||||
|
transforms = _transforms_for_pair(x, a_ij, b_gicp)
|
||||||
|
label = f"frames {args.frame_i} <- {args.frame_j}"
|
||||||
|
fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp)
|
||||||
|
pairs = ()
|
||||||
|
else:
|
||||||
|
if not pair_set.pairs:
|
||||||
|
raise SystemExit("no motion pairs rebuilt; loosen min-pair thresholds or check data")
|
||||||
|
if not 0 <= args.pair_index < len(pair_set.pairs):
|
||||||
|
raise SystemExit(
|
||||||
|
f"pair-index {args.pair_index} outside [0, {len(pair_set.pairs) - 1}] "
|
||||||
|
f"({len(pair_set.pairs)} pairs available)"
|
||||||
|
)
|
||||||
|
pairs = pair_set.pairs
|
||||||
|
frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
|
||||||
|
frames, pairs, args.pair_index, x
|
||||||
|
)
|
||||||
|
print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes")
|
||||||
|
|
||||||
|
if args.save_png is not None:
|
||||||
|
_print_pair_header(label, b_gicp, transforms)
|
||||||
|
_save_topdown_png(args.save_png, frame_i.points_xyz, frame_j.points_xyz, transforms)
|
||||||
|
|
||||||
|
if args.no_gui:
|
||||||
|
return 0
|
||||||
|
|
||||||
|
try:
|
||||||
|
import open3d # noqa: F401
|
||||||
|
except ImportError as exc:
|
||||||
|
raise SystemExit(
|
||||||
|
"Open3D is required for interactive view. "
|
||||||
|
"Install with: python -m pip install -e \".[open3d]\" "
|
||||||
|
"or use --no-gui --save-png out.png"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
_run_gui(
|
||||||
|
frames=frames,
|
||||||
|
pairs=pairs,
|
||||||
|
x=x,
|
||||||
|
start_index=args.pair_index,
|
||||||
|
voxel=args.voxel,
|
||||||
|
fixed_single_pair=fixed_single_pair,
|
||||||
|
)
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
Reference in New Issue
Block a user