Compare commits
16
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 lichun.qu
|
0ec0f40406 | ||
|
lichun.qu
|
32d8af8ed9 | ||
|
lichun.qu
|
d14ae74117 | ||
|
lichun.qu
|
c2da6dd192 | ||
|
lichun.qu
|
1233f8aafd | ||
 lichun.quandCursor
|
5ac50ad71f | ||
|
lichun.qu
|
3b3790ca2d | ||
|
lichun.quandCursor
|
6b44a495fb | ||
|
lichun.quandCursor
|
03fcee7e32 | ||
|
lichun.quandCursor
|
c2f99b94a2 | ||
|
lichun.quandCursor
|
2237be77a4 | ||
|
lichun.quandCursor
|
30f7e66db3 | ||
|
lichun.quandCursor
|
4ff176d184 | ||
|
lichun.quandCursor
|
ea06a3a523 | ||
|
lichun.quandCursor
|
e50a79b114 | ||
|
lichun.quandCursor
|
cf1fad7594 |
+27
-1
@@ -1,3 +1,29 @@
|
|||||||
__pycache__/
|
__pycache__/
|
||||||
*.pyc
|
*.py[cod]
|
||||||
|
.pytest_cache/
|
||||||
|
*.egg-info/
|
||||||
|
.eggs/
|
||||||
|
dist/
|
||||||
|
build/
|
||||||
|
examples/synthetic_session/
|
||||||
.venv/
|
.venv/
|
||||||
|
venv/
|
||||||
|
|
||||||
|
# Local IDE state
|
||||||
|
.vs/
|
||||||
|
|
||||||
|
# RTK-IMU calibration process artifacts stay local. Keep only the reviewed
|
||||||
|
# V3 result bundle explicitly listed below under version control.
|
||||||
|
artifacts/rtk_imu_calibration_v2/
|
||||||
|
artifacts/rtk_imu_calibration_v3/*
|
||||||
|
!artifacts/rtk_imu_calibration_v3/README.md
|
||||||
|
!artifacts/rtk_imu_calibration_v3/engineering_release_decision.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/heldout_nonconverged_retry.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection_refined.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/node_graph_free_information_selected_mechanical.json
|
||||||
|
!artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json
|
||||||
|
|||||||
@@ -1,12 +0,0 @@
|
|||||||
# 最终推荐值补充说明
|
|
||||||
|
|
||||||
两套后端各自结果均保留。最终推荐值又增加了一层不依赖外参X的交叉检查：只有同一运动对的Open3D B与small_gicp B相差不超过5 cm、0.5°时才进入最终求解；最终B数为39对。
|
|
||||||
|
|
||||||
最终推荐：
|
|
||||||
|
|
||||||
- 平移 `[1.297760, -0.000067, 0.720498] m`
|
|
||||||
- RPY `[-0.785151, 1.202661, -0.835510] deg`
|
|
||||||
- 第二批AX RMS：`0.07985 m / 0.96118°`
|
|
||||||
- 第一批22对辅助复核：`0.06067 m / 1.00298°`
|
|
||||||
|
|
||||||
完整结果见 `results/final_extrinsic_recommended.json`，选择摘要见 `results/final_summary.json`。执行完整 `run_all.ps1` 后，再执行 `run_consensus_finish.ps1` 可重建最终推荐结果。
|
|
||||||
@@ -1,194 +0,0 @@
|
|||||||
木兰宽松许可证，第2版
|
|
||||||
|
|
||||||
木兰宽松许可证，第2版
|
|
||||||
|
|
||||||
2020年1月 http://license.coscl.org.cn/MulanPSL2
|
|
||||||
|
|
||||||
您对“软件”的复制、使用、修改及分发受木兰宽松许可证，第2版（“本许可证”）的如下条款的约束：
|
|
||||||
|
|
||||||
0. 定义
|
|
||||||
|
|
||||||
“软件” 是指由“贡献”构成的许可在“本许可证”下的程序和相关文档的集合。
|
|
||||||
|
|
||||||
“贡献” 是指由任一“贡献者”许可在“本许可证”下的受版权法保护的作品。
|
|
||||||
|
|
||||||
“贡献者” 是指将受版权法保护的作品许可在“本许可证”下的自然人或“法人实体”。
|
|
||||||
|
|
||||||
“法人实体” 是指提交贡献的机构及其“关联实体”。
|
|
||||||
|
|
||||||
“关联实体” 是指，对“本许可证”下的行为方而言，控制、受控制或与其共同受控制的机构，此处的控制是
|
|
||||||
指有受控方或共同受控方至少50%直接或间接的投票权、资金或其他有价证券。
|
|
||||||
|
|
||||||
1. 授予版权许可
|
|
||||||
|
|
||||||
每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的版权许可，您可
|
|
||||||
以复制、使用、修改、分发其“贡献”，不论修改与否。
|
|
||||||
|
|
||||||
2. 授予专利许可
|
|
||||||
|
|
||||||
每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的（根据本条规定
|
|
||||||
撤销除外）专利许可，供您制造、委托制造、使用、许诺销售、销售、进口其“贡献”或以其他方式转移其“贡
|
|
||||||
献”。前述专利许可仅限于“贡献者”现在或将来拥有或控制的其“贡献”本身或其“贡献”与许可“贡献”时的“软
|
|
||||||
件”结合而将必然会侵犯的专利权利要求，不包括对“贡献”的修改或包含“贡献”的其他结合。如果您或您的“
|
|
||||||
关联实体”直接或间接地，就“软件”或其中的“贡献”对任何人发起专利侵权诉讼（包括反诉或交叉诉讼）或
|
|
||||||
其他专利维权行动，指控其侵犯专利权，则“本许可证”授予您对“软件”的专利许可自您提起诉讼或发起维权
|
|
||||||
行动之日终止。
|
|
||||||
|
|
||||||
3. 无商标许可
|
|
||||||
|
|
||||||
“本许可证”不提供对“贡献者”的商品名称、商标、服务标志或产品名称的商标许可，但您为满足第4条规定
|
|
||||||
的声明义务而必须使用除外。
|
|
||||||
|
|
||||||
4. 分发限制
|
|
||||||
|
|
||||||
您可以在任何媒介中将“软件”以源程序形式或可执行形式重新分发，不论修改与否，但您必须向接收者提供“
|
|
||||||
本许可证”的副本，并保留“软件”中的版权、商标、专利及免责声明。
|
|
||||||
|
|
||||||
5. 免责声明与责任限制
|
|
||||||
|
|
||||||
“软件”及其中的“贡献”在提供时不带任何明示或默示的担保。在任何情况下，“贡献者”或版权所有者不对
|
|
||||||
任何人因使用“软件”或其中的“贡献”而引发的任何直接或间接损失承担责任，不论因何种原因导致或者基于
|
|
||||||
何种法律理论，即使其曾被建议有此种损失的可能性。
|
|
||||||
|
|
||||||
6. 语言
|
|
||||||
|
|
||||||
“本许可证”以中英文双语表述，中英文版本具有同等法律效力。如果中英文版本存在任何冲突不一致，以中文
|
|
||||||
版为准。
|
|
||||||
|
|
||||||
条款结束
|
|
||||||
|
|
||||||
如何将木兰宽松许可证，第2版，应用到您的软件
|
|
||||||
|
|
||||||
如果您希望将木兰宽松许可证，第2版，应用到您的新软件，为了方便接收者查阅，建议您完成如下三步：
|
|
||||||
|
|
||||||
1， 请您补充如下声明中的空白，包括软件名、软件的首次发表年份以及您作为版权人的名字；
|
|
||||||
|
|
||||||
2， 请您在软件包的一级目录下创建以“LICENSE”为名的文件，将整个许可证文本放入该文件中；
|
|
||||||
|
|
||||||
3， 请将如下声明文本放入每个源文件的头部注释中。
|
|
||||||
|
|
||||||
Copyright (c) [Year] [name of copyright holder]
|
|
||||||
[Software Name] is licensed under Mulan PSL v2.
|
|
||||||
You can use this software according to the terms and conditions of the Mulan
|
|
||||||
PSL v2.
|
|
||||||
You may obtain a copy of Mulan PSL v2 at:
|
|
||||||
http://license.coscl.org.cn/MulanPSL2
|
|
||||||
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY
|
|
||||||
KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO
|
|
||||||
NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
|
|
||||||
See the Mulan PSL v2 for more details.
|
|
||||||
|
|
||||||
Mulan Permissive Software License，Version 2
|
|
||||||
|
|
||||||
Mulan Permissive Software License，Version 2 (Mulan PSL v2)
|
|
||||||
|
|
||||||
January 2020 http://license.coscl.org.cn/MulanPSL2
|
|
||||||
|
|
||||||
Your reproduction, use, modification and distribution of the Software shall
|
|
||||||
be subject to Mulan PSL v2 (this License) with the following terms and
|
|
||||||
conditions:
|
|
||||||
|
|
||||||
0. Definition
|
|
||||||
|
|
||||||
Software means the program and related documents which are licensed under
|
|
||||||
this License and comprise all Contribution(s).
|
|
||||||
|
|
||||||
Contribution means the copyrightable work licensed by a particular
|
|
||||||
Contributor under this License.
|
|
||||||
|
|
||||||
Contributor means the Individual or Legal Entity who licenses its
|
|
||||||
copyrightable work under this License.
|
|
||||||
|
|
||||||
Legal Entity means the entity making a Contribution and all its
|
|
||||||
Affiliates.
|
|
||||||
|
|
||||||
Affiliates means entities that control, are controlled by, or are under
|
|
||||||
common control with the acting entity under this License, ‘control’ means
|
|
||||||
direct or indirect ownership of at least fifty percent (50%) of the voting
|
|
||||||
power, capital or other securities of controlled or commonly controlled
|
|
||||||
entity.
|
|
||||||
|
|
||||||
1. Grant of Copyright License
|
|
||||||
|
|
||||||
Subject to the terms and conditions of this License, each Contributor hereby
|
|
||||||
grants to you a perpetual, worldwide, royalty-free, non-exclusive,
|
|
||||||
irrevocable copyright license to reproduce, use, modify, or distribute its
|
|
||||||
Contribution, with modification or not.
|
|
||||||
|
|
||||||
2. Grant of Patent License
|
|
||||||
|
|
||||||
Subject to the terms and conditions of this License, each Contributor hereby
|
|
||||||
grants to you a perpetual, worldwide, royalty-free, non-exclusive,
|
|
||||||
irrevocable (except for revocation under this Section) patent license to
|
|
||||||
make, have made, use, offer for sale, sell, import or otherwise transfer its
|
|
||||||
Contribution, where such patent license is only limited to the patent claims
|
|
||||||
owned or controlled by such Contributor now or in future which will be
|
|
||||||
necessarily infringed by its Contribution alone, or by combination of the
|
|
||||||
Contribution with the Software to which the Contribution was contributed.
|
|
||||||
The patent license shall not apply to any modification of the Contribution,
|
|
||||||
and any other combination which includes the Contribution. If you or your
|
|
||||||
Affiliates directly or indirectly institute patent litigation (including a
|
|
||||||
cross claim or counterclaim in a litigation) or other patent enforcement
|
|
||||||
activities against any individual or entity by alleging that the Software or
|
|
||||||
any Contribution in it infringes patents, then any patent license granted to
|
|
||||||
you under this License for the Software shall terminate as of the date such
|
|
||||||
litigation or activity is filed or taken.
|
|
||||||
|
|
||||||
3. No Trademark License
|
|
||||||
|
|
||||||
No trademark license is granted to use the trade names, trademarks, service
|
|
||||||
marks, or product names of Contributor, except as required to fulfill notice
|
|
||||||
requirements in section 4.
|
|
||||||
|
|
||||||
4. Distribution Restriction
|
|
||||||
|
|
||||||
You may distribute the Software in any medium with or without modification,
|
|
||||||
whether in source or executable forms, provided that you provide recipients
|
|
||||||
with a copy of this License and retain copyright, patent, trademark and
|
|
||||||
disclaimer statements in the Software.
|
|
||||||
|
|
||||||
5. Disclaimer of Warranty and Limitation of Liability
|
|
||||||
|
|
||||||
THE SOFTWARE AND CONTRIBUTION IN IT ARE PROVIDED WITHOUT WARRANTIES OF ANY
|
|
||||||
KIND, EITHER EXPRESS OR IMPLIED. IN NO EVENT SHALL ANY CONTRIBUTOR OR
|
|
||||||
COPYRIGHT HOLDER BE LIABLE TO YOU FOR ANY DAMAGES, INCLUDING, BUT NOT
|
|
||||||
LIMITED TO ANY DIRECT, OR INDIRECT, SPECIAL OR CONSEQUENTIAL DAMAGES ARISING
|
|
||||||
FROM YOUR USE OR INABILITY TO USE THE SOFTWARE OR THE CONTRIBUTION IN IT, NO
|
|
||||||
MATTER HOW IT’S CAUSED OR BASED ON WHICH LEGAL THEORY, EVEN IF ADVISED OF
|
|
||||||
THE POSSIBILITY OF SUCH DAMAGES.
|
|
||||||
|
|
||||||
6. Language
|
|
||||||
|
|
||||||
THIS LICENSE IS WRITTEN IN BOTH CHINESE AND ENGLISH, AND THE CHINESE VERSION
|
|
||||||
AND ENGLISH VERSION SHALL HAVE THE SAME LEGAL EFFECT. IN THE CASE OF
|
|
||||||
DIVERGENCE BETWEEN THE CHINESE AND ENGLISH VERSIONS, THE CHINESE VERSION
|
|
||||||
SHALL PREVAIL.
|
|
||||||
|
|
||||||
END OF THE TERMS AND CONDITIONS
|
|
||||||
|
|
||||||
How to Apply the Mulan Permissive Software License，Version 2
|
|
||||||
(Mulan PSL v2) to Your Software
|
|
||||||
|
|
||||||
To apply the Mulan PSL v2 to your work, for easy identification by
|
|
||||||
recipients, you are suggested to complete following three steps:
|
|
||||||
|
|
||||||
i. Fill in the blanks in following statement, including insert your software
|
|
||||||
name, the year of the first publication of your software, and your name
|
|
||||||
identified as the copyright owner;
|
|
||||||
|
|
||||||
ii. Create a file named "LICENSE" which contains the whole context of this
|
|
||||||
License in the first directory of your software package;
|
|
||||||
|
|
||||||
iii. Attach the statement to the appropriate annotated syntax at the
|
|
||||||
beginning of each source file.
|
|
||||||
|
|
||||||
Copyright (c) [Year] [name of copyright holder]
|
|
||||||
[Software Name] is licensed under Mulan PSL v2.
|
|
||||||
You can use this software according to the terms and conditions of the Mulan
|
|
||||||
PSL v2.
|
|
||||||
You may obtain a copy of Mulan PSL v2 at:
|
|
||||||
http://license.coscl.org.cn/MulanPSL2
|
|
||||||
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY
|
|
||||||
KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO
|
|
||||||
NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
|
|
||||||
See the Mulan PSL v2 for more details.
|
|
||||||
@@ -1,85 +0,0 @@
|
|||||||
# Pair 局部诊断与全局灵敏度扫描
|
|
||||||
|
|
||||||
本说明补充主 README。所有变换均采用：
|
|
||||||
|
|
||||||
```text
|
|
||||||
X = T_body_lidar
|
|
||||||
A_ij X = X B_ij
|
|
||||||
B_ij = T_Li_Lj（把站点 j 点云变到站点 i）
|
|
||||||
```
|
|
||||||
|
|
||||||
模式 4 相对模式 3 的数值差定义为：
|
|
||||||
|
|
||||||
```text
|
|
||||||
Delta_ij = B_ij^-1 * (X^-1 * A_ij * X)
|
|
||||||
```
|
|
||||||
|
|
||||||
打印的平移 xyz 和旋转 RPY 是 `Delta_ij` 在站点 j 雷达局部坐标系中的分量，不是屏幕坐标。3D 相机视角会改变画面中的“横向”，所以不能仅凭屏幕左右判断车体系 Y 或 yaw。
|
|
||||||
|
|
||||||
## Pair 0 当前诊断
|
|
||||||
|
|
||||||
Open3D 精筛 Pair 0（station 0 <- 1）的当前结果为：
|
|
||||||
|
|
||||||
```text
|
|
||||||
translation xyz = [-1.2535, +1.4157, +7.7441] cm
|
|
||||||
rotation RPY xyz = [-0.5355, -0.1093, +0.0247] deg
|
|
||||||
norm = 7.9716 cm / 0.547109 deg
|
|
||||||
```
|
|
||||||
|
|
||||||
因此这对主要表现为 roll/pitch 相关姿态差和相对 Z 差，yaw 仅约 `0.025 deg`，不应优先调 yaw。
|
|
||||||
|
|
||||||
## 可视化试验
|
|
||||||
|
|
||||||
查看原结果：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
$Repo = "D:\你的代码目录\calibration"
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_open3d_result.ps1" -PairIndex 0
|
|
||||||
```
|
|
||||||
|
|
||||||
试验车体系左乘 pitch `+0.2 deg`：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_open3d_result.ps1" -PairIndex 0 -LeftPitchDeg 0.2
|
|
||||||
```
|
|
||||||
|
|
||||||
此时按键含义：`3` 为 GICP 的 B，`4` 为当前最终 X 预测，`5` 为试验修正后的 X 预测。试验使用：
|
|
||||||
|
|
||||||
```text
|
|
||||||
X_test = DeltaR_body * X
|
|
||||||
```
|
|
||||||
|
|
||||||
这是真正的车体系左乘，会同时旋转 X 的旋转部分和平移向量；小角度下才近似等价于直接给 JSON 的 pitch 加相同角度。
|
|
||||||
|
|
||||||
## 全部运动对扫描
|
|
||||||
|
|
||||||
运行：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_sensitivity_scan.ps1" -PairIndex 0
|
|
||||||
```
|
|
||||||
|
|
||||||
程序扫描：
|
|
||||||
|
|
||||||
- pitch：`+0.1/+0.2/+0.3 deg`；
|
|
||||||
- 在这些 pitch 及零 pitch 附近扫描 roll：`-0.2/-0.1/+0.1 deg`；
|
|
||||||
- yaw `-0.2/+0.2 deg` 只作为低灵敏度对照；
|
|
||||||
- Open3D 全部精筛对、small_gicp 全部精筛对、跨后端共识对分别计算；
|
|
||||||
- 同时报告地面法向与高度残差；
|
|
||||||
- 使用 `0.05 m / 0.5 deg` 作为透明的归一化尺度，统计全局 RMS、改善对数和恶化对数。
|
|
||||||
|
|
||||||
输出位于 `results/diagnostics/`。JSON 保存逐对结果，CSV 便于横向比较。
|
|
||||||
|
|
||||||
## 接受规则
|
|
||||||
|
|
||||||
局部扫描只用于定位，程序不会覆盖 `final_extrinsic_recommended.json`。候选至少需要满足：
|
|
||||||
|
|
||||||
1. 不能只改善 Pair 0；全部精筛对的归一化 RMS 应下降；
|
|
||||||
2. 改善的运动对数量应多于恶化数量；
|
|
||||||
3. Open3D、small_gicp 与共识集合应给出相同方向的趋势；
|
|
||||||
4. 地面法向和高度约束不能明显恶化；
|
|
||||||
5. 多个代表性运动对的模式 4/5 可视化应同步改善。
|
|
||||||
|
|
||||||
如果只有 Pair 0 改善，应把它视为局部配准或场景问题，不修改全局 X。
|
|
||||||
|
|
||||||
对于当前仅含 yaw 的 RTK A，车体系 Z 轴平移在 AX=XB 中不可观。扫描程序会用 `X.z + 0.10 m` 做数值检查，但不会扫描或修改 z；z 必须由地面高度约束或外部量测确定。
|
|
||||||
@@ -1,226 +1,184 @@
|
|||||||
# LiDAR–双天线 RTK 手眼标定
|
# LiDAR–IMU 外参标定
|
||||||
|
|
||||||
本仓库提供一套可从原始 Medulla 记录复现的静态站点标定流程，求解三维激光雷达到后轮轴中心车体系的外参
|
用连续行驶中的 LiDAR 与 IMU 相对运动，估计安装外参与时间偏置：
|
||||||
|
|
||||||
```text
|
```text
|
||||||
X = T_body_lidar
|
p_IMU = T_IMU_lidar · p_lidar
|
||||||
```
|
```
|
||||||
|
|
||||||
约定 `T_A_B` 将 B 系坐标变换到 A 系。对任意站点 i、j：
|
**当前阶段：** 算法与合成自检已闭环；已提供 `tools/export_rscap_to_v1.py`（N300 `.rscap` + H32 dlog/MSOP → 中间格式）；**合格实车验收尚未完成**，故正式外参尚未对实车落盘交付。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 先看什么（对外三份就够）
|
||||||
|
|
||||||
|
|
||||||
|
| 顺序 | 文档 | 用途 |
|
||||||
|
| --- | -------------------------------------- | -------------------- |
|
||||||
|
| 1 | **本 README** | 做什么、怎么跑、结果怎么判 |
|
||||||
|
| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + 原始数据导出命令 |
|
||||||
|
| 3 | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采（合格数据要求） |
|
||||||
|
|
||||||
|
|
||||||
|
其余（方法细述、测试说明、源码职责、CHANGELOG）给深入阅读 / 改代码时用，见文末。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 1. 输入 / 输出
|
||||||
|
|
||||||
|
|
||||||
|
| 输入 | 说明 |
|
||||||
|
| --------- | ----------------------------------------------------------- |
|
||||||
|
| `imu.csv` | `t,gx,gy,gz,ax,ay,az`（秒；rad/s；m/s²）；`t` 用设备时间 |
|
||||||
|
| 雷达会话目录 | `frames_index.csv` + `frames/*.npz`（米制 XYZ） |
|
||||||
|
| 车辆 YAML | 轴向与时间语义；外参真值可空（`config/vehicle_installation.template.yaml`） |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
| 输出 | 说明 |
|
||||||
|
| ------------------ | ---------------------- |
|
||||||
|
| `T_IMU_lidar.json` | 外参 |
|
||||||
|
| `time_offset.json` | `t_imu = t_lidar + δt` |
|
||||||
|
| `summary.json` | 状态、残差、可观性 |
|
||||||
|
|
||||||
|
|
||||||
|
新车原始数据导出（H32 dlog/zip + HI13 rscap）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\export_rscap_to_v1.py `
|
||||||
|
--imu-rscap path\to\hi13r4-imu.rscap `
|
||||||
|
--imu-kind hi13 `
|
||||||
|
--lidar-dlog path\to\session_or_recovered.zip `
|
||||||
|
--host-start 2026-08-08T17:40:05 `
|
||||||
|
--host-end 2026-08-08T17:45:15 `
|
||||||
|
--out path\to\session_v1 `
|
||||||
|
--require-difop
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2. 一键复现（合成，不需实车）
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
cd <本仓库根目录>
|
||||||
|
python -m pip install -e ".[dev]"
|
||||||
|
python -m pip install -e ".[open3d]" # 推荐
|
||||||
|
powershell -File tools\reproduce_synthetic.ps1
|
||||||
|
```
|
||||||
|
|
||||||
|
证明：链路可跑通，能收回已知 yaw / δt。
|
||||||
|
不证明：实车安装精度、平移可交付。
|
||||||
|
|
||||||
|
产物在 `examples/synthetic_session/out/`（含 `summary.json` 与 `motion_pairs.json`）。叠点查看：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
# 优先读取 summary 同目录的 motion_pairs.json，按需加载点云（无需重算配准）
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar examples\synthetic_session\lidar `
|
||||||
|
--summary examples\synthetic_session\out\summary.json `
|
||||||
|
--pair-index 0
|
||||||
|
```
|
||||||
|
|
||||||
|
旧标定目录若缺少缓存，可只补导出运动对（不重求解外参）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\export_motion_pairs_for_viz.py `
|
||||||
|
--lidar path\to\lidar `
|
||||||
|
--imu path\to\imu.csv `
|
||||||
|
--summary path\to\out\summary.json
|
||||||
|
```
|
||||||
|
|
||||||
|
键 `1`–`4` 切换叠点模式；`N`/`P` 切换运动对。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 3. 真实数据怎么跑
|
||||||
|
|
||||||
|
1. 按采集清单录制（设备时间；静止 + 低速转弯；有结构场景）
|
||||||
|
2. 导出中间格式（第1节命令）
|
||||||
|
3. 填写车辆 YAML 的轴向与时间语义
|
||||||
|
4. 标定：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m imu_lidar.cli run `
|
||||||
|
--vehicle-config config\vehicle_installation.template.yaml `
|
||||||
|
--imu path\to\session_v1\imu.csv `
|
||||||
|
--lidar path\to\session_v1\lidar `
|
||||||
|
--output path\to\out `
|
||||||
|
--mode rotation_only `
|
||||||
|
--time-offset-search-s 2.0
|
||||||
|
```
|
||||||
|
|
||||||
|
1. 看 `summary.json`，再叠点 / 用验证会话复核后才交付
|
||||||
|
|
||||||
|
|
||||||
|
| 模式 | 交付 | 成功标志 |
|
||||||
|
| ------------------- | ------- | --------------------------- |
|
||||||
|
| `rotation_only`（先做） | 旋转 + δt | `rotation_only_accepted` |
|
||||||
|
| `full_se3`（激励够再试） | + 可观平移 | `full_se3_accepted`（否则平移拒绝） |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
| `summary.json` 状态 | 含义 |
|
||||||
|
| ---------------------------------------------- | ----------- |
|
||||||
|
| `rotation_only_accepted` / `full_se3_accepted` | 可进入验证 |
|
||||||
|
| `full_se3_rejected_due_to_observability` | 旋转可用，平移不交 |
|
||||||
|
| `blocked` | **不可作安装参数** |
|
||||||
|
|
||||||
|
|
||||||
|
预期量级：旋转约 0.5°–2°；水平平移数厘米～十几厘米；无坡时竖直常不可观。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 4. 方法（一句话）
|
||||||
|
|
||||||
|
关键帧雷达配准得 **B**，同区间 IMU 预积分得 **A**，解 `R_A R_X ≈ R_X R_B`；再估 δt。可观时才在 `full_se3` 下交平移。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 5. 试验边界（勿误读）
|
||||||
|
|
||||||
|
|
||||||
|
| | 合成 pytest | 旧车 S2 线下 |
|
||||||
|
| ------ | ------------------------ | ----------------- |
|
||||||
|
| 目的 | 回归算法 | 验证旧主机时间数据上链路能跑完 |
|
||||||
|
| 期望 | `rotation_only_accepted` | `blocked`**（预期）** |
|
||||||
|
| 当安装参数？ | 否 | **否** |
|
||||||
|
|
||||||
|
|
||||||
|
细节：[tests/README.md](tests/README.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 6. 仓库结构与其余文档
|
||||||
|
|
||||||
```text
|
```text
|
||||||
A_ij = T_W_Bi^-1 T_W_Bj # RTK 给出的车体相对运动
|
imu_lidar/ 算法与 CLI
|
||||||
B_ij = T_Li_Lj # GICP 给出的雷达相对运动
|
config/ 车辆配置模板
|
||||||
A_ij X = X B_ij
|
docs/ 采集清单、数据格式、方法细述
|
||||||
```
|
tools/ 导出、合成复现、可视化
|
||||||
|
tests/ 自动化测试
|
||||||
当前部署建议仍采用 [results/01_previous_two_batches/final_extrinsic_deployment.json](results/01_previous_two_batches/final_extrinsic_deployment.json)。data4 是一次独立重算，结果与部署值相差约 `1.592 cm / 0.234°`，但自身 AX 残差更高，因此只作为候选和稳定性证据，不自动替换部署值。
|
|
||||||
|
|
||||||
## 标定总流程
|
|
||||||
|
|
||||||
```mermaid
|
|
||||||
flowchart LR
|
|
||||||
raw["原始站点 dlog / RTK、IMU rscap"] --> export["分别解析并统一到时间轴"]
|
|
||||||
export --> assoc["按每个 LiDAR 帧关联 RTK/IMU，导出 NPZ"]
|
|
||||||
assoc --> prep["每站选一帧，构建 RTK 车体位姿 A"]
|
|
||||||
prep --> b1["small_gicp 求 B"]
|
|
||||||
prep --> b2["Open3D GICP 求 B"]
|
|
||||||
b1 --> gate["与 X 无关的质量筛选及双后端一致性"]
|
|
||||||
b2 --> gate
|
|
||||||
gate --> solve["AX=XB + 地面约束求 X"]
|
|
||||||
solve --> check["残差、bootstrap、条件数、跨批复核和 3D 可视化"]
|
|
||||||
```
|
|
||||||
|
|
||||||
流程有两个原始数据入口：
|
|
||||||
|
|
||||||
- 旧式数据：LiDAR 和 `GPS-POST-Z` 位于每个站点 dlog 中，使用 `export_legacy_stations.ps1`。
|
|
||||||
- 新式多传感器数据：LiDAR 位于逐站 dlog，RTK 与 IMU 是独立 `.rscap`，使用 `export_multisensor_stations.ps1`。处理顺序是统一时间轴、分别解析、按 LiDAR 帧关联、导出 NPZ。
|
|
||||||
|
|
||||||
IMU 会在新式数据中原样解析并随 LiDAR 帧关联保存，但当前 LiDAR–RTK 外参求解不使用 IMU，也不做运动畸变校正，因为每一站采集点云时车辆静止。IMU 外参应使用单独的激励数据和专用标定流程求解。
|
|
||||||
|
|
||||||
## 三批数据的角色
|
|
||||||
|
|
||||||
| 数据 | 原始格式 | 站点 | RTK 情况 | 在本仓库中的角色 |
|
|
||||||
|---|---|---:|---|---|
|
|
||||||
| 第一批 | 逐站 dlog，内嵌 GPS-POST-Z | 38 | 约 10 秒一条，部分站仅 1–11 个有效样本 | 辅助复核，不承担主要求解 |
|
|
||||||
| 第二批 | 逐站 dlog，内嵌 GPS-POST-Z | 38 | 每站约 125–412 个有效样本，航向稳定 | 现部署外参的主要求解数据 |
|
|
||||||
| data4 | 逐站 LiDAR dlog + 独立 RTK/IMU rscap | 34 | 11,678 个 LiDAR 帧均成功关联 fixed RTK、heading 和 IMU | 独立重算与跨批比较 |
|
|
||||||
|
|
||||||
原始数据体积较大，不在 Git 仓库中。复现者应从云盘取得第一批、第二批或 data4 的原始目录，并在命令行传入路径。
|
|
||||||
|
|
||||||
## 环境
|
|
||||||
|
|
||||||
- Windows PowerShell 5.1 或 PowerShell 7
|
|
||||||
- Python 3.10+
|
|
||||||
- `pip install -r requirements.txt`
|
|
||||||
- `small_gicp` 后端需要可导入 `small_gicp`；Open3D 后端需要 `open3d`
|
|
||||||
|
|
||||||
所有脚本从自身位置推导仓库根目录。数据和输出路径均由参数传入，不依赖开发者电脑上的固定路径。
|
|
||||||
|
|
||||||
## 从原始数据开始复现
|
|
||||||
|
|
||||||
以下路径只表示格式，请替换为自己的目录。
|
|
||||||
|
|
||||||
### A. 第一批、第二批旧式 dlog
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
$Repo = "D:\你的代码目录\calibration"
|
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_legacy_stations.ps1" `
|
|
||||||
-DataRoot "D:\你的数据目录\batch2_raw" `
|
|
||||||
-OutputRoot "D:\你的输出目录\batch2_export"
|
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_legacy_dataset.ps1" `
|
|
||||||
-ExportRoot "D:\你的输出目录\batch2_export" `
|
|
||||||
-Output "D:\你的输出目录\batch2_prepared" `
|
|
||||||
-HeadingOffsetDeg 21.226 `
|
|
||||||
-AntennaLever -0.320,-0.365,0.620 `
|
|
||||||
-ExpectedStations 38 `
|
|
||||||
-HeadingStdLimitDeg 0.5
|
|
||||||
```
|
|
||||||
|
|
||||||
第一批采用同一导出方式，但应在导出命令显式添加 `-RtkMaxDtMs 15000`；因其 RTK 稀疏，准备阶段也不建议沿用 `0.5°` 的严格站内航向离散度阈值。上述 `21.226°` 和杆臂 `[-0.320,-0.365,0.620] m` 是本项目已有两批数据采用的配置，不是通用常数；换车或改变天线安装后必须重新确认。
|
|
||||||
|
|
||||||
### B. data4 式独立 RTK/IMU rscap
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\export_multisensor_stations.ps1" `
|
|
||||||
-DataRoot "D:\你的数据目录\data4_raw" `
|
|
||||||
-RtkCapture "D:\你的数据目录\captures\rtk.rscap" `
|
|
||||||
-ImuCapture "D:\你的数据目录\captures\imu.rscap" `
|
|
||||||
-OutputRoot "D:\你的输出目录\data4_export"
|
|
||||||
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\prepare_multisensor_dataset.ps1" `
|
|
||||||
-CombinedRoot "D:\你的输出目录\data4_export\combined" `
|
|
||||||
-Output "D:\你的输出目录\data4_prepared" `
|
|
||||||
-HeadingOffsetDeg 21.226 `
|
|
||||||
-AntennaLever -0.320,-0.365,0.620 `
|
|
||||||
-ExpectedStations 34
|
|
||||||
```
|
|
||||||
|
|
||||||
### C. 运行标定
|
|
||||||
|
|
||||||
单批数据同时运行 small_gicp、Open3D GICP 和跨后端共识：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_single_dataset.ps1" `
|
|
||||||
-Prepared "D:\你的输出目录\data4_prepared" `
|
|
||||||
-OutputRoot "D:\你的输出目录\data4_calibration" `
|
|
||||||
-BodyHeight 0.2335
|
|
||||||
```
|
|
||||||
|
|
||||||
复现本仓库“第二批求解、第一批辅助复核”的历史流程：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" `
|
|
||||||
-Batch1Prepared "D:\你的输出目录\batch1_prepared" `
|
|
||||||
-Batch2Prepared "D:\你的输出目录\batch2_prepared" `
|
|
||||||
-OutputRoot "D:\你的输出目录\two_batch_calibration"
|
|
||||||
```
|
|
||||||
|
|
||||||
`BodyHeight=0.2335 m` 是后轮轴中心离地高度，用于把雷达地面平面约束转换到车体原点；它不是雷达离地高度。换轮胎、胎压或车体载荷后应重新测量并评估不确定度。
|
|
||||||
|
|
||||||
## 如何判断结果
|
|
||||||
|
|
||||||
不能仅凭 `AX=XB` 残差或一张叠图宣称外参正确。至少同时检查：
|
|
||||||
|
|
||||||
1. B 的配准质量：收敛、有效对应点数/比例、RMSE、Hessian 信息矩阵特征值与条件数、small_gicp 与 Open3D 的相对运动一致性。
|
|
||||||
2. X 的可观性：加权雅可比条件数越接近 1 越均衡；极大值说明存在弱方向，但没有脱离尺度和数据分布的单一硬阈值。本次 data4 为 `7.44`，不是病态。
|
|
||||||
3. AX 残差分布：看 RMS、median、P90/P95、max 和逐对异常，而不是只看均值。
|
|
||||||
4. 重采样稳定性：bootstrap 的六自由度标准差及置信区间。
|
|
||||||
5. 跨批检查：同一 X 在独立批次的残差，以及两批独立求出的 X 之 SE(3) 差。
|
|
||||||
6. 3D 可视化：模式 3 的 B 与模式 4 的 `X^-1 A X` 是否都使相同墙面、杆件和地面重合。
|
|
||||||
|
|
||||||
当前没有把“增量小于某个值”当成绝对真值判据。工程筛查可先关注多数优质运动对是否约在厘米级到数厘米、亚度级；但阈值必须结合场景尺度、点云分辨率、RTK 质量和车辆运动幅度制定。模式 3 已错位时优先检查 B；模式 3 正常而模式 4 系统性错位时，再检查 A、坐标约定和 X。
|
|
||||||
|
|
||||||
## 结果摘要
|
|
||||||
|
|
||||||
| 结果 | 平移 xyz (m) | roll/pitch/yaw (deg) | AX 平移/旋转 RMS | 结论 |
|
|
||||||
|---|---|---|---|---|
|
|
||||||
| 部署值（第二批求解） | `[1.297760,-0.000067,0.720498]` | `[-0.785151,1.202661,-0.835510]` | `0.07985 m / 0.96118°`，39 对 | 当前建议部署 |
|
|
||||||
| data4 独立候选 | `[1.300376,-0.001707,0.704877]` | `[-0.791892,1.393823,-0.970743]` | `0.11762 m / 1.24257°`，26 对 | 独立候选，不替换部署值 |
|
|
||||||
|
|
||||||
data4 候选相对部署值变化 `1.592 cm / 0.234°`，其中 z 低 `1.562 cm`。旧部署值作用于 data4 的残差约 `0.11953 m / 1.24836°`；data4 候选作用于历史第二批的残差约 `0.07931 m / 0.98926°`。两个 X 的跨批表现接近，当前数据不足以证明 data4 的较低 z 更接近真值。
|
|
||||||
|
|
||||||
## 仓库目录
|
|
||||||
|
|
||||||
```text
|
|
||||||
code/ 标定、配准筛选、共识、比较和可视化核心程序
|
|
||||||
tools/ 原始 dlog/rscap 解析、时间关联、NPZ 导出和数据准备
|
|
||||||
run/ 不含本机固定路径的 PowerShell 入口
|
|
||||||
results/ 历史两批、data4 与跨批比较三个结果目录
|
|
||||||
```
|
|
||||||
|
|
||||||
完整复现流程和所有主要文件职责均在本 README；`run/README.md`、`tools/README.md` 和 [results/README.md](results/README.md) 只是目录内快速索引。
|
|
||||||
|
|
||||||
## 代码、工具和运行入口职责
|
|
||||||
|
|
||||||
根 README 是本仓库唯一的完整复现说明。`run/README.md`、`tools/README.md` 和 `results/README.md` 只作为进入对应目录时的快速索引，不承载另一套流程。
|
|
||||||
|
|
||||||
### code：标定核心
|
|
||||||
|
|
||||||
| 文件 | 职责 |
|
|
||||||
|---|---|
|
|
||||||
| `rigorous_calibration.py` | 核心 CLI。`ground` 拟合每站地面；`pairs` 用 small_gicp 或 Open3D GICP 求 B 和质量指标；`calibrate` 联合 AX=XB 与地面约束求 X；`validate` 计算指定 X 的逐对残差。 |
|
|
||||||
| `refine_pairs.py` | 根据收敛、RMSE、对应关系、Hessian/信息矩阵和运动覆盖筛选 B；不读取 X，避免循环挑选。 |
|
|
||||||
| `cross_backend_filter.py` | 对齐 Open3D 与 small_gicp 的同一站点对，只保留两个后端相互一致的 B。 |
|
|
||||||
| `finalize_consensus.py` | 汇总历史两批的 consensus B、外参和第一批辅助复核。 |
|
|
||||||
| `summarize_results.py` | 汇总两个后端的外参、B 质量和跨批检查，生成推荐结果。 |
|
|
||||||
| `compare_extrinsics.py` | 在 SE(3) 上计算两套外参的严格相对平移和旋转差。 |
|
|
||||||
| `visualize_pair_3d.py` | 交互显示原始点云、RTK A、GICP B、`X^-1AX`，并打印 `B^-1(X^-1AX)` 数值增量。 |
|
|
||||||
| `scan_extrinsic_sensitivity.py` | 对 X 左乘小角度 roll/pitch/yaw 扰动，检查指定运动对的局部敏感方向。 |
|
|
||||||
|
|
||||||
### tools：原始数据到 prepared
|
|
||||||
|
|
||||||
| 文件 | 职责 |
|
|
||||||
|---|---|
|
|
||||||
| `frontlidar_dlog_export.py` | 从 Medulla dlog 导出传感器坐标系 `points_raw`；旧格式可同时匹配站内 GPS-POST-Z。 |
|
|
||||||
| `prepare_station_dataset.py` | 从旧式逐站导出中每站选择一帧，计算 yaw-only RTK 后轮轴位姿并生成 prepared。 |
|
|
||||||
| `build_multisensor_npz.py` | 将独立 LiDAR、RTK、IMU 统一到 LiDAR 帧索引并生成 combined NPZ。 |
|
|
||||||
| `prepare_multisensor_station_dataset.py` | 从 combined NPZ 选择每站静止帧，生成与旧流程相同的 prepared 接口。 |
|
|
||||||
| `rscap_v2/capture_format_v2.py` | 读取 rscap v2 文件头、原始记录块和文件尾。 |
|
|
||||||
| `rscap_v2/audit_capture_v2.py` | 审计 capture 完整性、时间范围和记录统计。 |
|
|
||||||
| `rscap_v2/parse_rtk_imu_v2.py` | 分别解析 RTK 与 IMU capture，输出 JSONL。 |
|
|
||||||
| `rscap_v2/pipeline_common*.py` | rscap 解析、时间处理和采集格式兼容的共用逻辑。 |
|
|
||||||
|
|
||||||
### run：推荐 PowerShell 入口
|
|
||||||
|
|
||||||
| 文件 | 职责 |
|
|
||||||
|---|---|
|
|
||||||
| `export_legacy_stations.ps1` | 批量导出 LiDAR 与 RTK 同在逐站 dlog 中的旧格式。 |
|
|
||||||
| `export_multisensor_stations.ps1` | 解析独立 RTK/IMU，导出逐站 LiDAR，并按 LiDAR 帧建立关联。 |
|
|
||||||
| `prepare_legacy_dataset.ps1` | 旧式导出结果转换为 prepared。 |
|
|
||||||
| `prepare_multisensor_dataset.ps1` | combined 多传感器结果转换为 prepared。 |
|
|
||||||
| `run_single_dataset.ps1` | 单批数据同时完成两个 GICP 后端、B 筛选、consensus 和 X 求解。 |
|
|
||||||
| `run_all.ps1` | 历史流程：第二批求解，第一批稀疏 RTK 数据作辅助复核。 |
|
|
||||||
| `run_consensus_finish.ps1` | 在已有两个后端 B 的基础上重新生成 consensus 和汇总结果。 |
|
|
||||||
| `run_sensitivity_scan.ps1` | 对历史运动对执行外参角度灵敏度扫描。 |
|
|
||||||
| `view_result.ps1` | 传入匹配的 frames、B 和 X，运行交互式 3D 可视化。 |
|
|
||||||
|
|
||||||
### 标定核心文件的数据关系
|
|
||||||
|
|
||||||
```text
|
|
||||||
原始 dlog/rscap
|
|
||||||
-> tools 导出和时间关联
|
|
||||||
-> prepared/{frames_all, body_poses_*.csv}
|
|
||||||
-> rigorous_calibration.py pairs 生成 A、B
|
|
||||||
-> refine_pairs.py 做与 X 无关的 B 筛选
|
|
||||||
-> cross_backend_filter.py 生成 consensus B
|
|
||||||
-> rigorous_calibration.py calibrate 联合 AX=XB 与地面约束求 X
|
|
||||||
-> validate / visualize_pair_3d.py 做数值和三维检查
|
|
||||||
```
|
```
|
||||||
|
|
||||||
|
|
||||||
## 重要限制
|
| 文档 | 何时看 |
|
||||||
|
| ------------------------------------------------ | ---------------- |
|
||||||
|
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 要看方法约定与实现状态表 |
|
||||||
|
| [tests/README.md](tests/README.md) | 要看合成用例 / S2 烟测记录 |
|
||||||
|
| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 要改源码 |
|
||||||
|
| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 要查改动史 |
|
||||||
|
|
||||||
- RTK 车体姿态当前是双天线 heading 构造的 yaw-only 轨迹；没有用 RTK pitch/roll 构造 A。
|
|
||||||
- 新式解析器保存 IMU 与 RTK pitch 等原始字段，但当前手眼方程未融合 IMU。
|
|
||||||
- 静止站点法不估计 LiDAR–RTK 时间偏移；时间戳关联必须在导出阶段通过审计。
|
|
||||||
- 地面约束负责 roll、pitch 和 z 的补充可观性，不会独立求出另一套六自由度外参。
|
|
||||||
- 仓库归档的是结果和轻量 B 文件，不包含云盘中的原始点云数据。
|
|
||||||
|
|
||||||
## 专题说明
|
改算法请同步职责说明与 CHANGELOG；改对外用法请更新本 README。
|
||||||
|
|
||||||
- [运动对诊断](PAIR_DIAGNOSTICS.md)
|
|
||||||
- [双后端共识筛选](CONSENSUS_SELECTION.md)
|
|
||||||
- [结果文件索引](results/README.md)
|
|
||||||
@@ -0,0 +1,177 @@
|
|||||||
|
# RTK-IMU 标定：流程、结果与复现
|
||||||
|
|
||||||
|
本文是本次 G90 双天线 RTK 与 HI13 IMU 标定的工程入口。算法代码在 `rtk_imu/`，命令入口在 `tools/`，正式审核结果在 `artifacts/rtk_imu_calibration_v3/`。原始 `.rscap`、统一导出数据、状态快照与 checkpoint 均只保留本地，不提交仓库。
|
||||||
|
|
||||||
|
## 一句话结论
|
||||||
|
|
||||||
|
RTK-IMU 旋转外参由双天线基线方向与水平静止重力约束获得；平移外参以机械测量为绝对基准，再通过 RTK+IMU 联合状态图、高动态转弯/坡道数据、独立 held-out 数据和旋转扰动测试进行一致性验证。
|
||||||
|
|
||||||
|
当前动态数据不足以独立高精度求出完整 XYZ 杆臂，因此它不是 data-only translation calibration。机械测量给出了杆臂的绝对值；在未参与标定的数据、转弯等杆臂敏感运动以及旋转外参扰动测试中，均未发现该机械外参存在明显矛盾或不稳定性。当前结果的正确表述是：**机械杆臂锚定，并经动态数据一致性验证的工程候选**。
|
||||||
|
|
||||||
|
## 当前工程候选与状态
|
||||||
|
|
||||||
|
坐标约定：`l_I = p_ANT1^I`，即 ANT1（主天线、左侧）相位中心在 IMU 坐标系中的位置。车体/RTK 安装坐标为：+X 为 ANT1（左）到 ANT2（右），+Y 为车辆前进方向且与 IMU +Y 同向，+Z 向上；GGA 参考点为 ANT1 相位中心，离地 `1.916499878 m`。
|
||||||
|
|
||||||
|
- 固定旋转来源：`R2G_gravity_level_prior`。
|
||||||
|
- 固定旋转近似 RPY（X/Y/Z）：`[0.454°, -0.003°, 0.012°]`。
|
||||||
|
- 当前工程候选杆臂：`l_I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||||
|
- 变换约定：`p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU`。
|
||||||
|
- 对应候选 `T_RTK_IMU`（四舍五入到 6 位小数）：
|
||||||
|
|
||||||
|
```text
|
||||||
|
[[ 1.000000, -0.000214, -0.000044, 0.451777],
|
||||||
|
[ 0.000214, 0.999969, -0.007929, 0.270363],
|
||||||
|
[ 0.000046, 0.007929, 0.999969, -0.729325],
|
||||||
|
[ 0.000000, 0.000000, 0.000000, 1.000000]]
|
||||||
|
```
|
||||||
|
|
||||||
|
当前门禁状态必须同时保留：
|
||||||
|
|
||||||
|
- `data_only_translation_accepted=false`：数据本身没有提供足够稳定的完整平移可观性。
|
||||||
|
- `engineering_translation_accepted=false`：独立传播验证仍存在公共加速度偏差，不能声称正式工程放行。
|
||||||
|
- `independent_extrinsic_sensitive_validation_passed=true`：杆臂敏感的高动态验证未发现机械杆臂冲突。
|
||||||
|
- `heldout_physical_validation_passed=true`、`rotation_sensitivity_passed=true`：固定候选在 held-out 物理残差和旋转扰动中保持一致。
|
||||||
|
|
||||||
|
因此不得描述为“data-only 标定平移”或“动态数据已精确细化机械杆臂”。完整、机器可读的结论见 [engineering_release_decision.json](artifacts/rtk_imu_calibration_v3/engineering_release_decision.json)。
|
||||||
|
|
||||||
|
## 求解流程
|
||||||
|
|
||||||
|
1. **统一原始数据导出。** 使用 G90/HI13 设备时间作为主时间轴；host receive time 仅用于诊断。G90 保留 GGA 质量、GNHPR 基线及质量、BESTNAVA Doppler velocity，必要时 PVTSLNA；HI13 保留 system time、gyro、accel、姿态/四元数和 host receive time。
|
||||||
|
2. **R0 连续性与质量控制。** 校验 checksum、RTK Fixed、设备时间单调性、IMU 覆盖、baseline jump 与测量间隔。轨迹连续性由 IMU 设备时间和预积分覆盖决定；孤立 HPR 缺失/Q5 只禁用或降权 HPR factor，不切断 IMU 轨迹。
|
||||||
|
3. **旋转外参。** R1b 从基线与 IMU 动态估计 ANT1→ANT2 在 IMU 中的 2DoF 方向；R2V 用基线与高质量 Doppler 速度作独立诊断；R2G 使用水平静止场地中的基线＋重力＋地面水平先验补齐完整旋转。R2G 是本次正式固定旋转来源，旧 GNHPR 三轴手眼只作诊断。
|
||||||
|
4. **节点状态图。** 每个 GNSS node 包含 `R,p,v,bg,ba` 的 15DoF 状态；相邻 node 由 covariance-whitened IMU preintegration 和 bias random walk 连接。BEST 约束 XYZ，GGA 仅在 BEST 缺失时约束 XY，Doppler 约束速度，HPR 是可选姿态/基线因子。
|
||||||
|
5. **机械锚定的平移验证。** 杆臂以机械值为基准，分别比较无先验 free、固定机械杆臂和软先验解。free 解只用于观测性诊断，不能因为数值收敛就替代机械值。
|
||||||
|
6. **独立验证。** 使用 circle、left-right、slope 的高动态非重叠窗口；再使用剩余 held-out 窗口、旋转 ±扰动敏感性、独立 innovation 和 propagation-bias root-cause audit 复核。
|
||||||
|
|
||||||
|
## 证据与限制
|
||||||
|
|
||||||
|
47 个非重叠标定窗口上的 free/fixed/prior 对比表明机械先验与数据拟合相容，但 posterior/prior 方差比没有显示足够的数据驱动细化，因此 `translation_refined_by_data=false`。剩余 267 个 frozen held-out 窗口的物理验证通过；然而独立传播创新在低速、低角速度区间同时出现位置和 Doppler 的同向偏差，等效为约 `0.20 m/s²` 的公共传播加速度误差。
|
||||||
|
|
||||||
|
该误差在 `|omega|` 很小时不能优先归因于杆臂速度项 `R*(omega × l)`，因此它不单独否决高动态杆臂敏感验证；但在传播模型根因关闭前，也不能把候选杆臂标为正式工程已放行。
|
||||||
|
|
||||||
|
主要审核证据：
|
||||||
|
|
||||||
|
- [47 窗口机械分支](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json)
|
||||||
|
- [held-out 验证](artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json)
|
||||||
|
- [独立创新审计](artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json)
|
||||||
|
- [传播偏差根因审计](artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json)
|
||||||
|
- [旋转敏感性](artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json)
|
||||||
|
|
||||||
|
## 如何复现
|
||||||
|
|
||||||
|
### 1. 准备环境和原始数据
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
cd <repository-root>
|
||||||
|
python -m pip install -e ".[dev]"
|
||||||
|
```
|
||||||
|
|
||||||
|
将三批 G90/HI13 原始 `.rscap` 会话放到本机的数据位置。数据位置不写入仓库；`tools/export_rtk_imu_unified.py` 中的会话配对清单必须与实际采集文件一致。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
$OUT = "artifacts\rtk_imu_calibration_v3\reproduce"
|
||||||
|
python tools\export_rtk_imu_unified.py --output-root "$OUT\unified" --overwrite
|
||||||
|
$MANIFEST = "$OUT\unified\manifest.json"
|
||||||
|
```
|
||||||
|
|
||||||
|
导出后应检查每个会话目录中的 `export_summary.json`，并确认 `manifest.json` 中记录的传感器时间轴没有被 host receive time 替换。
|
||||||
|
|
||||||
|
### 2. 复现固定旋转
|
||||||
|
|
||||||
|
`<flat-static-session-id>` 必须是已确认地面水平、车辆静止的会话；可多次传入 `--level-static`。R2V 只作独立诊断，不替代 R2G。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\run_rtk_imu_multisource.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--level-static <flat-static-session-id> `
|
||||||
|
--output "$OUT\r2g_multisource.json"
|
||||||
|
```
|
||||||
|
|
||||||
|
检查输出的 R2G 旋转与本 README 的近似 RPY 一致后，将该旋转固定为后续 node graph 的 `--rotation-rpy-deg 0.454 -0.003 0.012`。若 R2G 不一致，应停止，先复核天线方向、场地水平和时间/轴定义，不应继续求杆臂。
|
||||||
|
|
||||||
|
### 3. 复现窗口选择与无先验诊断
|
||||||
|
|
||||||
|
从 circle、left-right、slope 三类会话中选择高质量窗口；窗口不可共享 IMU/GNSS/HPR 样本。仓库提交的 `lever_information_window_selection*.json` 是本次审核所用选择结果，可用于对照。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\select_rtk_imu_windows_by_lever_information.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--circle-session <circle-session-id> `
|
||||||
|
--left-right-session <left-right-session-id> `
|
||||||
|
--slope-session <slope-session-id> `
|
||||||
|
--output "$OUT\selection.json" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
|
||||||
|
python tools\run_rtk_imu_node_graph_free_selected.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--selection "$OUT\selection.json" `
|
||||||
|
--output "$OUT\free_baseline.json" `
|
||||||
|
--start-name all `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
```
|
||||||
|
|
||||||
|
free solve 的作用是输出边缘化杆臂信息、协方差、最弱方向和多初值稳定性；本次数据若仍未达到完整 XYZ 可观性，不得扩大无先验求解规模来强行放行。
|
||||||
|
|
||||||
|
### 4. 复现机械杆臂分支和 held-out 验证
|
||||||
|
|
||||||
|
固定工程候选杆臂并使用同一批非重叠标定窗口。`states.npz` 和 checkpoint-dir 是本地过程产物，应保持被 `.gitignore` 排除。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\run_rtk_imu_mechanical_prior_branch.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--selection "$OUT\selection.json" `
|
||||||
|
--free-baseline "$OUT\free_baseline.json" `
|
||||||
|
--output "$OUT\mechanical_47_window.json" `
|
||||||
|
--state-output "$OUT\states.npz" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
|
||||||
|
python tools\run_rtk_imu_mechanical_prior_heldout.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--calibration-selection "$OUT\selection.json" `
|
||||||
|
--all-selection <all-nonoverlapping-selection.json> `
|
||||||
|
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||||
|
--output "$OUT\heldout.json" `
|
||||||
|
--checkpoint-dir "$OUT\heldout_checkpoints" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
```
|
||||||
|
|
||||||
|
随后运行独立 innovation、传播根因审计和旋转敏感性。它们不重新优化杆臂，不应被用于调 covariance、R2G 或机械先验。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\audit_rtk_imu_heldout_innovation.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--calibration-selection "$OUT\selection.json" `
|
||||||
|
--all-selection <all-nonoverlapping-selection.json> `
|
||||||
|
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||||
|
--output "$OUT\heldout_innovation.json" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
|
||||||
|
python tools\audit_rtk_imu_propagation_bias_root_cause.py `
|
||||||
|
--manifest $MANIFEST `
|
||||||
|
--calibration-selection "$OUT\selection.json" `
|
||||||
|
--all-selection <all-nonoverlapping-selection.json> `
|
||||||
|
--engineering-result "$OUT\mechanical_47_window.json" `
|
||||||
|
--output "$OUT\propagation_bias_root_cause.json" `
|
||||||
|
--rotation-rpy-deg 0.454 -0.003 0.012
|
||||||
|
```
|
||||||
|
|
||||||
|
最终仅汇总已生成的结果，不在 release 阶段重新拟合：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\finalize_rtk_imu_engineering_release.py `
|
||||||
|
--calibration "$OUT\mechanical_47_window.json" `
|
||||||
|
--heldout-postfit "$OUT\heldout.json" `
|
||||||
|
--innovation "$OUT\heldout_innovation.json" `
|
||||||
|
--sensitivity <rotation_sensitivity.json> `
|
||||||
|
--convergence-retry <heldout_retry.json> `
|
||||||
|
--propagation-root-cause "$OUT\propagation_bias_root_cause.json" `
|
||||||
|
--output "$OUT\engineering_release_decision.json"
|
||||||
|
```
|
||||||
|
|
||||||
|
## 相关文件
|
||||||
|
|
||||||
|
- [RTK-IMU 代码包](rtk_imu/README.md)
|
||||||
|
- [多源导出与旋转 V3 说明](docs/rtk_imu_multisource_v3.md)
|
||||||
|
- [engineering 6DoF 说明](docs/rtk_imu_engineering_6dof.md)
|
||||||
|
- [历史链路审计](docs/rtk_imu_calibration.md)
|
||||||
|
- [正式结果索引](artifacts/rtk_imu_calibration_v3/README.md)
|
||||||
@@ -0,0 +1,17 @@
|
|||||||
|
# RTK–IMU 标定产物说明
|
||||||
|
|
||||||
|
- `all_sessions/`：8 会话、5 s 平移节点、带 conditional rotation LOO 和 translation LOO 的当前完整基线。
|
||||||
|
- `rotation_hpr_time/`：改用 GNHPR 自带测量时刻后的全量 rotation-only 对照。
|
||||||
|
- `rotation_smoke/`：较早的 GGA 最近邻姿态时刻对照，不作为当前结果。
|
||||||
|
- `batch_0808_full_smoke/`：0808 三会话完整诊断。
|
||||||
|
- `batch_0815_rotation/`：0815 四会话 rotation-only 诊断。
|
||||||
|
- `single_smoke/`：早期单会话性能/数值冒烟，不作为当前结果。
|
||||||
|
|
||||||
|
每个正式运行目录包含：
|
||||||
|
|
||||||
|
- `dataset_audit.json`：样本数、固定解比例、共同时间范围和 ENU 原点。
|
||||||
|
- `rotation_result.json`：旋转、RPY、时间审计、GNHPR 候选、偏置、残差、协方差、逐会话指标和 LOO。
|
||||||
|
- `translation_result.json`：杆臂、平移、齐次矩阵、协方差/秩、位置/速度残差、偏置和 LOO。
|
||||||
|
- `summary.json`：供程序读取的最终状态和候选矩阵。
|
||||||
|
|
||||||
|
当前 `all_sessions/summary.json` 是 `diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性，禁止用于车辆配置。完整解释见 `docs/rtk_imu_calibration.md`。
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
{
|
||||||
|
"session_count": 8,
|
||||||
|
"sessions": [
|
||||||
|
{
|
||||||
|
"session_id": "priority_174005_174515",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 31000,
|
||||||
|
"rtk_samples": 4780,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.854602510460251,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16265.2325992,
|
||||||
|
16575.0298107
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465514786,
|
||||||
|
114.092169888,
|
||||||
|
29.4695
|
||||||
|
],
|
||||||
|
"imu_source": "31000 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_174905_175450",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 34499,
|
||||||
|
"rtk_samples": 5211,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8547303780464403,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16805.1862481,
|
||||||
|
17150.0907328
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4653424457,
|
||||||
|
114.092237385,
|
||||||
|
29.5366
|
||||||
|
],
|
||||||
|
"imu_source": "34499 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_175910_180530",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 37998,
|
||||||
|
"rtk_samples": 5786,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8567231247839613,
|
||||||
|
"common_time_span_s": [
|
||||||
|
17410.2121738,
|
||||||
|
17790.0458228
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654151935,
|
||||||
|
114.090796384,
|
||||||
|
29.5317
|
||||||
|
],
|
||||||
|
"imu_source": "37998 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "slope_190548_190730",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 10199,
|
||||||
|
"rtk_samples": 1578,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8757921419518377,
|
||||||
|
"common_time_span_s": [
|
||||||
|
36466.328875,
|
||||||
|
36568.2301441
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4652183602,
|
||||||
|
114.090839984,
|
||||||
|
29.9891
|
||||||
|
],
|
||||||
|
"imu_source": "10199 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "circle_193412_193642",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 14989,
|
||||||
|
"rtk_samples": 2162,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8288621646623496,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38170.3385255,
|
||||||
|
38317.8571971
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654799242,
|
||||||
|
114.092155912,
|
||||||
|
29.4779
|
||||||
|
],
|
||||||
|
"imu_source": "14989 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "loop_194223_195003",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 45801,
|
||||||
|
"rtk_samples": 6741,
|
||||||
|
"fixed_position_ratio": 0.9998516540572615,
|
||||||
|
"fixed_attitude_ratio": 0.8377095386441181,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38661.2831251,
|
||||||
|
39121.1709553
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654856957,
|
||||||
|
114.092151015,
|
||||||
|
29.4651
|
||||||
|
],
|
||||||
|
"imu_source": "45801 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "accel_195608_195958",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 23002,
|
||||||
|
"rtk_samples": 3456,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8457754629629629,
|
||||||
|
"common_time_span_s": [
|
||||||
|
39486.3574509,
|
||||||
|
39716.3199405
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465443014,
|
||||||
|
114.092179168,
|
||||||
|
29.5309
|
||||||
|
],
|
||||||
|
"imu_source": "23002 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "motion_sms_154023_154359",
|
||||||
|
"batch_id": "0819",
|
||||||
|
"imu_samples": 21556,
|
||||||
|
"rtk_samples": 3294,
|
||||||
|
"fixed_position_ratio": 0.49271402550091076,
|
||||||
|
"fixed_attitude_ratio": 0.4344262295081967,
|
||||||
|
"common_time_span_s": [
|
||||||
|
25829.8725525,
|
||||||
|
26027.4217529
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4651580863,
|
||||||
|
114.090773052,
|
||||||
|
36.5626
|
||||||
|
],
|
||||||
|
"imu_source": "21556 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,130 @@
|
|||||||
|
{
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996841792313951,
|
||||||
|
0.02499811511405725,
|
||||||
|
-0.002576050309343804
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.025002617750669198,
|
||||||
|
0.9996858874629868,
|
||||||
|
-0.0017307550243271697
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002531975526313299,
|
||||||
|
0.0017946164171362589,
|
||||||
|
0.9999951842143289
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rpy_deg": [
|
||||||
|
0.1028243313393031,
|
||||||
|
-0.1450716664951083,
|
||||||
|
-1.43269836393699
|
||||||
|
],
|
||||||
|
"gyro_bias_by_session_rad_s": {
|
||||||
|
"priority_174005_174515": [
|
||||||
|
-8.90944066727157e-05,
|
||||||
|
7.078609864979291e-05,
|
||||||
|
0.0001460390779870233
|
||||||
|
],
|
||||||
|
"priority_174905_175450": [
|
||||||
|
-4.327374730750894e-05,
|
||||||
|
-0.0001097548390022348,
|
||||||
|
3.000549725593387e-05
|
||||||
|
],
|
||||||
|
"priority_175910_180530": [
|
||||||
|
-7.036551812132934e-05,
|
||||||
|
-0.0008054961048184173,
|
||||||
|
1.3026193503057623e-05
|
||||||
|
],
|
||||||
|
"slope_190548_190730": [
|
||||||
|
6.035222904568755e-05,
|
||||||
|
-8.913884157613711e-06,
|
||||||
|
0.00015470949872434692
|
||||||
|
],
|
||||||
|
"circle_193412_193642": [
|
||||||
|
-3.696799268832588e-05,
|
||||||
|
-0.00019200076224600124,
|
||||||
|
-5.9567987995056394e-05
|
||||||
|
],
|
||||||
|
"loop_194223_195003": [
|
||||||
|
-0.0001979655352189002,
|
||||||
|
-0.00015937450336643818,
|
||||||
|
-4.343089303706036e-05
|
||||||
|
],
|
||||||
|
"accel_195608_195958": [
|
||||||
|
-7.841839817698704e-05,
|
||||||
|
-2.604932294598783e-06,
|
||||||
|
5.8586555655011475e-05
|
||||||
|
],
|
||||||
|
"motion_sms_154023_154359": [
|
||||||
|
0.00011745666467620586,
|
||||||
|
0.00021434926285955486,
|
||||||
|
-4.5147354343760625e-05
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"time_offset": {
|
||||||
|
"offset_s": 0.04000000000000031,
|
||||||
|
"peak_correlation": 0.5788788671828708,
|
||||||
|
"second_best_correlation": 0.5773967219219845,
|
||||||
|
"evaluated_samples": 12394,
|
||||||
|
"reliable": false
|
||||||
|
},
|
||||||
|
"applied_time_offset_s": 0.0,
|
||||||
|
"convention": {
|
||||||
|
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||||
|
"heading_sign": -1.0,
|
||||||
|
"pitch_sign": -1.0,
|
||||||
|
"roll_sign": 1.0
|
||||||
|
},
|
||||||
|
"convention_scores_deg": {
|
||||||
|
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
|
||||||
|
"north_cw__pitch_opposite": 1.737582794831228,
|
||||||
|
"heading_opposite__pitch_nose_up": 7.263259116648845,
|
||||||
|
"heading_opposite__pitch_opposite": 17.716319437929055
|
||||||
|
},
|
||||||
|
"pair_count": 731,
|
||||||
|
"residual_rms_deg": 1.6276839301413086,
|
||||||
|
"residual_median_deg": 0.5695938849052221,
|
||||||
|
"residual_p95_deg": 3.0903953005641345,
|
||||||
|
"rotation_std_deg": [
|
||||||
|
0.2340126126406699,
|
||||||
|
0.20048574131667432,
|
||||||
|
2.314692068045375
|
||||||
|
],
|
||||||
|
"information_singular_values": [
|
||||||
|
82178.77940373585,
|
||||||
|
60441.86300479194,
|
||||||
|
612.6358640387364
|
||||||
|
],
|
||||||
|
"per_session_rms_deg": {
|
||||||
|
"priority_174005_174515": 0.6134232617562615,
|
||||||
|
"priority_174905_175450": 2.2485932625979106,
|
||||||
|
"priority_175910_180530": 3.095861798757508,
|
||||||
|
"slope_190548_190730": 2.388478834012087,
|
||||||
|
"circle_193412_193642": 0.5629885032374518,
|
||||||
|
"loop_194223_195003": 0.6250976606325254,
|
||||||
|
"accel_195608_195958": 0.975856888271022,
|
||||||
|
"motion_sms_154023_154359": 1.3698317592842066
|
||||||
|
},
|
||||||
|
"loo_delta_deg": {
|
||||||
|
"priority_174005_174515": 0.7895996403426686,
|
||||||
|
"priority_174905_175450": 0.9224102138653388,
|
||||||
|
"priority_175910_180530": 5.144190932547379,
|
||||||
|
"slope_190548_190730": 0.24769087330915346,
|
||||||
|
"circle_193412_193642": 1.2637729378484346,
|
||||||
|
"loop_194223_195003": 0.4844375238260164,
|
||||||
|
"accel_195608_195958": 1.0976412402728102,
|
||||||
|
"motion_sms_154023_154359": 0.6896079810115167
|
||||||
|
},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||||
|
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||||
|
"GNHPR convention score gap=0.1063 deg",
|
||||||
|
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||||
|
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||||
|
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||||
|
"rotation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,65 @@
|
|||||||
|
{
|
||||||
|
"status": "diagnostic_not_accepted",
|
||||||
|
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||||
|
"rtk_frame_definition": "",
|
||||||
|
"rtk_reference_point": "",
|
||||||
|
"interpretation_blockers": [
|
||||||
|
"rotation quality gates failed",
|
||||||
|
"translation quality gates failed or were not run",
|
||||||
|
"RTK frame_definition is empty",
|
||||||
|
"RTK reference_point is empty"
|
||||||
|
],
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996841792313951,
|
||||||
|
0.02499811511405725,
|
||||||
|
-0.002576050309343804
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.025002617750669198,
|
||||||
|
0.9996858874629868,
|
||||||
|
-0.0017307550243271697
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002531975526313299,
|
||||||
|
0.0017946164171362589,
|
||||||
|
0.9999951842143289
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": [
|
||||||
|
0.7710937276592442,
|
||||||
|
0.5693018309069646,
|
||||||
|
-21.07303743539233
|
||||||
|
],
|
||||||
|
"T_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996841792313951,
|
||||||
|
0.02499811511405725,
|
||||||
|
-0.002576050309343804,
|
||||||
|
0.7710937276592442
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.025002617750669198,
|
||||||
|
0.9996858874629868,
|
||||||
|
-0.0017307550243271697,
|
||||||
|
0.5693018309069646
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002531975526313299,
|
||||||
|
0.0017946164171362589,
|
||||||
|
0.9999951842143289,
|
||||||
|
-21.07303743539233
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rotation_ok": false,
|
||||||
|
"translation_ok": false,
|
||||||
|
"rotation_result": "rotation_result.json",
|
||||||
|
"translation_result": "translation_result.json",
|
||||||
|
"dataset_audit": "dataset_audit.json"
|
||||||
|
}
|
||||||
@@ -0,0 +1,161 @@
|
|||||||
|
{
|
||||||
|
"lever_IMU_to_RTK_in_IMU_m": [
|
||||||
|
-0.7032597491310882,
|
||||||
|
-0.5505808768918061,
|
||||||
|
21.07590765040047
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": [
|
||||||
|
0.7710937276592442,
|
||||||
|
0.5693018309069646,
|
||||||
|
-21.07303743539233
|
||||||
|
],
|
||||||
|
"T_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996841792313951,
|
||||||
|
0.02499811511405725,
|
||||||
|
-0.002576050309343804,
|
||||||
|
0.7710937276592442
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.025002617750669198,
|
||||||
|
0.9996858874629868,
|
||||||
|
-0.0017307550243271697,
|
||||||
|
0.5693018309069646
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002531975526313299,
|
||||||
|
0.0017946164171362589,
|
||||||
|
0.9999951842143289,
|
||||||
|
-21.07303743539233
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"translation_std_m": [
|
||||||
|
0.367657527272358,
|
||||||
|
0.36764001580136974,
|
||||||
|
2.960858932902556
|
||||||
|
],
|
||||||
|
"lever_information_singular_values": [
|
||||||
|
7.467287688797444,
|
||||||
|
7.419771560016465,
|
||||||
|
0.11404687553142893
|
||||||
|
],
|
||||||
|
"lever_precision_rank": 3,
|
||||||
|
"position_residual_rms_xyz_m": [
|
||||||
|
0.0026057774092802665,
|
||||||
|
0.0026172203224947795,
|
||||||
|
0.0008707288139887198
|
||||||
|
],
|
||||||
|
"velocity_residual_rms_xyz_m_s": [
|
||||||
|
1.0344084464154262,
|
||||||
|
1.0018456371907998,
|
||||||
|
0.08189262443294992
|
||||||
|
],
|
||||||
|
"accel_bias_by_session_m_s2": {
|
||||||
|
"priority_174005_174515": [
|
||||||
|
-0.0033446448235936025,
|
||||||
|
0.04850567116751809,
|
||||||
|
0.014864908749227664
|
||||||
|
],
|
||||||
|
"priority_174905_175450": [
|
||||||
|
0.051484854191344014,
|
||||||
|
0.009750775340658668,
|
||||||
|
0.016837766756117954
|
||||||
|
],
|
||||||
|
"priority_175910_180530": [
|
||||||
|
-0.0012098409852818557,
|
||||||
|
0.04428237322299069,
|
||||||
|
0.015044444493724668
|
||||||
|
],
|
||||||
|
"slope_190548_190730": [
|
||||||
|
0.007720083607057322,
|
||||||
|
-0.07045791547639069,
|
||||||
|
0.011096872492579776
|
||||||
|
],
|
||||||
|
"circle_193412_193642": [
|
||||||
|
0.010695150095323893,
|
||||||
|
0.07124837152622766,
|
||||||
|
0.012181970165526993
|
||||||
|
],
|
||||||
|
"loop_194223_195003": [
|
||||||
|
0.017798177348527063,
|
||||||
|
0.026531175431114495,
|
||||||
|
0.013517325106459492
|
||||||
|
],
|
||||||
|
"accel_195608_195958": [
|
||||||
|
0.010843370014374661,
|
||||||
|
0.0021847264023688797,
|
||||||
|
0.012720554759472001
|
||||||
|
],
|
||||||
|
"motion_sms_154023_154359": [
|
||||||
|
-0.01917068926914634,
|
||||||
|
0.04878246850510192,
|
||||||
|
0.011986246228615108
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"knot_count_by_session": {
|
||||||
|
"priority_174005_174515": 62,
|
||||||
|
"priority_174905_175450": 69,
|
||||||
|
"priority_175910_180530": 76,
|
||||||
|
"slope_190548_190730": 21,
|
||||||
|
"circle_193412_193642": 30,
|
||||||
|
"loop_194223_195003": 92,
|
||||||
|
"accel_195608_195958": 46,
|
||||||
|
"motion_sms_154023_154359": 20
|
||||||
|
},
|
||||||
|
"loo_delta_m": {
|
||||||
|
"priority_174005_174515": [
|
||||||
|
-0.08631101169626099,
|
||||||
|
-0.16878866579372004,
|
||||||
|
-0.1969063529933237
|
||||||
|
],
|
||||||
|
"priority_174905_175450": [
|
||||||
|
0.052982832020427084,
|
||||||
|
0.23694572858391594,
|
||||||
|
0.5921699991558107
|
||||||
|
],
|
||||||
|
"priority_175910_180530": [
|
||||||
|
-0.03728037456994515,
|
||||||
|
-0.0587695607071852,
|
||||||
|
0.36745157813128415
|
||||||
|
],
|
||||||
|
"slope_190548_190730": [
|
||||||
|
0.06820599394639204,
|
||||||
|
0.13646009238200618,
|
||||||
|
0.8245922313333871
|
||||||
|
],
|
||||||
|
"circle_193412_193642": [
|
||||||
|
-0.06745036261863124,
|
||||||
|
-0.18645839932341524,
|
||||||
|
-0.8300513550738842
|
||||||
|
],
|
||||||
|
"loop_194223_195003": [
|
||||||
|
0.12045919921905746,
|
||||||
|
0.06572884705689208,
|
||||||
|
-1.774476169745249
|
||||||
|
],
|
||||||
|
"accel_195608_195958": [
|
||||||
|
0.019243586803303625,
|
||||||
|
-0.002985337002138544,
|
||||||
|
0.3653882998138158
|
||||||
|
],
|
||||||
|
"motion_sms_154023_154359": [
|
||||||
|
-0.0383153365081248,
|
||||||
|
0.027686850248512473,
|
||||||
|
0.507160468738487
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||||
|
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||||
|
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||||
|
"upstream rotation is not accepted, so translation is diagnostic only",
|
||||||
|
"translation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,62 @@
|
|||||||
|
{
|
||||||
|
"session_count": 3,
|
||||||
|
"sessions": [
|
||||||
|
{
|
||||||
|
"session_id": "priority_174005_174515",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 31000,
|
||||||
|
"rtk_samples": 4780,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.854602510460251,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16265.2325992,
|
||||||
|
16575.0298107
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465514786,
|
||||||
|
114.092169888,
|
||||||
|
29.4695
|
||||||
|
],
|
||||||
|
"imu_source": "31000 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_174905_175450",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 34499,
|
||||||
|
"rtk_samples": 5211,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8547303780464403,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16805.1862481,
|
||||||
|
17150.0907328
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4653424457,
|
||||||
|
114.092237385,
|
||||||
|
29.5366
|
||||||
|
],
|
||||||
|
"imu_source": "34499 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_175910_180530",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 37998,
|
||||||
|
"rtk_samples": 5786,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8567231247839613,
|
||||||
|
"common_time_span_s": [
|
||||||
|
17410.2121738,
|
||||||
|
17790.0458228
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654151935,
|
||||||
|
114.090796384,
|
||||||
|
29.5317
|
||||||
|
],
|
||||||
|
"imu_source": "37998 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,89 @@
|
|||||||
|
{
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996274297857513,
|
||||||
|
0.0272885099045028,
|
||||||
|
-0.0005821057674967719
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.027285914629035343,
|
||||||
|
0.9996193516550798,
|
||||||
|
0.0040780705652228135
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0006931686589101494,
|
||||||
|
-0.004060667909321538,
|
||||||
|
0.9999915152106744
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rpy_deg": [
|
||||||
|
-0.23265982849493025,
|
||||||
|
-0.03971564182674239,
|
||||||
|
-1.5635621825359411
|
||||||
|
],
|
||||||
|
"gyro_bias_by_session_rad_s": {
|
||||||
|
"priority_174005_174515": [
|
||||||
|
-9.355487703345326e-05,
|
||||||
|
6.022278219673199e-05,
|
||||||
|
0.0001599879605718389
|
||||||
|
],
|
||||||
|
"priority_174905_175450": [
|
||||||
|
-6.763782096065313e-05,
|
||||||
|
-0.00013271246982721386,
|
||||||
|
2.7304435743171578e-05
|
||||||
|
],
|
||||||
|
"priority_175910_180530": [
|
||||||
|
-4.9469524877665416e-05,
|
||||||
|
-0.0005605818923478396,
|
||||||
|
4.72341323992995e-06
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"time_offset": {
|
||||||
|
"offset_s": 0.07500000000000034,
|
||||||
|
"peak_correlation": 0.2075894836191556,
|
||||||
|
"second_best_correlation": 0.2075830757517665,
|
||||||
|
"evaluated_samples": 6286,
|
||||||
|
"reliable": false
|
||||||
|
},
|
||||||
|
"convention": {
|
||||||
|
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||||
|
"heading_sign": -1.0,
|
||||||
|
"pitch_sign": -1.0,
|
||||||
|
"roll_sign": 1.0
|
||||||
|
},
|
||||||
|
"convention_scores_deg": {
|
||||||
|
"north_cw__pitch_nose_up__roll_right_down": 2.124836124403544,
|
||||||
|
"north_cw__pitch_opposite": 2.246244073697405,
|
||||||
|
"heading_opposite__pitch_nose_up": 14.081722937345578,
|
||||||
|
"heading_opposite__pitch_opposite": 14.187723789068325
|
||||||
|
},
|
||||||
|
"pair_count": 347,
|
||||||
|
"residual_rms_deg": 2.121956852743709,
|
||||||
|
"residual_median_deg": 0.6620410270629032,
|
||||||
|
"residual_p95_deg": 4.932454679529714,
|
||||||
|
"rotation_std_deg": [
|
||||||
|
0.5378381823216046,
|
||||||
|
0.43991569563643235,
|
||||||
|
3.604673882923754
|
||||||
|
],
|
||||||
|
"information_singular_values": [
|
||||||
|
17058.156967522238,
|
||||||
|
11392.471369813427,
|
||||||
|
252.6038959367189
|
||||||
|
],
|
||||||
|
"per_session_rms_deg": {
|
||||||
|
"priority_174005_174515": 0.6074812737155098,
|
||||||
|
"priority_174905_175450": 2.2498284381524494,
|
||||||
|
"priority_175910_180530": 3.09642157433063
|
||||||
|
},
|
||||||
|
"loo_delta_deg": {},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||||
|
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||||
|
"GNHPR convention score gap=0.1214 deg",
|
||||||
|
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||||
|
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||||
|
"rotation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,57 @@
|
|||||||
|
{
|
||||||
|
"status": "diagnostic_not_accepted",
|
||||||
|
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996274297857513,
|
||||||
|
0.0272885099045028,
|
||||||
|
-0.0005821057674967719
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.027285914629035343,
|
||||||
|
0.9996193516550798,
|
||||||
|
0.0040780705652228135
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0006931686589101494,
|
||||||
|
-0.004060667909321538,
|
||||||
|
0.9999915152106744
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": [
|
||||||
|
1.017036626166409,
|
||||||
|
0.7992624136748191,
|
||||||
|
-22.250154010194304
|
||||||
|
],
|
||||||
|
"T_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996274297857513,
|
||||||
|
0.0272885099045028,
|
||||||
|
-0.0005821057674967719,
|
||||||
|
1.017036626166409
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.027285914629035343,
|
||||||
|
0.9996193516550798,
|
||||||
|
0.0040780705652228135,
|
||||||
|
0.7992624136748191
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0006931686589101494,
|
||||||
|
-0.004060667909321538,
|
||||||
|
0.9999915152106744,
|
||||||
|
-22.250154010194304
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rotation_ok": false,
|
||||||
|
"translation_ok": false,
|
||||||
|
"rotation_result": "rotation_result.json",
|
||||||
|
"translation_result": "translation_result.json",
|
||||||
|
"dataset_audit": "dataset_audit.json"
|
||||||
|
}
|
||||||
@@ -0,0 +1,90 @@
|
|||||||
|
{
|
||||||
|
"lever_IMU_to_RTK_in_IMU_m": [
|
||||||
|
-0.979425993211181,
|
||||||
|
-0.9170620761729408,
|
||||||
|
22.247297796687818
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": [
|
||||||
|
1.017036626166409,
|
||||||
|
0.7992624136748191,
|
||||||
|
-22.250154010194304
|
||||||
|
],
|
||||||
|
"T_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996274297857513,
|
||||||
|
0.0272885099045028,
|
||||||
|
-0.0005821057674967719,
|
||||||
|
1.017036626166409
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.027285914629035343,
|
||||||
|
0.9996193516550798,
|
||||||
|
0.0040780705652228135,
|
||||||
|
0.7992624136748191
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0006931686589101494,
|
||||||
|
-0.004060667909321538,
|
||||||
|
0.9999915152106744,
|
||||||
|
-22.250154010194304
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"translation_std_m": [
|
||||||
|
0.6410402171684149,
|
||||||
|
0.6447113787519164,
|
||||||
|
3.8963000265795396
|
||||||
|
],
|
||||||
|
"lever_information_singular_values": [
|
||||||
|
2.4406600443109308,
|
||||||
|
2.412462748498746,
|
||||||
|
0.06586096829714262
|
||||||
|
],
|
||||||
|
"lever_precision_rank": 3,
|
||||||
|
"position_residual_rms_xyz_m": [
|
||||||
|
0.0023101392989052683,
|
||||||
|
0.002794889168410421,
|
||||||
|
0.0005246719035281052
|
||||||
|
],
|
||||||
|
"velocity_residual_rms_xyz_m_s": [
|
||||||
|
1.1059333756244145,
|
||||||
|
1.368704336610928,
|
||||||
|
0.11603076744633863
|
||||||
|
],
|
||||||
|
"accel_bias_by_session_m_s2": {
|
||||||
|
"priority_174005_174515": [
|
||||||
|
0.015165392523580496,
|
||||||
|
0.10623623265620695,
|
||||||
|
0.015221547657584966
|
||||||
|
],
|
||||||
|
"priority_174905_175450": [
|
||||||
|
0.06725309871307823,
|
||||||
|
0.06785033078439083,
|
||||||
|
0.0171641139816057
|
||||||
|
],
|
||||||
|
"priority_175910_180530": [
|
||||||
|
0.015302724485125336,
|
||||||
|
0.10160587655627046,
|
||||||
|
0.015281702682303734
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"knot_count_by_session": {
|
||||||
|
"priority_174005_174515": 62,
|
||||||
|
"priority_174905_175450": 69,
|
||||||
|
"priority_175910_180530": 76
|
||||||
|
},
|
||||||
|
"loo_delta_m": {},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||||
|
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||||
|
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||||
|
"upstream rotation is not accepted, so translation is diagnostic only",
|
||||||
|
"translation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,81 @@
|
|||||||
|
{
|
||||||
|
"session_count": 4,
|
||||||
|
"sessions": [
|
||||||
|
{
|
||||||
|
"session_id": "slope_190548_190730",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 10199,
|
||||||
|
"rtk_samples": 1578,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8757921419518377,
|
||||||
|
"common_time_span_s": [
|
||||||
|
36466.328875,
|
||||||
|
36568.2301441
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4652183602,
|
||||||
|
114.090839984,
|
||||||
|
29.9891
|
||||||
|
],
|
||||||
|
"imu_source": "10199 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "circle_193412_193642",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 14989,
|
||||||
|
"rtk_samples": 2162,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8288621646623496,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38170.3385255,
|
||||||
|
38317.8571971
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654799242,
|
||||||
|
114.092155912,
|
||||||
|
29.4779
|
||||||
|
],
|
||||||
|
"imu_source": "14989 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "loop_194223_195003",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 45801,
|
||||||
|
"rtk_samples": 6741,
|
||||||
|
"fixed_position_ratio": 0.9998516540572615,
|
||||||
|
"fixed_attitude_ratio": 0.8377095386441181,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38661.2831251,
|
||||||
|
39121.1709553
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654856957,
|
||||||
|
114.092151015,
|
||||||
|
29.4651
|
||||||
|
],
|
||||||
|
"imu_source": "45801 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "accel_195608_195958",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 23002,
|
||||||
|
"rtk_samples": 3456,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8457754629629629,
|
||||||
|
"common_time_span_s": [
|
||||||
|
39486.3574509,
|
||||||
|
39716.3199405
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465443014,
|
||||||
|
114.092179168,
|
||||||
|
29.5309
|
||||||
|
],
|
||||||
|
"imu_source": "23002 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,96 @@
|
|||||||
|
{
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9999453769836999,
|
||||||
|
0.010155336209751328,
|
||||||
|
-0.002472285459453144
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.010163396322394882,
|
||||||
|
0.9999430054296535,
|
||||||
|
-0.003269750373547743
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002438939138240274,
|
||||||
|
0.003294698586865735,
|
||||||
|
0.9999915982332558
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rpy_deg": [
|
||||||
|
0.18877322677308359,
|
||||||
|
-0.13974105765052158,
|
||||||
|
-0.5823314723802753
|
||||||
|
],
|
||||||
|
"gyro_bias_by_session_rad_s": {
|
||||||
|
"slope_190548_190730": [
|
||||||
|
5.517563410757097e-05,
|
||||||
|
-1.0668622945796592e-05,
|
||||||
|
0.00015474647141927933
|
||||||
|
],
|
||||||
|
"circle_193412_193642": [
|
||||||
|
-4.092656265416791e-05,
|
||||||
|
-0.00017676217236757262,
|
||||||
|
-5.914138553067106e-05
|
||||||
|
],
|
||||||
|
"loop_194223_195003": [
|
||||||
|
-0.000196149370561509,
|
||||||
|
-0.0001486662046316796,
|
||||||
|
-3.6375768145656054e-05
|
||||||
|
],
|
||||||
|
"accel_195608_195958": [
|
||||||
|
-7.483644204529091e-05,
|
||||||
|
-4.17320520562614e-06,
|
||||||
|
5.861303135056825e-05
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"time_offset": {
|
||||||
|
"offset_s": 0.04000000000000031,
|
||||||
|
"peak_correlation": 0.7578237026149359,
|
||||||
|
"second_best_correlation": 0.757682618781891,
|
||||||
|
"evaluated_samples": 5443,
|
||||||
|
"reliable": false
|
||||||
|
},
|
||||||
|
"convention": {
|
||||||
|
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||||
|
"heading_sign": -1.0,
|
||||||
|
"pitch_sign": -1.0,
|
||||||
|
"roll_sign": 1.0
|
||||||
|
},
|
||||||
|
"convention_scores_deg": {
|
||||||
|
"north_cw__pitch_nose_up__roll_right_down": 0.9457675569035681,
|
||||||
|
"north_cw__pitch_opposite": 1.064327146928918,
|
||||||
|
"heading_opposite__pitch_nose_up": 5.753424726280977,
|
||||||
|
"heading_opposite__pitch_opposite": 13.362267974377751
|
||||||
|
},
|
||||||
|
"pair_count": 351,
|
||||||
|
"residual_rms_deg": 0.9428350359826574,
|
||||||
|
"residual_median_deg": 0.4993317818354644,
|
||||||
|
"residual_p95_deg": 1.5023594730593923,
|
||||||
|
"rotation_std_deg": [
|
||||||
|
0.191917244945292,
|
||||||
|
0.16245924204992213,
|
||||||
|
2.5710977852526464
|
||||||
|
],
|
||||||
|
"information_singular_values": [
|
||||||
|
125441.66767899474,
|
||||||
|
90572.5161990362,
|
||||||
|
496.5407001788204
|
||||||
|
],
|
||||||
|
"per_session_rms_deg": {
|
||||||
|
"slope_190548_190730": 2.386329048920402,
|
||||||
|
"circle_193412_193642": 0.560395991888806,
|
||||||
|
"loop_194223_195003": 0.6239401903763562,
|
||||||
|
"accel_195608_195958": 0.9797575195873669
|
||||||
|
},
|
||||||
|
"loo_delta_deg": {},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||||
|
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||||
|
"GNHPR convention score gap=0.1186 deg",
|
||||||
|
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||||
|
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||||
|
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||||
|
"rotation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
{
|
||||||
|
"status": "diagnostic_not_accepted",
|
||||||
|
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9999453769836999,
|
||||||
|
0.010155336209751328,
|
||||||
|
-0.002472285459453144
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.010163396322394882,
|
||||||
|
0.9999430054296535,
|
||||||
|
-0.003269750373547743
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002438939138240274,
|
||||||
|
0.003294698586865735,
|
||||||
|
0.9999915982332558
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": null,
|
||||||
|
"T_RTK_IMU": null,
|
||||||
|
"rotation_ok": false,
|
||||||
|
"translation_ok": null,
|
||||||
|
"rotation_result": "rotation_result.json",
|
||||||
|
"translation_result": null,
|
||||||
|
"dataset_audit": "dataset_audit.json"
|
||||||
|
}
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
{
|
||||||
|
"session_count": 8,
|
||||||
|
"sessions": [
|
||||||
|
{
|
||||||
|
"session_id": "priority_174005_174515",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 31000,
|
||||||
|
"rtk_samples": 4780,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.854602510460251,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16265.2325992,
|
||||||
|
16575.0298107
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465514786,
|
||||||
|
114.092169888,
|
||||||
|
29.4695
|
||||||
|
],
|
||||||
|
"imu_source": "31000 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_174905_175450",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 34499,
|
||||||
|
"rtk_samples": 5211,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8547303780464403,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16805.1862481,
|
||||||
|
17150.0907328
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4653424457,
|
||||||
|
114.092237385,
|
||||||
|
29.5366
|
||||||
|
],
|
||||||
|
"imu_source": "34499 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_175910_180530",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 37998,
|
||||||
|
"rtk_samples": 5786,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8567231247839613,
|
||||||
|
"common_time_span_s": [
|
||||||
|
17410.2121738,
|
||||||
|
17790.0458228
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654151935,
|
||||||
|
114.090796384,
|
||||||
|
29.5317
|
||||||
|
],
|
||||||
|
"imu_source": "37998 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "slope_190548_190730",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 10199,
|
||||||
|
"rtk_samples": 1578,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8757921419518377,
|
||||||
|
"common_time_span_s": [
|
||||||
|
36466.328875,
|
||||||
|
36568.2301441
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4652183602,
|
||||||
|
114.090839984,
|
||||||
|
29.9891
|
||||||
|
],
|
||||||
|
"imu_source": "10199 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "circle_193412_193642",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 14989,
|
||||||
|
"rtk_samples": 2162,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8288621646623496,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38170.3385255,
|
||||||
|
38317.8571971
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654799242,
|
||||||
|
114.092155912,
|
||||||
|
29.4779
|
||||||
|
],
|
||||||
|
"imu_source": "14989 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "loop_194223_195003",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 45801,
|
||||||
|
"rtk_samples": 6741,
|
||||||
|
"fixed_position_ratio": 0.9998516540572615,
|
||||||
|
"fixed_attitude_ratio": 0.8377095386441181,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38661.2831251,
|
||||||
|
39121.1709553
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654856957,
|
||||||
|
114.092151015,
|
||||||
|
29.4651
|
||||||
|
],
|
||||||
|
"imu_source": "45801 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "accel_195608_195958",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 23002,
|
||||||
|
"rtk_samples": 3456,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8457754629629629,
|
||||||
|
"common_time_span_s": [
|
||||||
|
39486.3574509,
|
||||||
|
39716.3199405
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465443014,
|
||||||
|
114.092179168,
|
||||||
|
29.5309
|
||||||
|
],
|
||||||
|
"imu_source": "23002 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "motion_sms_154023_154359",
|
||||||
|
"batch_id": "0819",
|
||||||
|
"imu_samples": 21556,
|
||||||
|
"rtk_samples": 3294,
|
||||||
|
"fixed_position_ratio": 0.49271402550091076,
|
||||||
|
"fixed_attitude_ratio": 0.4344262295081967,
|
||||||
|
"common_time_span_s": [
|
||||||
|
25829.8725525,
|
||||||
|
26027.4217529
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4651580863,
|
||||||
|
114.090773052,
|
||||||
|
36.5626
|
||||||
|
],
|
||||||
|
"imu_source": "21556 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,119 @@
|
|||||||
|
{
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996841792313951,
|
||||||
|
0.02499811511405725,
|
||||||
|
-0.002576050309343804
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.025002617750669198,
|
||||||
|
0.9996858874629868,
|
||||||
|
-0.0017307550243271697
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002531975526313299,
|
||||||
|
0.0017946164171362589,
|
||||||
|
0.9999951842143289
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rpy_deg": [
|
||||||
|
0.1028243313393031,
|
||||||
|
-0.1450716664951083,
|
||||||
|
-1.43269836393699
|
||||||
|
],
|
||||||
|
"gyro_bias_by_session_rad_s": {
|
||||||
|
"priority_174005_174515": [
|
||||||
|
-8.90944066727157e-05,
|
||||||
|
7.078609864979291e-05,
|
||||||
|
0.0001460390779870233
|
||||||
|
],
|
||||||
|
"priority_174905_175450": [
|
||||||
|
-4.327374730750894e-05,
|
||||||
|
-0.0001097548390022348,
|
||||||
|
3.000549725593387e-05
|
||||||
|
],
|
||||||
|
"priority_175910_180530": [
|
||||||
|
-7.036551812132934e-05,
|
||||||
|
-0.0008054961048184173,
|
||||||
|
1.3026193503057623e-05
|
||||||
|
],
|
||||||
|
"slope_190548_190730": [
|
||||||
|
6.035222904568755e-05,
|
||||||
|
-8.913884157613711e-06,
|
||||||
|
0.00015470949872434692
|
||||||
|
],
|
||||||
|
"circle_193412_193642": [
|
||||||
|
-3.696799268832588e-05,
|
||||||
|
-0.00019200076224600124,
|
||||||
|
-5.9567987995056394e-05
|
||||||
|
],
|
||||||
|
"loop_194223_195003": [
|
||||||
|
-0.0001979655352189002,
|
||||||
|
-0.00015937450336643818,
|
||||||
|
-4.343089303706036e-05
|
||||||
|
],
|
||||||
|
"accel_195608_195958": [
|
||||||
|
-7.841839817698704e-05,
|
||||||
|
-2.604932294598783e-06,
|
||||||
|
5.8586555655011475e-05
|
||||||
|
],
|
||||||
|
"motion_sms_154023_154359": [
|
||||||
|
0.00011745666467620586,
|
||||||
|
0.00021434926285955486,
|
||||||
|
-4.5147354343760625e-05
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"time_offset": {
|
||||||
|
"offset_s": 0.04000000000000031,
|
||||||
|
"peak_correlation": 0.5788788671828708,
|
||||||
|
"second_best_correlation": 0.5773967219219845,
|
||||||
|
"evaluated_samples": 12394,
|
||||||
|
"reliable": false
|
||||||
|
},
|
||||||
|
"convention": {
|
||||||
|
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||||
|
"heading_sign": -1.0,
|
||||||
|
"pitch_sign": -1.0,
|
||||||
|
"roll_sign": 1.0
|
||||||
|
},
|
||||||
|
"convention_scores_deg": {
|
||||||
|
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
|
||||||
|
"north_cw__pitch_opposite": 1.737582794831228,
|
||||||
|
"heading_opposite__pitch_nose_up": 7.263259116648845,
|
||||||
|
"heading_opposite__pitch_opposite": 17.716319437929055
|
||||||
|
},
|
||||||
|
"pair_count": 731,
|
||||||
|
"residual_rms_deg": 1.6276839301413086,
|
||||||
|
"residual_median_deg": 0.5695938849052221,
|
||||||
|
"residual_p95_deg": 3.0903953005641345,
|
||||||
|
"rotation_std_deg": [
|
||||||
|
0.2340126126406699,
|
||||||
|
0.20048574131667432,
|
||||||
|
2.314692068045375
|
||||||
|
],
|
||||||
|
"information_singular_values": [
|
||||||
|
82178.77940373585,
|
||||||
|
60441.86300479194,
|
||||||
|
612.6358640387364
|
||||||
|
],
|
||||||
|
"per_session_rms_deg": {
|
||||||
|
"priority_174005_174515": 0.6134232617562615,
|
||||||
|
"priority_174905_175450": 2.2485932625979106,
|
||||||
|
"priority_175910_180530": 3.095861798757508,
|
||||||
|
"slope_190548_190730": 2.388478834012087,
|
||||||
|
"circle_193412_193642": 0.5629885032374518,
|
||||||
|
"loop_194223_195003": 0.6250976606325254,
|
||||||
|
"accel_195608_195958": 0.975856888271022,
|
||||||
|
"motion_sms_154023_154359": 1.3698317592842066
|
||||||
|
},
|
||||||
|
"loo_delta_deg": {},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||||
|
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||||
|
"GNHPR convention score gap=0.1063 deg",
|
||||||
|
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||||
|
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||||
|
"rotation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
{
|
||||||
|
"status": "diagnostic_not_accepted",
|
||||||
|
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9996841792313951,
|
||||||
|
0.02499811511405725,
|
||||||
|
-0.002576050309343804
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.025002617750669198,
|
||||||
|
0.9996858874629868,
|
||||||
|
-0.0017307550243271697
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.002531975526313299,
|
||||||
|
0.0017946164171362589,
|
||||||
|
0.9999951842143289
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": null,
|
||||||
|
"T_RTK_IMU": null,
|
||||||
|
"rotation_ok": false,
|
||||||
|
"translation_ok": null,
|
||||||
|
"rotation_result": "rotation_result.json",
|
||||||
|
"translation_result": null,
|
||||||
|
"dataset_audit": "dataset_audit.json"
|
||||||
|
}
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
{
|
||||||
|
"session_count": 8,
|
||||||
|
"sessions": [
|
||||||
|
{
|
||||||
|
"session_id": "priority_174005_174515",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 31000,
|
||||||
|
"rtk_samples": 4780,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.854602510460251,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16265.2325992,
|
||||||
|
16575.0298107
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465514786,
|
||||||
|
114.092169888,
|
||||||
|
29.4695
|
||||||
|
],
|
||||||
|
"imu_source": "31000 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_174905_175450",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 34499,
|
||||||
|
"rtk_samples": 5211,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8547303780464403,
|
||||||
|
"common_time_span_s": [
|
||||||
|
16805.1862481,
|
||||||
|
17150.0907328
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4653424457,
|
||||||
|
114.092237385,
|
||||||
|
29.5366
|
||||||
|
],
|
||||||
|
"imu_source": "34499 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "priority_175910_180530",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 37998,
|
||||||
|
"rtk_samples": 5786,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8567231247839613,
|
||||||
|
"common_time_span_s": [
|
||||||
|
17410.2121738,
|
||||||
|
17790.0458228
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654151935,
|
||||||
|
114.090796384,
|
||||||
|
29.5317
|
||||||
|
],
|
||||||
|
"imu_source": "37998 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "slope_190548_190730",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 10199,
|
||||||
|
"rtk_samples": 1578,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8757921419518377,
|
||||||
|
"common_time_span_s": [
|
||||||
|
36466.328875,
|
||||||
|
36568.2301441
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4652183602,
|
||||||
|
114.090839984,
|
||||||
|
29.9891
|
||||||
|
],
|
||||||
|
"imu_source": "10199 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "circle_193412_193642",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 14989,
|
||||||
|
"rtk_samples": 2162,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8288621646623496,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38170.3385255,
|
||||||
|
38317.8571971
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654799242,
|
||||||
|
114.092155912,
|
||||||
|
29.4779
|
||||||
|
],
|
||||||
|
"imu_source": "14989 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "loop_194223_195003",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 45801,
|
||||||
|
"rtk_samples": 6741,
|
||||||
|
"fixed_position_ratio": 0.9998516540572615,
|
||||||
|
"fixed_attitude_ratio": 0.8377095386441181,
|
||||||
|
"common_time_span_s": [
|
||||||
|
38661.2831251,
|
||||||
|
39121.1709553
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654856957,
|
||||||
|
114.092151015,
|
||||||
|
29.4651
|
||||||
|
],
|
||||||
|
"imu_source": "45801 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "accel_195608_195958",
|
||||||
|
"batch_id": "0815",
|
||||||
|
"imu_samples": 23002,
|
||||||
|
"rtk_samples": 3456,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8457754629629629,
|
||||||
|
"common_time_span_s": [
|
||||||
|
39486.3574509,
|
||||||
|
39716.3199405
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.465443014,
|
||||||
|
114.092179168,
|
||||||
|
29.5309
|
||||||
|
],
|
||||||
|
"imu_source": "23002 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"session_id": "motion_sms_154023_154359",
|
||||||
|
"batch_id": "0819",
|
||||||
|
"imu_samples": 21556,
|
||||||
|
"rtk_samples": 3294,
|
||||||
|
"fixed_position_ratio": 0.49271402550091076,
|
||||||
|
"fixed_attitude_ratio": 0.4344262295081967,
|
||||||
|
"common_time_span_s": [
|
||||||
|
25829.8725525,
|
||||||
|
26027.4217529
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4651580863,
|
||||||
|
114.090773052,
|
||||||
|
36.5626
|
||||||
|
],
|
||||||
|
"imu_source": "21556 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,119 @@
|
|||||||
|
{
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
-0.999754381938718,
|
||||||
|
0.021803011844914608,
|
||||||
|
0.003975483470215314
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.021793866945340735,
|
||||||
|
0.9997597719617833,
|
||||||
|
-0.0023293197487744975
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.0040253146336934244,
|
||||||
|
-0.002242106467980424,
|
||||||
|
-0.9999893848440022
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rpy_deg": [
|
||||||
|
-179.8715356137635,
|
||||||
|
0.23063416255936714,
|
||||||
|
178.75119441523574
|
||||||
|
],
|
||||||
|
"gyro_bias_by_session_rad_s": {
|
||||||
|
"priority_174005_174515": [
|
||||||
|
-0.00018079953472427648,
|
||||||
|
-9.3706764772758e-05,
|
||||||
|
-1.6471539455612585e-05
|
||||||
|
],
|
||||||
|
"priority_174905_175450": [
|
||||||
|
-0.0001527801061740737,
|
||||||
|
-0.0001164065722445753,
|
||||||
|
-5.5002757975692905e-05
|
||||||
|
],
|
||||||
|
"priority_175910_180530": [
|
||||||
|
0.00010732176614734042,
|
||||||
|
-9.217694744503191e-05,
|
||||||
|
0.00037171055222583177
|
||||||
|
],
|
||||||
|
"slope_190548_190730": [
|
||||||
|
-0.0001400513048024026,
|
||||||
|
-9.490085614046164e-05,
|
||||||
|
-7.313488292136488e-05
|
||||||
|
],
|
||||||
|
"circle_193412_193642": [
|
||||||
|
-5.9488404294769446e-05,
|
||||||
|
-0.00029434501957892675,
|
||||||
|
0.000102661594942897
|
||||||
|
],
|
||||||
|
"loop_194223_195003": [
|
||||||
|
-0.0002980828626939094,
|
||||||
|
9.137138703447405e-05,
|
||||||
|
4.430930647242609e-05
|
||||||
|
],
|
||||||
|
"accel_195608_195958": [
|
||||||
|
5.9388956272647935e-05,
|
||||||
|
9.900621296014341e-06,
|
||||||
|
8.065768586636302e-05
|
||||||
|
],
|
||||||
|
"motion_sms_154023_154359": [
|
||||||
|
0.0016442968972331072,
|
||||||
|
0.00012387305081956675,
|
||||||
|
8.435528364463815e-08
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"time_offset": {
|
||||||
|
"offset_s": 0.08500000000000035,
|
||||||
|
"peak_correlation": 0.46191352508231565,
|
||||||
|
"second_best_correlation": 0.460889710037625,
|
||||||
|
"evaluated_samples": 12384,
|
||||||
|
"reliable": false
|
||||||
|
},
|
||||||
|
"convention": {
|
||||||
|
"name": "heading_opposite__pitch_nose_up",
|
||||||
|
"heading_sign": 1.0,
|
||||||
|
"pitch_sign": -1.0,
|
||||||
|
"roll_sign": 1.0
|
||||||
|
},
|
||||||
|
"convention_scores_deg": {
|
||||||
|
"north_cw__pitch_nose_up__roll_right_down": 1.6671304616155285,
|
||||||
|
"north_cw__pitch_opposite": 1.7955511909491206,
|
||||||
|
"heading_opposite__pitch_nose_up": 1.6669796415371239,
|
||||||
|
"heading_opposite__pitch_opposite": 19.709790964275243
|
||||||
|
},
|
||||||
|
"pair_count": 6164,
|
||||||
|
"residual_rms_deg": 1.6669796415371239,
|
||||||
|
"residual_median_deg": 0.6097138083592458,
|
||||||
|
"residual_p95_deg": 2.9952512236242987,
|
||||||
|
"rotation_std_deg": [
|
||||||
|
1.0204560247927847,
|
||||||
|
0.06460681216547948,
|
||||||
|
0.11838973994567728
|
||||||
|
],
|
||||||
|
"information_singular_values": [
|
||||||
|
818353.8002481281,
|
||||||
|
235774.70975965378,
|
||||||
|
3151.7390576048515
|
||||||
|
],
|
||||||
|
"per_session_rms_deg": {
|
||||||
|
"priority_174005_174515": 0.7068100152136895,
|
||||||
|
"priority_174905_175450": 1.7590065687347056,
|
||||||
|
"priority_175910_180530": 3.0674316757114104,
|
||||||
|
"slope_190548_190730": 2.07580304513421,
|
||||||
|
"circle_193412_193642": 0.6574485751543728,
|
||||||
|
"loop_194223_195003": 0.6594363752510715,
|
||||||
|
"accel_195608_195958": 0.7631418065216562,
|
||||||
|
"motion_sms_154023_154359": 3.709386878989403
|
||||||
|
},
|
||||||
|
"loo_delta_deg": {},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||||
|
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||||
|
"GNHPR convention score gap=0.0002 deg",
|
||||||
|
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||||
|
"empirical best GNHPR convention differs from protocol expectation; manual verification required",
|
||||||
|
"rotation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
{
|
||||||
|
"status": "diagnostic_not_accepted",
|
||||||
|
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
-0.999754381938718,
|
||||||
|
0.021803011844914608,
|
||||||
|
0.003975483470215314
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.021793866945340735,
|
||||||
|
0.9997597719617833,
|
||||||
|
-0.0023293197487744975
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.0040253146336934244,
|
||||||
|
-0.002242106467980424,
|
||||||
|
-0.9999893848440022
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": null,
|
||||||
|
"T_RTK_IMU": null,
|
||||||
|
"rotation_ok": false,
|
||||||
|
"translation_ok": null,
|
||||||
|
"rotation_result": "rotation_result.json",
|
||||||
|
"translation_result": null,
|
||||||
|
"dataset_audit": "dataset_audit.json"
|
||||||
|
}
|
||||||
@@ -0,0 +1,24 @@
|
|||||||
|
{
|
||||||
|
"session_count": 1,
|
||||||
|
"sessions": [
|
||||||
|
{
|
||||||
|
"session_id": "priority_175910_180530",
|
||||||
|
"batch_id": "0808",
|
||||||
|
"imu_samples": 37998,
|
||||||
|
"rtk_samples": 5786,
|
||||||
|
"fixed_position_ratio": 1.0,
|
||||||
|
"fixed_attitude_ratio": 0.8567231247839613,
|
||||||
|
"common_time_span_s": [
|
||||||
|
17410.2121738,
|
||||||
|
17790.0458228
|
||||||
|
],
|
||||||
|
"origin_geodetic": [
|
||||||
|
30.4654151935,
|
||||||
|
114.090796384,
|
||||||
|
29.5317
|
||||||
|
],
|
||||||
|
"imu_source": "37998 normalized samples",
|
||||||
|
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,76 @@
|
|||||||
|
{
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9974493913373024,
|
||||||
|
-0.07135460605626225,
|
||||||
|
0.0017977528752911507
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0713431523484532,
|
||||||
|
0.997435050761847,
|
||||||
|
0.005785682733905998
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.0022059768426676615,
|
||||||
|
-0.00564266836413856,
|
||||||
|
0.9999816468115312
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"rpy_deg": [
|
||||||
|
-0.32330358477785043,
|
||||||
|
0.1263932653005637,
|
||||||
|
4.0911470587568495
|
||||||
|
],
|
||||||
|
"gyro_bias_by_session_rad_s": {
|
||||||
|
"priority_175910_180530": [
|
||||||
|
-0.00010509442179307857,
|
||||||
|
-0.0001426783269500731,
|
||||||
|
3.086292346981605e-05
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"time_offset": {
|
||||||
|
"offset_s": -0.0949999999999998,
|
||||||
|
"peak_correlation": 0.1329249352233157,
|
||||||
|
"second_best_correlation": 0.13247995527618098,
|
||||||
|
"evaluated_samples": 2316,
|
||||||
|
"reliable": false
|
||||||
|
},
|
||||||
|
"convention": {
|
||||||
|
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||||
|
"heading_sign": -1.0,
|
||||||
|
"pitch_sign": -1.0,
|
||||||
|
"roll_sign": 1.0
|
||||||
|
},
|
||||||
|
"convention_scores_deg": {
|
||||||
|
"north_cw__pitch_nose_up__roll_right_down": 2.8406184024910064,
|
||||||
|
"north_cw__pitch_opposite": 2.8942770966870084,
|
||||||
|
"heading_opposite__pitch_nose_up": 3.8836333647028383,
|
||||||
|
"heading_opposite__pitch_opposite": 8.244842702849073
|
||||||
|
},
|
||||||
|
"pair_count": 91,
|
||||||
|
"residual_rms_deg": 2.8406184024910064,
|
||||||
|
"residual_median_deg": 1.624215282655001,
|
||||||
|
"residual_p95_deg": 6.087913500787023,
|
||||||
|
"rotation_std_deg": [
|
||||||
|
1.6029337786454214,
|
||||||
|
1.0761604678920118,
|
||||||
|
6.305203755276785
|
||||||
|
],
|
||||||
|
"information_singular_values": [
|
||||||
|
2941.184567996004,
|
||||||
|
1268.1548949190246,
|
||||||
|
82.52753961323997
|
||||||
|
],
|
||||||
|
"per_session_rms_deg": {
|
||||||
|
"priority_175910_180530": 2.8406184024910064
|
||||||
|
},
|
||||||
|
"loo_delta_deg": {},
|
||||||
|
"ok": false,
|
||||||
|
"notes": [
|
||||||
|
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||||
|
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||||
|
"GNHPR convention score gap=0.0537 deg",
|
||||||
|
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||||
|
"rotation failed one or more strict acceptance gates"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
{
|
||||||
|
"status": "diagnostic_not_accepted",
|
||||||
|
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||||
|
"R_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9974493913373024,
|
||||||
|
-0.07135460605626225,
|
||||||
|
0.0017977528752911507
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0713431523484532,
|
||||||
|
0.997435050761847,
|
||||||
|
0.005785682733905998
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.0022059768426676615,
|
||||||
|
-0.00564266836413856,
|
||||||
|
0.9999816468115312
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"t_RTK_IMU_m": null,
|
||||||
|
"T_RTK_IMU": null,
|
||||||
|
"rotation_ok": false,
|
||||||
|
"translation_ok": null,
|
||||||
|
"rotation_result": "rotation_result.json",
|
||||||
|
"translation_result": null,
|
||||||
|
"dataset_audit": "dataset_audit.json"
|
||||||
|
}
|
||||||
@@ -0,0 +1,24 @@
|
|||||||
|
# RTK-IMU V3 正式结果集
|
||||||
|
|
||||||
|
本目录只提交复现工程结论所需的审核结果。运行过程中的调试输出、状态快照、逐窗口 checkpoint 和旧版结果保留在本机，由仓库根目录 `.gitignore` 排除。
|
||||||
|
|
||||||
|
## 当前结论
|
||||||
|
|
||||||
|
- 固定旋转来源：`R2G_gravity_level_prior`。
|
||||||
|
- 工程候选杆臂：`l_I = p_ANT1^I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||||
|
- `data_only_translation_accepted=false`。
|
||||||
|
- `engineering_translation_accepted=false`，原因是独立传播验证仍受公共加速度偏差影响。
|
||||||
|
- `independent_extrinsic_sensitive_validation_passed=true`；当前结果可描述为机械杆臂经动态数据一致性验证的工程候选，不得描述为 data-only 平移标定结果。
|
||||||
|
|
||||||
|
## 文件说明
|
||||||
|
|
||||||
|
- `engineering_release_decision.json`：最终状态、变换和放行判定。
|
||||||
|
- `mechanical_prior_engineering_47_window.json`：相同 47 个非重叠窗口上的 free/fixed/prior 对比。
|
||||||
|
- `node_graph_free_information_selected_mechanical.json`：冻结的无先验 free-solve 基线。
|
||||||
|
- `lever_information_window_selection*.json`：窗口选择及实际边缘信息复核。
|
||||||
|
- `mechanical_prior_engineering_heldout.json`：未参与标定窗口的 held-out 验证。
|
||||||
|
- `heldout_independent_innovation.json`、`heldout_nonconverged_retry.json`：独立创新与失败窗口重试结果。
|
||||||
|
- `mechanical_prior_rotation_sensitivity.json`：固定候选杆臂的旋转扰动敏感性结果。
|
||||||
|
- `propagation_bias_root_cause_audit.json`：独立传播公共加速度误差根因审计。
|
||||||
|
|
||||||
|
结果 JSON 是审核快照；需要重新生成时应通过 `tools/` 中相应入口运行，且不得提交运行产生的 checkpoint 或 `.npz` 状态文件。
|
||||||
@@ -0,0 +1,128 @@
|
|||||||
|
{
|
||||||
|
"scope": "final mechanical-prior RTK-IMU engineering release decision",
|
||||||
|
"no_refit_performed": true,
|
||||||
|
"data_only_full_free_called": false,
|
||||||
|
"bootstrap_called": false,
|
||||||
|
"loo_called": false,
|
||||||
|
"covariance_retuned": false,
|
||||||
|
"new_window_selection_called": false,
|
||||||
|
"parser_R0_modified": false,
|
||||||
|
"data_only_translation_accepted": false,
|
||||||
|
"translation_refined_by_data": false,
|
||||||
|
"mechanical_prior_consistent_with_calibration": true,
|
||||||
|
"heldout_physical_validation_passed": true,
|
||||||
|
"heldout_physical_gate_checks": {
|
||||||
|
"all_267_converged_after_retry": true,
|
||||||
|
"BEST_position_vector_p95_le_0p20_m": true,
|
||||||
|
"Doppler_vector_p95_le_0p50_m_s": true,
|
||||||
|
"HPR_normalized_p95_le_4": true,
|
||||||
|
"preintegration_normalized_p95_le_3": true
|
||||||
|
},
|
||||||
|
"heldout_statistical_scale_passed": false,
|
||||||
|
"heldout_postfit_chi_square_per_dof": 0.12733913101711092,
|
||||||
|
"heldout_covariance_underdispersion_warning": true,
|
||||||
|
"independent_heldout_innovation_passed": false,
|
||||||
|
"common_constant_acceleration_error_detected": true,
|
||||||
|
"independent_propagation_validation_passed": false,
|
||||||
|
"independent_extrinsic_sensitive_validation_passed": true,
|
||||||
|
"rotation_sensitivity_passed": true,
|
||||||
|
"engineering_translation_acceptance_formula": "mechanical_prior_consistent_with_calibration AND heldout_physical_validation_passed AND independent_heldout_innovation_passed AND rotation_sensitivity_passed",
|
||||||
|
"engineering_translation_accepted": false,
|
||||||
|
"result_nature": "mechanically anchored + dynamically validated",
|
||||||
|
"summary": "Translation is mechanically anchored and dynamically validated. The current dataset does not independently observe translation accurately enough for data-only calibration, and does not provide meaningful refinement beyond the mechanical prior.",
|
||||||
|
"forbidden_descriptions": [
|
||||||
|
"data-only calibrated translation",
|
||||||
|
"dynamically refined mechanical lever"
|
||||||
|
],
|
||||||
|
"candidate_l_I_engineering_m": [
|
||||||
|
-0.45180151590212486,
|
||||||
|
-0.26447498198536895,
|
||||||
|
0.7314656114613277
|
||||||
|
],
|
||||||
|
"candidate_T_RTK_IMU": [
|
||||||
|
[
|
||||||
|
0.9999999761265028,
|
||||||
|
-0.00021395911860711003,
|
||||||
|
-4.436766104011944e-05,
|
||||||
|
0.45177737170031523
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.00021360059847267874,
|
||||||
|
0.9999685415936667,
|
||||||
|
-0.007929072948303898,
|
||||||
|
0.27036301129056084
|
||||||
|
],
|
||||||
|
[
|
||||||
|
4.606276276360097e-05,
|
||||||
|
0.007929063282050246,
|
||||||
|
0.9999685634227163,
|
||||||
|
-0.7293247665913783
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"candidate_T_IMU_RTK": [
|
||||||
|
[
|
||||||
|
0.9999999761265029,
|
||||||
|
0.00021360059847267876,
|
||||||
|
4.606276276360098e-05,
|
||||||
|
-0.45180151590212486
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-0.00021395911860711008,
|
||||||
|
0.9999685415936669,
|
||||||
|
0.007929063282050248,
|
||||||
|
-0.264474981985369
|
||||||
|
],
|
||||||
|
[
|
||||||
|
-4.436766104011945e-05,
|
||||||
|
-0.0079290729483039,
|
||||||
|
0.9999685634227164,
|
||||||
|
0.7314656114613278
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
1.0
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"candidate_transform_inverse_error_norm": 1.3597553244868544e-16,
|
||||||
|
"l_I_engineering_m": null,
|
||||||
|
"T_RTK_IMU": null,
|
||||||
|
"T_IMU_RTK": null,
|
||||||
|
"transform_convention": {
|
||||||
|
"equation": "p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU",
|
||||||
|
"translation": "t_RTK_IMU = -R_RTK_IMU * l_I",
|
||||||
|
"RTK_origin": "ANT1 phase center"
|
||||||
|
},
|
||||||
|
"rotation_source": "R2G_gravity_level_prior",
|
||||||
|
"translation_conditional_on_rotation": true,
|
||||||
|
"evidence": {
|
||||||
|
"calibration_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_47_window.json",
|
||||||
|
"heldout_postfit_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_heldout.json",
|
||||||
|
"innovation_path": "artifacts\\rtk_imu_calibration_v3\\heldout_independent_innovation.json",
|
||||||
|
"sensitivity_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_rotation_sensitivity.json",
|
||||||
|
"convergence_retry_path": "artifacts\\rtk_imu_calibration_v3\\heldout_nonconverged_retry.json",
|
||||||
|
"propagation_root_cause_path": "artifacts\\rtk_imu_calibration_v3\\propagation_bias_root_cause_audit.json",
|
||||||
|
"posterior_prior_variance_ratio": [
|
||||||
|
0.9841028247436912,
|
||||||
|
0.9831529061304226,
|
||||||
|
0.9921155953185713
|
||||||
|
],
|
||||||
|
"heldout_convergence_after_retry": 1.0,
|
||||||
|
"rotation_sensitivity_summary": {
|
||||||
|
"max_abs_delta_l_xyz_m": [
|
||||||
|
0.0024980444199615426,
|
||||||
|
0.0006588674774769543,
|
||||||
|
0.0007172660986609625
|
||||||
|
],
|
||||||
|
"max_delta_l_norm_m": 0.0025582764807350012,
|
||||||
|
"max_transform_translation_delta_norm_m": 0.00686033304738171
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
+1302
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,52 @@
|
|||||||
|
# RTK–IMU 候选数据清点（2026-08-20）
|
||||||
|
|
||||||
|
本目录记录 0808、0815、0819 三批 LiDAR/IMU 会话所对应的 G90 RTK 原始记录与当前导出状态。此处只做数据血缘和可用性评估，尚未求解 `T_RTK_IMU`。
|
||||||
|
|
||||||
|
## 时间与质量约定
|
||||||
|
|
||||||
|
- 原始 RTK 为 Wheeltec G90 V2 `.rscap`，包含 `$GNGGA/$GPGGA` 位置和 `$GNHPR` heading/pitch/roll。
|
||||||
|
- 切窗使用 NMEA 报文自带的测量 UTC，再通过每个会话的 IMU device→host affine clock 映射到 IMU 设备时间。
|
||||||
|
- 主机接收时间比 NMEA 测量时间晚约 3–4 s，且有波动；不能按接收时间直接切窗。
|
||||||
|
- 当前项目门控按 GGA `fix_quality=4` 和 HPR `heading_quality∈{4,5}` 判断固定位置/有效航向。
|
||||||
|
- `heading_valid` 未覆盖全部 GGA 行主要因为 GGA 与 HPR 频率不同、最近邻匹配阈值为 80 ms；不代表该会话航向整体失效。
|
||||||
|
|
||||||
|
## 数据映射与质量
|
||||||
|
|
||||||
|
详细机器可读清单见 `rtk_session_inventory.csv`。
|
||||||
|
|
||||||
|
| 会话 | RTK结论 | 适合的标定作用 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| `priority_174005_174515` | 100% GGA质量4；有效航向约309 s；yaw变化约485° | 多圈 yaw 与 XY 杠杆臂 |
|
||||||
|
| `priority_174905_175450` | 100% GGA质量4；Pitch跨度约11.4°；XY约58×22 m | 强候选：yaw、pitch、XY/Z耦合解除 |
|
||||||
|
| `priority_175910_180530` | 100% GGA质量4；Pitch跨度约13.6°；XY约144×59 m | 最强候选：长基线、pitch、平移 |
|
||||||
|
| `slope_190548_190730` | 100% GGA质量4；Pitch跨度约7.4°；约102 s | 坡度/Pitch补充 |
|
||||||
|
| `circle_193412_193642` | 100% GGA质量4；yaw变化约445° | 平面旋转与XY杠杆臂 |
|
||||||
|
| `loop_194223_195003` | 仅1个异常GGA；yaw累计变化约1203°；约460 s | 最强 yaw/多圈转弯候选 |
|
||||||
|
| `accel_195608_195958` | 100% GGA质量4；XY约46×23 m；约230 s | 加减速、速度和水平杠杆臂 |
|
||||||
|
| `motion_sms_154023_154359` | 全窗仅约49%为质量4；可用连续子段约100 s | 仅用15:41:28.2–15:43:08.15固定解/有效航向段 |
|
||||||
|
|
||||||
|
## 导出状态
|
||||||
|
|
||||||
|
- 0808 当前 `sessions_v2_device_affine` 原先没有 RTK CSV，本次已从原始 G90 `.rscap` 按 NMEA 测量 UTC补导三个会话，未覆盖旧文件。
|
||||||
|
- 0815 `sessions_v2_device_affine` 与 dense5 slope 已经采用同一测量时间导出规则，无需重导。
|
||||||
|
- 0819 `sessions_v2_device_affine` 与 dense5 motion 已经采用同一规则；需要在求解器中按质量与时间连续段过滤，而不是重新解释为全窗固定解。
|
||||||
|
- 0808 旧 `sessions_v1_host_aligned_00` RTK CSV 不含 `t_measurement_utc_s` 字段，只保留作历史对照；新求解应使用 `sessions_v2_device_affine`。
|
||||||
|
|
||||||
|
## 已发现的 LiDAR–RTK 资料边界
|
||||||
|
|
||||||
|
`D:\data\calibration_usable_20260808\rtk_lidar_station_report*` 保存的是静止站点候选：27个站点、29个候选段，并非包含 `T_RTK_lidar`、协方差和留一验证的正式手眼结果。本轮在 0808 数据目录的 JSON/YAML/Markdown/CSV/日志中没有找到 `T_RTK_lidar` 矩阵。若要通过链式关系得到 LiDAR–IMU，需要继续定位原手眼结果及其坐标约定：
|
||||||
|
|
||||||
|
```text
|
||||||
|
T_IMU_lidar = inverse(T_RTK_IMU) @ T_RTK_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
## 初步可行性判断
|
||||||
|
|
||||||
|
这些数据足以启动直接 RTK–IMU 标定，且比当前纯 LiDAR–IMU Phase-B 更有希望约束 XY：RTK 提供绝对位置，GNHPR 提供航向和 Pitch，多会话包含长基线、转弯、加减速与坡度。仍需注意：
|
||||||
|
|
||||||
|
1. GNHPR roll 的变化仅约0.006°–0.065°，不能指望它提供有效 roll 激励。
|
||||||
|
2. 应先用 RTK heading/pitch 角速度与 IMU gyro 做残余时间偏置和坐标轴验证，再求旋转。
|
||||||
|
3. 平移应使用 RTK绝对位置 + IMU预积分的联合状态模型，估计共享 `T_RTK_IMU`、每会话速度/bias；不应把RTK轨迹简单二次差分后直接最小二乘。
|
||||||
|
4. `motion_sms` 必须仅使用其连续固定解子段。
|
||||||
|
5. 跨0808/0815/0819时应使用每会话IMU bias，外参共享，并检查安装期间是否发生机械变动。
|
||||||
|
|
||||||
@@ -0,0 +1,9 @@
|
|||||||
|
session,batch,raw_rtk_rscap,current_rtk_csv,rows,fixed_gga_ratio,fixed_heading_valid_rows,valid_duration_s,xy_robust_span_x_m,xy_robust_span_y_m,altitude_robust_span_m,heading_unwrapped_span_deg,pitch_robust_span_deg,roll_robust_span_deg,notes
|
||||||
|
priority_174005_174515,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174005_174515\rtk.csv,4780,1.0000,4085,309.45,12.045,17.380,0.112,485.411,3.462,0.012,newly exported from NMEA measurement UTC
|
||||||
|
priority_174905_175450,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174905_175450\rtk.csv,5211,1.0000,4454,344.90,58.436,21.923,0.277,350.616,11.379,0.008,newly exported from NMEA measurement UTC
|
||||||
|
priority_175910_180530,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_175910_180530\rtk.csv,5786,1.0000,4957,379.85,143.775,58.657,0.971,260.213,13.574,0.065,newly exported from NMEA measurement UTC
|
||||||
|
slope_190548_190730,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-110519.251_e0edf32b-c82b-4a38-b5df-2c0e4cac364f.rscap,D:\data\0815\sessions_v2_device_affine\slope_190548_190730\rtk.csv,1578,1.0000,1382,101.90,11.354,18.567,0.714,155.356,7.399,0.026,root is named 0815 but raw measurement date is 2026-08-14 local
|
||||||
|
circle_193412_193642,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-113355.356_b4b37794-d4d6-433e-87e9-037cad5517d1.rscap,D:\data\0815\sessions_v2_device_affine\circle_193412_193642\rtk.csv,2162,1.0000,1792,147.25,10.105,10.277,0.097,444.994,2.834,0.006,raw capture has truncated tail but target messages are checksum-valid
|
||||||
|
loop_194223_195003,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-114200.914_36911c82-f8c0-451b-99e3-f5b663ec6115.rscap,D:\data\0815\sessions_v2_device_affine\loop_194223_195003\rtk.csv,6741,0.9999,5647,459.90,11.555,18.800,0.104,1202.601,2.727,0.009,one malformed/non-fixed GGA excluded
|
||||||
|
accel_195608_195958,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-115547.472_8722f326-8314-4db1-9ec4-bce185c54a78.rscap,D:\data\0815\sessions_v2_device_affine\accel_195608_195958\rtk.csv,3456,1.0000,2923,229.90,45.542,22.610,0.130,188.236,3.248,0.010,measurement-time export already present
|
||||||
|
motion_sms_154023_154359,0819,D:\data\0819\raw_serial_capture_v2\wheeltec-g90_20260819-074023.329_3d9da6eb-7ef8-4f7b-9262-9193328238b0.rscap,D:\data\0819\dense5\sessions_v2_device_affine\motion_sms_154023_154359\rtk.csv,3294,0.4924,1431,99.95,12.679,17.356,0.839,308.864,8.985,0.020,use only 15:41:28.200-15:43:08.150 fixed+valid sub-window
|
||||||
|
@@ -1,43 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Compare two T_body_lidar JSON files in parameter space and on SE(3)."""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
|
|
||||||
def main() -> int:
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--reference", type=Path, required=True)
|
|
||||||
parser.add_argument("--candidate", type=Path, required=True)
|
|
||||||
parser.add_argument("--output", type=Path, required=True)
|
|
||||||
args = parser.parse_args()
|
|
||||||
reference = json.loads(args.reference.read_text(encoding="utf-8-sig"))
|
|
||||||
candidate = json.loads(args.candidate.read_text(encoding="utf-8-sig"))
|
|
||||||
a = np.asarray(reference["matrix_4x4"], dtype=float)
|
|
||||||
b = np.asarray(candidate["matrix_4x4"], dtype=float)
|
|
||||||
delta = np.linalg.inv(a) @ b
|
|
||||||
result = {
|
|
||||||
"convention": "delta = inverse(reference) @ candidate",
|
|
||||||
"reference": str(args.reference.resolve()),
|
|
||||||
"candidate": str(args.candidate.resolve()),
|
|
||||||
"candidate_minus_reference_translation_xyz_m": (b[:3, 3] - a[:3, 3]).tolist(),
|
|
||||||
"candidate_minus_reference_rpy_xyz_deg": (
|
|
||||||
np.asarray(candidate["rotation_rpy_deg_xyz"], float)
|
|
||||||
- np.asarray(reference["rotation_rpy_deg_xyz"], float)
|
|
||||||
).tolist(),
|
|
||||||
"relative_translation_norm_m": float(np.linalg.norm(delta[:3, 3])),
|
|
||||||
"relative_rotation_deg": float(np.degrees(Rotation.from_matrix(delta[:3, :3]).magnitude())),
|
|
||||||
"relative_matrix_4x4": delta.tolist(),
|
|
||||||
}
|
|
||||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
args.output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
print(json.dumps(result, ensure_ascii=False, indent=2))
|
|
||||||
return 0
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
raise SystemExit(main())
|
|
||||||
@@ -1,81 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Keep common A/B edges on which Open3D and small_gicp agree, without using X."""
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
|
|
||||||
def key(meta):
|
|
||||||
return int(meta[0]), int(meta[1])
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--open3d-pairs", required=True)
|
|
||||||
parser.add_argument("--small-pairs", required=True)
|
|
||||||
parser.add_argument("--output", required=True)
|
|
||||||
parser.add_argument("--audit")
|
|
||||||
parser.add_argument("--max-translation", type=float, default=0.05)
|
|
||||||
parser.add_argument("--max-rotation", type=float, default=0.50)
|
|
||||||
parser.add_argument("--min-pairs", type=int, default=25)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
with np.load(args.open3d_pairs, allow_pickle=False) as source:
|
|
||||||
open_a = np.asarray(source["A"], float)
|
|
||||||
open_b = np.asarray(source["B"], float)
|
|
||||||
open_meta = np.asarray(source["meta"], float)
|
|
||||||
station_times = np.asarray(source["station_times"])
|
|
||||||
rtk_dt = np.asarray(source["rtk_nearest_dt_s"])
|
|
||||||
with np.load(args.small_pairs, allow_pickle=False) as source:
|
|
||||||
small = {key(meta): np.asarray(b, float)
|
|
||||||
for meta, b in zip(source["meta"], source["B"])}
|
|
||||||
|
|
||||||
keep, audit = [], []
|
|
||||||
for meta, b_open in zip(open_meta, open_b):
|
|
||||||
edge = key(meta)
|
|
||||||
if edge not in small:
|
|
||||||
audit.append({"i": edge[0], "j": edge[1], "accepted": False,
|
|
||||||
"reason": "not_in_small_gicp_refined"})
|
|
||||||
keep.append(False)
|
|
||||||
continue
|
|
||||||
delta = np.linalg.inv(b_open) @ small[edge]
|
|
||||||
translation = float(np.linalg.norm(delta[:3, 3]))
|
|
||||||
rotation = float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude()))
|
|
||||||
accepted = translation <= args.max_translation and rotation <= args.max_rotation
|
|
||||||
keep.append(accepted)
|
|
||||||
audit.append({
|
|
||||||
"i": edge[0], "j": edge[1],
|
|
||||||
"open3d_small_translation_m": translation,
|
|
||||||
"open3d_small_rotation_deg": rotation,
|
|
||||||
"accepted": accepted,
|
|
||||||
"reason": "" if accepted else "backend_disagreement",
|
|
||||||
})
|
|
||||||
keep = np.asarray(keep, bool)
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
np.savez_compressed(
|
|
||||||
output, A=open_a[keep], B=open_b[keep], meta=open_meta[keep],
|
|
||||||
station_times=station_times, rtk_nearest_dt_s=rtk_dt,
|
|
||||||
backend=np.asarray("open3d_gicp_cross_backend_consensus"),
|
|
||||||
)
|
|
||||||
audit_path = Path(args.audit or output.with_suffix(".consensus.json"))
|
|
||||||
audit_path.write_text(json.dumps({
|
|
||||||
"selection_is_X_independent": True,
|
|
||||||
"B_source": "Open3D; small_gicp is used only as an agreement gate",
|
|
||||||
"max_translation_m": args.max_translation,
|
|
||||||
"max_rotation_deg": args.max_rotation,
|
|
||||||
"input_open3d_pairs": len(open_b),
|
|
||||||
"accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"pairs": audit,
|
|
||||||
}, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
if np.count_nonzero(keep) < args.min_pairs:
|
|
||||||
raise RuntimeError(f"only {np.count_nonzero(keep)} consensus pairs")
|
|
||||||
print(json.dumps({"accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,58 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Publish the cross-backend-consensus result as the recommended deliverable."""
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--consensus-extrinsic", required=True)
|
|
||||||
parser.add_argument("--consensus-check", required=True)
|
|
||||||
parser.add_argument("--open3d-extrinsic", required=True)
|
|
||||||
parser.add_argument("--small-extrinsic", required=True)
|
|
||||||
parser.add_argument("--output", required=True)
|
|
||||||
parser.add_argument("--summary", required=True)
|
|
||||||
args = parser.parse_args()
|
|
||||||
consensus = json.loads(Path(args.consensus_extrinsic).read_text(encoding="utf-8-sig"))
|
|
||||||
check = json.loads(Path(args.consensus_check).read_text(encoding="utf-8-sig"))
|
|
||||||
open3d = json.loads(Path(args.open3d_extrinsic).read_text(encoding="utf-8-sig"))
|
|
||||||
small = json.loads(Path(args.small_extrinsic).read_text(encoding="utf-8-sig"))
|
|
||||||
summary = {
|
|
||||||
"recommended_method": "Open3D B gated by Open3D-small_gicp cross-backend agreement",
|
|
||||||
"selection_is_X_independent": True,
|
|
||||||
"second_batch_role": "estimation (dense RTK)",
|
|
||||||
"first_batch_role": "auxiliary check only (sparse RTK)",
|
|
||||||
"consensus": {
|
|
||||||
"translation_m": consensus["translation_m"],
|
|
||||||
"rotation_rpy_deg_xyz": consensus["rotation_rpy_deg_xyz"],
|
|
||||||
"estimation": consensus["estimation"]["residuals"],
|
|
||||||
"bootstrap_std": consensus["bootstrap"]["std"],
|
|
||||||
"batch1_auxiliary": check["metrics"],
|
|
||||||
},
|
|
||||||
"separate_backend_results": {
|
|
||||||
"open3d_gicp": {
|
|
||||||
"translation_m": open3d["translation_m"],
|
|
||||||
"rotation_rpy_deg_xyz": open3d["rotation_rpy_deg_xyz"],
|
|
||||||
},
|
|
||||||
"small_gicp": {
|
|
||||||
"translation_m": small["translation_m"],
|
|
||||||
"rotation_rpy_deg_xyz": small["rotation_rpy_deg_xyz"],
|
|
||||||
},
|
|
||||||
},
|
|
||||||
"warning": "AX rotation RMS remains about one degree; this is not centimetre-grade absolute certification.",
|
|
||||||
}
|
|
||||||
published = dict(consensus)
|
|
||||||
published["selection"] = {
|
|
||||||
"method": summary["recommended_method"],
|
|
||||||
"selection_is_X_independent": True,
|
|
||||||
"consensus_pair_threshold": "Open3D-small_gicp B delta <= 0.05 m and <= 0.50 deg",
|
|
||||||
"warning": summary["warning"],
|
|
||||||
}
|
|
||||||
Path(args.output).write_text(json.dumps(published, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
Path(args.summary).write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,87 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""X-independent second-stage filter for stationary A/B pairs."""
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from rigorous_calibration import read_pairs, rotation_angle_deg
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--pairs", required=True)
|
|
||||||
parser.add_argument("--quality-json", required=True)
|
|
||||||
parser.add_argument("--output", required=True)
|
|
||||||
parser.add_argument("--audit")
|
|
||||||
parser.add_argument("--min-pairs", type=int, default=25)
|
|
||||||
parser.add_argument("--min-inlier-ratio", type=float, default=0.70)
|
|
||||||
parser.add_argument("--max-inlier-rmse", type=float, default=0.13)
|
|
||||||
parser.add_argument("--max-rotation-invariant-error", type=float, default=0.75)
|
|
||||||
parser.add_argument("--reverse-translation-tolerance", type=float, default=0.05)
|
|
||||||
parser.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
a_array, b_array, meta, _ = read_pairs(args.pairs)
|
|
||||||
quality = json.loads(Path(args.quality_json).read_text(encoding="utf-8-sig"))
|
|
||||||
reports = {(int(item["i"]), int(item["j"])): item for item in quality["pairs"]}
|
|
||||||
keep, audit = [], []
|
|
||||||
for a_ij, b_ij, item_meta in zip(a_array, b_array, meta):
|
|
||||||
key = (int(item_meta[0]), int(item_meta[1]))
|
|
||||||
report = reports[key]
|
|
||||||
heldout = report["heldout_symmetric"]
|
|
||||||
reverse = report["forward_reverse"]
|
|
||||||
invariant = abs(rotation_angle_deg(a_ij[:3, :3]) - rotation_angle_deg(b_ij[:3, :3]))
|
|
||||||
reasons = []
|
|
||||||
if heldout["inlier_ratio"] < args.min_inlier_ratio:
|
|
||||||
reasons.append("overlap_ratio")
|
|
||||||
if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
|
|
||||||
reasons.append("heldout_rmse")
|
|
||||||
if invariant > args.max_rotation_invariant_error:
|
|
||||||
reasons.append("rotation_conjugacy_invariant")
|
|
||||||
if reverse["translation_m"] > args.reverse_translation_tolerance:
|
|
||||||
reasons.append("forward_reverse_translation")
|
|
||||||
if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
|
|
||||||
reasons.append("forward_reverse_rotation")
|
|
||||||
accepted = not reasons
|
|
||||||
keep.append(accepted)
|
|
||||||
audit.append({
|
|
||||||
"i": key[0], "j": key[1], "heldout_inlier_ratio": heldout["inlier_ratio"],
|
|
||||||
"heldout_inlier_rmse_m": heldout["inlier_rmse_m"],
|
|
||||||
"rotation_invariant_error_deg": invariant,
|
|
||||||
"reverse_translation_m": reverse["translation_m"],
|
|
||||||
"reverse_rotation_deg": reverse["rotation_deg"],
|
|
||||||
"accepted": accepted, "rejection_reasons": reasons,
|
|
||||||
})
|
|
||||||
keep = np.asarray(keep, bool)
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
with np.load(args.pairs, allow_pickle=False) as source:
|
|
||||||
np.savez_compressed(
|
|
||||||
output, A=a_array[keep], B=b_array[keep], meta=meta[keep],
|
|
||||||
station_times=np.asarray(source["station_times"]),
|
|
||||||
rtk_nearest_dt_s=np.asarray(source["rtk_nearest_dt_s"]),
|
|
||||||
backend=np.asarray(source["backend"]),
|
|
||||||
)
|
|
||||||
audit_path = Path(args.audit or output.with_suffix(".refinement.json"))
|
|
||||||
audit_path.write_text(json.dumps({
|
|
||||||
"selection_is_X_independent": True,
|
|
||||||
"criteria": {
|
|
||||||
"min_inlier_ratio": args.min_inlier_ratio,
|
|
||||||
"max_inlier_rmse_m": args.max_inlier_rmse,
|
|
||||||
"max_rotation_invariant_error_deg": args.max_rotation_invariant_error,
|
|
||||||
"reverse_translation_tolerance_m": args.reverse_translation_tolerance,
|
|
||||||
"reverse_rotation_tolerance_deg": args.reverse_rotation_tolerance,
|
|
||||||
},
|
|
||||||
"input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"pairs": audit,
|
|
||||||
}, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
if np.count_nonzero(keep) < args.min_pairs:
|
|
||||||
raise RuntimeError(f"only {np.count_nonzero(keep)} refined pairs; need {args.min_pairs}")
|
|
||||||
print(json.dumps({"input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
|
|
||||||
"output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,769 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Rigorous stationary LiDAR / dual-antenna RTK hand-eye calibration.
|
|
||||||
|
|
||||||
Convention: T_A_B maps points from frame B into frame A.
|
|
||||||
X = T_body_lidar, A_ij = T_W_Bi^-1 T_W_Bj, B_ij = T_Li_Lj,
|
|
||||||
therefore A_ij X = X B_ij. Raw sensor-frame points_raw are used.
|
|
||||||
"""
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import csv
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
import time
|
|
||||||
from dataclasses import dataclass
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.optimize import least_squares
|
|
||||||
from scipy.spatial import cKDTree
|
|
||||||
|
|
||||||
|
|
||||||
def skew(v):
|
|
||||||
x, y, z = v
|
|
||||||
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
|
|
||||||
|
|
||||||
|
|
||||||
def so3_exp(v):
|
|
||||||
angle = float(np.linalg.norm(v))
|
|
||||||
if angle < 1e-12:
|
|
||||||
return np.eye(3) + skew(v)
|
|
||||||
k = skew(np.asarray(v, float) / angle)
|
|
||||||
return np.eye(3) + math.sin(angle) * k + (1.0 - math.cos(angle)) * k @ k
|
|
||||||
|
|
||||||
|
|
||||||
def so3_log(rotation):
|
|
||||||
cosine = float(np.clip((np.trace(rotation) - 1.0) / 2.0, -1.0, 1.0))
|
|
||||||
angle = math.acos(cosine)
|
|
||||||
vee = np.array([
|
|
||||||
rotation[2, 1] - rotation[1, 2],
|
|
||||||
rotation[0, 2] - rotation[2, 0],
|
|
||||||
rotation[1, 0] - rotation[0, 1],
|
|
||||||
])
|
|
||||||
if angle < 1e-9:
|
|
||||||
return vee / 2.0
|
|
||||||
if abs(math.pi - angle) < 1e-5:
|
|
||||||
values, vectors = np.linalg.eigh((rotation + np.eye(3)) / 2.0)
|
|
||||||
return vectors[:, int(np.argmax(values))] * angle
|
|
||||||
return vee * angle / (2.0 * math.sin(angle))
|
|
||||||
|
|
||||||
|
|
||||||
def quat_to_rotation(q):
|
|
||||||
x, y, z, w = np.asarray(q, float) / np.linalg.norm(q)
|
|
||||||
return np.array([
|
|
||||||
[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
|
|
||||||
[2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
|
|
||||||
[2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)],
|
|
||||||
])
|
|
||||||
|
|
||||||
|
|
||||||
def rotation_to_quat(rotation):
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
return Rotation.from_matrix(rotation).as_quat()
|
|
||||||
|
|
||||||
|
|
||||||
def rpy_deg(rotation):
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
return Rotation.from_matrix(rotation).as_euler("xyz", degrees=True).tolist()
|
|
||||||
|
|
||||||
|
|
||||||
def make_transform(translation, rotation):
|
|
||||||
transform = np.eye(4)
|
|
||||||
transform[:3, :3] = rotation
|
|
||||||
transform[:3, 3] = translation
|
|
||||||
return transform
|
|
||||||
|
|
||||||
|
|
||||||
def params_transform(params):
|
|
||||||
return make_transform(params[:3], so3_exp(params[3:]))
|
|
||||||
|
|
||||||
|
|
||||||
def inverse_transform(transform):
|
|
||||||
answer = np.eye(4)
|
|
||||||
answer[:3, :3] = transform[:3, :3].T
|
|
||||||
answer[:3, 3] = -answer[:3, :3] @ transform[:3, 3]
|
|
||||||
return answer
|
|
||||||
|
|
||||||
|
|
||||||
def transform_points(points, transform):
|
|
||||||
return points @ transform[:3, :3].T + transform[:3, 3]
|
|
||||||
|
|
||||||
|
|
||||||
def rotation_angle_deg(rotation):
|
|
||||||
return math.degrees(np.linalg.norm(so3_log(rotation)))
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class PoseSeries:
|
|
||||||
time: np.ndarray
|
|
||||||
transforms: np.ndarray
|
|
||||||
|
|
||||||
|
|
||||||
def read_poses(path):
|
|
||||||
timestamps, transforms = [], []
|
|
||||||
with Path(path).open(encoding="utf-8-sig", newline="") as stream:
|
|
||||||
reader = csv.DictReader(stream)
|
|
||||||
required = ("time", "x", "y", "z", "qx", "qy", "qz", "qw")
|
|
||||||
missing = [key for key in required if key not in (reader.fieldnames or [])]
|
|
||||||
if missing:
|
|
||||||
raise ValueError(f"{path}: missing pose fields {missing}")
|
|
||||||
for row in reader:
|
|
||||||
timestamps.append(float(row["time"]))
|
|
||||||
translation = np.array([float(row[k]) for k in ("x", "y", "z")])
|
|
||||||
quaternion = np.array([float(row[k]) for k in ("qx", "qy", "qz", "qw")])
|
|
||||||
transforms.append(make_transform(translation, quat_to_rotation(quaternion)))
|
|
||||||
order = np.argsort(timestamps)
|
|
||||||
return PoseSeries(np.asarray(timestamps)[order], np.asarray(transforms)[order])
|
|
||||||
|
|
||||||
|
|
||||||
def nearest_pose(series, timestamp):
|
|
||||||
index = int(np.argmin(np.abs(series.time - timestamp)))
|
|
||||||
return series.transforms[index], float(abs(series.time[index] - timestamp))
|
|
||||||
|
|
||||||
|
|
||||||
def npz_files(root):
|
|
||||||
files = sorted(Path(root).rglob("*.npz"))
|
|
||||||
if not files:
|
|
||||||
raise FileNotFoundError(f"no NPZ files under {root}")
|
|
||||||
return files
|
|
||||||
|
|
||||||
|
|
||||||
def load_npz_xyz(path, min_range=1.0, max_range=50.0):
|
|
||||||
with np.load(path, allow_pickle=False) as data:
|
|
||||||
if "points_raw" not in data:
|
|
||||||
raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden")
|
|
||||||
raw = np.asarray(data["points_raw"], dtype=np.float64)
|
|
||||||
timestamp = float(np.ravel(data["unix_time_ns"])[0]) / 1e9
|
|
||||||
counter = int(np.ravel(data["frame_counter"])[0])
|
|
||||||
distance = raw[:, 0] * 0.001
|
|
||||||
azimuth = np.deg2rad(raw[:, 1])
|
|
||||||
altitude = np.deg2rad(raw[:, 2])
|
|
||||||
valid = (
|
|
||||||
np.isfinite(distance + azimuth + altitude)
|
|
||||||
& (distance >= min_range)
|
|
||||||
& (distance <= max_range)
|
|
||||||
)
|
|
||||||
distance, azimuth, altitude = distance[valid], azimuth[valid], altitude[valid]
|
|
||||||
xyz = np.column_stack((
|
|
||||||
distance * np.cos(altitude) * np.cos(azimuth),
|
|
||||||
distance * np.cos(altitude) * np.sin(azimuth),
|
|
||||||
distance * np.sin(altitude),
|
|
||||||
))
|
|
||||||
return timestamp, counter, xyz
|
|
||||||
|
|
||||||
|
|
||||||
def load_stations(root, min_range, max_range, z_min=None, z_max=None):
|
|
||||||
stations = []
|
|
||||||
for path in npz_files(root):
|
|
||||||
timestamp, counter, xyz = load_npz_xyz(path, min_range, max_range)
|
|
||||||
if z_min is not None:
|
|
||||||
xyz = xyz[(xyz[:, 2] >= z_min) & (xyz[:, 2] <= z_max)]
|
|
||||||
stations.append((timestamp, counter, path, xyz))
|
|
||||||
stations.sort(key=lambda item: item[0])
|
|
||||||
return stations
|
|
||||||
|
|
||||||
|
|
||||||
def split_holdout(points, fraction, phase):
|
|
||||||
stride = max(int(round(1.0 / fraction)), 2)
|
|
||||||
index = np.arange(len(points))
|
|
||||||
holdout = ((index + phase) % stride) == 0
|
|
||||||
return points[~holdout], points[holdout]
|
|
||||||
|
|
||||||
|
|
||||||
def make_o3d_cloud(points, voxel):
|
|
||||||
import open3d as o3d
|
|
||||||
cloud = o3d.geometry.PointCloud()
|
|
||||||
cloud.points = o3d.utility.Vector3dVector(np.asarray(points, float))
|
|
||||||
return cloud.voxel_down_sample(voxel)
|
|
||||||
|
|
||||||
|
|
||||||
def align_open3d(target, source, initial, voxels, correspondences, iterations):
|
|
||||||
import open3d as o3d
|
|
||||||
registration = o3d.pipelines.registration
|
|
||||||
estimate = registration.TransformationEstimationForGeneralizedICP()
|
|
||||||
criteria = registration.ICPConvergenceCriteria(max_iteration=iterations)
|
|
||||||
transform, stages = np.asarray(initial, float), []
|
|
||||||
final_target = final_source = final_answer = None
|
|
||||||
started = time.perf_counter()
|
|
||||||
for voxel, correspondence in zip(voxels, correspondences):
|
|
||||||
target_cloud = make_o3d_cloud(target, voxel)
|
|
||||||
source_cloud = make_o3d_cloud(source, voxel)
|
|
||||||
answer = registration.registration_generalized_icp(
|
|
||||||
source_cloud, target_cloud, correspondence, transform, estimate, criteria
|
|
||||||
)
|
|
||||||
transform = np.asarray(answer.transformation, float)
|
|
||||||
stages.append({
|
|
||||||
"voxel_m": voxel,
|
|
||||||
"max_correspondence_m": correspondence,
|
|
||||||
"fitness": float(answer.fitness),
|
|
||||||
"inlier_rmse_m": float(answer.inlier_rmse),
|
|
||||||
"target_points": len(target_cloud.points),
|
|
||||||
"source_points": len(source_cloud.points),
|
|
||||||
})
|
|
||||||
final_target, final_source, final_answer = target_cloud, source_cloud, answer
|
|
||||||
information = registration.get_information_matrix_from_point_clouds(
|
|
||||||
final_source, final_target, correspondences[-1], transform
|
|
||||||
)
|
|
||||||
inliers = int(round(float(final_answer.fitness) * len(final_source.points)))
|
|
||||||
return {
|
|
||||||
"transform": transform,
|
|
||||||
"hessian": np.asarray(information, float),
|
|
||||||
"converged": None,
|
|
||||||
"iterations": None,
|
|
||||||
"num_inliers": inliers,
|
|
||||||
"objective": float(final_answer.inlier_rmse ** 2 * max(inliers, 1)),
|
|
||||||
"elapsed_sec": time.perf_counter() - started,
|
|
||||||
"stages": stages,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def align_small_gicp(target, source, initial, voxels, correspondences, iterations, threads):
|
|
||||||
import small_gicp
|
|
||||||
transform, stages, result = np.asarray(initial, float), [], None
|
|
||||||
started = time.perf_counter()
|
|
||||||
for voxel, correspondence in zip(voxels, correspondences):
|
|
||||||
result = small_gicp.align(
|
|
||||||
np.ascontiguousarray(target),
|
|
||||||
np.ascontiguousarray(source),
|
|
||||||
transform,
|
|
||||||
registration_type="GICP",
|
|
||||||
downsampling_resolution=voxel,
|
|
||||||
max_correspondence_distance=correspondence,
|
|
||||||
num_threads=threads,
|
|
||||||
max_iterations=iterations,
|
|
||||||
rotation_epsilon=math.radians(0.005),
|
|
||||||
translation_epsilon=0.0005,
|
|
||||||
verbose=False,
|
|
||||||
)
|
|
||||||
transform = np.asarray(result.T_target_source, float)
|
|
||||||
stages.append({
|
|
||||||
"voxel_m": voxel,
|
|
||||||
"max_correspondence_m": correspondence,
|
|
||||||
"converged": bool(result.converged),
|
|
||||||
"iterations": int(result.iterations),
|
|
||||||
"num_inliers": int(result.num_inliers),
|
|
||||||
"objective": float(result.error),
|
|
||||||
})
|
|
||||||
return {
|
|
||||||
"transform": transform,
|
|
||||||
"hessian": np.asarray(result.H, float),
|
|
||||||
"converged": bool(result.converged),
|
|
||||||
"iterations": int(result.iterations),
|
|
||||||
"num_inliers": int(result.num_inliers),
|
|
||||||
"objective": float(result.error),
|
|
||||||
"elapsed_sec": time.perf_counter() - started,
|
|
||||||
"stages": stages,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def align_backend(backend, target, source, initial, args):
|
|
||||||
if backend == "open3d":
|
|
||||||
return align_open3d(
|
|
||||||
target, source, initial, args.voxels, args.correspondences, args.iterations
|
|
||||||
)
|
|
||||||
return align_small_gicp(
|
|
||||||
target, source, initial, args.voxels, args.correspondences,
|
|
||||||
args.iterations, args.threads
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def symmetric_heldout_metrics(target_fit, target_holdout, source_fit, source_holdout,
|
|
||||||
transform, threshold):
|
|
||||||
transformed_source_fit = transform_points(source_fit, transform)
|
|
||||||
transformed_source_holdout = transform_points(source_holdout, transform)
|
|
||||||
forward = cKDTree(target_fit).query(transformed_source_holdout, workers=-1)[0]
|
|
||||||
reverse = cKDTree(transformed_source_fit).query(target_holdout, workers=-1)[0]
|
|
||||||
distances = np.concatenate((forward, reverse))
|
|
||||||
inliers = distances[distances <= threshold]
|
|
||||||
return {
|
|
||||||
"evaluated": int(len(distances)),
|
|
||||||
"inliers": int(len(inliers)),
|
|
||||||
"inlier_ratio": float(len(inliers) / max(len(distances), 1)),
|
|
||||||
"inlier_rmse_m": float(np.sqrt(np.mean(inliers**2))) if len(inliers) else None,
|
|
||||||
"median_m": float(np.median(distances)),
|
|
||||||
"p90_m": float(np.quantile(distances, 0.90)),
|
|
||||||
"p95_m": float(np.quantile(distances, 0.95)),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def hessian_metrics(hessian, characteristic_length=10.0):
|
|
||||||
hessian = 0.5 * (np.asarray(hessian, float) + np.asarray(hessian, float).T)
|
|
||||||
scale = np.diag([1.0 / characteristic_length] * 3 + [1.0] * 3)
|
|
||||||
scaled = scale.T @ hessian @ scale
|
|
||||||
values, vectors = np.linalg.eigh(scaled)
|
|
||||||
largest = max(float(np.max(np.abs(values))), np.finfo(float).eps)
|
|
||||||
positive = values[values > largest * 1e-9]
|
|
||||||
condition = float(positive[-1] / positive[0]) if len(positive) else float("inf")
|
|
||||||
return {
|
|
||||||
"native_order": ["rx_rad", "ry_rad", "rz_rad", "tx_m", "ty_m", "tz_m"],
|
|
||||||
"scaled_eigenvalues": values.tolist(),
|
|
||||||
"effective_rank": int(len(positive)),
|
|
||||||
"scaled_condition_number": condition,
|
|
||||||
"weakest_scaled_direction": vectors[:, int(np.argmin(values))].tolist(),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def transform_difference(reference, candidate):
|
|
||||||
delta = inverse_transform(reference) @ candidate
|
|
||||||
return {
|
|
||||||
"translation_m": float(np.linalg.norm(delta[:3, 3])),
|
|
||||||
"rotation_deg": rotation_angle_deg(delta[:3, :3]),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def loop_metrics(transforms):
|
|
||||||
loops = []
|
|
||||||
for (i, j), b_ij in transforms.items():
|
|
||||||
for (j2, k), b_jk in transforms.items():
|
|
||||||
if j2 != j or (i, k) not in transforms:
|
|
||||||
continue
|
|
||||||
loops.append(transform_difference(transforms[(i, k)], b_ij @ b_jk))
|
|
||||||
if not loops:
|
|
||||||
return {"count": 0}
|
|
||||||
translation = np.array([item["translation_m"] for item in loops])
|
|
||||||
rotation = np.array([item["rotation_deg"] for item in loops])
|
|
||||||
return {
|
|
||||||
"count": len(loops),
|
|
||||||
"translation_rms_m": float(np.sqrt(np.mean(translation**2))),
|
|
||||||
"translation_p95_m": float(np.quantile(translation, 0.95)),
|
|
||||||
"rotation_rms_deg": float(np.sqrt(np.mean(rotation**2))),
|
|
||||||
"rotation_p95_deg": float(np.quantile(rotation, 0.95)),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_ground(args):
|
|
||||||
stations = load_stations(args.frames, args.min_range, args.max_range)
|
|
||||||
rows = []
|
|
||||||
for timestamp, counter, _, xyz in stations:
|
|
||||||
roi = xyz[(xyz[:, 2] >= args.z_min) & (xyz[:, 2] <= args.z_max)]
|
|
||||||
if len(roi) < args.min_inliers:
|
|
||||||
continue
|
|
||||||
cloud = make_o3d_cloud(roi, args.voxel)
|
|
||||||
plane, indexes = cloud.segment_plane(
|
|
||||||
args.distance_threshold, 3, args.ransac_iterations
|
|
||||||
)
|
|
||||||
normal = np.asarray(plane[:3], float)
|
|
||||||
norm = np.linalg.norm(normal)
|
|
||||||
normal, distance = normal / norm, float(plane[3] / norm)
|
|
||||||
if distance < 0:
|
|
||||||
normal, distance = -normal, -distance
|
|
||||||
points = np.asarray(cloud.points)[indexes]
|
|
||||||
rms = float(np.sqrt(np.mean((points @ normal + distance) ** 2)))
|
|
||||||
if len(indexes) >= args.min_inliers and rms <= args.max_rms:
|
|
||||||
rows.append([timestamp, *normal, distance, len(indexes), rms, counter])
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
with output.open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
writer = csv.writer(stream)
|
|
||||||
writer.writerow(["time", "nx", "ny", "nz", "d", "inliers", "rms_m", "frame_counter"])
|
|
||||||
writer.writerows(rows)
|
|
||||||
print(json.dumps({"planes": len(rows), "output": str(output.resolve())}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_pairs(args):
|
|
||||||
if len(args.voxels) != len(args.correspondences):
|
|
||||||
raise ValueError("--voxels and --correspondences must have equal lengths")
|
|
||||||
stations = load_stations(
|
|
||||||
args.frames, args.min_range, args.max_range, args.z_min, args.z_max
|
|
||||||
)
|
|
||||||
body = read_poses(args.body)
|
|
||||||
if len(stations) < args.min_stations:
|
|
||||||
raise ValueError(f"need at least {args.min_stations} stations, got {len(stations)}")
|
|
||||||
body_poses, body_dt = [], []
|
|
||||||
for timestamp, _, _, xyz in stations:
|
|
||||||
if len(xyz) < args.min_roi_points:
|
|
||||||
raise ValueError(f"station at {timestamp} has only {len(xyz)} ROI points")
|
|
||||||
pose, dt = nearest_pose(body, timestamp + args.time_offset)
|
|
||||||
body_poses.append(pose)
|
|
||||||
body_dt.append(dt)
|
|
||||||
body_poses = np.asarray(body_poses)
|
|
||||||
split = [split_holdout(station[3], args.holdout_fraction, i)
|
|
||||||
for i, station in enumerate(stations)]
|
|
||||||
rng = np.random.default_rng(args.seed)
|
|
||||||
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
|
|
||||||
accepted_transforms = {}
|
|
||||||
for i in range(len(stations)):
|
|
||||||
for j in range(i + args.min_gap, min(len(stations), i + args.max_gap + 1)):
|
|
||||||
a_ij = inverse_transform(body_poses[i]) @ body_poses[j]
|
|
||||||
translation = float(np.linalg.norm(a_ij[:2, 3]))
|
|
||||||
rotation = rotation_angle_deg(a_ij[:3, :3])
|
|
||||||
if translation < args.min_translation and rotation < args.min_rotation:
|
|
||||||
continue
|
|
||||||
initial_b = a_ij.copy() # X0=I; no measured extrinsic.
|
|
||||||
target_fit, target_holdout = split[i]
|
|
||||||
source_fit, source_holdout = split[j]
|
|
||||||
forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
|
|
||||||
heldout = symmetric_heldout_metrics(
|
|
||||||
target_fit, target_holdout, source_fit, source_holdout,
|
|
||||||
forward["transform"], args.evaluation_distance
|
|
||||||
)
|
|
||||||
hessian = hessian_metrics(forward["hessian"])
|
|
||||||
reverse_answer = align_backend(
|
|
||||||
args.backend, source_fit, target_fit, inverse_transform(initial_b), args
|
|
||||||
)
|
|
||||||
reverse = transform_difference(
|
|
||||||
forward["transform"], inverse_transform(reverse_answer["transform"])
|
|
||||||
)
|
|
||||||
multistart = []
|
|
||||||
for _ in range(args.multistart):
|
|
||||||
perturb = np.r_[
|
|
||||||
rng.normal(0.0, args.multistart_translation_sigma, 3),
|
|
||||||
np.deg2rad(rng.normal(0.0, args.multistart_rotation_sigma, 3)),
|
|
||||||
]
|
|
||||||
candidate = align_backend(
|
|
||||||
args.backend, target_fit, source_fit,
|
|
||||||
params_transform(perturb) @ initial_b, args
|
|
||||||
)
|
|
||||||
multistart.append(transform_difference(forward["transform"], candidate["transform"]))
|
|
||||||
stable = [
|
|
||||||
item["translation_m"] <= args.multistart_translation_tolerance
|
|
||||||
and item["rotation_deg"] <= args.multistart_rotation_tolerance
|
|
||||||
for item in multistart
|
|
||||||
]
|
|
||||||
success_rate = float(np.mean(stable)) if stable else 1.0
|
|
||||||
reasons = []
|
|
||||||
if forward["converged"] is False:
|
|
||||||
reasons.append("backend_not_converged")
|
|
||||||
if heldout["inlier_ratio"] < args.min_inlier_ratio:
|
|
||||||
reasons.append("heldout_inlier_ratio")
|
|
||||||
if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
|
|
||||||
reasons.append("heldout_inlier_rmse")
|
|
||||||
if hessian["effective_rank"] < 6:
|
|
||||||
reasons.append("hessian_rank")
|
|
||||||
if hessian["scaled_condition_number"] > args.max_hessian_condition:
|
|
||||||
reasons.append("hessian_condition")
|
|
||||||
if reverse["translation_m"] > args.reverse_translation_tolerance:
|
|
||||||
reasons.append("forward_reverse_translation")
|
|
||||||
if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
|
|
||||||
reasons.append("forward_reverse_rotation")
|
|
||||||
if success_rate < args.min_multistart_success:
|
|
||||||
reasons.append("multistart_instability")
|
|
||||||
accepted = not reasons
|
|
||||||
report = {
|
|
||||||
"i": i, "j": j,
|
|
||||||
"lidar_time_i": stations[i][0], "lidar_time_j": stations[j][0],
|
|
||||||
"frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1],
|
|
||||||
"rtk_translation_m": translation, "rtk_rotation_deg": rotation,
|
|
||||||
"nearest_rtk_dt_i_s": body_dt[i], "nearest_rtk_dt_j_s": body_dt[j],
|
|
||||||
"initial_B_source": "X0=identity; B0=A (no measured extrinsic)",
|
|
||||||
"B_ij_4x4": forward["transform"].tolist(),
|
|
||||||
"backend": args.backend, "backend_converged": forward["converged"],
|
|
||||||
"backend_iterations": forward["iterations"],
|
|
||||||
"backend_num_inliers": forward["num_inliers"],
|
|
||||||
"backend_objective": forward["objective"],
|
|
||||||
"backend_elapsed_sec": forward["elapsed_sec"],
|
|
||||||
"multiscale_stages": forward["stages"],
|
|
||||||
"heldout_symmetric": heldout, "hessian": hessian,
|
|
||||||
"forward_reverse": reverse,
|
|
||||||
"multistart": {"runs": len(multistart), "success_rate": success_rate,
|
|
||||||
"deltas": multistart},
|
|
||||||
"accepted": accepted, "rejection_reasons": reasons,
|
|
||||||
}
|
|
||||||
reports.append(report)
|
|
||||||
print(f"{args.backend} {i:02d}->{j:02d} rmse={heldout['inlier_rmse_m']} "
|
|
||||||
f"ratio={heldout['inlier_ratio']:.3f} accepted={accepted}")
|
|
||||||
if accepted:
|
|
||||||
accepted_a.append(a_ij)
|
|
||||||
accepted_b.append(forward["transform"])
|
|
||||||
accepted_meta.append([i, j, stations[i][0], stations[j][0]])
|
|
||||||
accepted_transforms[(i, j)] = forward["transform"]
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
np.savez_compressed(
|
|
||||||
output, A=np.asarray(accepted_a), B=np.asarray(accepted_b),
|
|
||||||
meta=np.asarray(accepted_meta),
|
|
||||||
station_times=np.asarray([item[0] for item in stations]),
|
|
||||||
rtk_nearest_dt_s=np.asarray(body_dt), backend=np.asarray(args.backend),
|
|
||||||
)
|
|
||||||
quality = {
|
|
||||||
"schema_version": 2,
|
|
||||||
"backend": args.backend,
|
|
||||||
"transform_convention": "B_ij=T_Li_Lj maps station j points into station i",
|
|
||||||
"raw_point_field": "points_raw",
|
|
||||||
"measured_extrinsic_used_as_initial": False,
|
|
||||||
"stations": len(stations), "candidate_pairs": len(reports),
|
|
||||||
"accepted_pairs": len(accepted_a),
|
|
||||||
"parameters": vars(args),
|
|
||||||
"accepted_loop_closure": loop_metrics(accepted_transforms),
|
|
||||||
"pairs": reports,
|
|
||||||
}
|
|
||||||
quality["parameters"].pop("func", None)
|
|
||||||
quality_path = Path(args.quality_json or output.with_suffix(".quality.json"))
|
|
||||||
quality_path.write_text(json.dumps(quality, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
csv_path = Path(args.quality_csv or output.with_suffix(".quality.csv"))
|
|
||||||
with csv_path.open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
fields = ["i", "j", "rtk_translation_m", "rtk_rotation_deg",
|
|
||||||
"heldout_inlier_ratio", "heldout_inlier_rmse_m",
|
|
||||||
"hessian_rank", "hessian_condition", "reverse_translation_m",
|
|
||||||
"reverse_rotation_deg", "multistart_success_rate", "accepted",
|
|
||||||
"rejection_reasons"]
|
|
||||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
|
||||||
writer.writeheader()
|
|
||||||
for item in reports:
|
|
||||||
writer.writerow({
|
|
||||||
"i": item["i"], "j": item["j"],
|
|
||||||
"rtk_translation_m": item["rtk_translation_m"],
|
|
||||||
"rtk_rotation_deg": item["rtk_rotation_deg"],
|
|
||||||
"heldout_inlier_ratio": item["heldout_symmetric"]["inlier_ratio"],
|
|
||||||
"heldout_inlier_rmse_m": item["heldout_symmetric"]["inlier_rmse_m"],
|
|
||||||
"hessian_rank": item["hessian"]["effective_rank"],
|
|
||||||
"hessian_condition": item["hessian"]["scaled_condition_number"],
|
|
||||||
"reverse_translation_m": item["forward_reverse"]["translation_m"],
|
|
||||||
"reverse_rotation_deg": item["forward_reverse"]["rotation_deg"],
|
|
||||||
"multistart_success_rate": item["multistart"]["success_rate"],
|
|
||||||
"accepted": item["accepted"],
|
|
||||||
"rejection_reasons": ";".join(item["rejection_reasons"]),
|
|
||||||
})
|
|
||||||
if len(accepted_a) < args.min_pairs:
|
|
||||||
raise RuntimeError(f"only {len(accepted_a)} accepted pairs; need {args.min_pairs}")
|
|
||||||
print(json.dumps({
|
|
||||||
"backend": args.backend, "stations": len(stations),
|
|
||||||
"candidate_pairs": len(reports), "accepted_pairs": len(accepted_a),
|
|
||||||
"output": str(output.resolve()), "quality_json": str(quality_path.resolve()),
|
|
||||||
"loop": quality["accepted_loop_closure"],
|
|
||||||
}, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def read_planes(path):
|
|
||||||
planes = []
|
|
||||||
with Path(path).open(encoding="utf-8-sig", newline="") as stream:
|
|
||||||
for row in csv.DictReader(stream):
|
|
||||||
normal = np.array([float(row[k]) for k in ("nx", "ny", "nz")])
|
|
||||||
norm = np.linalg.norm(normal)
|
|
||||||
normal, distance = normal / norm, float(row["d"]) / norm
|
|
||||||
if distance < 0:
|
|
||||||
normal, distance = -normal, -distance
|
|
||||||
planes.append([*normal, distance])
|
|
||||||
return np.asarray(planes)
|
|
||||||
|
|
||||||
|
|
||||||
def read_pairs(path):
|
|
||||||
with np.load(path, allow_pickle=False) as data:
|
|
||||||
return (np.asarray(data["A"], float), np.asarray(data["B"], float),
|
|
||||||
np.asarray(data["meta"], float), len(data["station_times"]))
|
|
||||||
|
|
||||||
|
|
||||||
def calibration_residual(params, a_array, b_array, planes, args):
|
|
||||||
x = params_transform(params)
|
|
||||||
values = []
|
|
||||||
for a_ij, b_ij in zip(a_array, b_array):
|
|
||||||
error = inverse_transform(a_ij @ x) @ x @ b_ij
|
|
||||||
values.extend((error[:3, 3] / args.translation_sigma).tolist())
|
|
||||||
values.extend((so3_log(error[:3, :3]) / math.radians(args.rotation_sigma)).tolist())
|
|
||||||
body_up = np.array([0.0, 0.0, 1.0])
|
|
||||||
for plane in planes:
|
|
||||||
normal_body = x[:3, :3] @ plane[:3]
|
|
||||||
values.extend((np.cross(normal_body, body_up) / args.plane_normal_sigma).tolist())
|
|
||||||
body_distance = plane[3] - float(normal_body @ x[:3, 3])
|
|
||||||
values.append((body_distance - args.body_height) / args.plane_height_sigma)
|
|
||||||
return np.asarray(values)
|
|
||||||
|
|
||||||
|
|
||||||
def pair_metrics(a_array, b_array, x):
|
|
||||||
translation, rotation, rows = [], [], []
|
|
||||||
for index, (a_ij, b_ij) in enumerate(zip(a_array, b_array)):
|
|
||||||
predicted = inverse_transform(x) @ a_ij @ x
|
|
||||||
delta = inverse_transform(b_ij) @ predicted
|
|
||||||
t = float(np.linalg.norm(delta[:3, 3]))
|
|
||||||
r = rotation_angle_deg(delta[:3, :3])
|
|
||||||
translation.append(t); rotation.append(r)
|
|
||||||
rows.append({"pair_index": index, "translation_m": t, "rotation_deg": r})
|
|
||||||
translation, rotation = np.asarray(translation), np.asarray(rotation)
|
|
||||||
def stats(values):
|
|
||||||
return {
|
|
||||||
"rms": float(np.sqrt(np.mean(values**2))),
|
|
||||||
"median": float(np.median(values)),
|
|
||||||
"p90": float(np.quantile(values, 0.90)),
|
|
||||||
"p95": float(np.quantile(values, 0.95)),
|
|
||||||
"max": float(np.max(values)),
|
|
||||||
}
|
|
||||||
return {"pairs": len(rows), "translation_m": stats(translation),
|
|
||||||
"rotation_deg": stats(rotation), "per_pair": rows}
|
|
||||||
|
|
||||||
|
|
||||||
def solve_extrinsic(a_array, b_array, planes, args):
|
|
||||||
rng = np.random.default_rng(args.seed)
|
|
||||||
starts = [np.zeros(6)]
|
|
||||||
for _ in range(args.solver_multistart - 1):
|
|
||||||
starts.append(np.r_[
|
|
||||||
rng.normal(0.0, args.start_translation_sigma, 3),
|
|
||||||
np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
|
|
||||||
])
|
|
||||||
candidates = []
|
|
||||||
lower = np.r_[[-5.0] * 3, [-math.pi] * 3]
|
|
||||||
upper = np.r_[[5.0] * 3, [math.pi] * 3]
|
|
||||||
for start in starts:
|
|
||||||
answer = least_squares(
|
|
||||||
calibration_residual, np.clip(start, lower, upper),
|
|
||||||
args=(a_array, b_array, planes, args),
|
|
||||||
bounds=(lower, upper), loss="huber", f_scale=1.5,
|
|
||||||
x_scale="jac", max_nfev=args.max_nfev,
|
|
||||||
)
|
|
||||||
candidates.append(answer)
|
|
||||||
best = min(candidates, key=lambda item: item.cost)
|
|
||||||
return best, candidates
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_calibrate(args):
|
|
||||||
a_array, b_array, meta, stations = read_pairs(args.pairs)
|
|
||||||
planes = read_planes(args.ground_planes)
|
|
||||||
best, candidates = solve_extrinsic(a_array, b_array, planes, args)
|
|
||||||
x = params_transform(best.x)
|
|
||||||
residual = calibration_residual(best.x, a_array, b_array, planes, args)
|
|
||||||
absolute = np.abs(residual)
|
|
||||||
weights = np.ones_like(residual)
|
|
||||||
weights[absolute > 1.5] = 1.5 / absolute[absolute > 1.5]
|
|
||||||
weighted_jacobian = best.jac * np.sqrt(weights)[:, None]
|
|
||||||
singular = np.linalg.svd(weighted_jacobian, compute_uv=False)
|
|
||||||
condition = float(singular[0] / max(singular[-1], 1e-15))
|
|
||||||
dof = max(len(residual) - 6, 1)
|
|
||||||
covariance = np.linalg.pinv(weighted_jacobian.T @ weighted_jacobian) * float(
|
|
||||||
np.sum(weights * residual**2) / dof
|
|
||||||
)
|
|
||||||
sigma = np.sqrt(np.maximum(np.diag(covariance), 0.0))
|
|
||||||
candidate_summary = []
|
|
||||||
for item in candidates:
|
|
||||||
candidate_x = params_transform(item.x)
|
|
||||||
candidate_summary.append({
|
|
||||||
"cost": float(item.cost), "success": bool(item.success),
|
|
||||||
**transform_difference(x, candidate_x),
|
|
||||||
})
|
|
||||||
bootstrap = []
|
|
||||||
rng = np.random.default_rng(args.seed + 1)
|
|
||||||
for _ in range(args.bootstrap):
|
|
||||||
indexes = rng.integers(0, len(a_array), len(a_array))
|
|
||||||
answer = least_squares(
|
|
||||||
calibration_residual, best.x,
|
|
||||||
args=(a_array[indexes], b_array[indexes], planes, args),
|
|
||||||
loss="huber", f_scale=1.5, x_scale="jac", max_nfev=args.max_nfev,
|
|
||||||
)
|
|
||||||
bootstrap.append(np.r_[answer.x[:3], rpy_deg(so3_exp(answer.x[3:]))])
|
|
||||||
bootstrap = np.asarray(bootstrap)
|
|
||||||
result = {
|
|
||||||
"schema_version": 2,
|
|
||||||
"success": bool(best.success),
|
|
||||||
"message": best.message,
|
|
||||||
"convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
|
|
||||||
"equation": "A_ij X = X B_ij",
|
|
||||||
"measured_extrinsic_used_as_initial": False,
|
|
||||||
"translation_m": x[:3, 3].tolist(),
|
|
||||||
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
|
|
||||||
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
|
|
||||||
"matrix_4x4": x.tolist(),
|
|
||||||
"estimation": {"stations": stations, "pairs": len(a_array),
|
|
||||||
"residuals": pair_metrics(a_array, b_array, x)},
|
|
||||||
"ground": {
|
|
||||||
"planes": len(planes),
|
|
||||||
"body_origin_height_above_ground_m": args.body_height,
|
|
||||||
"formula": "d_lidar - (R_X n_lidar)^T t_X - body_height",
|
|
||||||
},
|
|
||||||
"linearized_one_sigma": {
|
|
||||||
"translation_m": sigma[:3].tolist(),
|
|
||||||
"rotation_deg": np.rad2deg(sigma[3:]).tolist(),
|
|
||||||
"warning": "conditional local estimate; bootstrap is the primary stability check",
|
|
||||||
},
|
|
||||||
"weighted_jacobian_condition_number": condition,
|
|
||||||
"solver_multistart": {
|
|
||||||
"runs": len(candidates), "candidates_relative_to_best": candidate_summary,
|
|
||||||
},
|
|
||||||
"bootstrap": {
|
|
||||||
"runs": len(bootstrap),
|
|
||||||
"order": ["x_m", "y_m", "z_m", "roll_deg", "pitch_deg", "yaw_deg"],
|
|
||||||
"std": np.std(bootstrap, axis=0, ddof=1).tolist() if len(bootstrap) > 1 else None,
|
|
||||||
"p025": np.quantile(bootstrap, 0.025, axis=0).tolist() if len(bootstrap) else None,
|
|
||||||
"p975": np.quantile(bootstrap, 0.975, axis=0).tolist() if len(bootstrap) else None,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
print(json.dumps(result, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def cmd_validate(args):
|
|
||||||
result = json.loads(Path(args.extrinsic).read_text(encoding="utf-8-sig"))
|
|
||||||
x = np.asarray(result["matrix_4x4"], float)
|
|
||||||
a_array, b_array, meta, stations = read_pairs(args.pairs)
|
|
||||||
metrics = pair_metrics(a_array, b_array, x)
|
|
||||||
for row, pair_meta in zip(metrics["per_pair"], meta):
|
|
||||||
row.update({"i": int(pair_meta[0]), "j": int(pair_meta[1])})
|
|
||||||
report = {
|
|
||||||
"role": "auxiliary check only; first-batch RTK is sparse",
|
|
||||||
"blind_with_respect_to_X": True,
|
|
||||||
"note": "No AX residual was used to select these pairs",
|
|
||||||
"stations": stations, "metrics": metrics,
|
|
||||||
}
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
output.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
print(json.dumps(report, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
def build_parser():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
commands = parser.add_subparsers(dest="command", required=True)
|
|
||||||
ground = commands.add_parser("ground")
|
|
||||||
ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True)
|
|
||||||
ground.add_argument("--min-range", type=float, default=1.0); ground.add_argument("--max-range", type=float, default=30.0)
|
|
||||||
ground.add_argument("--z-min", type=float, default=-1.4); ground.add_argument("--z-max", type=float, default=-0.4)
|
|
||||||
ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025)
|
|
||||||
ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500)
|
|
||||||
ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground)
|
|
||||||
|
|
||||||
pairs = commands.add_parser("pairs")
|
|
||||||
pairs.add_argument("--backend", choices=["open3d", "small_gicp"], required=True)
|
|
||||||
pairs.add_argument("--frames", required=True); pairs.add_argument("--body", required=True)
|
|
||||||
pairs.add_argument("--output", required=True); pairs.add_argument("--quality-json"); pairs.add_argument("--quality-csv")
|
|
||||||
pairs.add_argument("--time-offset", type=float, default=0.0)
|
|
||||||
pairs.add_argument("--min-stations", type=int, default=30); pairs.add_argument("--min-pairs", type=int, default=25)
|
|
||||||
pairs.add_argument("--min-gap", type=int, default=1); pairs.add_argument("--max-gap", type=int, default=5)
|
|
||||||
pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0)
|
|
||||||
pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0)
|
|
||||||
pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0)
|
|
||||||
pairs.add_argument("--min-roi-points", type=int, default=1000)
|
|
||||||
pairs.add_argument("--holdout-fraction", type=float, default=0.20)
|
|
||||||
pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08])
|
|
||||||
pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25])
|
|
||||||
pairs.add_argument("--iterations", type=int, default=60); pairs.add_argument("--threads", type=int, default=8)
|
|
||||||
pairs.add_argument("--evaluation-distance", type=float, default=0.25)
|
|
||||||
pairs.add_argument("--min-inlier-ratio", type=float, default=0.35); pairs.add_argument("--max-inlier-rmse", type=float, default=0.16)
|
|
||||||
pairs.add_argument("--max-hessian-condition", type=float, default=1e8)
|
|
||||||
pairs.add_argument("--reverse-translation-tolerance", type=float, default=0.08)
|
|
||||||
pairs.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
|
|
||||||
pairs.add_argument("--multistart", type=int, default=2)
|
|
||||||
pairs.add_argument("--multistart-translation-sigma", type=float, default=0.30)
|
|
||||||
pairs.add_argument("--multistart-rotation-sigma", type=float, default=3.0)
|
|
||||||
pairs.add_argument("--multistart-translation-tolerance", type=float, default=0.08)
|
|
||||||
pairs.add_argument("--multistart-rotation-tolerance", type=float, default=0.50)
|
|
||||||
pairs.add_argument("--min-multistart-success", type=float, default=0.50)
|
|
||||||
pairs.add_argument("--seed", type=int, default=20260721); pairs.set_defaults(func=cmd_pairs)
|
|
||||||
|
|
||||||
calibrate = commands.add_parser("calibrate")
|
|
||||||
calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True)
|
|
||||||
calibrate.add_argument("--output", required=True)
|
|
||||||
calibrate.add_argument("--translation-sigma", type=float, default=0.05)
|
|
||||||
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
|
|
||||||
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
|
|
||||||
calibrate.add_argument("--plane-height-sigma", type=float, default=0.03)
|
|
||||||
calibrate.add_argument("--body-height", type=float, default=0.2335)
|
|
||||||
calibrate.add_argument("--solver-multistart", type=int, default=12)
|
|
||||||
calibrate.add_argument("--start-translation-sigma", type=float, default=1.0)
|
|
||||||
calibrate.add_argument("--start-rotation-sigma", type=float, default=20.0)
|
|
||||||
calibrate.add_argument("--bootstrap", type=int, default=100)
|
|
||||||
calibrate.add_argument("--max-nfev", type=int, default=1000)
|
|
||||||
calibrate.add_argument("--seed", type=int, default=20260721); calibrate.set_defaults(func=cmd_calibrate)
|
|
||||||
|
|
||||||
validate = commands.add_parser("validate")
|
|
||||||
validate.add_argument("--pairs", required=True); validate.add_argument("--extrinsic", required=True)
|
|
||||||
validate.add_argument("--output", required=True); validate.set_defaults(func=cmd_validate)
|
|
||||||
return parser
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
args = build_parser().parse_args()
|
|
||||||
args.func(args)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,311 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Scan body-left RPY corrections locally and validate them over every B pair.
|
|
||||||
|
|
||||||
This command is diagnostic only. It never writes or replaces an extrinsic JSON.
|
|
||||||
"""
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import csv
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from rigorous_calibration import (
|
|
||||||
inverse_transform, read_pairs, read_planes, rotation_angle_deg, rpy_deg,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def statistics(values):
|
|
||||||
values = np.asarray(values, float)
|
|
||||||
return {
|
|
||||||
"rms": float(np.sqrt(np.mean(values ** 2))),
|
|
||||||
"median": float(np.median(values)),
|
|
||||||
"p90": float(np.quantile(values, 0.90)),
|
|
||||||
"p95": float(np.quantile(values, 0.95)),
|
|
||||||
"max": float(np.max(values)),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def body_left_rpy(x, rpy_correction_deg):
|
|
||||||
correction = np.eye(4)
|
|
||||||
correction[:3, :3] = Rotation.from_euler(
|
|
||||||
"xyz", np.asarray(rpy_correction_deg, float), degrees=True
|
|
||||||
).as_matrix()
|
|
||||||
return correction @ x
|
|
||||||
|
|
||||||
|
|
||||||
def pair_delta(a_ij, b_ij, x):
|
|
||||||
predicted = inverse_transform(x) @ a_ij @ x
|
|
||||||
delta = inverse_transform(b_ij) @ predicted
|
|
||||||
translation = np.asarray(delta[:3, 3], float)
|
|
||||||
return {
|
|
||||||
"translation_xyz_m": translation.tolist(),
|
|
||||||
"translation_xyz_cm": (100.0 * translation).tolist(),
|
|
||||||
"translation_norm_m": float(np.linalg.norm(translation)),
|
|
||||||
"rotation_rpy_deg_xyz": rpy_deg(delta[:3, :3]),
|
|
||||||
"rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def ground_metrics(planes, x, body_height):
|
|
||||||
if len(planes) == 0:
|
|
||||||
return None
|
|
||||||
up = np.array([0.0, 0.0, 1.0])
|
|
||||||
tilt_deg, height_m = [], []
|
|
||||||
for plane in planes:
|
|
||||||
normal_body = x[:3, :3] @ plane[:3]
|
|
||||||
normal_body /= np.linalg.norm(normal_body)
|
|
||||||
tilt_deg.append(math.degrees(math.atan2(
|
|
||||||
np.linalg.norm(np.cross(normal_body, up)),
|
|
||||||
float(np.clip(normal_body @ up, -1.0, 1.0)),
|
|
||||||
)))
|
|
||||||
height_m.append(
|
|
||||||
float(plane[3] - normal_body @ x[:3, 3] - body_height)
|
|
||||||
)
|
|
||||||
return {
|
|
||||||
"normal_tilt_deg": statistics(tilt_deg),
|
|
||||||
"height_residual_m": statistics(height_m),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def evaluate(label, correction, a_array, b_array, meta, x, pair_index,
|
|
||||||
translation_scale, rotation_scale, planes, body_height):
|
|
||||||
candidate_x = body_left_rpy(x, correction)
|
|
||||||
per_pair = []
|
|
||||||
translation, rotation, normalized = [], [], []
|
|
||||||
for index, (a_ij, b_ij, pair_meta) in enumerate(zip(a_array, b_array, meta)):
|
|
||||||
item = pair_delta(a_ij, b_ij, candidate_x)
|
|
||||||
item.update({
|
|
||||||
"pair_index": index,
|
|
||||||
"i": int(pair_meta[0]),
|
|
||||||
"j": int(pair_meta[1]),
|
|
||||||
})
|
|
||||||
t = item["translation_norm_m"]
|
|
||||||
r = item["rotation_angle_deg"]
|
|
||||||
translation.append(t)
|
|
||||||
rotation.append(r)
|
|
||||||
normalized.append(math.hypot(t / translation_scale, r / rotation_scale))
|
|
||||||
per_pair.append(item)
|
|
||||||
return {
|
|
||||||
"label": label,
|
|
||||||
"body_left_rpy_correction_deg_xyz": list(map(float, correction)),
|
|
||||||
"candidate_extrinsic": {
|
|
||||||
"translation_m": candidate_x[:3, 3].tolist(),
|
|
||||||
"rotation_rpy_deg_xyz": rpy_deg(candidate_x[:3, :3]),
|
|
||||||
},
|
|
||||||
"all_pairs": {
|
|
||||||
"count": len(per_pair),
|
|
||||||
"translation_m": statistics(translation),
|
|
||||||
"rotation_deg": statistics(rotation),
|
|
||||||
"normalized_pair_score": statistics(normalized),
|
|
||||||
"normalized_global_rms": float(np.sqrt(np.mean(np.asarray(normalized) ** 2))),
|
|
||||||
},
|
|
||||||
"selected_pair": per_pair[pair_index],
|
|
||||||
"ground": ground_metrics(planes, candidate_x, body_height),
|
|
||||||
"per_pair": per_pair,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def candidate_grid(pitch_values, roll_values, yaw_values):
|
|
||||||
answer = [("baseline", (0.0, 0.0, 0.0))]
|
|
||||||
for pitch in pitch_values:
|
|
||||||
answer.append((f"pitch_{pitch:+.3f}", (0.0, pitch, 0.0)))
|
|
||||||
for pitch in (0.0, *pitch_values):
|
|
||||||
for roll in roll_values:
|
|
||||||
answer.append((
|
|
||||||
f"pitch_{pitch:+.3f}_roll_{roll:+.3f}",
|
|
||||||
(roll, pitch, 0.0),
|
|
||||||
))
|
|
||||||
for yaw in yaw_values:
|
|
||||||
answer.append((f"yaw_{yaw:+.3f}_diagnostic", (0.0, 0.0, yaw)))
|
|
||||||
unique = []
|
|
||||||
seen = set()
|
|
||||||
for label, values in answer:
|
|
||||||
key = tuple(round(float(value), 12) for value in values)
|
|
||||||
if key not in seen:
|
|
||||||
seen.add(key)
|
|
||||||
unique.append((label, values))
|
|
||||||
return unique
|
|
||||||
|
|
||||||
|
|
||||||
def z_observability(a_array, x, test_shift_m):
|
|
||||||
shift = np.eye(4)
|
|
||||||
shift[2, 3] = test_shift_m
|
|
||||||
shifted_x = shift @ x
|
|
||||||
effects = []
|
|
||||||
for a_ij in a_array:
|
|
||||||
before = inverse_transform(x) @ a_ij @ x
|
|
||||||
after = inverse_transform(shifted_x) @ a_ij @ shifted_x
|
|
||||||
delta = inverse_transform(before) @ after
|
|
||||||
effects.append((
|
|
||||||
float(np.linalg.norm(delta[:3, 3])),
|
|
||||||
rotation_angle_deg(delta[:3, :3]),
|
|
||||||
))
|
|
||||||
effects = np.asarray(effects, float)
|
|
||||||
maximum = np.max(effects, axis=0)
|
|
||||||
return {
|
|
||||||
"body_left_z_test_shift_m": test_shift_m,
|
|
||||||
"max_predicted_motion_change_translation_m": float(maximum[0]),
|
|
||||||
"max_predicted_motion_change_rotation_deg": float(maximum[1]),
|
|
||||||
"numerically_unobservable": bool(maximum[0] < 1e-10 and maximum[1] < 1e-10),
|
|
||||||
"note": "AX pairs cannot determine X.z when every A rotation preserves body Z; use ground/external height constraints.",
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--pairs", required=True)
|
|
||||||
parser.add_argument("--extrinsic", required=True)
|
|
||||||
parser.add_argument("--output", required=True)
|
|
||||||
parser.add_argument("--csv")
|
|
||||||
parser.add_argument("--ground-planes")
|
|
||||||
parser.add_argument("--pair-index", type=int, default=0)
|
|
||||||
parser.add_argument("--pitch-values", nargs="+", type=float, default=[0.1, 0.2, 0.3])
|
|
||||||
parser.add_argument("--roll-values", nargs="+", type=float, default=[-0.2, -0.1, 0.1])
|
|
||||||
parser.add_argument("--yaw-values", nargs="+", type=float, default=[-0.2, 0.2])
|
|
||||||
parser.add_argument("--translation-scale", type=float, default=0.05)
|
|
||||||
parser.add_argument("--rotation-scale", type=float, default=0.5)
|
|
||||||
parser.add_argument("--body-height", type=float, default=0.2335)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
a_array, b_array, meta, stations = read_pairs(args.pairs)
|
|
||||||
if not 0 <= args.pair_index < len(a_array):
|
|
||||||
raise IndexError(f"pair-index {args.pair_index} outside [0,{len(a_array)-1}]")
|
|
||||||
with Path(args.extrinsic).open(encoding="utf-8-sig") as stream:
|
|
||||||
x = np.asarray(json.load(stream)["matrix_4x4"], float)
|
|
||||||
planes = read_planes(args.ground_planes) if args.ground_planes else np.empty((0, 4))
|
|
||||||
|
|
||||||
candidates = [
|
|
||||||
evaluate(
|
|
||||||
label, correction, a_array, b_array, meta, x, args.pair_index,
|
|
||||||
args.translation_scale, args.rotation_scale, planes, args.body_height,
|
|
||||||
)
|
|
||||||
for label, correction in candidate_grid(
|
|
||||||
args.pitch_values, args.roll_values, args.yaw_values
|
|
||||||
)
|
|
||||||
]
|
|
||||||
baseline = candidates[0]
|
|
||||||
baseline_scores = np.asarray([
|
|
||||||
math.hypot(
|
|
||||||
item["translation_norm_m"] / args.translation_scale,
|
|
||||||
item["rotation_angle_deg"] / args.rotation_scale,
|
|
||||||
)
|
|
||||||
for item in baseline["per_pair"]
|
|
||||||
])
|
|
||||||
base_global = baseline["all_pairs"]["normalized_global_rms"]
|
|
||||||
for candidate in candidates:
|
|
||||||
scores = np.asarray([
|
|
||||||
math.hypot(
|
|
||||||
item["translation_norm_m"] / args.translation_scale,
|
|
||||||
item["rotation_angle_deg"] / args.rotation_scale,
|
|
||||||
)
|
|
||||||
for item in candidate["per_pair"]
|
|
||||||
])
|
|
||||||
delta = scores - baseline_scores
|
|
||||||
candidate["comparison_to_baseline"] = {
|
|
||||||
"normalized_global_rms_change": float(
|
|
||||||
candidate["all_pairs"]["normalized_global_rms"] - base_global
|
|
||||||
),
|
|
||||||
"improved_pairs": int(np.sum(delta < -1e-12)),
|
|
||||||
"worsened_pairs": int(np.sum(delta > 1e-12)),
|
|
||||||
"unchanged_pairs": int(np.sum(np.abs(delta) <= 1e-12)),
|
|
||||||
"median_per_pair_score_change": float(np.median(delta)),
|
|
||||||
"global_consistency_signal": bool(
|
|
||||||
candidate["all_pairs"]["normalized_global_rms"] < base_global
|
|
||||||
and np.sum(delta < -1e-12) > np.sum(delta > 1e-12)
|
|
||||||
),
|
|
||||||
}
|
|
||||||
|
|
||||||
ranking = sorted(
|
|
||||||
candidates,
|
|
||||||
key=lambda item: item["all_pairs"]["normalized_global_rms"],
|
|
||||||
)
|
|
||||||
report = {
|
|
||||||
"schema_version": 1,
|
|
||||||
"diagnostic_only": True,
|
|
||||||
"extrinsic_was_modified": False,
|
|
||||||
"equation": "delta_ij = B_ij^-1 * (X^-1 * A_ij * X)",
|
|
||||||
"correction_convention": "X_test = DeltaR_body * X; DeltaR uses fixed body xyz RPY axes",
|
|
||||||
"component_frame": "delta translation/RPY components are in station-j LiDAR coordinates, not screen axes",
|
|
||||||
"selection_rule": (
|
|
||||||
"Never accept a correction from selected_pair alone. Require improvement over all "
|
|
||||||
"refined pairs, directional consistency across pairs, acceptable ground constraints, "
|
|
||||||
"and independent visual review. This script never overwrites X."
|
|
||||||
),
|
|
||||||
"pairs_file": str(Path(args.pairs).resolve()),
|
|
||||||
"extrinsic_file": str(Path(args.extrinsic).resolve()),
|
|
||||||
"stations": stations,
|
|
||||||
"pairs": len(a_array),
|
|
||||||
"selected_pair_index": args.pair_index,
|
|
||||||
"selected_pair_stations": [int(meta[args.pair_index, 0]), int(meta[args.pair_index, 1])],
|
|
||||||
"normalization": {
|
|
||||||
"translation_scale_m": args.translation_scale,
|
|
||||||
"rotation_scale_deg": args.rotation_scale,
|
|
||||||
},
|
|
||||||
"z_observability": z_observability(a_array, x, 0.10),
|
|
||||||
"ranking_by_all_pair_normalized_rms": [
|
|
||||||
{
|
|
||||||
"rank": rank,
|
|
||||||
"label": item["label"],
|
|
||||||
"body_left_rpy_correction_deg_xyz": item["body_left_rpy_correction_deg_xyz"],
|
|
||||||
"normalized_global_rms": item["all_pairs"]["normalized_global_rms"],
|
|
||||||
**item["comparison_to_baseline"],
|
|
||||||
}
|
|
||||||
for rank, item in enumerate(ranking, 1)
|
|
||||||
],
|
|
||||||
"candidates": candidates,
|
|
||||||
}
|
|
||||||
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
output.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
csv_path = Path(args.csv) if args.csv else output.with_suffix(".csv")
|
|
||||||
with csv_path.open("w", encoding="utf-8", newline="") as stream:
|
|
||||||
fields = [
|
|
||||||
"label", "roll_correction_deg", "pitch_correction_deg", "yaw_correction_deg",
|
|
||||||
"selected_pair_translation_cm", "selected_pair_rotation_deg",
|
|
||||||
"all_pair_translation_rms_m", "all_pair_rotation_rms_deg",
|
|
||||||
"normalized_global_rms", "normalized_global_rms_change",
|
|
||||||
"improved_pairs", "worsened_pairs", "global_consistency_signal",
|
|
||||||
"ground_normal_tilt_rms_deg", "ground_height_rms_m",
|
|
||||||
]
|
|
||||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
|
||||||
writer.writeheader()
|
|
||||||
for item in candidates:
|
|
||||||
correction = item["body_left_rpy_correction_deg_xyz"]
|
|
||||||
ground = item["ground"]
|
|
||||||
comparison = item["comparison_to_baseline"]
|
|
||||||
writer.writerow({
|
|
||||||
"label": item["label"],
|
|
||||||
"roll_correction_deg": correction[0],
|
|
||||||
"pitch_correction_deg": correction[1],
|
|
||||||
"yaw_correction_deg": correction[2],
|
|
||||||
"selected_pair_translation_cm": item["selected_pair"]["translation_norm_m"] * 100.0,
|
|
||||||
"selected_pair_rotation_deg": item["selected_pair"]["rotation_angle_deg"],
|
|
||||||
"all_pair_translation_rms_m": item["all_pairs"]["translation_m"]["rms"],
|
|
||||||
"all_pair_rotation_rms_deg": item["all_pairs"]["rotation_deg"]["rms"],
|
|
||||||
"normalized_global_rms": item["all_pairs"]["normalized_global_rms"],
|
|
||||||
"normalized_global_rms_change": comparison["normalized_global_rms_change"],
|
|
||||||
"improved_pairs": comparison["improved_pairs"],
|
|
||||||
"worsened_pairs": comparison["worsened_pairs"],
|
|
||||||
"global_consistency_signal": comparison["global_consistency_signal"],
|
|
||||||
"ground_normal_tilt_rms_deg": None if ground is None else ground["normal_tilt_deg"]["rms"],
|
|
||||||
"ground_height_rms_m": None if ground is None else ground["height_residual_m"]["rms"],
|
|
||||||
})
|
|
||||||
|
|
||||||
print(json.dumps({
|
|
||||||
"diagnostic_only": True,
|
|
||||||
"selected_pair": baseline["selected_pair"],
|
|
||||||
"z_observability": report["z_observability"],
|
|
||||||
"top_all_pair_candidates": report["ranking_by_all_pair_normalized_rms"][:8],
|
|
||||||
"output": str(output.resolve()),
|
|
||||||
"csv": str(csv_path.resolve()),
|
|
||||||
}, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,87 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Build a concise backend comparison and select the recommended result."""
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--open3d", required=True)
|
|
||||||
parser.add_argument("--small", required=True)
|
|
||||||
parser.add_argument("--open3d-quality", required=True)
|
|
||||||
parser.add_argument("--small-quality", required=True)
|
|
||||||
parser.add_argument("--open3d-check", required=True)
|
|
||||||
parser.add_argument("--small-check", required=True)
|
|
||||||
parser.add_argument("--output", required=True)
|
|
||||||
parser.add_argument("--recommended-output", required=True)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
open_result = json.loads(Path(args.open3d).read_text(encoding="utf-8-sig"))
|
|
||||||
small_result = json.loads(Path(args.small).read_text(encoding="utf-8-sig"))
|
|
||||||
open_quality = json.loads(Path(args.open3d_quality).read_text(encoding="utf-8-sig"))
|
|
||||||
small_quality = json.loads(Path(args.small_quality).read_text(encoding="utf-8-sig"))
|
|
||||||
open_check = json.loads(Path(args.open3d_check).read_text(encoding="utf-8-sig"))
|
|
||||||
small_check = json.loads(Path(args.small_check).read_text(encoding="utf-8-sig"))
|
|
||||||
x_open = np.asarray(open_result["matrix_4x4"], float)
|
|
||||||
x_small = np.asarray(small_result["matrix_4x4"], float)
|
|
||||||
delta = np.linalg.inv(x_open) @ x_small
|
|
||||||
|
|
||||||
def compact(result, quality, check):
|
|
||||||
estimate = result["estimation"]["residuals"]
|
|
||||||
auxiliary = check["metrics"]
|
|
||||||
return {
|
|
||||||
"translation_m": result["translation_m"],
|
|
||||||
"rotation_rpy_deg_xyz": result["rotation_rpy_deg_xyz"],
|
|
||||||
"estimation_pairs": estimate["pairs"],
|
|
||||||
"estimation_translation_rms_m": estimate["translation_m"]["rms"],
|
|
||||||
"estimation_rotation_rms_deg": estimate["rotation_deg"]["rms"],
|
|
||||||
"bootstrap_std": result["bootstrap"]["std"],
|
|
||||||
"initial_B_loop_closure": quality["accepted_loop_closure"],
|
|
||||||
"batch1_auxiliary_pairs": auxiliary["pairs"],
|
|
||||||
"batch1_auxiliary_translation_rms_m": auxiliary["translation_m"]["rms"],
|
|
||||||
"batch1_auxiliary_rotation_rms_deg": auxiliary["rotation_deg"]["rms"],
|
|
||||||
}
|
|
||||||
|
|
||||||
summary = {
|
|
||||||
"recommended_backend": "open3d_gicp",
|
|
||||||
"selection_reason": (
|
|
||||||
"The two X estimates agree closely; Open3D has lower second-batch AX residual, "
|
|
||||||
"better B loop closure, and lower first-batch auxiliary residual."
|
|
||||||
),
|
|
||||||
"coordinate_convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
|
|
||||||
"measured_extrinsic_used_as_initial": False,
|
|
||||||
"second_batch_role": "estimation (dense RTK)",
|
|
||||||
"first_batch_role": "auxiliary check only (sparse RTK)",
|
|
||||||
"backend_difference": {
|
|
||||||
"translation_m": float(np.linalg.norm(delta[:3, 3])),
|
|
||||||
"rotation_deg": float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude())),
|
|
||||||
},
|
|
||||||
"open3d_gicp": compact(open_result, open_quality, open_check),
|
|
||||||
"small_gicp": compact(small_result, small_quality, small_check),
|
|
||||||
"important_limit": (
|
|
||||||
"Backend agreement is strong, but AX rotation RMS remains about one degree. "
|
|
||||||
"This is not a centimetre-grade absolute certification."
|
|
||||||
),
|
|
||||||
}
|
|
||||||
output = Path(args.output)
|
|
||||||
output.parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
output.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
||||||
recommended = dict(open_result)
|
|
||||||
recommended["selection"] = {
|
|
||||||
"recommended_backend": "open3d_gicp",
|
|
||||||
"comparison_summary": str(output.name),
|
|
||||||
"backend_difference": summary["backend_difference"],
|
|
||||||
"warning": summary["important_limit"],
|
|
||||||
}
|
|
||||||
Path(args.recommended_output).write_text(
|
|
||||||
json.dumps(recommended, ensure_ascii=False, indent=2), encoding="utf-8"
|
|
||||||
)
|
|
||||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,169 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion."""
|
|
||||||
import argparse
|
|
||||||
import json
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from rigorous_calibration import (
|
|
||||||
inverse_transform, load_stations, rotation_angle_deg, rpy_deg, transform_points,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
COLORS = {
|
|
||||||
"target": [0.10, 0.65, 1.00],
|
|
||||||
"source": [1.00, 0.35, 0.05],
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def cloud(o3d, points, color, voxel):
|
|
||||||
item = o3d.geometry.PointCloud()
|
|
||||||
item.points = o3d.utility.Vector3dVector(points)
|
|
||||||
item = item.voxel_down_sample(voxel)
|
|
||||||
item.paint_uniform_color(color)
|
|
||||||
return item
|
|
||||||
|
|
||||||
|
|
||||||
def delta_components(reference, candidate):
|
|
||||||
"""Components of reference^-1*candidate, plus coordinate-invariant norms."""
|
|
||||||
delta = inverse_transform(reference) @ candidate
|
|
||||||
translation = np.asarray(delta[:3, 3], float)
|
|
||||||
return {
|
|
||||||
"translation_xyz_cm": (translation * 100.0).tolist(),
|
|
||||||
"translation_norm_cm": float(np.linalg.norm(translation) * 100.0),
|
|
||||||
"rotation_rpy_deg_xyz": rpy_deg(delta[:3, :3]),
|
|
||||||
"rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def body_left_rpy(x, rpy_correction_deg):
|
|
||||||
correction = np.eye(4)
|
|
||||||
correction[:3, :3] = Rotation.from_euler(
|
|
||||||
"xyz", np.asarray(rpy_correction_deg, float), degrees=True
|
|
||||||
).as_matrix()
|
|
||||||
return correction @ x
|
|
||||||
|
|
||||||
|
|
||||||
def print_delta(name, reference, candidate):
|
|
||||||
item = delta_components(reference, candidate)
|
|
||||||
tx, ty, tz = item["translation_xyz_cm"]
|
|
||||||
roll, pitch, yaw = item["rotation_rpy_deg_xyz"]
|
|
||||||
print(
|
|
||||||
f"{name}: B^-1*motion translation xyz = "
|
|
||||||
f"[{tx:+.4f}, {ty:+.4f}, {tz:+.4f}] cm; "
|
|
||||||
f"rpy xyz = [{roll:+.4f}, {pitch:+.4f}, {yaw:+.4f}] deg; "
|
|
||||||
f"norm = {item['translation_norm_cm']:.4f} cm / "
|
|
||||||
f"{item['rotation_angle_deg']:.6f} deg"
|
|
||||||
)
|
|
||||||
return item
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
import open3d as o3d
|
|
||||||
|
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
parser.add_argument("--frames", required=True)
|
|
||||||
parser.add_argument("--pairs", required=True)
|
|
||||||
parser.add_argument("--extrinsic", required=True)
|
|
||||||
parser.add_argument("--pair-index", type=int, default=0)
|
|
||||||
parser.add_argument("--voxel", type=float, default=0.10)
|
|
||||||
parser.add_argument(
|
|
||||||
"--left-rpy-deg", nargs=3, type=float, default=[0.0, 0.0, 0.0],
|
|
||||||
metavar=("ROLL", "PITCH", "YAW"),
|
|
||||||
help="optional body-frame left correction applied as DeltaR_body * X",
|
|
||||||
)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
stations = load_stations(args.frames, 1.0, 60.0)
|
|
||||||
with np.load(args.pairs, allow_pickle=False) as data:
|
|
||||||
if len(stations) != len(data["station_times"]):
|
|
||||||
raise ValueError(
|
|
||||||
f"frames contain {len(stations)} stations but pair file records "
|
|
||||||
f"{len(data['station_times'])}"
|
|
||||||
)
|
|
||||||
if not 0 <= args.pair_index < len(data["A"]):
|
|
||||||
raise IndexError(
|
|
||||||
f"pair-index {args.pair_index} outside [0,{len(data['A']) - 1}]"
|
|
||||||
)
|
|
||||||
a_ij = np.asarray(data["A"][args.pair_index], float)
|
|
||||||
b_gicp = np.asarray(data["B"][args.pair_index], float)
|
|
||||||
i, j = np.asarray(data["meta"][args.pair_index, :2], int)
|
|
||||||
|
|
||||||
with open(args.extrinsic, encoding="utf-8-sig") as stream:
|
|
||||||
result = json.load(stream)
|
|
||||||
x = np.asarray(result["matrix_4x4"], float)
|
|
||||||
b_calibrated = inverse_transform(x) @ a_ij @ x
|
|
||||||
|
|
||||||
transforms = {
|
|
||||||
"1 raw": np.eye(4),
|
|
||||||
"2 RTK initial (X0=I)": a_ij,
|
|
||||||
"3 GICP B": b_gicp,
|
|
||||||
"4 calibrated X^-1 A X": b_calibrated,
|
|
||||||
}
|
|
||||||
correction = np.asarray(args.left_rpy_deg, float)
|
|
||||||
if np.any(np.abs(correction) > 0.0):
|
|
||||||
x_test = body_left_rpy(x, correction)
|
|
||||||
transforms[
|
|
||||||
f"5 test body-left RPY {correction.tolist()} deg"
|
|
||||||
] = inverse_transform(x_test) @ a_ij @ x_test
|
|
||||||
|
|
||||||
target = stations[i][3]
|
|
||||||
source = stations[j][3]
|
|
||||||
print(f"pair_index={args.pair_index}, station {i} <- {j}")
|
|
||||||
print("blue = target station i; orange = source station j after selected transform")
|
|
||||||
print("keys: 1 raw | 2 RTK initial | 3 GICP | 4 calibrated | 5 test correction | Q/Esc exit")
|
|
||||||
print(
|
|
||||||
"IMPORTANT: delta xyz/rpy are components of B^-1*(X^-1*A*X), expressed "
|
|
||||||
"in station-j LiDAR coordinates; screen-left/right depends on the 3D camera view."
|
|
||||||
)
|
|
||||||
baseline = print_delta("mode 4 minus mode 3", b_gicp, b_calibrated)
|
|
||||||
roll, pitch, yaw = np.abs(baseline["rotation_rpy_deg_xyz"])
|
|
||||||
if max(roll, pitch) > max(0.10, 2.0 * yaw):
|
|
||||||
print("diagnosis: roll/pitch components dominate yaw; do not prioritize yaw tuning for this pair.")
|
|
||||||
tx, ty, tz = np.abs(baseline["translation_xyz_cm"])
|
|
||||||
if tz > max(tx, ty):
|
|
||||||
print("diagnosis: the largest translation component is relative Z, not lateral XY.")
|
|
||||||
body_up = np.array([0.0, 0.0, 1.0])
|
|
||||||
if np.linalg.norm(a_ij[:3, :3] @ body_up - body_up) < 1e-8:
|
|
||||||
print(
|
|
||||||
"observability: this A preserves the body Z axis, so body-left X.z "
|
|
||||||
"translation is unobservable from this pair; use ground/external height constraints."
|
|
||||||
)
|
|
||||||
if "5 test body-left RPY " + str(correction.tolist()) + " deg" in transforms:
|
|
||||||
print_delta("mode 5 minus mode 3", b_gicp, list(transforms.values())[-1])
|
|
||||||
|
|
||||||
viewer = o3d.visualization.VisualizerWithKeyCallback()
|
|
||||||
viewer.create_window("Rigorous LiDAR registration inspection - 3D", 1400, 900)
|
|
||||||
target_cloud = cloud(o3d, target, COLORS["target"], args.voxel)
|
|
||||||
source_cloud = cloud(o3d, source, COLORS["source"], args.voxel)
|
|
||||||
viewer.add_geometry(target_cloud)
|
|
||||||
viewer.add_geometry(source_cloud)
|
|
||||||
axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0)
|
|
||||||
viewer.add_geometry(axes)
|
|
||||||
current = np.eye(4)
|
|
||||||
|
|
||||||
def select(name):
|
|
||||||
def callback(vis):
|
|
||||||
nonlocal current
|
|
||||||
desired = transforms[name]
|
|
||||||
source_cloud.transform(desired @ inverse_transform(current))
|
|
||||||
current = desired
|
|
||||||
vis.update_geometry(source_cloud)
|
|
||||||
if name == "3 GICP B":
|
|
||||||
print(f"{name}: reference registration B; delta = 0")
|
|
||||||
else:
|
|
||||||
print_delta(name + " minus mode 3", b_gicp, desired)
|
|
||||||
return False
|
|
||||||
return callback
|
|
||||||
|
|
||||||
for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4"), ord("5")), transforms):
|
|
||||||
viewer.register_key_callback(key, select(name))
|
|
||||||
viewer.get_render_option().background_color = np.array([0.02, 0.02, 0.02])
|
|
||||||
viewer.get_render_option().point_size = 2.0
|
|
||||||
viewer.run()
|
|
||||||
viewer.destroy_window()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -0,0 +1,51 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "S2_old_validation"
|
||||||
|
body_frame:
|
||||||
|
name: "rear_axle_center"
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "S2_old_smoke"
|
||||||
|
installed_at: "unknown"
|
||||||
|
notes: "Smoke-test on old S2 host-time data only. Not for delivery."
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "HI13_old_S2"
|
||||||
|
raw_frame:
|
||||||
|
axes: "as exported HI91"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "frontlidar"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "Cartesian metres from points_raw spherical conversion"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "host_utc_receive_of_serial_chunk"
|
||||||
|
lidar_timestamp_source: "unix_time_ns_from_dlog_export"
|
||||||
|
lidar_frame_time_definition: "frame midpoint approx from unix_time_ns"
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_m: null
|
||||||
|
rotation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_deg: null
|
||||||
@@ -0,0 +1,86 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "outdoor_usable_20260808"
|
||||||
|
body_frame:
|
||||||
|
name: "base_link"
|
||||||
|
# 车体约定：后轮轴中心在地面投影为原点附近参考；X 前 / Y 左 / Z 上
|
||||||
|
# translation_m 的 Z 使用「离地高度」；后轮轴中心离地 294 mm
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
reference_point: "rear_axle_center_xy__z_above_ground"
|
||||||
|
rear_axle_height_above_ground_m: 0.294
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "20260808_priority_windows"
|
||||||
|
installed_at: "2026-08-08"
|
||||||
|
notes: >
|
||||||
|
HI13R4 + H32 DLogCapture. Body +X forward: LiDAR and IMU at positive X.
|
||||||
|
CAD sheet may draw +X rearward; numbers below are body-frame.
|
||||||
|
Z is height above ground = CAD height at axle + 0.294 m (axle AGL).
|
||||||
|
LiDAR CAD dZ is 1637.499879 mm relative to the axle reference. Phase-center
|
||||||
|
AGL adds rear-axle height 294 mm and the 63.5 mm phase-center offset.
|
||||||
|
IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up).
|
||||||
|
LiDAR Cartesian in NPZ assumed body-aligned (X forward).
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "HI13R4"
|
||||||
|
raw_frame:
|
||||||
|
# HI13R4 用户手册 2.4：右-前-上 (RFU)
|
||||||
|
axes: "X right, Y forward, Z up (RFU)"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
# X/Y：后轮轴中心 → IMU；Z：离地 = CAD 0.8925 + 0.294
|
||||||
|
translation_m: [2.574126255, 0.0365, 1.1865]
|
||||||
|
# body <- imu : p_body = R_body_imu * p_imu
|
||||||
|
# R_body_imu = [[0,1,0],[-1,0,0],[0,0,1]] (fwd=imu_y, left=-imu_x, up=imu_z)
|
||||||
|
rotation_matrix_body_imu: [[0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
source: "CAD X/Y in body (+X forward); Z = CAD axle-height + 294mm AGL + HI13R4 RFU"
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "RSLidarH32"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
# X/Y：后轮轴中心 → 雷达
|
||||||
|
# Z离地 = CAD dZ 1.637499879 + 后轮轴离地 0.294 + 相位中心偏移 0.0635
|
||||||
|
translation_m: [2.522276859, 0.000020526, 1.994999879]
|
||||||
|
rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
source: "CAD X/Y in body (+X forward); Z AGL = CAD dZ 1.637499879 + axle AGL 0.294 + phase-center offset 0.0635; attitude = body"
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "hi13_device_timestamp_ms_seconds"
|
||||||
|
lidar_timestamp_source: "h32_msop_device_timestamp_seconds"
|
||||||
|
lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint"
|
||||||
|
host_bridge: "MSOP HostReceiveUtcTicks + IMU receive_utc_ticks"
|
||||||
|
|
||||||
|
# Derived prior for p_IMU = R_IMU_lidar * p_lidar + t_IMU_lidar
|
||||||
|
# t_body = t_lidar_body - t_imu_body
|
||||||
|
# t_IMU_lidar = R_IMU_body * t_body, R_IMU_lidar = R_IMU_body * R_body_lidar
|
||||||
|
derived_T_IMU_lidar_prior:
|
||||||
|
R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||||
|
t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
|
||||||
|
notes: >
|
||||||
|
Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd).
|
||||||
|
Relative Z = 1.994999879 - 1.1865 = 0.808499879 m.
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: true
|
||||||
|
sigma_m: [0.05, 0.05, 0.05]
|
||||||
|
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||||
|
rotation_prior:
|
||||||
|
enabled: true
|
||||||
|
sigma_deg: 15.0
|
||||||
|
R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
@@ -0,0 +1,51 @@
|
|||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
vehicle:
|
||||||
|
vehicle_id: "example_vehicle"
|
||||||
|
body_frame:
|
||||||
|
name: "base_link"
|
||||||
|
axes: "X forward, Y left, Z up"
|
||||||
|
unit: m
|
||||||
|
|
||||||
|
installation:
|
||||||
|
installation_id: "example_install"
|
||||||
|
installed_at: "unknown"
|
||||||
|
notes: "V1 example config. Mount translations may stay null."
|
||||||
|
|
||||||
|
sensors:
|
||||||
|
imu:
|
||||||
|
model: "unknown_imu"
|
||||||
|
raw_frame:
|
||||||
|
axes: "declare after black-box tests: e.g. out_x=forward, out_y=left, out_z=up"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
lidar:
|
||||||
|
model: "unknown_lidar"
|
||||||
|
points_field: points
|
||||||
|
raw_frame:
|
||||||
|
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
||||||
|
driver_axis_remapped: false
|
||||||
|
mount_in_body:
|
||||||
|
translation_m: null
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
|
||||||
|
rtk:
|
||||||
|
frame_definition: ""
|
||||||
|
reference_point: ""
|
||||||
|
existing_T_RTK_LIDAR_file: ""
|
||||||
|
|
||||||
|
time:
|
||||||
|
imu_timestamp_source: "device_or_file_clock_seconds"
|
||||||
|
lidar_timestamp_source: "frame_midpoint_seconds"
|
||||||
|
lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint"
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_m: null
|
||||||
|
rotation_prior:
|
||||||
|
enabled: false
|
||||||
|
sigma_deg: null
|
||||||
@@ -0,0 +1,209 @@
|
|||||||
|
# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题
|
||||||
|
|
||||||
|
> 数据：`D:\data\calibration_usable_20260808`
|
||||||
|
> 可用会话：`sessions_v1_host_aligned`（三优先窗）
|
||||||
|
> 当前结果目录：各窗 `out_fixed_dt0/`
|
||||||
|
> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`
|
||||||
|
> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml`
|
||||||
|
> 约定外参：`p_IMU = T_IMU_lidar · p_lidar`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 一句话结论
|
||||||
|
|
||||||
|
**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。**
|
||||||
|
三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 当前可用结果（`out_fixed_dt0`）
|
||||||
|
|
||||||
|
约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。
|
||||||
|
|
||||||
|
| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
|
||||||
|
|----|------|----|---------------------|--------------|----------|--------------|
|
||||||
|
| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
|
||||||
|
| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° |
|
||||||
|
| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° |
|
||||||
|
|
||||||
|
- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。
|
||||||
|
- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。
|
||||||
|
- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。
|
||||||
|
|
||||||
|
### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
|
||||||
|
- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
|
||||||
|
[ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ],
|
||||||
|
[-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
|
||||||
|
- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
|
||||||
|
[ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ],
|
||||||
|
[ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
|
||||||
|
- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
|
||||||
|
[ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
|
||||||
|
[ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 相对历史失败轮次
|
||||||
|
|
||||||
|
| 轮次 | 问题 | 结果 |
|
||||||
|
|------|------|------|
|
||||||
|
| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° |
|
||||||
|
| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） |
|
||||||
|
| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 已澄清并写入配置的坐标系 / 先验
|
||||||
|
|
||||||
|
### 3.1 车体与传感器
|
||||||
|
|
||||||
|
- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。
|
||||||
|
- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。
|
||||||
|
- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。
|
||||||
|
- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。
|
||||||
|
|
||||||
|
### 3.2 安装量（`translation_m`）
|
||||||
|
|
||||||
|
| 传感器 | X / Y（后轮轴中心） | Z（离地） |
|
||||||
|
|--------|---------------------|-----------|
|
||||||
|
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
|
||||||
|
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
|
||||||
|
|
||||||
|
- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
|
||||||
|
- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。
|
||||||
|
|
||||||
|
### 3.3 导出外参先验
|
||||||
|
|
||||||
|
- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。
|
||||||
|
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
|
||||||
|
- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 现存问题清单
|
||||||
|
|
||||||
|
### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移）
|
||||||
|
|
||||||
|
- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。
|
||||||
|
- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。
|
||||||
|
- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。
|
||||||
|
- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。
|
||||||
|
- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。
|
||||||
|
|
||||||
|
### P2. 平面运动导致竖直平移弱可观
|
||||||
|
|
||||||
|
- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。
|
||||||
|
- 流水线门控已给出 `translation_accepted=false`。
|
||||||
|
- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。
|
||||||
|
|
||||||
|
### P3. 时间偏移若再自由估计会被带偏（已规避，需保持）
|
||||||
|
|
||||||
|
- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。
|
||||||
|
- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。
|
||||||
|
- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。
|
||||||
|
|
||||||
|
### P4. 单窗低残差 ≠ 外参正确（历史教训）
|
||||||
|
|
||||||
|
- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。
|
||||||
|
- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
|
||||||
|
- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。
|
||||||
|
|
||||||
|
### P5. 旋转软先验尚未做无先验对照
|
||||||
|
|
||||||
|
- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。
|
||||||
|
- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。
|
||||||
|
|
||||||
|
### P6. 文档与操作约定未完全同步（工程）
|
||||||
|
|
||||||
|
- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
|
||||||
|
- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 不该做 / 可以做
|
||||||
|
|
||||||
|
| 动作 | 建议 |
|
||||||
|
|------|------|
|
||||||
|
| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） |
|
||||||
|
| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
|
||||||
|
| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 |
|
||||||
|
| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** |
|
||||||
|
| 无先验 / 大 σ 旋转对照 | **建议做** |
|
||||||
|
| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
|
||||||
|
| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 建议下一步顺序
|
||||||
|
|
||||||
|
1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。
|
||||||
|
2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。
|
||||||
|
3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。
|
||||||
|
4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 常用路径与命令
|
||||||
|
|
||||||
|
```text
|
||||||
|
数据根：
|
||||||
|
D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
|
||||||
|
|
||||||
|
结果：
|
||||||
|
...\priority_XXXX\out_fixed_dt0\summary.json
|
||||||
|
...\priority_XXXX\out_fixed_dt0\motion_pairs.json
|
||||||
|
...\calibration_manifest_fixed_dt0.json
|
||||||
|
```
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
# 可视化（rotation_only 默认模式4用 GICP 平移）
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
|
||||||
|
--summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
|
||||||
|
--pair-index 0
|
||||||
|
|
||||||
|
# 若要看「坏 Δp」导致的错层效果：
|
||||||
|
# --mode4-translation imu
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. 问题优先级（跟踪用）
|
||||||
|
|
||||||
|
| ID | 严重度 | 状态 | 标题 |
|
||||||
|
|----|--------|------|------|
|
||||||
|
| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 |
|
||||||
|
| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 |
|
||||||
|
| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） |
|
||||||
|
| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
|
||||||
|
| P5 | 中 | 待做 | 无旋转先验对照 |
|
||||||
|
| P6 | 低 | 待做 | README/交付物同步 |
|
||||||
@@ -0,0 +1,195 @@
|
|||||||
|
# LiDAR–IMU 外参标定说明
|
||||||
|
|
||||||
|
**用途：** 方法约定与实现状态（深入阅读）。日常使用请先看根目录 [`README.md`](../README.md)。
|
||||||
|
|
||||||
|
本文说明标定目标、约定、流水线与实现状态。代码在 `imu_lidar/`。场地采集见 [`标定流程与采集清单.md`](标定流程与采集清单.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 目标与约定
|
||||||
|
|
||||||
|
估计安装外参：
|
||||||
|
|
||||||
|
```text
|
||||||
|
p_IMU = T_IMU_lidar · p_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
约定：`T_A_B` 表示把 **B 系点**变换到 **A 系**。
|
||||||
|
|
||||||
|
相对运动手眼模型：
|
||||||
|
|
||||||
|
```text
|
||||||
|
A_ij ≈ IMU 在 [t_i, t_j] 的相对运动（预积分）
|
||||||
|
B_ij ≈ LiDAR 在同时间段的相对运动（关键帧配准）
|
||||||
|
A X ≈ X B
|
||||||
|
X = T_IMU_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
旋转子问题（常规主交付）：
|
||||||
|
|
||||||
|
```text
|
||||||
|
R_A R_X = R_X R_B
|
||||||
|
→ R_IMU_lidar
|
||||||
|
```
|
||||||
|
|
||||||
|
若已有完整六自由度外参，且另有 `T_RTK_lidar`，可链式得到：
|
||||||
|
|
||||||
|
```text
|
||||||
|
T_lidar_IMU = inverse(T_IMU_lidar)
|
||||||
|
T_RTK_IMU = T_RTK_lidar @ T_lidar_IMU
|
||||||
|
```
|
||||||
|
|
||||||
|
**不要**把 IMU 加速度二次积分成轨迹，再当作绝对位姿去做完整六自由度手眼。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 交付分层
|
||||||
|
|
||||||
|
| 层级 | 交付 | 数据最低要求 |
|
||||||
|
|---|---|---|
|
||||||
|
| 第一步 | 旋转 + 时间偏置 δt | **设备时间戳**；静止 + 低速转弯 /「8」字；结构化场景 |
|
||||||
|
| 第二步 | 上一步 + 可观的水平平移 | 更多转弯半径与加减速 |
|
||||||
|
| 第三步 | 完整六自由度（含可靠竖直分量） | 缓坡俯仰激励，或外测垂直杆臂先验 |
|
||||||
|
|
||||||
|
可观性不过关 → 只交旋转，不强交“假精确”六自由度。
|
||||||
|
仅主机接收时间、或残差未过门控的结果 → **不要当作正式安装参数**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 总体原则
|
||||||
|
|
||||||
|
1. **时间同步优先于外参**：δt 未对齐时，旋转与平移都不可信。使用设备时间戳。
|
||||||
|
2. **先求旋转，再求平移**：旋转通常更稳；平面运动下竖直平移常常不可观。
|
||||||
|
3. **用连续运动标定**：停车多站、再靠站间长积分，不适合作为纯 IMU 外参主流程。
|
||||||
|
4. **可观性门控**：过不了就降级交付。
|
||||||
|
5. **残差小 ≠ 标定对**：需叠点云 / 跨会话等独立验证。
|
||||||
|
6. **首轮建议低速**：先保证配准与时间对齐；点云去畸变可选。
|
||||||
|
7. **安装参数不写死在源码**：轴向与时间语义进 YAML；机械尺寸可作检查，不能伪装成已标定平移。
|
||||||
|
8. **注意耦合**：时间相关峰很弱时，δt、航向角与陀螺零偏可能互相补偿，结果不可当真。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 流水线（现行实现）
|
||||||
|
|
||||||
|
```text
|
||||||
|
vehicle_config
|
||||||
|
→ timestamp_audit
|
||||||
|
→ imu_audit（静止零偏等）
|
||||||
|
→ time_offset：粗估 δt
|
||||||
|
→ keyframes / [可选] deskew
|
||||||
|
→ motion_pairs：完整 IMU 预积分 + 配准 B
|
||||||
|
→ rotation_handeye：加权求解 R
|
||||||
|
→ （有候选 R 时）精修 δt，必要时交替重建运动对
|
||||||
|
→ joint_optimizer：精修旋转与陀螺零偏；可观且 full_se3 时再估平移等
|
||||||
|
→ finalize
|
||||||
|
```
|
||||||
|
|
||||||
|
| 模块 | 文件 | 职责 |
|
||||||
|
|---|---|---|
|
||||||
|
| 配置 | `vehicle_config.py` | 读安装 YAML |
|
||||||
|
| IO | `imu_io.py` / `lidar_io.py` | 标准 CSV / 帧目录 |
|
||||||
|
| 质检 | `timestamp_audit.py` / `imu_audit.py` | 时间域、静止零偏 |
|
||||||
|
| δt | `time_offset.py` | 粗估 + 有符号精修 |
|
||||||
|
| 运动 | `keyframes.py` / `registration.py` / `lidar_deskew.py` | 关键帧、配准、可选去畸变 |
|
||||||
|
| IMU 侧 | `imu_preintegration.py` / `motion_pairs.py` | 预积分与运动对 |
|
||||||
|
| 求解 | `rotation_handeye.py` / `joint_optimizer.py` / `observability.py` | 手眼、联合精修、门控 |
|
||||||
|
| 编排 | `pipeline.py` / `cli.py` / `finalize.py` | 入口与落盘 |
|
||||||
|
|
||||||
|
输入中间格式见 [V1_数据格式.md](V1_数据格式.md)。改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||||
|
|
||||||
|
### 运行示例
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m pip install -e ".[dev]"
|
||||||
|
python -m pip install -e ".[open3d]" # 可选
|
||||||
|
|
||||||
|
python -m imu_lidar.cli plan --mode rotation_only
|
||||||
|
|
||||||
|
python -m imu_lidar.cli run `
|
||||||
|
--vehicle-config config\vehicle_installation.template.yaml `
|
||||||
|
--imu path\to\imu.csv `
|
||||||
|
--lidar path\to\lidar_session `
|
||||||
|
--output path\to\output `
|
||||||
|
--mode rotation_only `
|
||||||
|
--time-offset-search-s 2.0
|
||||||
|
```
|
||||||
|
|
||||||
|
合成自检：`python tools\generate_synthetic_session.py` 后跑 CLI，再 `python -m pytest -q`。
|
||||||
|
|
||||||
|
### 结果状态
|
||||||
|
|
||||||
|
| status | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| `rotation_only_accepted` | 旋转过门，可交旋转与报告中的 δt |
|
||||||
|
| `full_se3_accepted` | 可观且联合优化通过，可交完整 `T` |
|
||||||
|
| `full_se3_rejected_due_to_observability` | 旋转可用，平移未接受 |
|
||||||
|
| `blocked` | 质检 / δt / 手眼残差等硬门失败，**不交付** |
|
||||||
|
|
||||||
|
`rotation_only` 模式下：不要把未标定的机械平移拼进 4×4 伪装成完整标定。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 采集要点
|
||||||
|
|
||||||
|
单趟动态会话：
|
||||||
|
|
||||||
|
```text
|
||||||
|
[静止 20–30 s] → [低速激励 3–8 min] → [再静止 10–20 s]
|
||||||
|
```
|
||||||
|
|
||||||
|
优先激励：
|
||||||
|
|
||||||
|
- 低速「8」字 / 左右圆（旋转主激励）
|
||||||
|
- 直线加减速（有助于时间对齐与水平平移）
|
||||||
|
- 缓坡（仅完整六自由度需要）：约 3°～8°，连续长度优先 ≥ 20～30 m
|
||||||
|
|
||||||
|
硬条件：IMU / LiDAR **设备时间戳**；结构化场景；标定全程安装不得改动。
|
||||||
|
|
||||||
|
更完整的现场清单见 [标定流程与采集清单.md](标定流程与采集清单.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 数学上允许与禁止
|
||||||
|
|
||||||
|
禁止：加速度二次积分当真值轨迹；把只有旋转的相对运动硬补成完整六自由度；用最终外参反向筛边掩盖失败。
|
||||||
|
|
||||||
|
允许：预积分旋转手眼求旋转；在可观时用预积分残差联合估计平移，并精修零偏等辅助量。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 车辆配置
|
||||||
|
|
||||||
|
使用 `--vehicle-config vehicle_installation.yaml`。
|
||||||
|
模板中安装平移/旋转保持未标定状态，直至实测确认；禁止写死某车杆臂冒充结果。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. 实现状态（当前阶段）
|
||||||
|
|
||||||
|
本仓库**仅此一条**标定路径：连续运动关键帧 + IMU 预积分。对外总览与「合成 / 旧车 / 合格数据预期」见根目录 [`README.md`](../README.md) §0。
|
||||||
|
|
||||||
|
| 项 | 状态 |
|
||||||
|
|---|---|
|
||||||
|
| 连续运动关键帧标定流水线 | 已实现（现行唯一路径） |
|
||||||
|
| 加权预积分 / 加权手眼 | 已实现 |
|
||||||
|
| 旋转预积分因子 + 有符号 δt 精修 | 已实现 |
|
||||||
|
| 完整预积分（含速度/位移增量）与可观时的平移优化 | 已实现 |
|
||||||
|
| 可观性门控 / `rotation_only` | 已实现 |
|
||||||
|
| 合成数据 pytest / 一键复现 | 已实现（证明链路与已知 yaw/δt，不证明实车精度） |
|
||||||
|
| 旧车主机时间烟测（S2） | 线下可跑；预期 `blocked`，不当交付 |
|
||||||
|
| 设备时间新车数据正式验收 | **待做** |
|
||||||
|
| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`（N300 rscap + H32 dlog/MSOP → V1；dlog 用 DIFOP 通道角） |
|
||||||
|
|
||||||
|
细项见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 9. 相关文档
|
||||||
|
|
||||||
|
| 内容 | 路径 | 备注 |
|
||||||
|
|---|---|---|
|
||||||
|
| 对外总览（优先） | 根目录 [`README.md`](../README.md) | 日常入口 |
|
||||||
|
| 采集清单 | [`标定流程与采集清单.md`](标定流程与采集清单.md) | 现场 |
|
||||||
|
| 数据格式 / 导出 | [`V1_数据格式.md`](V1_数据格式.md) | 中间格式 |
|
||||||
|
| 文件职责说明 | [`imu_lidar/文件职责说明.md`](../imu_lidar/文件职责说明.md) | 改代码 |
|
||||||
|
| 改动史 | [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md) | 改代码 |
|
||||||
+109
@@ -0,0 +1,109 @@
|
|||||||
|
# V1 标准中间数据格式
|
||||||
|
|
||||||
|
**用途：** 标定程序读入的 CSV/NPZ 约定，以及新车原始数据如何导出。总览见根目录 [README](../README.md)。
|
||||||
|
|
||||||
|
## 从原始数据导出
|
||||||
|
|
||||||
|
**推荐（新 H32 + HI13）：** HI13 `.rscap` + 雷达 Medulla dlog / recovered zip（raw MSOP + DIFOP）。
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\export_rscap_to_v1.py `
|
||||||
|
--imu-rscap path\to\hi13r4-imu.rscap `
|
||||||
|
--imu-kind hi13 `
|
||||||
|
--lidar-dlog path\to\session_or_dlog_or_recovered.zip `
|
||||||
|
--host-start 2026-08-08T17:40:05 `
|
||||||
|
--host-end 2026-08-08T17:45:15 `
|
||||||
|
--out path\to\session_v1 `
|
||||||
|
--frame-stride 5 `
|
||||||
|
--require-difop
|
||||||
|
```
|
||||||
|
|
||||||
|
`--lidar-dlog` 可为：标准 `dobject/`+`dobject_recording/` 目录，或 recovered zip（`indices.log` + `data.bin`）。
|
||||||
|
`--imu-kind`：`hi13` / `n300` / `auto`（默认按文件名推断）。
|
||||||
|
`--host-start/end`：按本地墙钟切窗（仅裁剪；标定主轴仍是设备时间）。
|
||||||
|
默认 DObject：`frontlidar-msop-raw`、`frontlidar-difop-raw`。
|
||||||
|
|
||||||
|
**兼容旧 MSOP-only `.rscap`：**
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools\export_rscap_to_v1.py `
|
||||||
|
--imu-rscap path\to\n300.rscap `
|
||||||
|
--lidar-rscap path\to\h32_msop.rscap `
|
||||||
|
--out path\to\session_v1 `
|
||||||
|
--frame-stride 1
|
||||||
|
```
|
||||||
|
|
||||||
|
产出：`imu.csv`、`lidar/`（含 `frames_index.csv`）、`export_summary.json`。
|
||||||
|
标定主轴仍是**设备时间**；同时写出**主机 UTC 接收时间**，用于把雷达帧桥接到 IMU 设备钟（禁止把两边设备时间第一帧强行重合）。
|
||||||
|
|
||||||
|
## IMU
|
||||||
|
|
||||||
|
文件：`imu.csv` 或 `imu.npz`
|
||||||
|
|
||||||
|
### CSV
|
||||||
|
|
||||||
|
```text
|
||||||
|
t,gx,gy,gz,ax,ay,az,t_host_utc_s,receive_utc_ticks
|
||||||
|
0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81,1754646005.123,6389...
|
||||||
|
...
|
||||||
|
```
|
||||||
|
|
||||||
|
| 列 | 含义 | 单位 |
|
||||||
|
|---|---|---|
|
||||||
|
| t | IMU 设备时钟时间 | s |
|
||||||
|
| gx,gy,gz | 角速度 | rad/s |
|
||||||
|
| ax,ay,az | 比力/加速度 | m/s² |
|
||||||
|
| t_host_utc_s | 主机 UTC 接收时间（Unix） | s |
|
||||||
|
| receive_utc_ticks | 同上，.NET UTC ticks | — |
|
||||||
|
|
||||||
|
### NPZ
|
||||||
|
|
||||||
|
数组：`t (N,)`, `gyro (N,3)`, `acc (N,3)`，含义同上。
|
||||||
|
|
||||||
|
> IMU 与 LiDAR 的时间原点可以不同。流水线会估计常值偏置：`t_imu = t_lidar + delta_t`。
|
||||||
|
|
||||||
|
## LiDAR
|
||||||
|
|
||||||
|
目录结构：
|
||||||
|
|
||||||
|
```text
|
||||||
|
lidar_session/
|
||||||
|
├── frames_index.csv
|
||||||
|
└── frames/
|
||||||
|
├── frame_00000.npz
|
||||||
|
├── frame_00001.npz
|
||||||
|
└── ...
|
||||||
|
```
|
||||||
|
|
||||||
|
### frames_index.csv
|
||||||
|
|
||||||
|
```text
|
||||||
|
frame_id,filename,t_start,t_end,host_receive_utc_ticks,t_host_utc_s,host_receive_utc_end_ticks,t_host_utc_end_s
|
||||||
|
0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22
|
||||||
|
```
|
||||||
|
|
||||||
|
| 列 | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| t_start / t_end | H32 MSOP **设备时间**（秒） |
|
||||||
|
| t_host_utc_s / t_host_utc_end_s | 帧首/末包 **HostReceiveUtcTicks** → Unix 秒 |
|
||||||
|
|
||||||
|
也兼容旧列名 `file`。对齐脚本用主机 UTC 把 `t_*` 重写到 IMU 设备钟后再跑标定。
|
||||||
|
|
||||||
|
### 每帧 NPZ
|
||||||
|
|
||||||
|
- `points`: `float64/float32`，形状 `(N, 3)`，LiDAR 直角坐标系，单位米
|
||||||
|
|
||||||
|
## 时间不同步能不能用？
|
||||||
|
|
||||||
|
可以，前提是：
|
||||||
|
|
||||||
|
1. 两边都覆盖同一段**有角速度激励**的物理运动（尤其是转弯）；
|
||||||
|
2. 偏置近似为**常数**（短会话）；
|
||||||
|
3. `--time-offset-search-s` 足够覆盖可能的偏移（默认 ±1 s，可加大）。
|
||||||
|
|
||||||
|
若两段数据完全不是同一趟行驶，或只有静止 IMU、没有重叠运动，则无法估 δt，标定会被 `blocked`。
|
||||||
|
|
||||||
|
## 最小可用会话
|
||||||
|
|
||||||
|
- IMU：建议含静止段 + 运动段，采样率稳定
|
||||||
|
- LiDAR：建议 ≥ 20 帧，场景有墙/柱等结构，含转弯
|
||||||
@@ -0,0 +1,168 @@
|
|||||||
|
# RTK–IMU 标定链路审计与当前方案
|
||||||
|
|
||||||
|
> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
|
||||||
|
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
|
||||||
|
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。
|
||||||
|
|
||||||
|
## 结论
|
||||||
|
|
||||||
|
当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。
|
||||||
|
|
||||||
|
G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。
|
||||||
|
|
||||||
|
双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。
|
||||||
|
|
||||||
|
当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。
|
||||||
|
|
||||||
|
## 坐标系和参考点
|
||||||
|
|
||||||
|
统一输出约定：
|
||||||
|
|
||||||
|
```text
|
||||||
|
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
|
||||||
|
```
|
||||||
|
|
||||||
|
RTK 车固坐标定义：
|
||||||
|
|
||||||
|
- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
|
||||||
|
- +Y：车辆前进方向，与 IMU +Y 同向。
|
||||||
|
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
|
||||||
|
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。
|
||||||
|
|
||||||
|
位置参考点：
|
||||||
|
|
||||||
|
- GGA 为 ANT1/主天线相位中心。
|
||||||
|
- 相位中心离地高度为 1.916499878 m。
|
||||||
|
|
||||||
|
## 求解链路
|
||||||
|
|
||||||
|
### R0：协议、数据质量和时间
|
||||||
|
|
||||||
|
1. 校验 NMEA checksum。
|
||||||
|
2. GGA 位置只接收 Q=4 固定解。
|
||||||
|
3. GNHPR 姿态只接收 Q=4 固定解；Q=5 浮点解仅进入诊断统计，不参与标定。
|
||||||
|
4. 保留 GNHPR 自己的卫星数、差分龄期和基站号，不能再用 GGA 卫星数替代 HPR 质量。
|
||||||
|
5. 姿态使用 `hpr_measurement_utc_s` 映射后的 IMU 设备时间。
|
||||||
|
6. 遇到 HPR 无效、浮点、时间间隔异常或基线跳变时切断连续段，运动对不得跨断点。
|
||||||
|
|
||||||
|
### T0：残余时间偏移
|
||||||
|
|
||||||
|
只使用可观测的有符号 heading 角速度与 IMU `gyro_z` 做相关扫描。角速度模长会混入不可观的绕基线旋转，不再作为时间审计依据。
|
||||||
|
|
||||||
|
时间偏移只用于初始化/诊断。只有相关峰足够高、与次峰分离且峰宽足够窄时才应用，否则保持 0 s。
|
||||||
|
|
||||||
|
### R1：双天线基线一致性
|
||||||
|
|
||||||
|
对每个连续固定解片段构造 0.75 s、1.5 s、3.0 s 的相对运动。以安装先验 `u_IMU=[1,0,0]` 检验：
|
||||||
|
|
||||||
|
```text
|
||||||
|
angle(u_RTK(t0), u_RTK(t1))
|
||||||
|
≈ angle(u_IMU, ΔR_IMU(t0,t1) u_IMU)
|
||||||
|
```
|
||||||
|
|
||||||
|
该标量约束不虚构 RTK 的前向轴和上向轴。输出整体、逐会话 RMS/P95、分轴诊断及最坏时间区间。所有会话的总权重归一，避免高激励或样本更多的会话支配结果。
|
||||||
|
|
||||||
|
### R2：旧式完整 HPR 诊断
|
||||||
|
|
||||||
|
为兼容历史输出，将基线补成零 roll 的数学坐标架，再运行完整手眼。这个结果仅用于暴露符号错误、异常会话和旧结果变化，不能作为可交付外参。
|
||||||
|
|
||||||
|
LOO 删除一个会话后，会重新优化剩余会话的陀螺零偏，不再固定全量数据的 nuisance 参数。
|
||||||
|
|
||||||
|
### T1：平移
|
||||||
|
|
||||||
|
只有完整三自由度旋转“可观且通过”时，才允许进入杆臂和平移求解。当前条件不满足，因此：
|
||||||
|
|
||||||
|
- 不运行平移优化；
|
||||||
|
- 不输出 `translation_result.json`；
|
||||||
|
- `t_RTK_IMU_m` 和 `T_RTK_IMU` 为 null；
|
||||||
|
- 历史巨大 Z、米级不确定度和约 1 m/s 速度残差不再消耗优化时间。
|
||||||
|
|
||||||
|
## 当前 8 会话结果
|
||||||
|
|
||||||
|
结果目录：`artifacts/rtk_imu_calibration_v2/all_sessions`。
|
||||||
|
|
||||||
|
```text
|
||||||
|
可观测基线一致性：
|
||||||
|
RMS / median / P95 = 0.593665 / 0.146040 / 1.212972 deg
|
||||||
|
priority_175910 RMS / P95 = 1.316292 / 2.472570 deg
|
||||||
|
slope RMS / P95 = 1.264514 / 2.475899 deg
|
||||||
|
最坏区间 = priority_175910，约 5.084 deg
|
||||||
|
baseline gate = passed
|
||||||
|
|
||||||
|
时间偏移：
|
||||||
|
全局候选 = +0.005 s
|
||||||
|
峰值相关 = 0.909019
|
||||||
|
近峰宽度 = [-0.110, +0.170] s
|
||||||
|
实际应用 = 0 s
|
||||||
|
|
||||||
|
旧式完整 HPR 诊断：
|
||||||
|
RPY = [-0.132765, -0.297433, -1.558358] deg
|
||||||
|
RMS / P95 = 1.172397 / 2.404977 deg
|
||||||
|
std = [0.615416, 0.437419, 3.687145] deg
|
||||||
|
priority_175910 re-optimized LOO = 1.698555 deg
|
||||||
|
max re-optimized LOO = 1.699427 deg（删除 slope）
|
||||||
|
legacy numeric gate = failed
|
||||||
|
full attitude observable = false
|
||||||
|
|
||||||
|
平移：
|
||||||
|
frozen / not run
|
||||||
|
```
|
||||||
|
|
||||||
|
旧结果中的 `priority_175910` 条件 LOO 为 5.14°。严格剔除 Q5、会话等权、断段保护以及 LOO 重估零偏后，该会话完整诊断 LOO 降为 1.699°。它和坡道会话仍是主要异常源，但现在异常集中在基线仰角通道，而不是 heading：这更符合原始 HPR 中 Q5、低卫星数和 pitch 大幅波动的事实。
|
||||||
|
|
||||||
|
## 已排除或仍存在的漏洞
|
||||||
|
|
||||||
|
- 已修复：heading 误当车前方向。
|
||||||
|
- 已修复：GNHPR pitch 误当纵向车体 pitch。
|
||||||
|
- 已修复：不可观 roll 注入完整姿态。
|
||||||
|
- 已修复：Q5 浮点 HPR 进入标定。
|
||||||
|
- 已修复：HPR 质量字段在导出时丢失。
|
||||||
|
- 已修复：相对运动跨越无效段或跳变。
|
||||||
|
- 已修复：样本多的会话权重过大。
|
||||||
|
- 已修复：LOO 固定全量会话零偏。
|
||||||
|
- 已修复：时间相关使用三轴角速度模长。
|
||||||
|
- 已修复：旋转未通过仍继续优化平移。
|
||||||
|
- 仍存在：当前运动不能稳定地从 GGA 速度补全车前/车上方向。
|
||||||
|
- 仍存在：基线绕轴自由度没有独立传感器观测。
|
||||||
|
- 仍存在：`priority_175910` 和坡道的基线仰角存在局部异常。
|
||||||
|
|
||||||
|
## 如何得到可交付的完整旋转
|
||||||
|
|
||||||
|
按优先级建议：
|
||||||
|
|
||||||
|
1. 采一组专用数据：空旷区域、全程 RTK fixed、较长直线加减速、左右转、坡道上下行，保留原始 GGA/GNHPR/IMU 时间和全部质量字段。
|
||||||
|
2. 用高质量前向速度补第二根轴。只在速度足够高、航向变化平缓的区间用 GGA course，并在同一优化中建模 ANT1 杆臂、非完整车辆侧向速度约束和时间偏移。
|
||||||
|
3. 用静止重力补上向轴时，必须把“地面水平/车辆静止”写成显式先验，并将结果标记为安装先验约束解，而不是双天线数据独立解。
|
||||||
|
4. 若能取得 G90 内部融合后的完整 INS 姿态、第三天线、轮速/转角或可靠车体姿态源，优先作为第二独立方向。
|
||||||
|
5. 完整旋转通过留出验证后，才恢复平移；平移应同时估计杆臂、速度、加计偏置，并检查垂向高程基准。
|
||||||
|
|
||||||
|
当前数据上尝试用平滑 GGA 速度直接补全姿态，结果随平滑窗口明显变化，受低速、转弯杆臂和高程差分噪声影响，不能进入正式结果。
|
||||||
|
|
||||||
|
## 与主流开源方法的对应
|
||||||
|
|
||||||
|
- Kalibr：角速度相关适合作为时间偏移初始化，不应在宽峰时强行采用候选值。
|
||||||
|
- iKalibr：采用连续时间轨迹联合估计时空参数，并强调充分激励；适合后续专用数据。
|
||||||
|
- MINS：异步测量插值并把传感器外参、时间和 nuisance 状态一起估计。
|
||||||
|
- GICI-LIB：因子图中显式进行 GNSS/INS 初始化、质量控制和异常值处理。
|
||||||
|
|
||||||
|
本项目暂不直接引入这些大型框架，而是吸收其原则：先保证物理可观测性和数据质量，再进行联合优化；不能用自由状态吸收错误模型。
|
||||||
|
|
||||||
|
参考：
|
||||||
|
|
||||||
|
- Unicore N4 Reference Commands Manual
|
||||||
|
- Unicore UM982 User Manual
|
||||||
|
- https://github.com/Unsigned-Long/iKalibr
|
||||||
|
- https://github.com/ethz-asl/kalibr
|
||||||
|
- https://github.com/rpng/MINS
|
||||||
|
- https://github.com/chichengcn/gici-open
|
||||||
|
|
||||||
|
## 代码入口
|
||||||
|
|
||||||
|
- `rtk_imu/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。
|
||||||
|
- `rtk_imu/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。
|
||||||
|
- `rtk_imu/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。
|
||||||
|
- `rtk_imu/rtk_imu_replay.py`：坐标定义、参考点、平移冻结和 JSON 输出。
|
||||||
|
- `tools/rscap_v2/g90_rtk.py`：原始 GNHPR 解析及质量字段。
|
||||||
|
- `tools/export_g90_rtk_to_sessions.py`：GNHPR 质量字段导出。
|
||||||
|
- `tools/run_rtk_imu_calibration.py`：端到端命令行入口。
|
||||||
|
- `tests/test_rtk_imu_calibration.py`：轴定义、Q4/Q5、时间和预积分回归测试。
|
||||||
@@ -0,0 +1,108 @@
|
|||||||
|
# RTK–IMU Engineering 6DoF 分支
|
||||||
|
|
||||||
|
该分支与 V3 `data_only` 严格链路并列，不改变其门禁结论：
|
||||||
|
|
||||||
|
```text
|
||||||
|
data_only_6dof_accepted = false
|
||||||
|
```
|
||||||
|
|
||||||
|
工程分支固定使用真实水平静止场地得到的 R2G 旋转，默认参考值为
|
||||||
|
`RPY=[0.4543066, -0.0026392, 0.0122384] deg`，输出始终记录：
|
||||||
|
|
||||||
|
```text
|
||||||
|
rotation_source = R2G_gravity_level_prior
|
||||||
|
translation_conditional_on_rotation = true
|
||||||
|
```
|
||||||
|
|
||||||
|
## 条件模型
|
||||||
|
|
||||||
|
估计量是 `l_I = p_ANT1^I`。每个连续质量段使用 HI13 设备时间、raw gyro/acc 动态预积分，融合 RTK Fixed 的 GGA/BESTNAVA 位置、BESTNAVA Doppler velocity 和 Q4 GNHPR 的 ANT1→ANT2 基线。逐段 nuisance state 包含初始姿态、IMU 原点初始位置、速度、gyro bias 和 acc bias。HI13 absolute quaternion/RPY（尤其 absolute yaw）不进入机械外参因子；R2V 仅作诊断。
|
||||||
|
|
||||||
|
静止相关因子分为两类：
|
||||||
|
|
||||||
|
- `gravity_candidate`：连续约 1.5 s 的低角速度、gyro/acc 方差稳定且加速度模长接近重力；它不能等价为静止。
|
||||||
|
- `zupt_static`：在 gravity candidate 基础上，必须同时有至少约 1 s 的连续 BESTNAVA Doppler 速度接近 0。高速或匀速直线不会加入 ZUPT。
|
||||||
|
|
||||||
|
连续段复用 R0 质量断点：checksum 无效、定位非 Fixed、GNHPR 非 Q4、设备时间回跳、HPR 测量间隙、baseline jump、位置测量间隙或 IMU gap 都会切段，禁止跨断点预积分。 断后片段还必须至少包含 6 个求解节点且持续不少于 5 s；更短的局部欠约束微段不会跨断点拼接，而是直接不进入杆臂优化。
|
||||||
|
|
||||||
|
## 高程与残差口径
|
||||||
|
|
||||||
|
- GGA 只约束 XY；GGA MSL altitude 不定义也不参与 ENU-Z。
|
||||||
|
- ENU-Z reference 只来自有效 Fixed BESTNAVA altitude；BESTNAVA 才约束 XYZ。
|
||||||
|
- 输出分别为 `gga_xy_residual`、`bestnava_xyz_residual` 和 `doppler_velocity_residual`。未进入 Z factor 的 GGA 高度不进入垂向残差统计。
|
||||||
|
|
||||||
|
## 可观性与验收
|
||||||
|
|
||||||
|
杆臂可观性不再使用全状态最小奇异向量。状态分为杆臂 `l` 与 nuisance state，对优化 Hessian 计算 Schur complement：
|
||||||
|
|
||||||
|
```text
|
||||||
|
H_l_marg = H_ll - H_ln pinv(H_nn) H_nl
|
||||||
|
```
|
||||||
|
|
||||||
|
只对该 3×3 marginal lever information 做 SVD，并输出：
|
||||||
|
|
||||||
|
- `l_I_marginal_covariance_m2`、`l_I_std_m`
|
||||||
|
- `lever_information_singular_values`
|
||||||
|
- `lever_information_condition_number`
|
||||||
|
- `lever_precision_rank`
|
||||||
|
- `weakest_lever_direction_I`
|
||||||
|
|
||||||
|
门禁分为两层：
|
||||||
|
|
||||||
|
- `solver_health_gates` 只判断优化是否收敛、数值是否有限且残差未发散。
|
||||||
|
- `engineering_acceptance_gates` 使用更严格的杆臂 marginal std/information/rank/condition、BESTNAVA XYZ、GGA XY、Doppler velocity、LOO、bootstrap、旋转敏感性和可选手量一致性。
|
||||||
|
|
||||||
|
任一核心门禁失败时，`engineering_6dof_accepted=false`。没有完整执行 bootstrap 和 18 组旋转敏感性时，两项门禁明确为 false，不会把阶段性 base/LOO 结果误标为正式放行。
|
||||||
|
|
||||||
|
Bootstrap 按 session 有放回抽样，并保留重复 session 的 multiplicity；重复抽中的 session 会重复贡献其全部连续段。
|
||||||
|
|
||||||
|
## 机械杆臂软先验与双解输出
|
||||||
|
|
||||||
|
`--manual-l-i-m` 仅在同时提供 `--manual-l-i-std-m` 或完整
|
||||||
|
`--manual-l-i-covariance-m2` 时才成为白化高斯软因子。求解器始终先运行无先验
|
||||||
|
`free_solution`，再运行 `prior_constrained_solution`；输出还包含机械参考及两者到
|
||||||
|
参考的差值。当前活动 engineering 解为 prior-constrained 解（若启用），但 acceptance
|
||||||
|
额外要求 free-solve 也与机械参考一致，软先验不能掩盖不可观或数据矛盾。
|
||||||
|
|
||||||
|
示例（数值需使用实际机械测量的 1σ，不可把示例值当作默认）：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
--manual-l-i-m -0.45072 -0.25682 0.73208 `
|
||||||
|
--manual-l-i-std-m <sigma_x_m> <sigma_y_m> <sigma_z_m>
|
||||||
|
```
|
||||||
|
## 坐标转换
|
||||||
|
|
||||||
|
```text
|
||||||
|
T_RTK_IMU: p_RTK = R_RTK_IMU p_IMU - R_RTK_IMU l_I
|
||||||
|
T_IMU_RTK: p_IMU = R_RTK_IMU^T p_RTK + l_I
|
||||||
|
```
|
||||||
|
|
||||||
|
RTK 原点是 ANT1，因此 `T_IMU_RTK.translation == l_I`，两矩阵必须互逆。
|
||||||
|
|
||||||
|
## 分阶段运行
|
||||||
|
|
||||||
|
第一阶段默认只运行 base fit、marginal observability、residual audit 和 LOO：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools/run_rtk_imu_engineering_6dof.py `
|
||||||
|
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||||
|
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_base_loo.json `
|
||||||
|
--session <session-a> `
|
||||||
|
--session <session-b> `
|
||||||
|
--session <session-c>
|
||||||
|
```
|
||||||
|
|
||||||
|
只有 base/LOO 合理后，才显式启动全量验证：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python tools/run_rtk_imu_engineering_6dof.py `
|
||||||
|
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||||
|
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_full.json `
|
||||||
|
--session <session-a> `
|
||||||
|
--session <session-b> `
|
||||||
|
--session <session-c> `
|
||||||
|
--run-bootstrap --bootstrap-repetitions 40 --bootstrap-seed 0 `
|
||||||
|
--run-rotation-sensitivity
|
||||||
|
```
|
||||||
|
|
||||||
|
正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。
|
||||||
@@ -0,0 +1,175 @@
|
|||||||
|
# RTK–IMU 多源原始数据与旋转标定 V3
|
||||||
|
|
||||||
|
## 可行性结论
|
||||||
|
|
||||||
|
重构方向正确且必要。原始捕获中确实存在旧导出链路忽略的 BESTNAVA、
|
||||||
|
PVTSLNA 和 HI13 姿态/四元数。它们可以补充速度、质量、绝对姿态和静止
|
||||||
|
重力约束，但本批数据尚不能通过完整旋转门禁：
|
||||||
|
|
||||||
|
- BESTNAVA 约 0.8–1 Hz，严格高速样本很少。
|
||||||
|
- 当前动态主要是平面 yaw，R1b 的两个倾斜自由度仍弱可观。
|
||||||
|
- HI13 四元数与 GNSS 真北存在稳定但非机械的 yaw 偏差，可能来自磁偏角或
|
||||||
|
内部导航融合。
|
||||||
|
- 两个明确水平静止会话的 R2G 很稳定，但它是水平/重力先验约束解。
|
||||||
|
|
||||||
|
因此，本次重构显著提高了诊断能力，也防止错误外参进入平移；它没有把信息
|
||||||
|
不足包装成“标定成功”。
|
||||||
|
|
||||||
|
## 三批原始数据
|
||||||
|
|
||||||
|
扫描根目录：
|
||||||
|
|
||||||
|
- 0808：D:\data\raw_serial_capture_v2，仅选 20260808。
|
||||||
|
- 0815：D:\data\0815\raw_serial_capture_v2；目录实际包含 20260812–14。
|
||||||
|
- 0819：D:\data\0819\raw_serial_capture_v2。
|
||||||
|
|
||||||
|
按文件名捕获开始时间在 1.5 s 内一一配对：
|
||||||
|
|
||||||
|
| 批次 | G90/HI13 配对 | GNHPR | BESTNAVA | PVTSLNA | Q4 HPR | Fixed Doppler |
|
||||||
|
|---|---:|---:|---:|---:|---:|---:|
|
||||||
|
| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 |
|
||||||
|
| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 |
|
||||||
|
| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 |
|
||||||
|
|
||||||
|
共完成 55 组统一导出。0815 有一组 G90 没有匹配 HI13，manifest 将其列为
|
||||||
|
unmatched。发现一条 NMEA 时间为 0913A：保留原始行，设备时间留空并隔离，
|
||||||
|
没有回退到 host receive time。
|
||||||
|
|
||||||
|
速度不等于高速激励。增加速度 ≥1.5 m/s、水平速度标准差 ≤0.25 m/s 后，
|
||||||
|
三个批次分别只剩 44、8、0 条候选。
|
||||||
|
|
||||||
|
## 统一导出
|
||||||
|
|
||||||
|
输出根目录：D:\data\rtk_imu_unified_v3。
|
||||||
|
|
||||||
|
每组捕获：
|
||||||
|
|
||||||
|
- imu.npz：HI13 system_time、gyro、accel、RPY、WXYZ quaternion、磁场、
|
||||||
|
PPS stamp、温度、气压和 host receive UTC。
|
||||||
|
- rtk.csv：每条原生异步 GGA/GNHPR/BESTNAVA/PVTSLNA 单独成行，不再把
|
||||||
|
HPR/BEST/PVT 最近邻挂到 GGA。
|
||||||
|
- export_summary.json：原始捕获摘要、报文数量、四元数范数和设备到 host
|
||||||
|
的仿射时钟诊断。
|
||||||
|
- manifest.json：全部会话、批次、目录和 unmatched 记录。
|
||||||
|
|
||||||
|
时间规则：
|
||||||
|
|
||||||
|
1. HI13 system_time 是 IMU 主时间轴。
|
||||||
|
2. BESTNAVA/PVTSLNA 使用 GNSS week/TOW 和 leap seconds。
|
||||||
|
3. GGA/GNHPR 使用报文自己的 UTC time-of-day。
|
||||||
|
4. RTK GNSS 测量时刻通过 HI13 device→host 仿射模型映射进 HI13 设备时钟。
|
||||||
|
5. host receive time 只用于跨时钟桥接、延迟和抖动诊断，绝不替代采样时刻。
|
||||||
|
6. checksum 无效、时间畸形或非固定解记录保留，但不进入求解。
|
||||||
|
|
||||||
|
## 新旋转链路
|
||||||
|
|
||||||
|
### R1b：基线在 IMU 中的 2DoF 方向
|
||||||
|
|
||||||
|
对连续 Q4 GNHPR 基线和 HI13 陀螺相对旋转使用不变量：
|
||||||
|
|
||||||
|
angle(b_ENU(t0), b_ENU(t1))
|
||||||
|
= angle(b_IMU, DeltaR_IMU b_IMU)
|
||||||
|
|
||||||
|
估计 b_IMU 的两个倾斜自由度及逐会话陀螺零偏。安装信息只用于选择 +X
|
||||||
|
半球，并施加明确记录的弱 20° 先验。按会话等权，输出残差、协方差和信息
|
||||||
|
奇异值。
|
||||||
|
|
||||||
|
### R2V：基线 + Doppler velocity
|
||||||
|
|
||||||
|
筛选条件：
|
||||||
|
|
||||||
|
- BESTNAVA position 为 SOL_COMPUTED/NARROW_INT；
|
||||||
|
- velocity 为 SOL_COMPUTED/DOPPLER_VELOCITY；
|
||||||
|
- checksum 有效；
|
||||||
|
- 水平速度 ≥1.5 m/s、速度标准差 ≤0.25 m/s；
|
||||||
|
- |IMU gyro_z| ≤3°/s；
|
||||||
|
- 速度方向与横向基线接近正交；
|
||||||
|
- GNHPR Q4，并有时间邻近的合法 HI13 quaternion。
|
||||||
|
|
||||||
|
基线给车右，Doppler velocity 给车前，叉积给车上。HI13 quaternion 的
|
||||||
|
body/world 和 ENU/NED 候选全部评分，只保留残差最小者。该方法会把 HI13
|
||||||
|
导航 yaw 偏差带入候选，因此必须和 R2G 交叉验证。
|
||||||
|
|
||||||
|
### R2G：基线 + 水平静止重力
|
||||||
|
|
||||||
|
只允许调用方明确标记的水平静止会话。本次使用：
|
||||||
|
|
||||||
|
- 0819_20260819_072130（flat_static_hdg207）
|
||||||
|
- 0819_20260819_073045（flat_static_hdg082）
|
||||||
|
|
||||||
|
每 10 s 分块，要求 gyro norm ≤0.35°/s，且加速度模长距标准重力不超过
|
||||||
|
0.15 m/s²。R1b 提供车右，加速度中值提供车上，叉积得到车前。
|
||||||
|
|
||||||
|
### R3：稳定性与正式门禁
|
||||||
|
|
||||||
|
R2V/R2G 都输出：
|
||||||
|
|
||||||
|
- 样本和会话数量；
|
||||||
|
- SO(3) RMS/P95；
|
||||||
|
- 旋转向量样本标准差和均值协方差；
|
||||||
|
- leave-one-session；
|
||||||
|
- 10 样本或 10 s block-out。
|
||||||
|
|
||||||
|
只有 R1b 可观、R2V 和 R2G 各自稳定、二者差异 ≤2° 时，才将
|
||||||
|
translation_unlocked 设为 true。旧 GNHPR 三轴手眼不参与正式门禁。
|
||||||
|
|
||||||
|
## 首轮实测
|
||||||
|
|
||||||
|
R1b：
|
||||||
|
|
||||||
|
b_IMU = [0.999999976, -0.000213959, -0.000044368]
|
||||||
|
tilt_yz = [-0.0123, -0.0025] deg
|
||||||
|
pairs = 2091
|
||||||
|
RMS / P95 = 0.8086 / 1.7105 deg
|
||||||
|
information singular values = [1.176e-2, 8.476e-4]
|
||||||
|
std = [27.77, 7.47] deg
|
||||||
|
gate = failed
|
||||||
|
|
||||||
|
点估计接近 +X 是安装先验与名义轴一致的结果；巨大协方差说明不能宣称
|
||||||
|
数据独立估出了这两个小角。
|
||||||
|
|
||||||
|
R2V：
|
||||||
|
|
||||||
|
qualifying samples / sessions = 41 / 2
|
||||||
|
selected quaternion convention = HI13_q_body_to_ENU
|
||||||
|
RPY = [0.7532, 0.0708, -9.2944] deg
|
||||||
|
RMS / P95 = 1.8436 / 3.4975 deg
|
||||||
|
max leave-one-session = 3.4519 deg
|
||||||
|
gate = failed
|
||||||
|
|
||||||
|
R2G：
|
||||||
|
|
||||||
|
level-static blocks / sessions = 53 / 2
|
||||||
|
RPY = [0.4543, -0.0026, 0.0122] deg
|
||||||
|
RMS / P95 = 0.2542 / 0.2732 deg
|
||||||
|
max leave-one-session = 0.2688 deg
|
||||||
|
gate = passed (level/gravity-prior constrained)
|
||||||
|
|
||||||
|
R2V 与 R2G 的 SO(3) 差异为 9.3119°。R3 最终：
|
||||||
|
|
||||||
|
full_rotation_accepted = false
|
||||||
|
translation_unlocked = false
|
||||||
|
|
||||||
|
## 下一轮数据要求
|
||||||
|
|
||||||
|
1. BESTNAVA 改为至少 10 Hz，并确认 Doppler velocity 与 GNHPR 使用同一
|
||||||
|
GNSS week/TOW 输出周期。
|
||||||
|
2. 每个日期都录制多段 ≥3 m/s、持续 20–30 s 的正向直线；包含不同方位，
|
||||||
|
避免单一磁环境和单一会话支配。
|
||||||
|
3. 为 R1b 增加可控的 roll/pitch 激励；只有平面 yaw 无法稳定估出横向
|
||||||
|
基线的两个微小倾斜角。
|
||||||
|
4. 每个日期至少录两种车头方位的明确水平静止段，检验 HI13 重力和绝对
|
||||||
|
quaternion 的跨日期稳定性。
|
||||||
|
5. 若 HI13 quaternion 的 yaw 来自磁融合，应获取其导航坐标定义、磁偏角
|
||||||
|
设置和融合状态；否则 R2V 只使用其 roll/pitch，yaw 由 GNSS 基线和速度
|
||||||
|
决定。
|
||||||
|
6. 上述门禁通过前继续冻结杆臂和平移。
|
||||||
|
|
||||||
|
## 实现入口
|
||||||
|
|
||||||
|
- tools/rscap_v2/g90_rtk.py：GGA/GNHPR/BESTNAVA/PVTSLNA 和双 checksum。
|
||||||
|
- tools/rscap_v2/hi13_imu.py：HI91 system_time、惯性、姿态和四元数。
|
||||||
|
- tools/export_rtk_imu_unified.py：55 组配对、原生异步导出和断点续导。
|
||||||
|
- rtk_imu/rtk_imu_multisource.py：R1b/R2V/R2G/R3。
|
||||||
|
- tools/run_rtk_imu_multisource.py：多源旋转命令行入口。
|
||||||
|
- artifacts/rtk_imu_calibration_v3/multisource_result.json：首轮 R3 结果。
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
# 纯 LiDAR–IMU：标定流程与采集清单
|
||||||
|
|
||||||
|
**用途：** 现场怎么采合格数据（看完根目录 [README](../README.md) 后再看本文即可）。
|
||||||
|
算法命令与结果判读以 README 为准；改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||||
|
|
||||||
|
仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐采集与流程要点。
|
||||||
|
目标外参：`p_IMU = T_IMU_lidar · p_lidar`（`T_A_B` 表示把 B 系点变到 A 系）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 原则（先读）
|
||||||
|
|
||||||
|
1. **时间对齐优先**：时钟偏置未对准时，旋转与平移都不可信；正式数据用**设备时间戳**。
|
||||||
|
2. **先旋转，再平移**：旋转通常更稳；平面低速时竖直方向平移常常不可观。
|
||||||
|
3. **用连续运动**：停车多站适合 RTK 手眼，不适合作为纯 IMU 外参主流程。
|
||||||
|
4. **可观才交平移**：激励不够就只交旋转，不强交“假精确”六自由度。
|
||||||
|
5. **残差小 ≠ 标定对**：需叠点云、跨会话等独立验证。
|
||||||
|
|
||||||
|
相对运动模型：
|
||||||
|
|
||||||
|
```text
|
||||||
|
A ≈ IMU 预积分相对运动（关键帧区间）
|
||||||
|
B ≈ 雷达关键帧配准相对运动
|
||||||
|
R_A · R_X ≈ R_X · R_B → 先求旋转
|
||||||
|
完整模式且可观时再求平移 t
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 端到端流程（与现行代码一致）
|
||||||
|
|
||||||
|
```text
|
||||||
|
确认轴向 / 单位 / 时间语义
|
||||||
|
→ 现场采集（首尾静止 + 低速多转弯；建议 ≥2 段独立会话）
|
||||||
|
→ 导出标准中间格式（imu.csv + lidar 会话目录）
|
||||||
|
→ 质检（时间 / IMU）不通过则停
|
||||||
|
→ 粗估时间偏置 δt
|
||||||
|
→ 关键帧 → 配准得 B；完整 IMU 预积分得 A（旋转/速度/位移增量）
|
||||||
|
→ 加权旋转手眼得 R
|
||||||
|
→ 用 R 精修 δt，必要时重新组对再解 R（可交替数轮）
|
||||||
|
→ 联合精修 R 与常值陀螺零偏
|
||||||
|
→ full_se3 且可观：再估重力、关键帧速度、时变零偏与平移 t
|
||||||
|
→ 写出 T / δt / summary → 叠点云 / 跨会话验证后交付
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
| 步骤 | 现行模块 | 说明 |
|
||||||
|
| --- | -------------------------------------------------------------------- | --------------------------------------- |
|
||||||
|
| 质检 | `timestamp_audit` / `imu_audit` | 含静止段陀螺零偏初值 |
|
||||||
|
| 时间 | `time_offset` | 模长相关粗估 + 有符号三轴精修 |
|
||||||
|
| 运动对 | `keyframes` / `registration` / `imu_preintegration` / `motion_pairs` | 预积分始终算满；手眼先用旋转 |
|
||||||
|
| 旋转 | `rotation_handeye` | 加权手眼 |
|
||||||
|
| 精修 | `joint_optimizer` / `observability` | `rotation_only` 到旋转为止；`full_se3` 可观才碰平移 |
|
||||||
|
|
||||||
|
|
||||||
|
点云去畸变（`lidar_deskew`）可选；低速首轮可不依赖。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 3. 采集设计
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 3.1 单趟会话结构
|
||||||
|
|
||||||
|
```text
|
||||||
|
静止 20–30 s → 连续运动 3–8 min → 再静止 10–20 s
|
||||||
|
```
|
||||||
|
|
||||||
|
运动优先：低速「8」字 / 左右圆；再补加减速直线。
|
||||||
|
要可靠竖直方向外参时，另加缓坡（约 3°～8°，有效长度优先 ≥20～30 m），或改用外测垂直尺寸先验。
|
||||||
|
|
||||||
|
### 3.2 会话安排
|
||||||
|
|
||||||
|
|
||||||
|
| 会话 | 作用 |
|
||||||
|
| ----- | ---------------------------------- |
|
||||||
|
| A | 主标定 |
|
||||||
|
| B | 独立验证（**同一场地**换一条不完全相同的路线即可，不参与求外参） |
|
||||||
|
| C（可选） | 不同速度/路线，测稳定性 |
|
||||||
|
|
||||||
|
|
||||||
|
安装全程不得改动。多会话不要跨会话拼运动对。
|
||||||
|
|
||||||
|
### 3.3 录制字段（原始，勿先做姿态融合）
|
||||||
|
|
||||||
|
- IMU：设备时间、陀螺、加速度（建议同时留主机接收时间便于排查）
|
||||||
|
- LiDAR：每帧起止时间（最好有包级/逐点时间）、原始点云
|
||||||
|
|
||||||
|
中间格式见 `[V1_数据格式.md](V1_数据格式.md)`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 4. 现场 Checklist
|
||||||
|
|
||||||
|
**出发前**
|
||||||
|
|
||||||
|
- [ ] 安装固定；草图/卷尺粗测仅作参考，不当真值
|
||||||
|
- [ ] 单位与轴向确认；设备时间可写盘
|
||||||
|
- [ ] 结构化路线（墙/杆/路缘），避开空旷无特征区
|
||||||
|
- [ ] 存储与供电充足
|
||||||
|
|
||||||
|
**录制中**
|
||||||
|
|
||||||
|
- [ ] 首尾静止；中间有明显左右转与加减速
|
||||||
|
- [ ] 不改安装、不切换时间源
|
||||||
|
- [ ] 记录会话 ID、天气、异常（急刹、掉包等）
|
||||||
|
|
||||||
|
**当场快查**
|
||||||
|
|
||||||
|
- [ ] IMU 静止段平稳，转弯时角速度明显
|
||||||
|
- [ ] 点云帧数/点数正常，无明显大面积丢帧
|
||||||
|
- [ ] 雷达与 IMU 时间覆盖同一时段
|
||||||
|
|
||||||
|
**回实验室**
|
||||||
|
|
||||||
|
- [ ] 已导出中间格式并通过质检
|
||||||
|
- [ ] 本次目标：`rotation_only` 还是尝试 `full_se3`
|
||||||
|
- [ ] 若要竖直方向：确认真有俯仰/高度激励，否则降级交付
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 5. 精度预期
|
||||||
|
|
||||||
|
|
||||||
|
| 量 | 较现实范围 | 说明 |
|
||||||
|
| ---------- | --------- | ---------------- |
|
||||||
|
| 旋转 | 约 0.5°–2° | 最精确部分 |
|
||||||
|
| 水平平移 | 数厘米～十几厘米 | 强依赖配准、激励与同步 |
|
||||||
|
| 竖直 / 部分杠杆臂 | 往往更差甚至不可观 | 无高度激励时无法得出“精确 z” |
|
||||||
|
|
||||||
|
|
||||||
|
如果条件有限，优先交付：**可靠旋转 + δt + 可观的平移分量（若有）+ 明确限制说明**。
|
||||||
|
|
||||||
|
满足条件后的模式与成功标志见根目录 [`README.md`](../README.md) §0。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 6. 交付物建议
|
||||||
|
|
||||||
|
程序默认写出：`T_IMU_lidar.json`、`time_offset.json`、`summary.json`。
|
||||||
|
完整报告目录还可补充：可观性结论、跨会话对比、运动对质量表、限制说明（尤其竖直方向与时间同步方式）。
|
||||||
|
|
||||||
|
```text
|
||||||
|
静止 + 激励录制（多会话）
|
||||||
|
→ 质检 → 估 δt → 关键帧 A/B → 先解 R
|
||||||
|
→ 精修 δt 与 R → 可观则求 t → 验证后交付
|
||||||
|
```
|
||||||
|
|
||||||
+209
@@ -0,0 +1,209 @@
|
|||||||
|
# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题
|
||||||
|
|
||||||
|
> 数据：`D:\data\calibration_usable_20260808`
|
||||||
|
> 可用会话：`sessions_v1_host_aligned`（三优先窗）
|
||||||
|
> 当前结果目录：各窗 `out_fixed_dt0/`
|
||||||
|
> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json`
|
||||||
|
> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml`
|
||||||
|
> 约定外参：`p_IMU = T_IMU_lidar · p_lidar`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 一句话结论
|
||||||
|
|
||||||
|
**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。**
|
||||||
|
三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 当前可用结果（`out_fixed_dt0`）
|
||||||
|
|
||||||
|
约定：`p_IMU = T_IMU_lidar · p_lidar`；本轮交付 **仅旋转**，`t = [0,0,0]`，`time_offset_s = 0`。
|
||||||
|
|
||||||
|
| 窗 | 状态 | δt | roll/pitch/yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior |
|
||||||
|
|----|------|----|---------------------|--------------|----------|--------------|
|
||||||
|
| `priority_174005_174515` | `rotation_only_accepted` | 0 | −0.398 / +0.108 / **89.998** | 0.625 | 1374 | 0.413° |
|
||||||
|
| `priority_174905_175450` | 同上 | 0 | −0.316 / −0.352 / **90.002** | 0.293 | 1182 | 0.473° |
|
||||||
|
| `priority_175910_180530` | 同上 | 0 | −0.373 / −0.036 / **90.005** | 0.786 | 789 | 0.375° |
|
||||||
|
|
||||||
|
- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。
|
||||||
|
- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。
|
||||||
|
- 原始摘要：各窗 `out_fixed_dt0/summary.json`；总表 `calibration_manifest_fixed_dt0.json`。
|
||||||
|
|
||||||
|
### 2.1 窗1 `priority_174005_174515` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174005_174515\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.39806616272552936, 0.10842366761721789, 89.99788311264182]`
|
||||||
|
- quaternion_xyzw：`[-0.0031254048203223084, -0.0017872288323561246, 0.7070914597978358, 0.707112936622415]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[ 3.6946588133e-05, -0.9999758655693408, -0.0069474393698490 ],
|
||||||
|
[ 0.9999982088237712, 2.3798651350e-05, 0.0018925598731371 ],
|
||||||
|
[-0.0018923488575947, -0.0069474968493909, 0.9999740753156198 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.2 窗2 `priority_174905_175450` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_174905_175450\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.31554362742542746, -0.35231680831805484, 90.00193338170823]`
|
||||||
|
- quaternion_xyzw：`[0.00022698471903919405, -0.004121119694188399, 0.7071067019847445, 0.7070948146172911]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-3.3743238552e-05, -0.9999848355714863, -0.0055070399001942 ],
|
||||||
|
[ 0.9999810938467022, 1.2097239027e-07, -0.0061491421465438 ],
|
||||||
|
[ 0.0061490495645171, -0.0055071432752239, 0.9999659297008071 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2.3 窗3 `priority_175910_180530` — `R_IMU_lidar`
|
||||||
|
|
||||||
|
- 路径：`...\priority_175910_180530\out_fixed_dt0\summary.json`
|
||||||
|
- rpy_deg_xyz：`[-0.37335575043737196, -0.03585856981709423, 90.00538974486676]`
|
||||||
|
- quaternion_xyzw：`[-0.002082462199099642, -0.002525219418619018, 0.7071355299053291, 0.7070704554452735]`
|
||||||
|
|
||||||
|
```text
|
||||||
|
R =
|
||||||
|
[[-9.4068775206e-05, -0.9999787650354249, -0.0065161821101807 ],
|
||||||
|
[ 0.9999997997313592, -8.9988606603e-05, -0.0006264497522950 ],
|
||||||
|
[ 0.0006258500675082, -0.0065162397345547, 0.9999785732361544 ]]
|
||||||
|
t = [0, 0, 0]
|
||||||
|
```
|
||||||
|
|
||||||
|
### 相对历史失败轮次
|
||||||
|
|
||||||
|
| 轮次 | 问题 | 结果 |
|
||||||
|
|------|------|------|
|
||||||
|
| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° |
|
||||||
|
| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） |
|
||||||
|
| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 已澄清并写入配置的坐标系 / 先验
|
||||||
|
|
||||||
|
### 3.1 车体与传感器
|
||||||
|
|
||||||
|
- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。
|
||||||
|
- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。
|
||||||
|
- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。
|
||||||
|
- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。
|
||||||
|
|
||||||
|
### 3.2 安装量（`translation_m`）
|
||||||
|
|
||||||
|
| 传感器 | X / Y（后轮轴中心） | Z（离地） |
|
||||||
|
|--------|---------------------|-----------|
|
||||||
|
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
|
||||||
|
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
|
||||||
|
|
||||||
|
- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
|
||||||
|
- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。
|
||||||
|
|
||||||
|
### 3.3 导出外参先验
|
||||||
|
|
||||||
|
- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。
|
||||||
|
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
|
||||||
|
- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 现存问题清单
|
||||||
|
|
||||||
|
### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移）
|
||||||
|
|
||||||
|
- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。
|
||||||
|
- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。
|
||||||
|
- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。
|
||||||
|
- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。
|
||||||
|
- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。
|
||||||
|
|
||||||
|
### P2. 平面运动导致竖直平移弱可观
|
||||||
|
|
||||||
|
- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。
|
||||||
|
- 流水线门控已给出 `translation_accepted=false`。
|
||||||
|
- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。
|
||||||
|
|
||||||
|
### P3. 时间偏移若再自由估计会被带偏（已规避，需保持）
|
||||||
|
|
||||||
|
- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。
|
||||||
|
- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。
|
||||||
|
- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。
|
||||||
|
|
||||||
|
### P4. 单窗低残差 ≠ 外参正确（历史教训）
|
||||||
|
|
||||||
|
- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。
|
||||||
|
- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。
|
||||||
|
- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。
|
||||||
|
|
||||||
|
### P5. 旋转软先验尚未做无先验对照
|
||||||
|
|
||||||
|
- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。
|
||||||
|
- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。
|
||||||
|
|
||||||
|
### P6. 文档与操作约定未完全同步（工程）
|
||||||
|
|
||||||
|
- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。
|
||||||
|
- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 不该做 / 可以做
|
||||||
|
|
||||||
|
| 动作 | 建议 |
|
||||||
|
|------|------|
|
||||||
|
| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） |
|
||||||
|
| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** |
|
||||||
|
| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 |
|
||||||
|
| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** |
|
||||||
|
| 无先验 / 大 σ 旋转对照 | **建议做** |
|
||||||
|
| 冻结交付 `R` + `δt=0` 说明 | **建议做** |
|
||||||
|
| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 建议下一步顺序
|
||||||
|
|
||||||
|
1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。
|
||||||
|
2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。
|
||||||
|
3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。
|
||||||
|
4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 常用路径与命令
|
||||||
|
|
||||||
|
```text
|
||||||
|
数据根：
|
||||||
|
D:\data\calibration_usable_20260808\sessions_v1_host_aligned\
|
||||||
|
|
||||||
|
结果：
|
||||||
|
...\priority_XXXX\out_fixed_dt0\summary.json
|
||||||
|
...\priority_XXXX\out_fixed_dt0\motion_pairs.json
|
||||||
|
...\calibration_manifest_fixed_dt0.json
|
||||||
|
```
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
# 可视化（rotation_only 默认模式4用 GICP 平移）
|
||||||
|
python tools\visualize_pair_3d.py `
|
||||||
|
--lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar `
|
||||||
|
--summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json `
|
||||||
|
--pair-index 0
|
||||||
|
|
||||||
|
# 若要看「坏 Δp」导致的错层效果：
|
||||||
|
# --mode4-translation imu
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. 问题优先级（跟踪用）
|
||||||
|
|
||||||
|
| ID | 严重度 | 状态 | 标题 |
|
||||||
|
|----|--------|------|------|
|
||||||
|
| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 |
|
||||||
|
| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 |
|
||||||
|
| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） |
|
||||||
|
| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 |
|
||||||
|
| P5 | 中 | 待做 | 无旋转先验对照 |
|
||||||
|
| P6 | 低 | 待做 | README/交付物同步 |
|
||||||
@@ -0,0 +1,270 @@
|
|||||||
|
# `imu_lidar` 改动记录
|
||||||
|
|
||||||
|
本文件专门记录 `imu_lidar` 目录内的实现改动。
|
||||||
|
每条包含：**时间戳**、**改动内容**（以「原本怎么做 → 改成怎么做」书写）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-11 10:55 (UTC+8)
|
||||||
|
|
||||||
|
### 运动对缓存：标定落盘，可视化直读
|
||||||
|
|
||||||
|
- **原本**：`visualize_pair_3d` 每次启动都重新关键帧+配准+预积分，等同半次标定。
|
||||||
|
- **改成**：
|
||||||
|
- 标定成功后写出 `motion_pairs.json`（`motion_pairs_io.py` / `finalize`）。
|
||||||
|
- 可视化优先读缓存并对点云懒加载；`--rebuild-pairs` 可回退旧路径。
|
||||||
|
- 旧结果可用 `tools/export_motion_pairs_for_viz.py` 只补导出运动对，无需重求解外参。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-11 08:55 (UTC+8)
|
||||||
|
|
||||||
|
### 主机桥接后冻结 δt + 旋转先验软约束
|
||||||
|
|
||||||
|
- **原本**：手眼后 signed δt 精修可在弱 MSE 下降下连走数步（最远约 0.5 s）；旋转手眼无 CAD 先验，平面运动下 yaw 易掉进低残差错解。
|
||||||
|
- **改成**：
|
||||||
|
- CLI：`--fixed-time-offset-s`、`--no-signed-time-refine`、`--max-signed-refine-shift-s`。
|
||||||
|
- signed refine：默认 `|Δδt|≤0.05 s`，且要求 MSE 至少降约 2%。
|
||||||
|
- `rotation_handeye` 读取配置 `rotation_prior` 作初值/软约束。
|
||||||
|
- 主机 UTC 桥接会话建议：`--fixed-time-offset-s 0 --no-signed-time-refine`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-09 14:30 (UTC+8)
|
||||||
|
|
||||||
|
### 导出：HI13 IMU + recovered dlog zip + 墙钟切窗
|
||||||
|
|
||||||
|
- **原本**：IMU 只解 N300 FDILink；dlog 只认标准 `*.dorec`；无法按图上时段切窗。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/rscap_v2/hi13_imu.py`（HI91：g→m/s²、°/s→rad/s、设备 ms）。
|
||||||
|
- `h32_dlog` 支持 recovered zip（`indices.log` + `data.bin`），ZIP_STORED 成员按文件绝对 offset 直读。
|
||||||
|
- `export_rscap_to_v1.py`：`--imu-kind hi13|n300|auto`、多段 `--imu-rscap`、`--host-start/end` 切窗。
|
||||||
|
- 辅助脚本 `tools/export_usable_20260808_windows.py` 导出优先运动段。
|
||||||
|
- **未推送**（按用户要求本地改完即可）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-05 09:00 (UTC+8)
|
||||||
|
|
||||||
|
### 导出：支持 H32 DLogCapture（MSOP+DIFOP）→ V1
|
||||||
|
|
||||||
|
- **原本**：导出只读 H32 MSOP V2 `.rscap`，无 DIFOP，垂直角用默认 −16°…+16°。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/h32_dlog/`（dobject 索引、MSOP/DIFOP payload V1、DIFOP 通道角）。
|
||||||
|
- `export_rscap_to_v1.py` 增加 `--lidar-dlog`（与 `--lidar-rscap` 二选一）；默认用 DIFOP 角做 XYZ。
|
||||||
|
- `h32_msop.iter_h32_frames_from_packets` 供 dlog/rscap 共用拼帧。
|
||||||
|
- 单测 `tests/test_h32_dlog_export.py`；文档改为推荐 dlog 导出命令。
|
||||||
|
- **标定核心**（`imu_lidar/` 读 V1）未改。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-03 17:30 (UTC+8)
|
||||||
|
|
||||||
|
### 文档：精简对外阅读路径
|
||||||
|
|
||||||
|
- **原本**：README 很长，多份文档职责不清，外人易觉复杂。
|
||||||
|
- **改成**：README 改为短入口 +「对外三份就够」；采集清单 / 方法说明 / 测试说明 / 职责说明文首标明用途；细节仍保留在原文件。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2026-08-03 11:40 (UTC+8)
|
||||||
|
|
||||||
|
### 新增：N300/H32 `.rscap` → V1 中间格式导出
|
||||||
|
|
||||||
|
- **原本**：标定只接受 CSV/NPZ；新车原始录制需手工转换，无仓库内导出器。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/rscap_v2/`（V2 读取、N300 IMU、H32 MSOP 拼帧）与 `tools/export_rscap_to_v1.py`。
|
||||||
|
- 导出写入设备时间轴的 `imu.csv` + `lidar/`；支持 `--frame-stride` / `--max-points-per-frame`。
|
||||||
|
- 单元测试 `tests/test_export_rscap_helpers.py`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-08-01 11:40 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档：现状一览补充「合格数据」定义
|
||||||
|
|
||||||
|
- **原本**：§0 只写「合格数据拿到后」怎么跑，未写清何为合格。
|
||||||
|
- **改成**：根 `[README.md](../README.md)` §0 增加「什么叫合格数据」表（时间戳 / 会话 / 场景 / 格式 / 反例）及拿到后的模式与预期。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-08-01 11:30 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档：现状一览 + 去掉「方案」二分表述
|
||||||
|
|
||||||
|
- **原本**：对外说明仍偶发「方案二」等旧称呼；根 README 缺少一眼可读的阶段 / 合成 vs 旧车 / 合格数据预期；烟测配置与对比脚本文件名带 `scheme2`。
|
||||||
|
- **改成**：
|
||||||
|
- 根 `[README.md](../README.md)` 增加 §0「现状一览」；明确仓库只有一条连续运动标定路径。
|
||||||
|
- `[tests/README.md](../tests/README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明同步边界与阶段。
|
||||||
|
- `config/s2_old_smoke.yaml`、`tools/compare_s2_runs.py` 替换旧 `*scheme2*` 命名。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 18:10 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 配准可视化工具 + tests 说明（含 S2 线下记录）
|
||||||
|
|
||||||
|
- **原本**：无类似 RTK 仓库的运动对叠点 3D 查看；`tests/` 未说明合成 pytest 与 S2 旧数据线下试验的区别与结果。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `tools/visualize_pair_3d.py` / `view_pair.ps1`（键 1–4：原始 / IMU(X=I) / 雷达 B / `X⁻¹AX`；可 `--save-png`）。
|
||||||
|
- 新增 `[tests/README.md](../tests/README.md)`：自动化用例表 + S2 主机时间数据做了什么、结果为何 `blocked`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 17:20 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档同步 + 合成数据一键复现
|
||||||
|
|
||||||
|
- **原本**：`docs/标定流程与采集清单.md` 仍偏旧版「待写代码 / 因子图设想」；根 README 缺少清晰的一键复现入口与输入输出总表。
|
||||||
|
- **改成**：
|
||||||
|
- 采集清单与现行流水线对齐（完整预积分、δt↔R 交替、可观时再估平移）。
|
||||||
|
- 新增 `tools/reproduce_synthetic.py` / `.ps1`、`tools/show_calibration_report.py`；合成生成写入 `meta.json`；根 README 增加「系统输入输出 + 一键复现」。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 16:30 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档：移除已删除的静站路径表述，对外 README 重写
|
||||||
|
|
||||||
|
- **原本**：根 README / `docs` / 包说明仍对照已删除的静站路径与内部阶段黑话；`pyproject` 仍声明已删除的 `static_station` 包。
|
||||||
|
- **改成**：
|
||||||
|
- 删除旧静站文档；采集清单定为 `[docs/标定流程与采集清单.md](../docs/标定流程与采集清单.md)`。
|
||||||
|
- 根 `[README.md](../README.md)`、`[docs/IMU-LiDAR标定.md](../docs/IMU-LiDAR标定.md)`、本目录说明改为对外可读，只保留连续运动标定路径。
|
||||||
|
- `pyproject.toml` 仅保留 `imu_lidar` / `tools`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 14:00 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### Phase-C：完整 IMU 预积分 + 重力/速度/动态零偏（full_se3）
|
||||||
|
|
||||||
|
- **原本**：
|
||||||
|
- 运动对仅陀螺旋转预积分（`ΔR/Σ/J_bg`）；`t_A` 为空。
|
||||||
|
- 联合精修只估常值陀螺零偏修正；SE(3) 平移用经典手眼式 `(R_A-I)t ≈ R_X t_B`，无重力/速度/`b_a`。
|
||||||
|
- **改成**：
|
||||||
|
- `imu_preintegration.preintegrate_imu`：中值法积分 `ΔR/Δv/Δp`，传播 15 维误差态后输出 9×9 `Σ`（含 bias RW 过程噪声）与 9×3 `J_bg/J_ba`；保留 `preintegrate_gyro`。
|
||||||
|
- `motion_pairs` 始终调用完整预积分，写入 `delta_v/delta_p/cov9/J_bg9/J_ba`，`t_A_m=Δp`。
|
||||||
|
- `joint_optimizer`：`rotation_only` 仍 Phase-A；`full_se3` 可观时 Phase-C 联合估 `R_X,t_X,g,v_k,b_g,k,b_a,k`（关键帧 RW 先验）。
|
||||||
|
- `pipeline` 用静止加速度推重力初值；`summary.joint` 增加 `gravity_m_s2` / `accel_bias_m_s2`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 11:20 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档维护约定 + README 与现行实现对齐
|
||||||
|
|
||||||
|
- **原本**：根 README 与已删除的静站目录说明仍按「双路径并行」表述；部分模块说明未写明有符号 δt；改代码时 README 更新不完整。
|
||||||
|
- **改成**：
|
||||||
|
- 对外说明统一为**唯一连续运动标定路径**；流水线描述对齐有符号 δt 与联合精修。
|
||||||
|
- 根 README 增加「文档维护」表：每次改代码必须同步涉及的 README / 本 CHANGELOG。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 09:40 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 流水线：手眼未过门时仍尝试有符号 δt 精修
|
||||||
|
|
||||||
|
- **原本**：`rotation_handeye.ok=false`（如 RMS>5°）时立即 `blocked` 返回，阶段 A 的有符号 δt 精修根本不会执行。
|
||||||
|
- **改成**：只要可用运动对数 ≥3，即使用当前候选 `R` 做最多 2 轮有符号 δt 精修并重建运动对；精修后再按手眼门控决定是否 `blocked`。保证阶段 A 在困难数据上也能完整参与。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-31 09:20 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 阶段 A：标准旋转预积分因子 + 精确时间边界 + 有符号 δt 精修
|
||||||
|
|
||||||
|
- **原本**：
|
||||||
|
- 预积分只输出 `ΔR` 与启发式标量 weight/`σ`，区间端点用邻近 IMU 样本，无 `Σ`、无 `J_bg`。
|
||||||
|
- δt 仅靠角速度模长互相关粗估；手眼得到 `R` 后不再回头精修时间。
|
||||||
|
- 联合精修对零偏多用重积分或 `Exp(-δbΔt)` 近似，残差未按协方差白化，也无 `δb` 先验。
|
||||||
|
- **改成**：
|
||||||
|
- `imu_preintegration.preintegrate_gyro`：区间端点 **线性插值** 到精确 `t0/t1`；离散中值更新同时传播 `cov(Σ)` 与 `J_bg`（`ΔR(b+δb)≈ΔR Exp(J_bg δb)`）；weight 由 `trace(Σ)` + 激励/时长构造。
|
||||||
|
- `motion_pairs` metadata 增加 `cov`、`J_bg`，modeling 标记为 `gyro_preintegration_factor_phase_a`。
|
||||||
|
- `time_offset.refine_time_offset_signed`：用当前 `R_IMU_lidar` 把 LiDAR 角速度变到 IMU 系，在粗 δt 邻域做 **三轴有符号 MSE 精修**；仅当 MSE 下降且 **模长相关不劣化** 时才接受，避免 ICP 噪声带偏；`pipeline` 在手眼后与构对交替最多 2 轮。
|
||||||
|
- `joint_optimizer`：残差按 `Σ` **信息白化**；零偏用 `J_bg` 一阶修正；增加弱 `δb` 先验。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 2026-07-30 17:50 (UTC+8)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 第 1 步：帧间 IMU 轻量加强（加权预积分手眼）
|
||||||
|
|
||||||
|
- **原本**：`motion_pairs` 用 `integrate_gyro_rotation` 直接得到 `R_A`，各运动对等权进入 `rotation_handeye`；手眼残差不区分长短间隔与激励强弱。
|
||||||
|
- **改成**：
|
||||||
|
- 新增 `imu_preintegration.py`：对 `[t_i, t_j]` 做中值陀螺预积分，估计 `σ` 与 **pair weight**（偏短间隔、有角速度、低不确定度）。
|
||||||
|
- `motion_pairs` 改为调用 `preintegrate_gyro`，在 `metadata` 写入 `weight/duration_s/mean_gyro_norm/preint_sigma_rad/t_*_imu_s`，并增加 A/B 转角粗一致性过滤。
|
||||||
|
- `rotation_handeye` 改为 **√weight 加权** 的 Tsai 初值与 Huber 非线性精修；报告仍给未加权 RMS/中位数便于解读。
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 第 2 步：预积分残差联合精修（外参 + 陀螺零偏）
|
||||||
|
|
||||||
|
- **原本**：`joint_optimizer` 在手眼 `R_X` 基础上，仅在可观时用离散手眼平移式尝试 SE(3)；旋转侧不再用 IMU 过程模型，也不联合估零偏。
|
||||||
|
- **改成**：
|
||||||
|
- `joint_optimizer.solve_joint_extrinsic` 增加预积分旋转残差：`log(ΔRᵀ · R_X R_B R_Xᵀ)`，按 weight 加权。
|
||||||
|
- 联合变量增加陀螺零偏修正 `δb`：有 `imu` 时按区间 **重预积分**；否则用一阶修正 `ΔR(b+δb)≈ΔR Exp(-δbΔt)`。
|
||||||
|
- `pipeline` 把 `imu`、静止零偏、`δt` 传入 joint；summary 增加 `gyro_bias_rad_s`。
|
||||||
|
- 平移仍受可观性门控；`rotation_only` 时不交付平移。
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
### 文档
|
||||||
|
|
||||||
|
- **原本**：`imu_lidar/README.md` 仅模块列表，无逐次改动史。
|
||||||
|
- **改成**：新增本文件 `CHANGELOG.md`；模块说明中补充 `imu_preintegration.py` 与建模步骤描述。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 模板（以后追加用）
|
||||||
|
|
||||||
|
```markdown
|
||||||
|
## YYYY-MM-DD HH:MM (UTC+8)
|
||||||
|
|
||||||
|
### 标题
|
||||||
|
|
||||||
|
- **原本**：...
|
||||||
|
- **改成**：...
|
||||||
|
```
|
||||||
|
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""LiDAR–IMU calibration package (V1 runnable pipeline)."""
|
||||||
|
|
||||||
|
from .contracts import CalibrationMode, CalibrationStatus, TransformConvention
|
||||||
|
|
||||||
|
__all__ = ["CalibrationMode", "CalibrationStatus", "TransformConvention"]
|
||||||
@@ -0,0 +1,345 @@
|
|||||||
|
"""Command-line entry point for LiDAR–IMU calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
from datetime import datetime
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from .contracts import CalibrationMode, CalibrationRequest, CalibrationStatus, SessionInput
|
||||||
|
from .phase_a_replay import run_phase_a_replay
|
||||||
|
from .pipeline import describe_pipeline, run_calibration
|
||||||
|
|
||||||
|
|
||||||
|
def _format_progress_value(value: Any) -> str:
|
||||||
|
if isinstance(value, float):
|
||||||
|
return f"{value:.3f}"
|
||||||
|
if isinstance(value, (list, tuple, set)):
|
||||||
|
return "[" + ",".join(str(item) for item in value) + "]"
|
||||||
|
return str(value)
|
||||||
|
|
||||||
|
|
||||||
|
def _print_progress(event: dict[str, Any]) -> None:
|
||||||
|
"""Print one compact, immediately flushed progress line."""
|
||||||
|
|
||||||
|
timestamp = datetime.now().strftime("%H:%M:%S")
|
||||||
|
stage_index = event.get("stage_index", "?")
|
||||||
|
stage_total = event.get("stage_total", "?")
|
||||||
|
stage_name = event.get("stage", "unknown")
|
||||||
|
message = event.get("event", "progress")
|
||||||
|
fields = " ".join(
|
||||||
|
f"{key}={_format_progress_value(value)}"
|
||||||
|
for key, value in event.items()
|
||||||
|
if key not in {"stage_index", "stage_total", "stage", "event"}
|
||||||
|
and value is not None
|
||||||
|
)
|
||||||
|
suffix = f" | {fields}" if fields else ""
|
||||||
|
print(
|
||||||
|
f"[{timestamp}] [stage {stage_index}/{stage_total} {stage_name}] {message}{suffix}",
|
||||||
|
flush=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _parse_session_imu_specs(
|
||||||
|
specs: list[str] | None,
|
||||||
|
) -> dict[str, Path]:
|
||||||
|
result: dict[str, Path] = {}
|
||||||
|
for spec in specs or []:
|
||||||
|
if "=" not in spec:
|
||||||
|
raise SystemExit(
|
||||||
|
"--session-imu must use SESSION_ID=PATH syntax"
|
||||||
|
)
|
||||||
|
session_id, raw_path = spec.split("=", 1)
|
||||||
|
session_id = session_id.strip()
|
||||||
|
if not session_id or not raw_path.strip():
|
||||||
|
raise SystemExit(
|
||||||
|
"--session-imu must use non-empty SESSION_ID=PATH"
|
||||||
|
)
|
||||||
|
if session_id in result:
|
||||||
|
raise SystemExit(
|
||||||
|
f"duplicate --session-imu for {session_id}"
|
||||||
|
)
|
||||||
|
result[session_id] = Path(raw_path.strip())
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def _print_phase_a_progress(
|
||||||
|
event: str,
|
||||||
|
fields: dict[str, Any],
|
||||||
|
) -> None:
|
||||||
|
_print_progress(
|
||||||
|
{
|
||||||
|
"stage_index": "A",
|
||||||
|
"stage_total": "A",
|
||||||
|
"stage": "phase_a_replay",
|
||||||
|
"event": event,
|
||||||
|
**fields,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def build_parser() -> argparse.ArgumentParser:
|
||||||
|
parser = argparse.ArgumentParser(description="LiDAR–IMU extrinsic calibration (V1)")
|
||||||
|
subcommands = parser.add_subparsers(dest="command", required=True)
|
||||||
|
|
||||||
|
plan = subcommands.add_parser("plan", help="显示标定阶段，不读取数据")
|
||||||
|
plan.add_argument("--vehicle-config", help="车辆配置路径（仅展示，plan 不读取）")
|
||||||
|
plan.add_argument(
|
||||||
|
"--mode",
|
||||||
|
choices=[mode.value for mode in CalibrationMode],
|
||||||
|
default=CalibrationMode.ROTATION_ONLY.value,
|
||||||
|
)
|
||||||
|
|
||||||
|
run = subcommands.add_parser(
|
||||||
|
"run",
|
||||||
|
help="执行 V1 标定流水线（可重复 --imu/--lidar/--session-id 做多会话联合）",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--session-id",
|
||||||
|
action="append",
|
||||||
|
default=None,
|
||||||
|
help="会话 ID（可重复；与 --imu/--lidar 一一对应）",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--imu",
|
||||||
|
action="append",
|
||||||
|
required=True,
|
||||||
|
help="IMU CSV/NPZ 路径（可重复）",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--lidar",
|
||||||
|
action="append",
|
||||||
|
required=True,
|
||||||
|
help="LiDAR 会话目录（可重复）",
|
||||||
|
)
|
||||||
|
run.add_argument("--vehicle-config", required=True, help="车辆配置 YAML")
|
||||||
|
run.add_argument("--output", required=True, help="输出目录")
|
||||||
|
run.add_argument(
|
||||||
|
"--mode",
|
||||||
|
choices=[mode.value for mode in CalibrationMode],
|
||||||
|
default=CalibrationMode.ROTATION_ONLY.value,
|
||||||
|
)
|
||||||
|
run.add_argument("--max-iterations", type=int, default=2)
|
||||||
|
run.add_argument("--time-offset-search-s", type=float, default=1.0)
|
||||||
|
run.add_argument(
|
||||||
|
"--fixed-time-offset-s",
|
||||||
|
type=float,
|
||||||
|
default=None,
|
||||||
|
help="Skip |ω| δt search and use this constant (use 0 after host-UTC bridge)",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--session-time-offset-s",
|
||||||
|
action="append",
|
||||||
|
type=float,
|
||||||
|
default=None,
|
||||||
|
help="Per-session fixed time offset; repeat once per --imu/--lidar input",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--no-signed-time-refine",
|
||||||
|
action="store_true",
|
||||||
|
help="Disable signed 3-axis δt refine after hand-eye (recommended for host-bridged data)",
|
||||||
|
)
|
||||||
|
run.add_argument(
|
||||||
|
"--max-signed-refine-shift-s",
|
||||||
|
type=float,
|
||||||
|
default=0.05,
|
||||||
|
help="Max |Δδt| accepted by signed refine from the coarse estimate",
|
||||||
|
)
|
||||||
|
run.add_argument("--min-pair-rotation-deg", type=float, default=3.0)
|
||||||
|
run.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
||||||
|
run.add_argument("--min-registration-fitness", type=float, default=0.5)
|
||||||
|
run.add_argument("--max-imu-gap-s", type=float, default=0.05)
|
||||||
|
run.add_argument("--max-lidar-gap-s", type=float, default=1.0)
|
||||||
|
|
||||||
|
replay = subcommands.add_parser(
|
||||||
|
"phase-a-replay",
|
||||||
|
help="Replay Phase-A from cached motion pairs without rerunning GICP",
|
||||||
|
)
|
||||||
|
replay.add_argument("--motion-pairs", type=Path, required=True)
|
||||||
|
replay.add_argument("--vehicle-config", type=Path, required=True)
|
||||||
|
replay.add_argument("--output", type=Path, required=True)
|
||||||
|
replay.add_argument(
|
||||||
|
"--session-imu",
|
||||||
|
action="append",
|
||||||
|
default=None,
|
||||||
|
metavar="SESSION_ID=PATH",
|
||||||
|
help="Raw IMU mapping used only when cache lacks J_bg/cov",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--exclude-session",
|
||||||
|
action="append",
|
||||||
|
default=None,
|
||||||
|
help="Session ID to exclude; may be repeated",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--strong-rotation-min-deg",
|
||||||
|
type=float,
|
||||||
|
default=1.0,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--decorrelation-block-s",
|
||||||
|
type=float,
|
||||||
|
default=3.0,
|
||||||
|
help="Per-session time-block length used to decorrelate factors",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--max-pairs-per-block",
|
||||||
|
type=int,
|
||||||
|
default=1,
|
||||||
|
help="Maximum factors kept in each decorrelation block",
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--bias-prior-sigma-rad-s",
|
||||||
|
type=float,
|
||||||
|
default=0.002,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--yaw-std-max-deg",
|
||||||
|
type=float,
|
||||||
|
default=0.5,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--loo-yaw-range-max-deg",
|
||||||
|
type=float,
|
||||||
|
default=1.0,
|
||||||
|
)
|
||||||
|
replay.add_argument(
|
||||||
|
"--data-prior-difference-max-deg",
|
||||||
|
type=float,
|
||||||
|
default=1.0,
|
||||||
|
)
|
||||||
|
replay.add_argument("--max-nfev", type=int, default=200)
|
||||||
|
|
||||||
|
return parser
|
||||||
|
|
||||||
|
|
||||||
|
def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]:
|
||||||
|
imus = [Path(p) for p in args.imu]
|
||||||
|
lidars = [Path(p) for p in args.lidar]
|
||||||
|
if len(imus) != len(lidars):
|
||||||
|
raise SystemExit(f"--imu count ({len(imus)}) must match --lidar count ({len(lidars)})")
|
||||||
|
if args.session_id is None:
|
||||||
|
session_ids = [f"session{i}" for i in range(len(imus))]
|
||||||
|
else:
|
||||||
|
session_ids = list(args.session_id)
|
||||||
|
if len(session_ids) != len(imus):
|
||||||
|
raise SystemExit(
|
||||||
|
f"--session-id count ({len(session_ids)}) must match --imu/--lidar ({len(imus)})"
|
||||||
|
)
|
||||||
|
if args.session_time_offset_s is None:
|
||||||
|
session_offsets: list[float | None] = [None] * len(imus)
|
||||||
|
else:
|
||||||
|
session_offsets = list(args.session_time_offset_s)
|
||||||
|
if len(session_offsets) != len(imus):
|
||||||
|
raise SystemExit(
|
||||||
|
f"--session-time-offset-s count ({len(session_offsets)}) must match "
|
||||||
|
f"--imu/--lidar ({len(imus)})"
|
||||||
|
)
|
||||||
|
return tuple(
|
||||||
|
SessionInput(
|
||||||
|
session_id=sid,
|
||||||
|
imu_source=imu,
|
||||||
|
lidar_source=lidar,
|
||||||
|
fixed_time_offset_s=offset,
|
||||||
|
)
|
||||||
|
for sid, imu, lidar, offset in zip(session_ids, imus, lidars, session_offsets)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def main(argv: list[str] | None = None) -> int:
|
||||||
|
parser = build_parser()
|
||||||
|
args = parser.parse_args(argv)
|
||||||
|
|
||||||
|
if args.command == "plan":
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=Path(args.vehicle_config) if args.vehicle_config else None,
|
||||||
|
requested_mode=CalibrationMode(args.mode),
|
||||||
|
)
|
||||||
|
print("LiDAR–IMU calibration stages:")
|
||||||
|
print(f"requested mode: {request.requested_mode.value}")
|
||||||
|
for index, stage in enumerate(describe_pipeline(request), start=1):
|
||||||
|
print(f"{index}. {stage.name}: {stage.responsibility}")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
if args.command == "phase-a-replay":
|
||||||
|
summary = run_phase_a_replay(
|
||||||
|
motion_pairs_path=args.motion_pairs,
|
||||||
|
vehicle_config_path=args.vehicle_config,
|
||||||
|
output_directory=args.output,
|
||||||
|
imu_paths_by_session=_parse_session_imu_specs(
|
||||||
|
args.session_imu
|
||||||
|
),
|
||||||
|
excluded_sessions=set(args.exclude_session or []),
|
||||||
|
strong_rotation_min_deg=args.strong_rotation_min_deg,
|
||||||
|
decorrelation_block_s=args.decorrelation_block_s,
|
||||||
|
max_pairs_per_block=args.max_pairs_per_block,
|
||||||
|
bias_prior_sigma_rad_s=args.bias_prior_sigma_rad_s,
|
||||||
|
yaw_std_max_deg=args.yaw_std_max_deg,
|
||||||
|
leave_one_out_yaw_range_max_deg=(
|
||||||
|
args.loo_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
data_prior_difference_max_deg=(
|
||||||
|
args.data_prior_difference_max_deg
|
||||||
|
),
|
||||||
|
max_nfev=args.max_nfev,
|
||||||
|
progress_callback=_print_phase_a_progress,
|
||||||
|
)
|
||||||
|
print(f"status: {summary['status']}")
|
||||||
|
print(f"acceptance_checks: {summary['acceptance_checks']}")
|
||||||
|
for name, variant in summary["variants"].items():
|
||||||
|
print(
|
||||||
|
f"{name}: rpy_deg_xyz={variant['rpy_deg_xyz']} "
|
||||||
|
f"RMS={variant['residual_rms_deg']:.6f} "
|
||||||
|
f"P95={variant['residual_p95_deg']:.6f}"
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
"A1 marginalized yaw_std_deg: "
|
||||||
|
f"{summary['marginal_observability_A1']['yaw_std_deg']}"
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
"leave_one_out_yaw_range_deg: "
|
||||||
|
f"{summary['leave_one_out_yaw_range_deg']}"
|
||||||
|
)
|
||||||
|
print(f"report directory: {args.output}")
|
||||||
|
return 0 if (summary["accepted"] or summary.get("partial_accepted")) else 2
|
||||||
|
|
||||||
|
if args.command == "run":
|
||||||
|
sessions = _build_sessions(args)
|
||||||
|
request = CalibrationRequest(
|
||||||
|
vehicle_config=Path(args.vehicle_config),
|
||||||
|
sessions=sessions,
|
||||||
|
requested_mode=CalibrationMode(args.mode),
|
||||||
|
output_directory=Path(args.output),
|
||||||
|
max_iterations=args.max_iterations,
|
||||||
|
min_pair_rotation_deg=args.min_pair_rotation_deg,
|
||||||
|
min_pair_translation_m=args.min_pair_translation_m,
|
||||||
|
min_registration_fitness=args.min_registration_fitness,
|
||||||
|
max_imu_gap_s=args.max_imu_gap_s,
|
||||||
|
max_lidar_gap_s=args.max_lidar_gap_s,
|
||||||
|
time_offset_search_s=args.time_offset_search_s,
|
||||||
|
fixed_time_offset_s=args.fixed_time_offset_s,
|
||||||
|
enable_signed_time_refine=not args.no_signed_time_refine,
|
||||||
|
max_signed_refine_shift_s=args.max_signed_refine_shift_s,
|
||||||
|
)
|
||||||
|
result = run_calibration(request, progress_callback=_print_progress)
|
||||||
|
print(f"status: {result.status.value}")
|
||||||
|
print(f"message: {result.message}")
|
||||||
|
if result.time_offset_s is not None:
|
||||||
|
print(f"time_offset_s (first session; t_imu = t_lidar + dt): {result.time_offset_s:.6f}")
|
||||||
|
joint = (result.details or {}).get("joint") or {}
|
||||||
|
if joint:
|
||||||
|
print(f"merged_pair_count: {joint.get('merged_pair_count')}")
|
||||||
|
print(f"pair_counts_per_session: {joint.get('pair_counts_per_session')}")
|
||||||
|
if result.T_IMU_lidar is not None:
|
||||||
|
print("T_IMU_lidar:")
|
||||||
|
print(result.T_IMU_lidar)
|
||||||
|
print(f"report directory: {args.output}")
|
||||||
|
return 0 if result.status != CalibrationStatus.BLOCKED else 2
|
||||||
|
|
||||||
|
parser.error(f"unknown command {args.command}")
|
||||||
|
return 2
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,130 @@
|
|||||||
|
"""Shared contracts for the LiDAR–IMU calibration pipeline."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from enum import Enum
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
class TransformConvention(str, Enum):
|
||||||
|
"""The only transform convention used by this project."""
|
||||||
|
|
||||||
|
T_A_B = "T_A_B maps points from frame B into frame A"
|
||||||
|
|
||||||
|
|
||||||
|
class CalibrationMode(str, Enum):
|
||||||
|
ROTATION_ONLY = "rotation_only"
|
||||||
|
FULL_SE3 = "full_se3"
|
||||||
|
|
||||||
|
|
||||||
|
class CalibrationStatus(str, Enum):
|
||||||
|
NOT_RUN = "not_run"
|
||||||
|
BLOCKED = "blocked"
|
||||||
|
ROTATION_ONLY_ACCEPTED = "rotation_only_accepted"
|
||||||
|
ROTATION_ONLY_PRIOR_CONSTRAINED = "rotation_only_prior_constrained"
|
||||||
|
FULL_SE3_ACCEPTED = "full_se3_accepted"
|
||||||
|
FULL_SE3_REJECTED = "full_se3_rejected_due_to_observability"
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class SessionInput:
|
||||||
|
"""Input paths for one independently recorded session."""
|
||||||
|
|
||||||
|
session_id: str
|
||||||
|
imu_source: Path
|
||||||
|
lidar_source: Path
|
||||||
|
board_configuration_id: str | None = None
|
||||||
|
# Optional session-local override. The request-level value remains a
|
||||||
|
# backward-compatible fallback for batches whose timelines are all aligned.
|
||||||
|
fixed_time_offset_s: float | None = None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CalibrationRequest:
|
||||||
|
"""Top-level calibration request."""
|
||||||
|
|
||||||
|
vehicle_config: Path | None
|
||||||
|
sessions: tuple[SessionInput, ...] = ()
|
||||||
|
requested_mode: CalibrationMode = CalibrationMode.ROTATION_ONLY
|
||||||
|
output_directory: Path | None = None
|
||||||
|
max_iterations: int = 2
|
||||||
|
min_pair_rotation_deg: float = 3.0
|
||||||
|
min_pair_translation_m: float = 0.3
|
||||||
|
min_registration_fitness: float = 0.5
|
||||||
|
max_imu_gap_s: float = 0.05
|
||||||
|
max_lidar_gap_s: float = 1.0
|
||||||
|
time_offset_search_s: float = 1.0
|
||||||
|
# If set, skip |ω| search and use this constant (host-UTC-bridged sessions: 0).
|
||||||
|
fixed_time_offset_s: float | None = None
|
||||||
|
# Signed 3-axis refine after hand-eye; disable for already-bridged timelines.
|
||||||
|
enable_signed_time_refine: bool = True
|
||||||
|
# Reject signed refine steps that walk farther than this from the coarse δt.
|
||||||
|
max_signed_refine_shift_s: float = 0.05
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class CalibrationResult:
|
||||||
|
"""Result envelope written by finalize after pipeline gates."""
|
||||||
|
|
||||||
|
status: CalibrationStatus = CalibrationStatus.NOT_RUN
|
||||||
|
message: str = "Calibration has not been executed."
|
||||||
|
details: dict[str, Any] = field(default_factory=dict)
|
||||||
|
T_IMU_lidar: np.ndarray | None = None
|
||||||
|
time_offset_s: float | None = None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuSeries:
|
||||||
|
"""Normalized IMU samples.
|
||||||
|
|
||||||
|
``t_s`` is the native IMU clock in seconds (need not match LiDAR epoch).
|
||||||
|
Gyro must be rad/s; accelerometer must be m/s^2.
|
||||||
|
"""
|
||||||
|
|
||||||
|
t_s: np.ndarray
|
||||||
|
gyro_rad_s: np.ndarray
|
||||||
|
acc_m_s2: np.ndarray
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
object.__setattr__(self, "t_s", np.asarray(self.t_s, dtype=float).reshape(-1))
|
||||||
|
object.__setattr__(self, "gyro_rad_s", np.asarray(self.gyro_rad_s, dtype=float).reshape(-1, 3))
|
||||||
|
object.__setattr__(self, "acc_m_s2", np.asarray(self.acc_m_s2, dtype=float).reshape(-1, 3))
|
||||||
|
n = self.t_s.size
|
||||||
|
if self.gyro_rad_s.shape != (n, 3) or self.acc_m_s2.shape != (n, 3):
|
||||||
|
raise ValueError("IMU arrays must share the same length and have shape (N, 3)")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class LidarFrame:
|
||||||
|
"""One LiDAR sweep in Cartesian sensor coordinates."""
|
||||||
|
|
||||||
|
frame_id: str
|
||||||
|
t_start_s: float
|
||||||
|
t_end_s: float
|
||||||
|
points_xyz: np.ndarray
|
||||||
|
path: Path | None = None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def t_mid_s(self) -> float:
|
||||||
|
return 0.5 * (self.t_start_s + self.t_end_s)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MotionPair:
|
||||||
|
"""One relative-motion observation between keyframes i and j."""
|
||||||
|
|
||||||
|
session_id: str
|
||||||
|
i: int
|
||||||
|
j: int
|
||||||
|
t_i_s: float
|
||||||
|
t_j_s: float
|
||||||
|
R_A: np.ndarray
|
||||||
|
R_B: np.ndarray
|
||||||
|
t_A_m: np.ndarray | None = None
|
||||||
|
t_B_m: np.ndarray | None = None
|
||||||
|
fitness: float = 0.0
|
||||||
|
metadata: dict[str, Any] = field(default_factory=dict)
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
"""Package calibration outputs as JSON-friendly artifacts."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import CalibrationResult, CalibrationStatus
|
||||||
|
from .geometry import rotation_matrix_to_quaternion_xyzw, rpy_deg_xyz
|
||||||
|
|
||||||
|
|
||||||
|
def _to_serializable(value: Any) -> Any:
|
||||||
|
if isinstance(value, np.ndarray):
|
||||||
|
return value.tolist()
|
||||||
|
if isinstance(value, (np.floating, np.integer, np.bool_)):
|
||||||
|
return value.item()
|
||||||
|
if isinstance(value, Path):
|
||||||
|
return str(value)
|
||||||
|
if isinstance(value, dict):
|
||||||
|
return {str(k): _to_serializable(v) for k, v in value.items()}
|
||||||
|
if isinstance(value, (list, tuple)):
|
||||||
|
return [_to_serializable(v) for v in value]
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def finalize_result(
|
||||||
|
*,
|
||||||
|
status: CalibrationStatus,
|
||||||
|
message: str,
|
||||||
|
details: dict[str, Any],
|
||||||
|
T_IMU_lidar: np.ndarray | None = None,
|
||||||
|
time_offset_s: float | None = None,
|
||||||
|
output_directory: Path | None = None,
|
||||||
|
motion_pairs_payload: dict[str, Any] | None = None,
|
||||||
|
) -> CalibrationResult:
|
||||||
|
"""Build the result envelope and optionally write report files."""
|
||||||
|
|
||||||
|
result = CalibrationResult(
|
||||||
|
status=status,
|
||||||
|
message=message,
|
||||||
|
details=_to_serializable(details),
|
||||||
|
T_IMU_lidar=None if T_IMU_lidar is None else np.asarray(T_IMU_lidar, dtype=float),
|
||||||
|
time_offset_s=time_offset_s,
|
||||||
|
)
|
||||||
|
|
||||||
|
if output_directory is not None:
|
||||||
|
output_directory = Path(output_directory)
|
||||||
|
output_directory.mkdir(parents=True, exist_ok=True)
|
||||||
|
summary = {
|
||||||
|
"status": status.value,
|
||||||
|
"message": message,
|
||||||
|
"time_offset_s": time_offset_s,
|
||||||
|
"details": result.details,
|
||||||
|
}
|
||||||
|
if result.T_IMU_lidar is not None:
|
||||||
|
t = result.T_IMU_lidar
|
||||||
|
summary["T_IMU_lidar"] = {
|
||||||
|
"matrix": t.tolist(),
|
||||||
|
"translation_m": t[:3, 3].tolist(),
|
||||||
|
"rotation_quaternion_xyzw": rotation_matrix_to_quaternion_xyzw(t[:3, :3]).tolist(),
|
||||||
|
"rpy_deg_xyz": rpy_deg_xyz(t[:3, :3]).tolist(),
|
||||||
|
"convention": "p_IMU = T_IMU_lidar * p_lidar",
|
||||||
|
}
|
||||||
|
(output_directory / "T_IMU_lidar.json").write_text(
|
||||||
|
json.dumps(summary["T_IMU_lidar"], indent=2),
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
if time_offset_s is not None:
|
||||||
|
(output_directory / "time_offset.json").write_text(
|
||||||
|
json.dumps({"delta_t_s": time_offset_s, "definition": "t_imu = t_lidar + delta_t"}, indent=2),
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
if motion_pairs_payload is not None:
|
||||||
|
from .motion_pairs_io import save_motion_pairs
|
||||||
|
|
||||||
|
save_motion_pairs(output_directory / "motion_pairs.json", motion_pairs_payload)
|
||||||
|
summary["motion_pairs_file"] = "motion_pairs.json"
|
||||||
|
(output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
|
||||||
|
return result
|
||||||
@@ -0,0 +1,74 @@
|
|||||||
|
"""Small WGS84 geodesy helpers used by the RTK--IMU calibration path."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
WGS84_A_M = 6378137.0
|
||||||
|
WGS84_F = 1.0 / 298.257223563
|
||||||
|
WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
|
||||||
|
|
||||||
|
|
||||||
|
def geodetic_to_ecef(
|
||||||
|
latitude_deg: np.ndarray,
|
||||||
|
longitude_deg: np.ndarray,
|
||||||
|
altitude_m: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Convert WGS84 latitude/longitude/ellipsoidal height to ECEF metres."""
|
||||||
|
|
||||||
|
latitude = np.deg2rad(np.asarray(latitude_deg, dtype=float))
|
||||||
|
longitude = np.deg2rad(np.asarray(longitude_deg, dtype=float))
|
||||||
|
altitude = np.asarray(altitude_m, dtype=float)
|
||||||
|
latitude, longitude, altitude = np.broadcast_arrays(latitude, longitude, altitude)
|
||||||
|
sin_lat = np.sin(latitude)
|
||||||
|
cos_lat = np.cos(latitude)
|
||||||
|
radius = WGS84_A_M / np.sqrt(1.0 - WGS84_E2 * sin_lat**2)
|
||||||
|
x = (radius + altitude) * cos_lat * np.cos(longitude)
|
||||||
|
y = (radius + altitude) * cos_lat * np.sin(longitude)
|
||||||
|
z = (radius * (1.0 - WGS84_E2) + altitude) * sin_lat
|
||||||
|
return np.stack([x, y, z], axis=-1)
|
||||||
|
|
||||||
|
|
||||||
|
def geodetic_to_enu(
|
||||||
|
latitude_deg: np.ndarray,
|
||||||
|
longitude_deg: np.ndarray,
|
||||||
|
altitude_m: np.ndarray,
|
||||||
|
*,
|
||||||
|
origin_latitude_deg: float | None = None,
|
||||||
|
origin_longitude_deg: float | None = None,
|
||||||
|
origin_altitude_m: float | None = None,
|
||||||
|
) -> tuple[np.ndarray, tuple[float, float, float]]:
|
||||||
|
"""Convert WGS84 samples to a local east/north/up frame.
|
||||||
|
|
||||||
|
When no origin is supplied, the first finite sample is used. The returned
|
||||||
|
origin tuple is ``(latitude_deg, longitude_deg, altitude_m)``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
lat = np.asarray(latitude_deg, dtype=float).reshape(-1)
|
||||||
|
lon = np.asarray(longitude_deg, dtype=float).reshape(-1)
|
||||||
|
alt = np.asarray(altitude_m, dtype=float).reshape(-1)
|
||||||
|
if not (lat.size == lon.size == alt.size):
|
||||||
|
raise ValueError("latitude, longitude and altitude must have equal length")
|
||||||
|
finite = np.isfinite(lat) & np.isfinite(lon) & np.isfinite(alt)
|
||||||
|
if not np.any(finite):
|
||||||
|
raise ValueError("no finite geodetic sample")
|
||||||
|
first = int(np.flatnonzero(finite)[0])
|
||||||
|
lat0 = float(lat[first] if origin_latitude_deg is None else origin_latitude_deg)
|
||||||
|
lon0 = float(lon[first] if origin_longitude_deg is None else origin_longitude_deg)
|
||||||
|
alt0 = float(alt[first] if origin_altitude_m is None else origin_altitude_m)
|
||||||
|
|
||||||
|
ecef = geodetic_to_ecef(lat, lon, alt)
|
||||||
|
ecef0 = geodetic_to_ecef(np.array(lat0), np.array(lon0), np.array(alt0)).reshape(3)
|
||||||
|
delta = ecef - ecef0
|
||||||
|
phi = np.deg2rad(lat0)
|
||||||
|
lam = np.deg2rad(lon0)
|
||||||
|
rotation = np.array(
|
||||||
|
[
|
||||||
|
[-np.sin(lam), np.cos(lam), 0.0],
|
||||||
|
[-np.sin(phi) * np.cos(lam), -np.sin(phi) * np.sin(lam), np.cos(phi)],
|
||||||
|
[np.cos(phi) * np.cos(lam), np.cos(phi) * np.sin(lam), np.sin(phi)],
|
||||||
|
],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
return delta @ rotation.T, (lat0, lon0, alt0)
|
||||||
@@ -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,74 @@
|
|||||||
|
"""IMU adapters for the V1 standard intermediate format.
|
||||||
|
|
||||||
|
Accepted inputs
|
||||||
|
---------------
|
||||||
|
1. CSV with header:
|
||||||
|
t,gx,gy,gz,ax,ay,az
|
||||||
|
- ``t`` in seconds on the IMU clock
|
||||||
|
- gyro in rad/s
|
||||||
|
- accel in m/s^2
|
||||||
|
|
||||||
|
2. NPZ with arrays:
|
||||||
|
t, gyro, acc
|
||||||
|
shapes: (N,), (N,3), (N,3)
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import csv
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries
|
||||||
|
|
||||||
|
|
||||||
|
def load_imu_samples(path: Path | str) -> ImuSeries:
|
||||||
|
"""Load normalized IMU samples from CSV or NPZ."""
|
||||||
|
|
||||||
|
source = Path(path)
|
||||||
|
if not source.exists():
|
||||||
|
raise FileNotFoundError(source)
|
||||||
|
if source.suffix.lower() == ".csv":
|
||||||
|
return _load_imu_csv(source)
|
||||||
|
if source.suffix.lower() == ".npz":
|
||||||
|
return _load_imu_npz(source)
|
||||||
|
raise ValueError(f"unsupported IMU format '{source.suffix}' (use .csv or .npz)")
|
||||||
|
|
||||||
|
|
||||||
|
def _load_imu_csv(path: Path) -> ImuSeries:
|
||||||
|
required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
|
||||||
|
with path.open("r", encoding="utf-8-sig", newline="") as handle:
|
||||||
|
header = next(csv.reader(handle), [])
|
||||||
|
names = set(header)
|
||||||
|
if not set(required_order).issubset(names):
|
||||||
|
raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
|
||||||
|
usecols = [header.index(name) for name in required_order]
|
||||||
|
data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
|
||||||
|
t = np.asarray(data[:, 0], dtype=float).reshape(-1)
|
||||||
|
gyro = np.asarray(data[:, 1:4], dtype=float)
|
||||||
|
acc = np.asarray(data[:, 4:7], dtype=float)
|
||||||
|
order = np.argsort(t)
|
||||||
|
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||||
|
|
||||||
|
|
||||||
|
def _load_imu_npz(path: Path) -> ImuSeries:
|
||||||
|
with np.load(path) as payload:
|
||||||
|
keys = set(payload.files)
|
||||||
|
if not {"t", "gyro", "acc"}.issubset(keys):
|
||||||
|
raise ValueError(f"IMU NPZ must contain t, gyro, acc; got {sorted(keys)}")
|
||||||
|
t = np.asarray(payload["t"], dtype=float).reshape(-1)
|
||||||
|
gyro = np.asarray(payload["gyro"], dtype=float).reshape(-1, 3)
|
||||||
|
acc = np.asarray(payload["acc"], dtype=float).reshape(-1, 3)
|
||||||
|
order = np.argsort(t)
|
||||||
|
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||||
|
|
||||||
|
|
||||||
|
def save_imu_csv(path: Path | str, imu: ImuSeries) -> None:
|
||||||
|
"""Write IMU samples to the standard CSV format."""
|
||||||
|
|
||||||
|
destination = Path(path)
|
||||||
|
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
array = np.column_stack([imu.t_s, imu.gyro_rad_s, imu.acc_m_s2])
|
||||||
|
header = "t,gx,gy,gz,ax,ay,az"
|
||||||
|
np.savetxt(destination, array, delimiter=",", header=header, comments="")
|
||||||
@@ -0,0 +1,460 @@
|
|||||||
|
"""Frame-to-frame IMU preintegration (Phase-A rotation + Phase-C full factor).
|
||||||
|
|
||||||
|
Phase-A: ``ΔR``, 3×3 ``Σ``, ``J_bg``.
|
||||||
|
Phase-C: ``ΔR/Δv/Δp``, 9×9 ``Σ`` (with bias RW process noise), ``J_bg``/``J_ba``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .geometry import orthonormalize_rotation, so3_exp, so3_log, skew
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class GyroPreintegration:
|
||||||
|
"""Rotation-only preintegration on ``[t0, t1]`` (IMU clock)."""
|
||||||
|
|
||||||
|
delta_R: np.ndarray
|
||||||
|
duration_s: float
|
||||||
|
mean_gyro_norm: float
|
||||||
|
sigma_rad: float
|
||||||
|
weight: float
|
||||||
|
bias_rad_s: np.ndarray
|
||||||
|
cov: np.ndarray
|
||||||
|
J_bg: np.ndarray
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ImuPreintegration:
|
||||||
|
"""Full IMU preintegration on ``[t0, t1]`` (IMU clock).
|
||||||
|
|
||||||
|
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``.
|
||||||
|
``delta_v`` / ``delta_p`` are body-frame increments (no gravity).
|
||||||
|
Error-state order in ``cov`` / Jacobians: ``[δθ, δv, δp]`` (9).
|
||||||
|
``J_bg`` / ``J_ba`` are 9×3: first-order correction w.r.t. constant bias deltas.
|
||||||
|
"""
|
||||||
|
|
||||||
|
delta_R: np.ndarray
|
||||||
|
delta_v: np.ndarray
|
||||||
|
delta_p: np.ndarray
|
||||||
|
duration_s: float
|
||||||
|
mean_gyro_norm: float
|
||||||
|
sigma_rad: float
|
||||||
|
weight: float
|
||||||
|
gyro_bias_rad_s: np.ndarray
|
||||||
|
acc_bias_m_s2: np.ndarray
|
||||||
|
cov: np.ndarray
|
||||||
|
J_bg: np.ndarray
|
||||||
|
J_ba: np.ndarray
|
||||||
|
|
||||||
|
|
||||||
|
def _right_jacobian(phi: np.ndarray) -> np.ndarray:
|
||||||
|
"""SO(3) right Jacobian ``Jr(φ)`` with ``Exp(φ+δ)≈Exp(φ)Exp(Jr δ)``."""
|
||||||
|
|
||||||
|
phi = np.asarray(phi, dtype=float).reshape(3)
|
||||||
|
angle = float(np.linalg.norm(phi))
|
||||||
|
if angle < 1e-8:
|
||||||
|
return np.eye(3) - 0.5 * skew(phi)
|
||||||
|
axis = phi / angle
|
||||||
|
s = skew(axis)
|
||||||
|
return (
|
||||||
|
np.eye(3)
|
||||||
|
- ((1.0 - np.cos(angle)) / angle) * s
|
||||||
|
+ ((angle - np.sin(angle)) / angle) * (s @ s)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _interp_vec(times_s: np.ndarray, values: np.ndarray, t: float) -> np.ndarray:
|
||||||
|
"""Linear interpolate a 3-vector series at an exact time."""
|
||||||
|
|
||||||
|
return np.array(
|
||||||
|
[float(np.interp(t, times_s, values[:, axis])) for axis in range(3)],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _interp_gyro(times_s: np.ndarray, gyro_rad_s: np.ndarray, t: float) -> np.ndarray:
|
||||||
|
"""Linear interpolate gyro at an exact time."""
|
||||||
|
|
||||||
|
return _interp_vec(times_s, gyro_rad_s, t)
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_weight(duration_s: float, mean_gyro_norm: float, cov_trace: float) -> float:
|
||||||
|
"""Larger weight for short, excited, low-covariance intervals."""
|
||||||
|
|
||||||
|
duration_term = 1.0 / max(duration_s, 0.05)
|
||||||
|
excite_term = min(max(mean_gyro_norm, 1e-3), 1.0)
|
||||||
|
avg_var = max(cov_trace / 3.0, 1e-8)
|
||||||
|
return float(duration_term * excite_term / avg_var)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegrate_gyro(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
bias_rad_s: np.ndarray | None = None,
|
||||||
|
*,
|
||||||
|
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||||
|
) -> GyroPreintegration:
|
||||||
|
"""Discrete mid-point gyro preintegration with exact endpoints.
|
||||||
|
|
||||||
|
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``
|
||||||
|
via right-invariant updates ``ΔR ← ΔR Exp((ω-b) dt)``.
|
||||||
|
|
||||||
|
Also returns:
|
||||||
|
- ``cov``: 3×3 covariance of the right tangent noise on ``ΔR``
|
||||||
|
- ``J_bg``: ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
|
||||||
|
empty = GyroPreintegration(
|
||||||
|
delta_R=np.eye(3),
|
||||||
|
duration_s=0.0,
|
||||||
|
mean_gyro_norm=0.0,
|
||||||
|
sigma_rad=1e3,
|
||||||
|
weight=1e-6,
|
||||||
|
bias_rad_s=bias.copy(),
|
||||||
|
cov=np.eye(3) * 1e6,
|
||||||
|
J_bg=np.zeros((3, 3)),
|
||||||
|
)
|
||||||
|
if times_s.size < 2 or duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||||
|
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
if duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
delta_r = np.eye(3)
|
||||||
|
j_bg = np.zeros((3, 3))
|
||||||
|
cov = np.zeros((3, 3))
|
||||||
|
sigma2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||||
|
gyro_norms: list[float] = []
|
||||||
|
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Exact local endpoint interpolation. ``seg0`` and ``seg1`` are inside
|
||||||
|
# this adjacent sample interval, so scanning the full series with
|
||||||
|
# np.interp here would turn pair construction into quadratic work.
|
||||||
|
sample_dt = max(t_b - t_a, 1e-12)
|
||||||
|
u0 = (seg0 - t_a) / sample_dt
|
||||||
|
u1 = (seg1 - t_a) / sample_dt
|
||||||
|
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
|
||||||
|
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
|
||||||
|
omega = 0.5 * (g_a + g_b) - bias
|
||||||
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|
||||||
|
theta = omega * dt
|
||||||
|
jr = _right_jacobian(theta)
|
||||||
|
a_mat = so3_exp(-theta)
|
||||||
|
j_bg = a_mat @ j_bg - jr * dt
|
||||||
|
cov = a_mat @ cov @ a_mat.T + jr @ (sigma2 * dt * np.eye(3)) @ jr.T
|
||||||
|
delta_r = delta_r @ so3_exp(theta)
|
||||||
|
|
||||||
|
delta_r = orthonormalize_rotation(delta_r)
|
||||||
|
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||||
|
cov = 0.5 * (cov + cov.T)
|
||||||
|
cov = cov + np.eye(3) * 1e-12
|
||||||
|
if mean_gyro_norm < 0.02:
|
||||||
|
cov = cov * 4.0
|
||||||
|
cov_trace = float(np.trace(cov))
|
||||||
|
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||||
|
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||||
|
return GyroPreintegration(
|
||||||
|
delta_R=delta_r,
|
||||||
|
duration_s=duration,
|
||||||
|
mean_gyro_norm=mean_gyro_norm,
|
||||||
|
sigma_rad=sigma_rad,
|
||||||
|
weight=weight,
|
||||||
|
bias_rad_s=bias.copy(),
|
||||||
|
cov=cov,
|
||||||
|
J_bg=np.asarray(j_bg, dtype=float),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegrate_imu(
|
||||||
|
times_s: np.ndarray,
|
||||||
|
gyro_rad_s: np.ndarray,
|
||||||
|
acc_m_s2: np.ndarray,
|
||||||
|
t0: float,
|
||||||
|
t1: float,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
acc_bias_m_s2: np.ndarray | None = None,
|
||||||
|
*,
|
||||||
|
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||||
|
sigma_a_m_s2_sqrt_hz: float = 2.0e-2,
|
||||||
|
sigma_bg_rw_rad_s_sqrt_hz: float = 1.0e-5,
|
||||||
|
sigma_ba_rw_m_s2_sqrt_hz: float = 1.0e-3,
|
||||||
|
) -> ImuPreintegration:
|
||||||
|
"""Mid-point IMU preintegration with exact endpoints and bias-RW noise.
|
||||||
|
|
||||||
|
Discrete updates (right-invariant)::
|
||||||
|
|
||||||
|
ΔR ← ΔR Exp((ω-bg) dt)
|
||||||
|
Δv ← Δv + ΔR (a-ba) dt
|
||||||
|
Δp ← Δp + Δv_old dt + 0.5 ΔR (a-ba) dt²
|
||||||
|
|
||||||
|
Propagates a 15-DoF error state ``[δθ, δv, δp, δbg, δba]`` then returns the
|
||||||
|
top-left 9×9 covariance (bias RW already folded in) and 9×3 Jacobians.
|
||||||
|
"""
|
||||||
|
|
||||||
|
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||||
|
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||||
|
acc_m_s2 = np.asarray(acc_m_s2, dtype=float).reshape(-1, 3)
|
||||||
|
bg = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
ba = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
empty = ImuPreintegration(
|
||||||
|
delta_R=np.eye(3),
|
||||||
|
delta_v=np.zeros(3),
|
||||||
|
delta_p=np.zeros(3),
|
||||||
|
duration_s=0.0,
|
||||||
|
mean_gyro_norm=0.0,
|
||||||
|
sigma_rad=1e3,
|
||||||
|
weight=1e-6,
|
||||||
|
gyro_bias_rad_s=bg.copy(),
|
||||||
|
acc_bias_m_s2=ba.copy(),
|
||||||
|
cov=np.eye(9) * 1e6,
|
||||||
|
J_bg=np.zeros((9, 3)),
|
||||||
|
J_ba=np.zeros((9, 3)),
|
||||||
|
)
|
||||||
|
if times_s.size < 2 or acc_m_s2.shape != gyro_rad_s.shape:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||||
|
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||||
|
duration = float(max(t1 - t0, 0.0))
|
||||||
|
if duration <= 0:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||||
|
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||||
|
left = max(left, 0)
|
||||||
|
right = min(right, times_s.size - 1)
|
||||||
|
if right <= left:
|
||||||
|
return empty
|
||||||
|
|
||||||
|
delta_r = np.eye(3)
|
||||||
|
delta_v = np.zeros(3)
|
||||||
|
delta_p = np.zeros(3)
|
||||||
|
# Jacobians of [δθ, δv, δp] w.r.t. constant bias (accumulated analytically).
|
||||||
|
j_bg = np.zeros((9, 3))
|
||||||
|
j_ba = np.zeros((9, 3))
|
||||||
|
# 15×15 covariance: [θ, v, p, bg, ba]
|
||||||
|
cov15 = np.zeros((15, 15))
|
||||||
|
sg2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||||
|
sa2 = float(sigma_a_m_s2_sqrt_hz) ** 2
|
||||||
|
sbg2 = float(sigma_bg_rw_rad_s_sqrt_hz) ** 2
|
||||||
|
sba2 = float(sigma_ba_rw_m_s2_sqrt_hz) ** 2
|
||||||
|
gyro_norms: list[float] = []
|
||||||
|
|
||||||
|
for index in range(left, right):
|
||||||
|
t_a = float(times_s[index])
|
||||||
|
t_b = float(times_s[index + 1])
|
||||||
|
if t_b <= t0 or t_a >= t1:
|
||||||
|
continue
|
||||||
|
seg0 = max(t_a, t0)
|
||||||
|
seg1 = min(t_b, t1)
|
||||||
|
dt = seg1 - seg0
|
||||||
|
if dt <= 0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
sample_dt = max(t_b - t_a, 1e-12)
|
||||||
|
u0 = (seg0 - t_a) / sample_dt
|
||||||
|
u1 = (seg1 - t_a) / sample_dt
|
||||||
|
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
|
||||||
|
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
|
||||||
|
a_a = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
|
||||||
|
a_b = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
|
||||||
|
omega = 0.5 * (g_a + g_b) - bg
|
||||||
|
acc = 0.5 * (a_a + a_b) - ba
|
||||||
|
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||||
|
|
||||||
|
theta = omega * dt
|
||||||
|
jr = _right_jacobian(theta)
|
||||||
|
r_dt = so3_exp(theta)
|
||||||
|
r_mid = delta_r # rotate body accel into i0 frame before update
|
||||||
|
|
||||||
|
# Bias Jacobians (Forster-style first-order recursion).
|
||||||
|
j_r_bg = j_bg[0:3]
|
||||||
|
j_v_bg = j_bg[3:6]
|
||||||
|
j_p_bg = j_bg[6:9]
|
||||||
|
j_r_ba = j_ba[0:3]
|
||||||
|
j_v_ba = j_ba[3:6]
|
||||||
|
j_p_ba = j_ba[6:9]
|
||||||
|
|
||||||
|
acc_skew = skew(acc)
|
||||||
|
j_p_bg_new = j_p_bg + j_v_bg * dt - 0.5 * r_mid @ acc_skew @ j_r_bg * (dt**2)
|
||||||
|
j_v_bg_new = j_v_bg - r_mid @ acc_skew @ j_r_bg * dt
|
||||||
|
j_r_bg_new = r_dt.T @ j_r_bg - jr * dt
|
||||||
|
|
||||||
|
j_p_ba_new = j_p_ba + j_v_ba * dt - 0.5 * r_mid * (dt**2)
|
||||||
|
j_v_ba_new = j_v_ba - r_mid * dt
|
||||||
|
j_r_ba_new = r_dt.T @ j_r_ba
|
||||||
|
|
||||||
|
j_bg = np.vstack([j_r_bg_new, j_v_bg_new, j_p_bg_new])
|
||||||
|
j_ba = np.vstack([j_r_ba_new, j_v_ba_new, j_p_ba_new])
|
||||||
|
|
||||||
|
# Nominal state update (use pre-update Δv in position).
|
||||||
|
delta_p = delta_p + delta_v * dt + 0.5 * r_mid @ acc * (dt**2)
|
||||||
|
delta_v = delta_v + r_mid @ acc * dt
|
||||||
|
delta_r = orthonormalize_rotation(delta_r @ r_dt)
|
||||||
|
|
||||||
|
# Linearized error-state transition (15×15).
|
||||||
|
f = np.eye(15)
|
||||||
|
a_mat = so3_exp(-theta)
|
||||||
|
f[0:3, 0:3] = a_mat
|
||||||
|
f[0:3, 9:12] = -jr * dt
|
||||||
|
f[3:6, 0:3] = -r_mid @ acc_skew * dt
|
||||||
|
f[3:6, 12:15] = -r_mid * dt
|
||||||
|
f[6:9, 0:3] = -0.5 * r_mid @ acc_skew * (dt**2)
|
||||||
|
f[6:9, 3:6] = np.eye(3) * dt
|
||||||
|
f[6:9, 12:15] = -0.5 * r_mid * (dt**2)
|
||||||
|
|
||||||
|
# Noise: continuous densities σ²; Var(∫n dt)=σ² dt. Columns: n_g, n_a, n_bg, n_ba.
|
||||||
|
g_mat = np.zeros((15, 12))
|
||||||
|
g_mat[0:3, 0:3] = jr
|
||||||
|
g_mat[3:6, 3:6] = r_mid
|
||||||
|
g_mat[6:9, 3:6] = 0.5 * r_mid * dt
|
||||||
|
g_mat[9:12, 6:9] = np.eye(3)
|
||||||
|
g_mat[12:15, 9:12] = np.eye(3)
|
||||||
|
q = np.zeros((12, 12))
|
||||||
|
q[0:3, 0:3] = sg2 * dt * np.eye(3)
|
||||||
|
q[3:6, 3:6] = sa2 * dt * np.eye(3)
|
||||||
|
q[6:9, 6:9] = sbg2 * dt * np.eye(3)
|
||||||
|
q[9:12, 9:12] = sba2 * dt * np.eye(3)
|
||||||
|
cov15 = f @ cov15 @ f.T + g_mat @ q @ g_mat.T
|
||||||
|
|
||||||
|
delta_r = orthonormalize_rotation(delta_r)
|
||||||
|
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||||
|
cov9 = cov15[0:9, 0:9]
|
||||||
|
cov9 = 0.5 * (cov9 + cov9.T) + np.eye(9) * 1e-12
|
||||||
|
if mean_gyro_norm < 0.02:
|
||||||
|
cov9 = cov9.copy()
|
||||||
|
cov9[0:3, 0:3] = cov9[0:3, 0:3] * 4.0
|
||||||
|
cov_trace = float(np.trace(cov9[0:3, 0:3]))
|
||||||
|
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||||
|
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||||
|
return ImuPreintegration(
|
||||||
|
delta_R=delta_r,
|
||||||
|
delta_v=np.asarray(delta_v, dtype=float),
|
||||||
|
delta_p=np.asarray(delta_p, dtype=float),
|
||||||
|
duration_s=duration,
|
||||||
|
mean_gyro_norm=mean_gyro_norm,
|
||||||
|
sigma_rad=sigma_rad,
|
||||||
|
weight=weight,
|
||||||
|
gyro_bias_rad_s=bg.copy(),
|
||||||
|
acc_bias_m_s2=ba.copy(),
|
||||||
|
cov=np.asarray(cov9, dtype=float),
|
||||||
|
J_bg=np.asarray(j_bg, dtype=float),
|
||||||
|
J_ba=np.asarray(j_ba, dtype=float),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def apply_bias_correction_imu(
|
||||||
|
preint: ImuPreintegration,
|
||||||
|
delta_gyro_bias: np.ndarray | None = None,
|
||||||
|
delta_acc_bias: np.ndarray | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""First-order bias correction of ``ΔR/Δv/Δp``.
|
||||||
|
|
||||||
|
Returns ``(delta_R, delta_v, delta_p)``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
dbg = np.zeros(3) if delta_gyro_bias is None else np.asarray(delta_gyro_bias, dtype=float).reshape(3)
|
||||||
|
dba = np.zeros(3) if delta_acc_bias is None else np.asarray(delta_acc_bias, dtype=float).reshape(3)
|
||||||
|
j_bg = np.asarray(preint.J_bg, dtype=float).reshape(9, 3)
|
||||||
|
j_ba = np.asarray(preint.J_ba, dtype=float).reshape(9, 3)
|
||||||
|
delta_r = orthonormalize_rotation(preint.delta_R @ so3_exp(j_bg[0:3] @ dbg))
|
||||||
|
delta_v = preint.delta_v + j_bg[3:6] @ dbg + j_ba[3:6] @ dba
|
||||||
|
delta_p = preint.delta_p + j_bg[6:9] @ dbg + j_ba[6:9] @ dba
|
||||||
|
return delta_r, np.asarray(delta_v, dtype=float), np.asarray(delta_p, dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def relative_rotation_from_lidar(R_X: np.ndarray, R_B: np.ndarray) -> np.ndarray:
|
||||||
|
"""Map LiDAR relative rotation into IMU frame: ``R_X R_B R_X^T``."""
|
||||||
|
|
||||||
|
r_x = orthonormalize_rotation(R_X)
|
||||||
|
r_b = orthonormalize_rotation(R_B)
|
||||||
|
return orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||||||
|
|
||||||
|
|
||||||
|
def preintegration_rotation_residual(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
R_X: np.ndarray,
|
||||||
|
R_B: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""``log( delta_R^T * R_X R_B R_X^T )`` in so(3)."""
|
||||||
|
|
||||||
|
predicted = relative_rotation_from_lidar(R_X, R_B)
|
||||||
|
return so3_log(delta_R.T @ predicted)
|
||||||
|
|
||||||
|
|
||||||
|
def apply_bias_jacobian_correction(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
J_bg: np.ndarray,
|
||||||
|
delta_bias_rad_s: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""First-order update ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``."""
|
||||||
|
|
||||||
|
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
j_bg = np.asarray(J_bg, dtype=float).reshape(3, 3)
|
||||||
|
return orthonormalize_rotation(delta_R @ so3_exp(j_bg @ db))
|
||||||
|
|
||||||
|
|
||||||
|
def apply_constant_bias_correction(
|
||||||
|
delta_R: np.ndarray,
|
||||||
|
duration_s: float,
|
||||||
|
delta_bias_rad_s: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Legacy first-order correction when ``J_bg`` is unavailable.
|
||||||
|
|
||||||
|
``ΔR(b+δb) ≈ ΔR Exp(-δb Δt)`` (identity Jacobian approximation).
|
||||||
|
"""
|
||||||
|
|
||||||
|
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
return orthonormalize_rotation(delta_R @ so3_exp(-db * float(duration_s)))
|
||||||
|
|
||||||
|
|
||||||
|
def residual_whiten_matrix(cov: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return ``W`` such that ``W @ e`` is approximately information-whitened.
|
||||||
|
|
||||||
|
Accepts square ``n×n`` covariances (3×3 rotation or 9×9 full IMU).
|
||||||
|
"""
|
||||||
|
|
||||||
|
matrix = np.asarray(cov, dtype=float)
|
||||||
|
if matrix.ndim != 2 or matrix.shape[0] != matrix.shape[1]:
|
||||||
|
raise ValueError("cov must be square")
|
||||||
|
n = matrix.shape[0]
|
||||||
|
matrix = 0.5 * (matrix + matrix.T) + np.eye(n) * 1e-10
|
||||||
|
try:
|
||||||
|
info = np.linalg.inv(matrix)
|
||||||
|
return np.linalg.cholesky(info).T
|
||||||
|
except np.linalg.LinAlgError:
|
||||||
|
scale = 1.0 / max(float(np.sqrt(np.trace(matrix) / n)), 1e-6)
|
||||||
|
return np.eye(n) * scale
|
||||||
@@ -0,0 +1,977 @@
|
|||||||
|
"""Joint extrinsic refinement: Phase-A rotation factors + Phase-C SE(3) IMU factors."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Callable, Mapping
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, MotionPair
|
||||||
|
from .geometry import make_transform, orthonormalize_rotation, so3_exp, so3_log
|
||||||
|
from .imu_preintegration import (
|
||||||
|
apply_bias_jacobian_correction,
|
||||||
|
apply_constant_bias_correction,
|
||||||
|
preintegrate_gyro,
|
||||||
|
preintegration_rotation_residual,
|
||||||
|
residual_whiten_matrix,
|
||||||
|
)
|
||||||
|
from .observability import ObservabilityReport, analyze_observability
|
||||||
|
from .phase_a import phase_a_comparison_to_dict, solve_phase_a_comparison
|
||||||
|
from .rotation_handeye import select_strong_rotation_pairs
|
||||||
|
|
||||||
|
G_NORM = 9.80665
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class PhaseASessionResult:
|
||||||
|
session_id: str
|
||||||
|
pair_count: int
|
||||||
|
gyro_bias0_rad_s: np.ndarray
|
||||||
|
gyro_bias_rad_s: np.ndarray
|
||||||
|
residual_rms_deg: float
|
||||||
|
residual_median_deg: float
|
||||||
|
residual_p95_deg: float
|
||||||
|
outlier_fraction_gt_5deg: float
|
||||||
|
accepted: bool
|
||||||
|
included_in_final: bool
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointExtrinsicResult:
|
||||||
|
T_IMU_lidar: np.ndarray
|
||||||
|
translation_accepted: bool
|
||||||
|
residual_rms_rot_deg: float
|
||||||
|
residual_rms_trans_m: float
|
||||||
|
observability: ObservabilityReport
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None
|
||||||
|
accel_bias_m_s2: np.ndarray | None = None
|
||||||
|
gravity_m_s2: np.ndarray | None = None
|
||||||
|
gyro_bias_rad_s_per_session: dict[str, np.ndarray] = field(default_factory=dict)
|
||||||
|
phase_a_sessions: tuple[PhaseASessionResult, ...] = ()
|
||||||
|
phase_a_accepted: bool = False
|
||||||
|
phase_a_comparison: dict[str, Any] = field(default_factory=dict)
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_weight(pair: MotionPair) -> float:
|
||||||
|
weight = float(pair.metadata.get("weight", 1.0))
|
||||||
|
if not np.isfinite(weight) or weight <= 0:
|
||||||
|
return 1.0
|
||||||
|
return weight
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_j_bg(pair: MotionPair) -> np.ndarray | None:
|
||||||
|
raw = pair.metadata.get("J_bg")
|
||||||
|
if raw is None:
|
||||||
|
return None
|
||||||
|
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_cov(pair: MotionPair) -> np.ndarray:
|
||||||
|
raw = pair.metadata.get("cov")
|
||||||
|
if raw is None:
|
||||||
|
sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2))
|
||||||
|
return np.eye(3) * max(sigma, 1e-4) ** 2
|
||||||
|
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||||||
|
|
||||||
|
|
||||||
|
def _corrected_delta_r(
|
||||||
|
pair: MotionPair,
|
||||||
|
delta_bias: np.ndarray,
|
||||||
|
*,
|
||||||
|
imu: ImuSeries | None,
|
||||||
|
bias0: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
j_bg = _pair_j_bg(pair)
|
||||||
|
if j_bg is not None:
|
||||||
|
return apply_bias_jacobian_correction(pair.R_A, j_bg, delta_bias)
|
||||||
|
if imu is not None and "t_i_imu_s" in pair.metadata and "t_j_imu_s" in pair.metadata:
|
||||||
|
preint = preintegrate_gyro(
|
||||||
|
imu.t_s,
|
||||||
|
imu.gyro_rad_s,
|
||||||
|
float(pair.metadata["t_i_imu_s"]),
|
||||||
|
float(pair.metadata["t_j_imu_s"]),
|
||||||
|
bias0 + delta_bias,
|
||||||
|
)
|
||||||
|
return preint.delta_R
|
||||||
|
duration = float(pair.metadata.get("duration_s", max(pair.t_j_s - pair.t_i_s, 1e-3)))
|
||||||
|
return apply_constant_bias_correction(pair.R_A, duration, delta_bias)
|
||||||
|
|
||||||
|
|
||||||
|
def _gravity_basis(g0: np.ndarray) -> np.ndarray:
|
||||||
|
"""Return 3×2 orthonormal basis spanning the plane orthogonal to ``g0``."""
|
||||||
|
|
||||||
|
g = np.asarray(g0, dtype=float).reshape(3)
|
||||||
|
n = np.linalg.norm(g)
|
||||||
|
if n < 1e-9:
|
||||||
|
g = np.array([0.0, 0.0, -G_NORM])
|
||||||
|
n = G_NORM
|
||||||
|
g = g / n
|
||||||
|
axis = np.array([1.0, 0.0, 0.0]) if abs(g[0]) < 0.9 else np.array([0.0, 1.0, 0.0])
|
||||||
|
e1 = np.cross(g, axis)
|
||||||
|
e1 /= max(np.linalg.norm(e1), 1e-12)
|
||||||
|
e2 = np.cross(g, e1)
|
||||||
|
return np.column_stack([e1, e2])
|
||||||
|
|
||||||
|
|
||||||
|
def _gravity_from_params(xy: np.ndarray, g0: np.ndarray, basis: np.ndarray) -> np.ndarray:
|
||||||
|
raw = np.asarray(g0, dtype=float).reshape(3) + basis @ np.asarray(xy, dtype=float).reshape(2)
|
||||||
|
n = float(np.linalg.norm(raw))
|
||||||
|
if n < 1e-9:
|
||||||
|
return np.asarray(g0, dtype=float).reshape(3)
|
||||||
|
return raw * (G_NORM / n)
|
||||||
|
|
||||||
|
|
||||||
|
def _lidar_to_imu_relative(r_x: np.ndarray, t_x: np.ndarray, r_b: np.ndarray, t_b: np.ndarray):
|
||||||
|
"""Map LiDAR relative pose to IMU: ``T_A = T_X T_B T_X^{-1}``."""
|
||||||
|
|
||||||
|
r_a = orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||||||
|
t_a = (np.eye(3) - r_a) @ t_x + r_x @ t_b
|
||||||
|
return r_a, t_a
|
||||||
|
|
||||||
|
|
||||||
|
def _corrected_preint_quantities(
|
||||||
|
pair: MotionPair,
|
||||||
|
bg_i: np.ndarray,
|
||||||
|
ba_i: np.ndarray,
|
||||||
|
bg0: np.ndarray,
|
||||||
|
ba0: np.ndarray,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""First-order correct ΔR/Δv/Δp for keyframe biases vs preintegration biases."""
|
||||||
|
|
||||||
|
dbg = np.asarray(bg_i, dtype=float).reshape(3) - np.asarray(bg0, dtype=float).reshape(3)
|
||||||
|
dba = np.asarray(ba_i, dtype=float).reshape(3) - np.asarray(ba0, dtype=float).reshape(3)
|
||||||
|
j_bg = pair.metadata.get("J_bg9")
|
||||||
|
j_ba = pair.metadata.get("J_ba")
|
||||||
|
delta_v0 = np.asarray(pair.metadata.get("delta_v", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||||||
|
delta_p0 = (
|
||||||
|
np.asarray(pair.t_A_m, dtype=float).reshape(3)
|
||||||
|
if pair.t_A_m is not None
|
||||||
|
else np.asarray(pair.metadata.get("delta_p", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||||||
|
)
|
||||||
|
if j_bg is None or j_ba is None:
|
||||||
|
delta_r = apply_bias_jacobian_correction(
|
||||||
|
pair.R_A,
|
||||||
|
_pair_j_bg(pair) if _pair_j_bg(pair) is not None else np.zeros((3, 3)),
|
||||||
|
dbg,
|
||||||
|
)
|
||||||
|
return delta_r, delta_v0, delta_p0
|
||||||
|
j_bg_m = np.asarray(j_bg, dtype=float).reshape(9, 3)
|
||||||
|
j_ba_m = np.asarray(j_ba, dtype=float).reshape(9, 3)
|
||||||
|
delta_r = orthonormalize_rotation(pair.R_A @ so3_exp(j_bg_m[0:3] @ dbg))
|
||||||
|
delta_v = delta_v0 + j_bg_m[3:6] @ dbg + j_ba_m[3:6] @ dba
|
||||||
|
delta_p = delta_p0 + j_bg_m[6:9] @ dbg + j_ba_m[6:9] @ dba
|
||||||
|
return delta_r, delta_v, delta_p
|
||||||
|
|
||||||
|
|
||||||
|
def _build_nav_rotations(
|
||||||
|
keyframe_ids: list[int],
|
||||||
|
id_to_idx: dict[int, int],
|
||||||
|
consecutive_pairs: dict[tuple[int, int], MotionPair],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
t_x: np.ndarray,
|
||||||
|
) -> list[np.ndarray]:
|
||||||
|
"""Chain IMU orientations; restart at session/gap boundaries (no cross-link)."""
|
||||||
|
|
||||||
|
del id_to_idx
|
||||||
|
rotations = [np.eye(3) for _ in keyframe_ids]
|
||||||
|
for k in range(len(keyframe_ids) - 1):
|
||||||
|
a = keyframe_ids[k]
|
||||||
|
b = keyframe_ids[k + 1]
|
||||||
|
pair = consecutive_pairs.get((a, b))
|
||||||
|
if pair is None:
|
||||||
|
# Missing link or new session: start a fresh nav chain.
|
||||||
|
rotations[k + 1] = np.eye(3)
|
||||||
|
continue
|
||||||
|
t_b = np.zeros(3) if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float)
|
||||||
|
r_meas, _ = _lidar_to_imu_relative(r_x, t_x, pair.R_B, t_b)
|
||||||
|
rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas)
|
||||||
|
return rotations
|
||||||
|
|
||||||
|
|
||||||
|
def _solve_phase_c_se3(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
gyro_bias_linearization: np.ndarray,
|
||||||
|
gyro_bias_init: np.ndarray,
|
||||||
|
gravity_init: np.ndarray,
|
||||||
|
sigma_bg_rw: float = 1.0e-5,
|
||||||
|
sigma_ba_rw: float = 1.0e-3,
|
||||||
|
t_init: np.ndarray | None = None,
|
||||||
|
t_prior: np.ndarray | None = None,
|
||||||
|
t_prior_sigma_m: np.ndarray | float | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]:
|
||||||
|
"""Keyframe IMU factor optimization for full SE(3)."""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
usable = [pair for pair in pairs if pair.t_B_m is not None and "delta_v" in pair.metadata]
|
||||||
|
if len(usable) < 3:
|
||||||
|
notes.append("phase-C skipped: need pairs with full preintegration metadata")
|
||||||
|
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
|
||||||
|
return r_x, t0, gravity_init, gyro_bias_init, np.zeros(3), 1e9, 1e9, notes
|
||||||
|
|
||||||
|
# Keyframes: group by session, sort each session by IMU time (no cross-session chain).
|
||||||
|
stamp: dict[int, float] = {}
|
||||||
|
kf_session: dict[int, str] = {}
|
||||||
|
for pair in usable:
|
||||||
|
stamp[pair.i] = float(pair.metadata.get("t_i_imu_s", pair.t_i_s))
|
||||||
|
stamp[pair.j] = float(pair.metadata.get("t_j_imu_s", pair.t_j_s))
|
||||||
|
kf_session[pair.i] = pair.session_id
|
||||||
|
kf_session[pair.j] = pair.session_id
|
||||||
|
session_ids = sorted(set(kf_session.values()))
|
||||||
|
keyframe_ids: list[int] = []
|
||||||
|
for sid in session_ids:
|
||||||
|
local = [kid for kid, sess in kf_session.items() if sess == sid]
|
||||||
|
local.sort(key=lambda kid: stamp[kid])
|
||||||
|
keyframe_ids.extend(local)
|
||||||
|
k_count = len(keyframe_ids)
|
||||||
|
id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)}
|
||||||
|
|
||||||
|
consecutive_pairs: dict[tuple[int, int], MotionPair] = {}
|
||||||
|
for pair in usable:
|
||||||
|
if kf_session.get(pair.i) != kf_session.get(pair.j):
|
||||||
|
continue
|
||||||
|
if id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
|
||||||
|
consecutive_pairs[(pair.i, pair.j)] = pair
|
||||||
|
notes.append(
|
||||||
|
f"phase-C multi-session graph: sessions={len(session_ids)}, "
|
||||||
|
f"keyframes={k_count}, consecutive_links={len(consecutive_pairs)}"
|
||||||
|
)
|
||||||
|
|
||||||
|
g0 = np.asarray(gravity_init, dtype=float).reshape(3)
|
||||||
|
if np.linalg.norm(g0) < 1e-6:
|
||||||
|
g0 = np.array([0.0, 0.0, -G_NORM])
|
||||||
|
g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9))
|
||||||
|
basis = _gravity_basis(g0)
|
||||||
|
ba0 = np.zeros(3)
|
||||||
|
bg0 = np.asarray(gyro_bias_linearization, dtype=float).reshape(3)
|
||||||
|
bg_init = np.asarray(gyro_bias_init, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
# State: dθ(3), t(3), g_xy(2), v(3K), bg(3K), ba(3K)
|
||||||
|
n_v = 3 * k_count
|
||||||
|
n_b = 3 * k_count
|
||||||
|
dim = 3 + 3 + 2 + n_v + n_b + n_b
|
||||||
|
x0 = np.zeros(dim)
|
||||||
|
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
|
||||||
|
x0[3:6] = t0
|
||||||
|
t_prior_vec = None if t_prior is None else np.asarray(t_prior, dtype=float).reshape(3)
|
||||||
|
if t_prior_sigma_m is None:
|
||||||
|
t_sigma = np.array([0.05, 0.05, 0.05], dtype=float)
|
||||||
|
else:
|
||||||
|
t_sigma = np.asarray(t_prior_sigma_m, dtype=float).reshape(-1)
|
||||||
|
if t_sigma.size == 1:
|
||||||
|
t_sigma = np.full(3, float(t_sigma[0]), dtype=float)
|
||||||
|
# velocities start at 0; biases at prior
|
||||||
|
for idx in range(k_count):
|
||||||
|
x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg_init
|
||||||
|
|
||||||
|
whitened = []
|
||||||
|
for pair in usable:
|
||||||
|
cov9 = pair.metadata.get("cov9")
|
||||||
|
if cov9 is None:
|
||||||
|
cov = _pair_cov(pair)
|
||||||
|
cov9_m = np.eye(9)
|
||||||
|
cov9_m[0:3, 0:3] = cov
|
||||||
|
cov9_m[3:6, 3:6] = np.eye(3) * 0.25
|
||||||
|
cov9_m[6:9, 6:9] = np.eye(3) * 1.0
|
||||||
|
else:
|
||||||
|
cov9_m = np.asarray(cov9, dtype=float).reshape(9, 9)
|
||||||
|
whitened.append(residual_whiten_matrix(cov9_m))
|
||||||
|
|
||||||
|
def unpack(vec: np.ndarray):
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[0:3]) @ r_x)
|
||||||
|
t_opt = vec[3:6]
|
||||||
|
g_opt = _gravity_from_params(vec[6:8], g0, basis)
|
||||||
|
base = 8
|
||||||
|
vels = vec[base : base + n_v].reshape(k_count, 3)
|
||||||
|
base += n_v
|
||||||
|
bgs = vec[base : base + n_b].reshape(k_count, 3)
|
||||||
|
base += n_b
|
||||||
|
bas = vec[base : base + n_b].reshape(k_count, 3)
|
||||||
|
return r_opt, t_opt, g_opt, vels, bgs, bas
|
||||||
|
|
||||||
|
def residuals(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt, t_opt, g_opt, vels, bgs, bas = unpack(vec)
|
||||||
|
nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
|
||||||
|
out: list[np.ndarray] = []
|
||||||
|
|
||||||
|
for pair, whiten in zip(usable, whitened):
|
||||||
|
i_idx = id_to_idx[pair.i]
|
||||||
|
j_idx = id_to_idx[pair.j]
|
||||||
|
dt = float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s))
|
||||||
|
dt = max(dt, 1e-3)
|
||||||
|
delta_r, delta_v, delta_p = _corrected_preint_quantities(
|
||||||
|
pair, bgs[i_idx], bas[i_idx], bg0, ba0
|
||||||
|
)
|
||||||
|
t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
|
||||||
|
r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
|
||||||
|
r_i = nav_r[i_idx]
|
||||||
|
v_i = vels[i_idx]
|
||||||
|
v_j = vels[j_idx]
|
||||||
|
|
||||||
|
err_r = so3_log(delta_r.T @ r_meas)
|
||||||
|
err_v = v_j - v_i - g_opt * dt - r_i @ delta_v
|
||||||
|
err_p = r_i @ (t_meas - delta_p) - v_i * dt - 0.5 * g_opt * (dt**2)
|
||||||
|
err = np.concatenate([err_r, err_v, err_p])
|
||||||
|
w = np.sqrt(_pair_weight(pair))
|
||||||
|
out.append(w * (whiten @ err))
|
||||||
|
|
||||||
|
# Bias random-walk between consecutive keyframes (same session only).
|
||||||
|
for k in range(k_count - 1):
|
||||||
|
a = keyframe_ids[k]
|
||||||
|
b = keyframe_ids[k + 1]
|
||||||
|
if kf_session.get(a) != kf_session.get(b):
|
||||||
|
continue
|
||||||
|
dt = max(stamp[b] - stamp[a], 1e-3)
|
||||||
|
scale_g = 1.0 / (max(sigma_bg_rw, 1e-8) * np.sqrt(dt))
|
||||||
|
scale_a = 1.0 / (max(sigma_ba_rw, 1e-8) * np.sqrt(dt))
|
||||||
|
out.append(scale_g * (bgs[k + 1] - bgs[k]))
|
||||||
|
out.append(scale_a * (bas[k + 1] - bas[k]))
|
||||||
|
|
||||||
|
# Weak priors: first keyframe of each session + CAD/installation translation.
|
||||||
|
for sid in session_ids:
|
||||||
|
first = next(kid for kid in keyframe_ids if kf_session[kid] == sid)
|
||||||
|
idx0 = id_to_idx[first]
|
||||||
|
out.append(50.0 * (bgs[idx0] - bg_init))
|
||||||
|
out.append(20.0 * bas[idx0])
|
||||||
|
if t_prior_vec is not None:
|
||||||
|
out.append((t_opt - t_prior_vec) / np.maximum(t_sigma, 1e-3))
|
||||||
|
else:
|
||||||
|
out.append(0.2 * t_opt) # soft |t|~0 prior when no CAD prior
|
||||||
|
return np.concatenate(out)
|
||||||
|
|
||||||
|
# Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime.
|
||||||
|
opt = least_squares(residuals, x0, loss="huber", f_scale=0.05, max_nfev=80)
|
||||||
|
r_opt, t_opt, g_opt, vels, bgs, bas = unpack(opt.x)
|
||||||
|
|
||||||
|
rot_errs = []
|
||||||
|
trans_errs = []
|
||||||
|
nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
|
||||||
|
for pair in usable:
|
||||||
|
i_idx = id_to_idx[pair.i]
|
||||||
|
j_idx = id_to_idx[pair.j]
|
||||||
|
dt = max(float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s)), 1e-3)
|
||||||
|
delta_r, delta_v, delta_p = _corrected_preint_quantities(
|
||||||
|
pair, bgs[i_idx], bas[i_idx], bg0, ba0
|
||||||
|
)
|
||||||
|
t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
|
||||||
|
r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
|
||||||
|
r_i = nav_r[i_idx]
|
||||||
|
err_r = so3_log(delta_r.T @ r_meas)
|
||||||
|
err_p = r_i @ (t_meas - delta_p) - vels[i_idx] * dt - 0.5 * g_opt * (dt**2)
|
||||||
|
rot_errs.append(np.degrees(np.linalg.norm(err_r)))
|
||||||
|
trans_errs.append(float(np.linalg.norm(err_p)))
|
||||||
|
del delta_v, j_idx
|
||||||
|
|
||||||
|
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
|
||||||
|
trans_rms = float(np.sqrt(np.mean(np.square(trans_errs)))) if trans_errs else 1e9
|
||||||
|
bg_mean = np.mean(bgs, axis=0)
|
||||||
|
ba_mean = np.mean(bas, axis=0)
|
||||||
|
notes.append(
|
||||||
|
"phase-C SE3 (Δv/Δp + g + keyframe v/bias RW): "
|
||||||
|
f"keyframes={k_count}, pairs={len(usable)}, "
|
||||||
|
f"|t|={float(np.linalg.norm(t_opt)):.3f} m, "
|
||||||
|
f"|g|={float(np.linalg.norm(g_opt)):.3f}, "
|
||||||
|
f"trans_rms={trans_rms:.3f} m"
|
||||||
|
)
|
||||||
|
return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_gyro_bias0(pair: MotionPair, fallback: np.ndarray) -> np.ndarray:
|
||||||
|
raw = pair.metadata.get("gyro_bias0_rad_s")
|
||||||
|
if raw is None:
|
||||||
|
return np.asarray(fallback, dtype=float).reshape(3)
|
||||||
|
return np.asarray(raw, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
|
||||||
|
def _phase_a_bias_bases(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
*,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None,
|
||||||
|
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None,
|
||||||
|
) -> dict[str, np.ndarray]:
|
||||||
|
session_ids = sorted({pair.session_id for pair in pairs})
|
||||||
|
scalar = None
|
||||||
|
if gyro_bias_rad_s is not None:
|
||||||
|
scalar = np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
supplied = {} if gyro_bias_rad_s_by_session is None else gyro_bias_rad_s_by_session
|
||||||
|
bases: dict[str, np.ndarray] = {}
|
||||||
|
for sid in session_ids:
|
||||||
|
if sid in supplied:
|
||||||
|
bases[sid] = np.asarray(supplied[sid], dtype=float).reshape(3)
|
||||||
|
continue
|
||||||
|
pair = next(
|
||||||
|
(
|
||||||
|
item
|
||||||
|
for item in pairs
|
||||||
|
if item.session_id == sid and "gyro_bias0_rad_s" in item.metadata
|
||||||
|
),
|
||||||
|
None,
|
||||||
|
)
|
||||||
|
if pair is not None:
|
||||||
|
bases[sid] = np.asarray(pair.metadata["gyro_bias0_rad_s"], dtype=float).reshape(3)
|
||||||
|
elif scalar is not None:
|
||||||
|
bases[sid] = scalar.copy()
|
||||||
|
else:
|
||||||
|
bases[sid] = np.zeros(3)
|
||||||
|
return bases
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_distribution(errs_deg: list[float]) -> tuple[float, float, float, float, bool]:
|
||||||
|
if not errs_deg:
|
||||||
|
return 1e9, 1e9, 1e9, 1.0, False
|
||||||
|
errs = np.asarray(errs_deg, dtype=float)
|
||||||
|
rms = float(np.sqrt(np.mean(errs**2)))
|
||||||
|
median = float(np.median(errs))
|
||||||
|
p95 = float(np.percentile(errs, 95.0))
|
||||||
|
outlier_fraction = float(np.mean(errs > 5.0))
|
||||||
|
accepted = (
|
||||||
|
len(errs) >= 3
|
||||||
|
and rms < 1.5
|
||||||
|
and median < 0.5
|
||||||
|
and p95 < 1.5
|
||||||
|
and outlier_fraction <= 0.005
|
||||||
|
)
|
||||||
|
return rms, median, p95, outlier_fraction, accepted
|
||||||
|
|
||||||
|
|
||||||
|
def _solve_phase_a_rotation(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
r_seed: np.ndarray,
|
||||||
|
*,
|
||||||
|
bias_bases: Mapping[str, np.ndarray],
|
||||||
|
imu: ImuSeries | None,
|
||||||
|
bias_prior_sigma_rad_s: float,
|
||||||
|
preexcluded_session_ids: set[str] | None = None,
|
||||||
|
) -> tuple[
|
||||||
|
np.ndarray,
|
||||||
|
dict[str, np.ndarray],
|
||||||
|
tuple[PhaseASessionResult, ...],
|
||||||
|
list[MotionPair],
|
||||||
|
float,
|
||||||
|
bool,
|
||||||
|
list[str],
|
||||||
|
]:
|
||||||
|
notes: list[str] = []
|
||||||
|
all_session_ids = sorted({pair.session_id for pair in pairs})
|
||||||
|
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
|
||||||
|
|
||||||
|
def optimize(
|
||||||
|
active_pairs: list[MotionPair],
|
||||||
|
r0: np.ndarray,
|
||||||
|
bias_seed: Mapping[str, np.ndarray],
|
||||||
|
) -> tuple[np.ndarray, dict[str, np.ndarray]]:
|
||||||
|
session_ids = sorted({pair.session_id for pair in active_pairs})
|
||||||
|
session_index = {sid: index for index, sid in enumerate(session_ids)}
|
||||||
|
whiten = [residual_whiten_matrix(_pair_cov(pair)) for pair in active_pairs]
|
||||||
|
x0 = np.zeros(3 + 3 * len(session_ids))
|
||||||
|
for sid, index in session_index.items():
|
||||||
|
x0[3 + 3 * index : 6 + 3 * index] = np.asarray(bias_seed[sid], dtype=float)
|
||||||
|
|
||||||
|
def residual(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r0)
|
||||||
|
out: list[np.ndarray] = []
|
||||||
|
for pair, sqrt_info in zip(active_pairs, whiten):
|
||||||
|
index = session_index[pair.session_id]
|
||||||
|
bias = vec[3 + 3 * index : 6 + 3 * index]
|
||||||
|
base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
|
||||||
|
delta_r = _corrected_delta_r(
|
||||||
|
pair, bias - base, imu=imu, bias0=base
|
||||||
|
)
|
||||||
|
out.append(
|
||||||
|
sqrt_info
|
||||||
|
@ preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||||||
|
)
|
||||||
|
for sid, index in session_index.items():
|
||||||
|
bias = vec[3 + 3 * index : 6 + 3 * index]
|
||||||
|
out.append(prior_w * (bias - bias_bases[sid]))
|
||||||
|
return np.concatenate(out)
|
||||||
|
|
||||||
|
opt = least_squares(residual, x0, loss="huber", f_scale=1.0, max_nfev=200)
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r0)
|
||||||
|
biases = {
|
||||||
|
sid: opt.x[3 + 3 * index : 6 + 3 * index].copy()
|
||||||
|
for sid, index in session_index.items()
|
||||||
|
}
|
||||||
|
return r_opt, biases
|
||||||
|
|
||||||
|
def summarize(
|
||||||
|
r_opt: np.ndarray,
|
||||||
|
biases: Mapping[str, np.ndarray],
|
||||||
|
included: set[str],
|
||||||
|
) -> tuple[PhaseASessionResult, ...]:
|
||||||
|
results: list[PhaseASessionResult] = []
|
||||||
|
for sid in all_session_ids:
|
||||||
|
local_pairs = [pair for pair in pairs if pair.session_id == sid]
|
||||||
|
bias = np.asarray(biases.get(sid, bias_bases[sid]), dtype=float).reshape(3)
|
||||||
|
errs: list[float] = []
|
||||||
|
for pair in local_pairs:
|
||||||
|
base = _pair_gyro_bias0(pair, bias_bases[sid])
|
||||||
|
delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||||||
|
errs.append(float(np.degrees(np.linalg.norm(err))))
|
||||||
|
rms, median, p95, outlier, accepted = _rotation_distribution(errs)
|
||||||
|
results.append(
|
||||||
|
PhaseASessionResult(
|
||||||
|
session_id=sid,
|
||||||
|
pair_count=len(local_pairs),
|
||||||
|
gyro_bias0_rad_s=np.asarray(bias_bases[sid], dtype=float),
|
||||||
|
gyro_bias_rad_s=bias,
|
||||||
|
residual_rms_deg=rms,
|
||||||
|
residual_median_deg=median,
|
||||||
|
residual_p95_deg=p95,
|
||||||
|
outlier_fraction_gt_5deg=outlier,
|
||||||
|
accepted=accepted,
|
||||||
|
included_in_final=sid in included,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return tuple(results)
|
||||||
|
|
||||||
|
if not pairs:
|
||||||
|
return r_seed, dict(bias_bases), (), [], 1e9, False, ["no pairs for phase-A"]
|
||||||
|
|
||||||
|
r_first, biases_first = optimize(pairs, r_seed, bias_bases)
|
||||||
|
first = summarize(r_first, biases_first, set(all_session_ids))
|
||||||
|
accepted_ids = {item.session_id for item in first if item.accepted}
|
||||||
|
preexcluded = set() if preexcluded_session_ids is None else set(preexcluded_session_ids)
|
||||||
|
accepted_ids -= preexcluded
|
||||||
|
active_ids = set(all_session_ids)
|
||||||
|
r_final = r_first
|
||||||
|
biases_final = dict(biases_first)
|
||||||
|
if preexcluded and not accepted_ids:
|
||||||
|
active_ids = set()
|
||||||
|
notes.append(f"phase-A pre-gate excluded all sessions: {sorted(preexcluded)}")
|
||||||
|
elif accepted_ids and accepted_ids != active_ids:
|
||||||
|
active_ids = accepted_ids
|
||||||
|
active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
|
||||||
|
r_final, active_biases = optimize(active_pairs, r_first, biases_first)
|
||||||
|
biases_final.update(active_biases)
|
||||||
|
excluded = sorted(set(all_session_ids) - active_ids)
|
||||||
|
notes.append(f"phase-A excluded sessions after local/pre residual gate: {excluded}")
|
||||||
|
active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
|
||||||
|
final = summarize(r_final, biases_final, active_ids)
|
||||||
|
active_results = [item for item in final if item.included_in_final]
|
||||||
|
global_errs: list[float] = []
|
||||||
|
for pair in active_pairs:
|
||||||
|
bias = biases_final[pair.session_id]
|
||||||
|
base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
|
||||||
|
delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r_final, pair.R_B)
|
||||||
|
global_errs.append(float(np.degrees(np.linalg.norm(err))))
|
||||||
|
rot_rms, _, _, _, global_ok = _rotation_distribution(global_errs)
|
||||||
|
accepted = bool(active_results and global_ok and all(item.accepted for item in active_results))
|
||||||
|
notes.append(
|
||||||
|
f"phase-A session-local bias refine: sessions={len(active_ids)}/{len(all_session_ids)}, "
|
||||||
|
f"pairs={len(active_pairs)}, rms={rot_rms:.3f} deg"
|
||||||
|
)
|
||||||
|
return r_final, biases_final, final, active_pairs, rot_rms, accepted, notes
|
||||||
|
|
||||||
|
|
||||||
|
def _solve_joint_extrinsic_legacy(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
force_rotation_only: bool = False,
|
||||||
|
imu: ImuSeries | None = None,
|
||||||
|
delta_t_s: float = 0.0,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
gravity_init_m_s2: np.ndarray | None = None,
|
||||||
|
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
|
||||||
|
time_offset_s_by_session: Mapping[str, float] | None = None,
|
||||||
|
bias_prior_sigma_rad_s: float = 0.02,
|
||||||
|
enable_phase_c: bool | None = None,
|
||||||
|
t_init_m: np.ndarray | None = None,
|
||||||
|
t_prior_m: np.ndarray | None = None,
|
||||||
|
t_prior_sigma_m: np.ndarray | float | None = None,
|
||||||
|
) -> JointExtrinsicResult:
|
||||||
|
"""Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3)."""
|
||||||
|
|
||||||
|
del delta_t_s # reserved for future SE(3) time coupling
|
||||||
|
if enable_phase_c is None:
|
||||||
|
enable_phase_c = not force_rotation_only
|
||||||
|
|
||||||
|
usable = [pair for pair in pairs if pair.t_B_m is not None]
|
||||||
|
observability = analyze_observability(usable, r_x)
|
||||||
|
notes = list(observability.notes)
|
||||||
|
|
||||||
|
r = orthonormalize_rotation(np.asarray(r_x, dtype=float))
|
||||||
|
bias0 = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||||
|
t_seed = None if t_init_m is None else np.asarray(t_init_m, dtype=float).reshape(3)
|
||||||
|
weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
|
||||||
|
whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable]
|
||||||
|
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
|
||||||
|
|
||||||
|
def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray:
|
||||||
|
residuals = []
|
||||||
|
for pair, whiten in zip(usable, whitens):
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||||||
|
residuals.append(whiten @ err)
|
||||||
|
residuals.append(prior_w * delta_bias)
|
||||||
|
return np.concatenate(residuals) if residuals else np.zeros(0)
|
||||||
|
|
||||||
|
def residual_rot_bias(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
|
||||||
|
return rotation_residuals(r_opt, vec[3:])
|
||||||
|
|
||||||
|
if usable:
|
||||||
|
opt = least_squares(
|
||||||
|
residual_rot_bias,
|
||||||
|
np.zeros(6),
|
||||||
|
loss="huber",
|
||||||
|
f_scale=1.0,
|
||||||
|
max_nfev=200,
|
||||||
|
)
|
||||||
|
r = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r)
|
||||||
|
delta_bias = opt.x[3:]
|
||||||
|
bias_out = bias0 + delta_bias
|
||||||
|
notes.append(
|
||||||
|
"phase-A joint refine (single Σ whitening + J_bg): "
|
||||||
|
f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
|
||||||
|
f"pairs={len(usable)}"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
bias_out = bias0
|
||||||
|
delta_bias = np.zeros(3)
|
||||||
|
notes.append("no pairs for joint refine")
|
||||||
|
|
||||||
|
rot_errs = []
|
||||||
|
for pair in usable:
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
err = preintegration_rotation_residual(delta_r, r, pair.R_B)
|
||||||
|
rot_errs.append(np.degrees(np.linalg.norm(err)))
|
||||||
|
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
|
||||||
|
|
||||||
|
t = np.zeros(3) if t_seed is None else t_seed.copy()
|
||||||
|
translation_accepted = False
|
||||||
|
trans_rms = 1e9
|
||||||
|
gravity_out: np.ndarray | None = None
|
||||||
|
accel_bias_out: np.ndarray | None = None
|
||||||
|
|
||||||
|
if gravity_init_m_s2 is None:
|
||||||
|
gravity_init = np.array([0.0, 0.0, -G_NORM])
|
||||||
|
else:
|
||||||
|
gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3)
|
||||||
|
|
||||||
|
if t_prior_m is not None:
|
||||||
|
notes.append(
|
||||||
|
"using CAD/installation translation prior "
|
||||||
|
f"t={np.asarray(t_prior_m, dtype=float).reshape(3).tolist()}"
|
||||||
|
)
|
||||||
|
|
||||||
|
if (
|
||||||
|
enable_phase_c
|
||||||
|
and not force_rotation_only
|
||||||
|
and observability.translation_observable
|
||||||
|
and observability.rotation_observable
|
||||||
|
and len(usable) >= 5
|
||||||
|
):
|
||||||
|
r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3(
|
||||||
|
usable,
|
||||||
|
r,
|
||||||
|
gyro_bias_linearization=bias0,
|
||||||
|
gyro_bias_init=bias_out,
|
||||||
|
gravity_init=gravity_init,
|
||||||
|
t_init=t_seed if t_seed is not None else t_prior_m,
|
||||||
|
t_prior=t_prior_m,
|
||||||
|
t_prior_sigma_m=t_prior_sigma_m,
|
||||||
|
)
|
||||||
|
notes.extend(c_notes)
|
||||||
|
translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
|
||||||
|
if not translation_accepted:
|
||||||
|
# Prefer CAD prior over silent zero when motion SE3 is rejected.
|
||||||
|
if t_prior_m is not None:
|
||||||
|
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
notes.append(
|
||||||
|
"phase-C translation residual/gate failed; CAD translation is reported "
|
||||||
|
"as a prior only and is not accepted as calibration"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("phase-C translation residual/gate failed; keeping translation at zero")
|
||||||
|
t = np.zeros(3)
|
||||||
|
elif (
|
||||||
|
not force_rotation_only
|
||||||
|
and observability.translation_observable
|
||||||
|
and observability.rotation_observable
|
||||||
|
and len(usable) >= 5
|
||||||
|
):
|
||||||
|
# Legacy hand-eye translation fallback when Phase-C metadata missing.
|
||||||
|
def residual_se3(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
|
||||||
|
t_opt = vec[3:]
|
||||||
|
residuals = []
|
||||||
|
for pair, weight, whiten in zip(usable, weights, whitens):
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
residuals.append(
|
||||||
|
np.sqrt(weight) * (whiten @ preintegration_rotation_residual(delta_r, r_opt, pair.R_B))
|
||||||
|
)
|
||||||
|
pred = (pair.R_A - np.eye(3)) @ t_opt
|
||||||
|
meas = r_opt @ np.asarray(pair.t_B_m, dtype=float)
|
||||||
|
residuals.append(np.sqrt(weight) * (pred - meas))
|
||||||
|
if t_prior_m is not None:
|
||||||
|
sigma = np.asarray(t_prior_sigma_m if t_prior_sigma_m is not None else 0.05, dtype=float)
|
||||||
|
if sigma.size == 1:
|
||||||
|
sigma = np.full(3, float(sigma), dtype=float)
|
||||||
|
residuals.append((t_opt - np.asarray(t_prior_m, dtype=float).reshape(3)) / np.maximum(sigma, 1e-3))
|
||||||
|
return np.concatenate(residuals)
|
||||||
|
|
||||||
|
x_se3 = np.zeros(6)
|
||||||
|
if t_seed is not None:
|
||||||
|
x_se3[3:] = t_seed
|
||||||
|
elif t_prior_m is not None:
|
||||||
|
x_se3[3:] = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
opt_t = least_squares(residual_se3, x_se3, loss="huber", f_scale=0.05, max_nfev=200)
|
||||||
|
r = orthonormalize_rotation(so3_exp(opt_t.x[:3]) @ r)
|
||||||
|
t = opt_t.x[3:]
|
||||||
|
rot_errs = []
|
||||||
|
trans_errs = []
|
||||||
|
for pair in usable:
|
||||||
|
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||||
|
rot_errs.append(np.degrees(np.linalg.norm(preintegration_rotation_residual(delta_r, r, pair.R_B))))
|
||||||
|
pred = (pair.R_A - np.eye(3)) @ t
|
||||||
|
meas = r @ np.asarray(pair.t_B_m, dtype=float)
|
||||||
|
trans_errs.append(np.linalg.norm(pred - meas))
|
||||||
|
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs))))
|
||||||
|
trans_rms = float(np.sqrt(np.mean(np.square(trans_errs))))
|
||||||
|
translation_accepted = trans_rms < 0.5
|
||||||
|
notes.append(f"legacy translation refine rms={trans_rms:.3f} m")
|
||||||
|
if not translation_accepted:
|
||||||
|
notes.append("translation residual too large; keeping translation at zero")
|
||||||
|
t = np.zeros(3)
|
||||||
|
elif not force_rotation_only and t_prior_m is not None:
|
||||||
|
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
translation_accepted = False
|
||||||
|
notes.append(
|
||||||
|
"SE3 motion solve gated off; CAD translation is reported as a prior only "
|
||||||
|
"and is not accepted as calibration"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")
|
||||||
|
|
||||||
|
return JointExtrinsicResult(
|
||||||
|
T_IMU_lidar=make_transform(t, r),
|
||||||
|
translation_accepted=bool(translation_accepted and np.linalg.norm(t) > 0),
|
||||||
|
residual_rms_rot_deg=rot_rms,
|
||||||
|
residual_rms_trans_m=trans_rms,
|
||||||
|
observability=observability,
|
||||||
|
gyro_bias_rad_s=np.asarray(bias_out, dtype=float),
|
||||||
|
accel_bias_m_s2=None if accel_bias_out is None else np.asarray(accel_bias_out, dtype=float),
|
||||||
|
gravity_m_s2=None if gravity_out is None else np.asarray(gravity_out, dtype=float),
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def solve_joint_extrinsic(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
force_rotation_only: bool = False,
|
||||||
|
imu: ImuSeries | None = None,
|
||||||
|
delta_t_s: float = 0.0,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
|
||||||
|
time_offset_s_by_session: Mapping[str, float] | None = None,
|
||||||
|
preexcluded_session_ids: set[str] | None = None,
|
||||||
|
gravity_init_m_s2: np.ndarray | None = None,
|
||||||
|
bias_prior_sigma_rad_s: float = 0.002,
|
||||||
|
rotation_prior: np.ndarray | None = None,
|
||||||
|
rotation_prior_sigma_deg: float = 15.0,
|
||||||
|
phase_a_yaw_std_max_deg: float = 0.5,
|
||||||
|
phase_a_loo_yaw_range_max_deg: float = 1.0,
|
||||||
|
phase_a_data_prior_difference_max_deg: float = 1.0,
|
||||||
|
run_phase_a_leave_one_out: bool = True,
|
||||||
|
phase_a_progress_callback: (
|
||||||
|
Callable[[str, dict[str, Any]], None] | None
|
||||||
|
) = None,
|
||||||
|
enable_phase_c: bool | None = None,
|
||||||
|
t_init_m: np.ndarray | None = None,
|
||||||
|
t_prior_m: np.ndarray | None = None,
|
||||||
|
t_prior_sigma_m: np.ndarray | float | None = None,
|
||||||
|
) -> JointExtrinsicResult:
|
||||||
|
"""Run the corrected session-aware Phase-A and gate unfinished SE(3) stages."""
|
||||||
|
|
||||||
|
del gravity_init_m_s2, t_init_m, t_prior_sigma_m, imu, r_x
|
||||||
|
usable_input = [pair for pair in pairs if pair.t_B_m is not None]
|
||||||
|
bias_bases = _phase_a_bias_bases(
|
||||||
|
usable_input,
|
||||||
|
gyro_bias_rad_s=gyro_bias_rad_s,
|
||||||
|
gyro_bias_rad_s_by_session=gyro_bias_rad_s_by_session,
|
||||||
|
)
|
||||||
|
comparison = solve_phase_a_comparison(
|
||||||
|
usable_input,
|
||||||
|
gyro_bias_rad_s_by_session=bias_bases,
|
||||||
|
rotation_prior=rotation_prior,
|
||||||
|
rotation_prior_sigma_deg=rotation_prior_sigma_deg,
|
||||||
|
preexcluded_session_ids=preexcluded_session_ids,
|
||||||
|
bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
|
||||||
|
yaw_std_max_deg=phase_a_yaw_std_max_deg,
|
||||||
|
leave_one_out_yaw_range_max_deg=(
|
||||||
|
phase_a_loo_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
data_prior_difference_max_deg=(
|
||||||
|
phase_a_data_prior_difference_max_deg
|
||||||
|
),
|
||||||
|
run_leave_one_out=run_phase_a_leave_one_out,
|
||||||
|
progress_callback=phase_a_progress_callback,
|
||||||
|
)
|
||||||
|
primary = comparison.session_bg_data_only
|
||||||
|
r = primary.R_IMU_lidar
|
||||||
|
biases = primary.gyro_bias_rad_s_per_session
|
||||||
|
rot_rms = primary.residual_rms_deg
|
||||||
|
phase_a_accepted = comparison.accepted
|
||||||
|
notes = list(comparison.notes)
|
||||||
|
notes.append(
|
||||||
|
"phase-A primary=A1_session_bg_data_only; "
|
||||||
|
f"A0 RPY={comparison.fixed_bg_data_only.rpy_deg_xyz.tolist()}, "
|
||||||
|
f"A1 RPY={primary.rpy_deg_xyz.tolist()}, "
|
||||||
|
"A2 RPY="
|
||||||
|
f"{comparison.session_bg_with_rotation_prior.rpy_deg_xyz.tolist()}"
|
||||||
|
)
|
||||||
|
notes.append(
|
||||||
|
f"phase-A marginal yaw_std={comparison.marginal_observability.yaw_std_deg:.3f} deg, "
|
||||||
|
f"LOO yaw range={comparison.leave_one_out_yaw_range_deg:.3f} deg"
|
||||||
|
)
|
||||||
|
|
||||||
|
session_results_list: list[PhaseASessionResult] = [
|
||||||
|
PhaseASessionResult(
|
||||||
|
session_id=item.session_id,
|
||||||
|
pair_count=item.pair_count,
|
||||||
|
gyro_bias0_rad_s=item.gyro_bias0_rad_s,
|
||||||
|
gyro_bias_rad_s=item.gyro_bias_rad_s,
|
||||||
|
residual_rms_deg=item.residual_rms_deg,
|
||||||
|
residual_median_deg=item.residual_median_deg,
|
||||||
|
residual_p95_deg=item.residual_p95_deg,
|
||||||
|
outlier_fraction_gt_5deg=item.outlier_fraction_gt_5deg,
|
||||||
|
accepted=item.accepted,
|
||||||
|
included_in_final=True,
|
||||||
|
)
|
||||||
|
for item in primary.sessions
|
||||||
|
]
|
||||||
|
preexcluded = (
|
||||||
|
set()
|
||||||
|
if preexcluded_session_ids is None
|
||||||
|
else set(preexcluded_session_ids)
|
||||||
|
)
|
||||||
|
strong_all = select_strong_rotation_pairs(usable_input)
|
||||||
|
for session_id in sorted(preexcluded):
|
||||||
|
local_pairs = [
|
||||||
|
pair for pair in strong_all if pair.session_id == session_id
|
||||||
|
]
|
||||||
|
errors = [
|
||||||
|
float(
|
||||||
|
np.degrees(
|
||||||
|
np.linalg.norm(
|
||||||
|
preintegration_rotation_residual(
|
||||||
|
pair.R_A, r, pair.R_B
|
||||||
|
)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
for pair in local_pairs
|
||||||
|
]
|
||||||
|
rms, median, p95, outlier, accepted = _rotation_distribution(
|
||||||
|
errors
|
||||||
|
)
|
||||||
|
base = np.asarray(
|
||||||
|
bias_bases.get(session_id, np.zeros(3)), dtype=float
|
||||||
|
).reshape(3)
|
||||||
|
session_results_list.append(
|
||||||
|
PhaseASessionResult(
|
||||||
|
session_id=session_id,
|
||||||
|
pair_count=len(local_pairs),
|
||||||
|
gyro_bias0_rad_s=base,
|
||||||
|
gyro_bias_rad_s=base,
|
||||||
|
residual_rms_deg=rms,
|
||||||
|
residual_median_deg=median,
|
||||||
|
residual_p95_deg=p95,
|
||||||
|
outlier_fraction_gt_5deg=outlier,
|
||||||
|
accepted=accepted,
|
||||||
|
included_in_final=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
session_results = tuple(
|
||||||
|
sorted(session_results_list, key=lambda item: item.session_id)
|
||||||
|
)
|
||||||
|
usable = [
|
||||||
|
pair
|
||||||
|
for pair in strong_all
|
||||||
|
if pair.session_id not in preexcluded
|
||||||
|
]
|
||||||
|
base_observability = analyze_observability(usable, r)
|
||||||
|
marginal = comparison.marginal_observability
|
||||||
|
observability = ObservabilityReport(
|
||||||
|
rotation_observable=bool(
|
||||||
|
marginal.rank == 3
|
||||||
|
and marginal.yaw_std_deg <= phase_a_yaw_std_max_deg
|
||||||
|
),
|
||||||
|
translation_observable=base_observability.translation_observable,
|
||||||
|
condition_rotation=marginal.condition,
|
||||||
|
condition_translation=base_observability.condition_translation,
|
||||||
|
notes=tuple(
|
||||||
|
list(marginal.notes)
|
||||||
|
+ list(base_observability.notes)
|
||||||
|
),
|
||||||
|
)
|
||||||
|
notes.extend(observability.notes)
|
||||||
|
if time_offset_s_by_session is None:
|
||||||
|
notes.append(
|
||||||
|
f"legacy scalar time offset fixed during pair construction: {float(delta_t_s):.6f}s"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
fixed_offsets = {
|
||||||
|
str(sid): float(value) for sid, value in time_offset_s_by_session.items()
|
||||||
|
}
|
||||||
|
notes.append(
|
||||||
|
f"time offsets fixed during pair construction (not optimized): {fixed_offsets}"
|
||||||
|
)
|
||||||
|
|
||||||
|
for item in session_results:
|
||||||
|
notes.append(
|
||||||
|
f"phase-A session {item.session_id}: included={item.included_in_final}, "
|
||||||
|
f"pairs={item.pair_count}, rms={item.residual_rms_deg:.3f} deg, "
|
||||||
|
f"p95={item.residual_p95_deg:.3f} deg, "
|
||||||
|
f"|bias-bias0|={float(np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)):.3e}"
|
||||||
|
)
|
||||||
|
|
||||||
|
phase_c_requested = (not force_rotation_only) if enable_phase_c is None else bool(enable_phase_c)
|
||||||
|
t = np.zeros(3)
|
||||||
|
if not force_rotation_only:
|
||||||
|
if phase_c_requested:
|
||||||
|
notes.append(
|
||||||
|
"phase-B/C gated off: session-aware translation/gravity/navigation "
|
||||||
|
"states are not implemented yet"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("phase-C disabled; translation is not accepted")
|
||||||
|
if t_prior_m is not None:
|
||||||
|
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||||||
|
notes.append(
|
||||||
|
"CAD translation is reported as a prior only and is not accepted as calibration"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append("rotation-only extrinsic returned after corrected phase-A")
|
||||||
|
|
||||||
|
single_bias = None
|
||||||
|
if len(biases) == 1:
|
||||||
|
single_bias = np.asarray(next(iter(biases.values())), dtype=float)
|
||||||
|
return JointExtrinsicResult(
|
||||||
|
T_IMU_lidar=make_transform(t, r),
|
||||||
|
translation_accepted=False,
|
||||||
|
residual_rms_rot_deg=rot_rms,
|
||||||
|
residual_rms_trans_m=1e9,
|
||||||
|
observability=observability,
|
||||||
|
gyro_bias_rad_s=single_bias,
|
||||||
|
accel_bias_m_s2=None,
|
||||||
|
gravity_m_s2=None,
|
||||||
|
gyro_bias_rad_s_per_session={
|
||||||
|
sid: np.asarray(value, dtype=float) for sid, value in biases.items()
|
||||||
|
},
|
||||||
|
phase_a_sessions=session_results,
|
||||||
|
phase_a_accepted=phase_a_accepted,
|
||||||
|
phase_a_comparison=phase_a_comparison_to_dict(comparison),
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
"""LiDAR keyframe selection."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import LidarFrame
|
||||||
|
from .registration import register_lidar_pair
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class KeyframeSet:
|
||||||
|
indices: tuple[int, ...]
|
||||||
|
frames: tuple[LidarFrame, ...]
|
||||||
|
|
||||||
|
|
||||||
|
def build_keyframes(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
min_translation_m: float = 0.3,
|
||||||
|
min_rotation_deg: float = 3.0,
|
||||||
|
min_registration_fitness: float = 0.5,
|
||||||
|
max_frame_gap: int = 8,
|
||||||
|
) -> KeyframeSet:
|
||||||
|
"""Select keyframes with enough relative motion for hand-eye pairs."""
|
||||||
|
|
||||||
|
if not frames:
|
||||||
|
return KeyframeSet((), ())
|
||||||
|
|
||||||
|
selected = [0]
|
||||||
|
last = 0
|
||||||
|
for index in range(1, len(frames)):
|
||||||
|
if index - last > max_frame_gap:
|
||||||
|
selected.append(index)
|
||||||
|
last = index
|
||||||
|
continue
|
||||||
|
result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
|
||||||
|
if not result.ok or result.fitness < min_registration_fitness:
|
||||||
|
continue
|
||||||
|
if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
|
||||||
|
selected.append(index)
|
||||||
|
last = index
|
||||||
|
|
||||||
|
if selected[-1] != len(frames) - 1 and len(frames) > 1:
|
||||||
|
selected.append(len(frames) - 1)
|
||||||
|
|
||||||
|
unique = tuple(dict.fromkeys(selected))
|
||||||
|
return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))
|
||||||
@@ -0,0 +1,59 @@
|
|||||||
|
"""Coarse LiDAR deskew using a constant body rate over the sweep."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
from .geometry import so3_exp
|
||||||
|
from .time_offset import lidar_time_to_imu_time
|
||||||
|
|
||||||
|
|
||||||
|
def deskew_lidar_frames(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
imu: ImuSeries,
|
||||||
|
*,
|
||||||
|
delta_t_s: float,
|
||||||
|
R_IMU_lidar: np.ndarray | None = None,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
) -> list[LidarFrame]:
|
||||||
|
"""Return deskewed copies when extrinsic is known; otherwise return originals."""
|
||||||
|
|
||||||
|
if R_IMU_lidar is None:
|
||||||
|
return frames
|
||||||
|
|
||||||
|
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
r_li = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3).T
|
||||||
|
output: list[LidarFrame] = []
|
||||||
|
|
||||||
|
for frame in frames:
|
||||||
|
n = frame.points_xyz.shape[0]
|
||||||
|
if n < 10:
|
||||||
|
output.append(frame)
|
||||||
|
continue
|
||||||
|
t_mid_imu = lidar_time_to_imu_time(frame.t_mid_s, delta_t_s)
|
||||||
|
index = int(np.clip(np.searchsorted(imu.t_s, t_mid_imu), 1, imu.t_s.size - 1))
|
||||||
|
omega_lidar = r_li @ (imu.gyro_rad_s[index] - bias)
|
||||||
|
duration = max(frame.t_end_s - frame.t_start_s, 1e-3)
|
||||||
|
rel = np.linspace(-0.5, 0.5, n) * duration
|
||||||
|
deskewed = np.empty_like(frame.points_xyz)
|
||||||
|
# Piecewise-constant rotation over a few time bins.
|
||||||
|
bins = 12
|
||||||
|
edges = np.linspace(-0.5 * duration, 0.5 * duration, bins + 1)
|
||||||
|
for b in range(bins):
|
||||||
|
mask = (rel >= edges[b]) & (rel <= edges[b + 1] if b == bins - 1 else rel < edges[b + 1])
|
||||||
|
if not np.any(mask):
|
||||||
|
continue
|
||||||
|
tau = 0.5 * (edges[b] + edges[b + 1])
|
||||||
|
rot = so3_exp(omega_lidar * float(tau))
|
||||||
|
deskewed[mask] = frame.points_xyz[mask] @ rot.T
|
||||||
|
output.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=frame.frame_id,
|
||||||
|
t_start_s=frame.t_start_s,
|
||||||
|
t_end_s=frame.t_end_s,
|
||||||
|
points_xyz=deskewed,
|
||||||
|
path=frame.path,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return output
|
||||||
@@ -0,0 +1,127 @@
|
|||||||
|
"""LiDAR adapters for the V1 standard intermediate format.
|
||||||
|
|
||||||
|
Accepted input: a directory containing ``frames_index.csv`` and per-frame NPZ files.
|
||||||
|
|
||||||
|
frames_index.csv
|
||||||
|
----------------
|
||||||
|
frame_id,file,t_start,t_end
|
||||||
|
|
||||||
|
Each NPZ referenced by ``file`` must contain:
|
||||||
|
- points: float array shaped (N, 3) in LiDAR Cartesian coordinates (metres)
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import LidarFrame
|
||||||
|
|
||||||
|
|
||||||
|
def _read_frames_index(root: Path) -> tuple[np.ndarray, str]:
|
||||||
|
index_path = root / "frames_index.csv"
|
||||||
|
if not index_path.exists():
|
||||||
|
raise FileNotFoundError(f"missing frames_index.csv under {root}")
|
||||||
|
|
||||||
|
rows = np.genfromtxt(index_path, delimiter=",", names=True, dtype=None, encoding="utf-8")
|
||||||
|
if rows.ndim == 0:
|
||||||
|
rows = np.array([rows])
|
||||||
|
names = set(rows.dtype.names or ())
|
||||||
|
# NumPy may rename reserved name ``file`` to ``file_``.
|
||||||
|
file_key = "filename" if "filename" in names else ("file_" if "file_" in names else "file")
|
||||||
|
required = {"frame_id", "t_start", "t_end"}
|
||||||
|
if not required.issubset(names) or file_key not in names:
|
||||||
|
raise ValueError(
|
||||||
|
f"frames_index.csv must contain frame_id,{file_key}/filename,t_start,t_end; got {sorted(names)}"
|
||||||
|
)
|
||||||
|
return rows, file_key
|
||||||
|
|
||||||
|
|
||||||
|
def list_lidar_frame_entries(path: Path | str) -> list[tuple[str, float, float, Path]]:
|
||||||
|
"""Return ``(frame_id, t_start, t_end, npz_path)`` sorted by mid time (same as ``load_lidar_frames``)."""
|
||||||
|
|
||||||
|
root = Path(path)
|
||||||
|
rows, file_key = _read_frames_index(root)
|
||||||
|
entries: list[tuple[str, float, float, Path]] = []
|
||||||
|
for row in rows:
|
||||||
|
t0 = float(row["t_start"])
|
||||||
|
t1 = float(row["t_end"])
|
||||||
|
entries.append((str(row["frame_id"]), t0, t1, root / str(row[file_key])))
|
||||||
|
entries.sort(key=lambda item: 0.5 * (item[1] + item[2]))
|
||||||
|
return entries
|
||||||
|
|
||||||
|
|
||||||
|
def load_lidar_frame_at(root: Path | str, index: int) -> LidarFrame:
|
||||||
|
"""Load one frame by index in mid-time-sorted order (matches motion-pair ``i``/``j``)."""
|
||||||
|
|
||||||
|
entries = list_lidar_frame_entries(root)
|
||||||
|
if index < 0 or index >= len(entries):
|
||||||
|
raise IndexError(f"frame index {index} outside [0, {len(entries) - 1}] for {root}")
|
||||||
|
frame_id, t0, t1, npz_path = entries[index]
|
||||||
|
with np.load(npz_path) as payload:
|
||||||
|
if "points" not in payload.files:
|
||||||
|
raise ValueError(f"{npz_path} must contain array 'points'")
|
||||||
|
points = np.asarray(payload["points"], dtype=float)
|
||||||
|
if points.ndim != 2 or points.shape[1] < 3:
|
||||||
|
raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
|
||||||
|
return LidarFrame(
|
||||||
|
frame_id=frame_id,
|
||||||
|
t_start_s=t0,
|
||||||
|
t_end_s=t1,
|
||||||
|
points_xyz=points[:, :3],
|
||||||
|
path=npz_path,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def lidar_frame_count(path: Path | str) -> int:
|
||||||
|
return len(list_lidar_frame_entries(path))
|
||||||
|
|
||||||
|
|
||||||
|
def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
|
||||||
|
"""Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
|
||||||
|
|
||||||
|
root = Path(path)
|
||||||
|
rows, file_key = _read_frames_index(root)
|
||||||
|
|
||||||
|
frames: list[LidarFrame] = []
|
||||||
|
for row in rows:
|
||||||
|
frame_id = str(row["frame_id"])
|
||||||
|
rel = str(row[file_key])
|
||||||
|
npz_path = root / rel
|
||||||
|
with np.load(npz_path) as payload:
|
||||||
|
if "points" not in payload.files:
|
||||||
|
raise ValueError(f"{npz_path} must contain array 'points'")
|
||||||
|
points = np.asarray(payload["points"], dtype=float)
|
||||||
|
if points.ndim != 2 or points.shape[1] < 3:
|
||||||
|
raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
|
||||||
|
frames.append(
|
||||||
|
LidarFrame(
|
||||||
|
frame_id=frame_id,
|
||||||
|
t_start_s=float(row["t_start"]),
|
||||||
|
t_end_s=float(row["t_end"]),
|
||||||
|
points_xyz=points[:, :3],
|
||||||
|
path=npz_path,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
frames.sort(key=lambda frame: frame.t_mid_s)
|
||||||
|
return frames
|
||||||
|
|
||||||
|
|
||||||
|
def save_lidar_session(
|
||||||
|
root: Path | str,
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
points_dirname: str = "frames",
|
||||||
|
) -> None:
|
||||||
|
"""Write a LiDAR session directory in the standard intermediate format."""
|
||||||
|
|
||||||
|
destination = Path(root)
|
||||||
|
frames_dir = destination / points_dirname
|
||||||
|
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||||
|
index_rows: list[str] = ["frame_id,filename,t_start,t_end"]
|
||||||
|
for index, frame in enumerate(frames):
|
||||||
|
relative = f"{points_dirname}/frame_{index:05d}.npz"
|
||||||
|
np.savez_compressed(destination / relative, points=np.asarray(frame.points_xyz, dtype=float))
|
||||||
|
index_rows.append(f"{frame.frame_id},{relative},{frame.t_start_s:.9f},{frame.t_end_s:.9f}")
|
||||||
|
(destination / "frames_index.csv").write_text("\n".join(index_rows) + "\n", encoding="utf-8")
|
||||||
@@ -0,0 +1,220 @@
|
|||||||
|
"""Build IMU/LiDAR relative-motion pairs for hand-eye calibration."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Callable
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from time import perf_counter
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame, MotionPair
|
||||||
|
from .geometry import make_transform, rotation_angle_deg
|
||||||
|
from .imu_preintegration import preintegrate_imu
|
||||||
|
from .registration import register_lidar_pair
|
||||||
|
from .time_offset import lidar_time_to_imu_time
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MotionPairSet:
|
||||||
|
pairs: tuple[MotionPair, ...]
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def build_motion_pairs(
|
||||||
|
*,
|
||||||
|
session_id: str,
|
||||||
|
keyframes: list[LidarFrame],
|
||||||
|
keyframe_indices: list[int] | tuple[int, ...],
|
||||||
|
imu: ImuSeries,
|
||||||
|
delta_t_s: float,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
acc_bias_m_s2: np.ndarray | None = None,
|
||||||
|
min_rotation_deg: float = 3.0,
|
||||||
|
min_translation_m: float = 0.3,
|
||||||
|
min_registration_fitness: float = 0.5,
|
||||||
|
max_imu_gap_s: float = 0.05,
|
||||||
|
max_lidar_gap_s: float = 1.0,
|
||||||
|
all_frame_times_s: np.ndarray | None = None,
|
||||||
|
max_index_span: int = 4,
|
||||||
|
progress_callback: Callable[[dict[str, Any]], None] | None = None,
|
||||||
|
) -> MotionPairSet:
|
||||||
|
"""Create A/B motion pairs between nearby keyframes.
|
||||||
|
|
||||||
|
IMU side uses full Phase-C preintegration (``ΔR/Δv/Δp``, ``Σ9``, ``J_bg/J_ba``).
|
||||||
|
Rotation hand-eye still consumes ``R_A = ΔR`` only.
|
||||||
|
"""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
pairs: list[MotionPair] = []
|
||||||
|
rejected_fitness = 0
|
||||||
|
rejected_imu_gap = 0
|
||||||
|
rejected_lidar_gap = 0
|
||||||
|
frame_times = (
|
||||||
|
None
|
||||||
|
if all_frame_times_s is None
|
||||||
|
else np.asarray(all_frame_times_s, dtype=float).reshape(-1)
|
||||||
|
)
|
||||||
|
bias_g = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
bias_a = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float)
|
||||||
|
n = len(keyframes)
|
||||||
|
if n < 2:
|
||||||
|
return MotionPairSet((), ("need at least two keyframes",))
|
||||||
|
|
||||||
|
total_candidates = sum(max(n - span, 0) for span in range(1, max_index_span + 1))
|
||||||
|
processed_candidates = 0
|
||||||
|
started_at = perf_counter()
|
||||||
|
last_progress_at = started_at
|
||||||
|
|
||||||
|
def report_progress(*, event: str, span: int, force: bool = False) -> None:
|
||||||
|
nonlocal last_progress_at
|
||||||
|
if progress_callback is None:
|
||||||
|
return
|
||||||
|
now = perf_counter()
|
||||||
|
if not force and processed_candidates > 1 and now - last_progress_at < 10.0:
|
||||||
|
return
|
||||||
|
last_progress_at = now
|
||||||
|
progress_callback(
|
||||||
|
{
|
||||||
|
"event": event,
|
||||||
|
"processed_candidates": processed_candidates,
|
||||||
|
"total_candidates": total_candidates,
|
||||||
|
"progress_pct": 100.0 * processed_candidates / max(total_candidates, 1),
|
||||||
|
"current_span": span,
|
||||||
|
"max_span": max_index_span,
|
||||||
|
"accepted_pairs": len(pairs),
|
||||||
|
"rejected_fitness": rejected_fitness,
|
||||||
|
"rejected_imu_gap": rejected_imu_gap,
|
||||||
|
"rejected_lidar_gap": rejected_lidar_gap,
|
||||||
|
"elapsed_s": now - started_at,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
report_progress(event="start", span=1, force=True)
|
||||||
|
|
||||||
|
for span in range(1, max_index_span + 1):
|
||||||
|
for start in range(0, n - span):
|
||||||
|
processed_candidates += 1
|
||||||
|
report_progress(event="running", span=span)
|
||||||
|
i = start
|
||||||
|
j = start + span
|
||||||
|
frame_i = keyframes[i]
|
||||||
|
frame_j = keyframes[j]
|
||||||
|
source_i = int(keyframe_indices[i])
|
||||||
|
source_j = int(keyframe_indices[j])
|
||||||
|
if frame_times is not None:
|
||||||
|
lo = min(source_i, source_j)
|
||||||
|
hi = max(source_i, source_j)
|
||||||
|
local_times = frame_times[lo : hi + 1]
|
||||||
|
if local_times.size >= 2 and np.any(np.diff(local_times) > max_lidar_gap_s):
|
||||||
|
rejected_lidar_gap += 1
|
||||||
|
continue
|
||||||
|
reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
|
||||||
|
if not reg.ok:
|
||||||
|
continue
|
||||||
|
if reg.fitness < min_registration_fitness:
|
||||||
|
rejected_fitness += 1
|
||||||
|
continue
|
||||||
|
if reg.rotation_deg < min_rotation_deg and reg.translation_m < min_translation_m:
|
||||||
|
continue
|
||||||
|
|
||||||
|
t_i_imu = lidar_time_to_imu_time(frame_i.t_mid_s, delta_t_s)
|
||||||
|
t_j_imu = lidar_time_to_imu_time(frame_j.t_mid_s, delta_t_s)
|
||||||
|
if t_j_imu <= t_i_imu:
|
||||||
|
continue
|
||||||
|
if t_i_imu < imu.t_s[0] or t_j_imu > imu.t_s[-1]:
|
||||||
|
continue
|
||||||
|
imu_lo = max(int(np.searchsorted(imu.t_s, t_i_imu, side="right")) - 1, 0)
|
||||||
|
imu_hi = min(
|
||||||
|
int(np.searchsorted(imu.t_s, t_j_imu, side="left")) + 1,
|
||||||
|
imu.t_s.size,
|
||||||
|
)
|
||||||
|
if imu_hi - imu_lo >= 2 and np.any(
|
||||||
|
np.diff(imu.t_s[imu_lo:imu_hi]) > max_imu_gap_s
|
||||||
|
):
|
||||||
|
rejected_imu_gap += 1
|
||||||
|
continue
|
||||||
|
|
||||||
|
preint = preintegrate_imu(
|
||||||
|
imu.t_s,
|
||||||
|
imu.gyro_rad_s,
|
||||||
|
imu.acc_m_s2,
|
||||||
|
t_i_imu,
|
||||||
|
t_j_imu,
|
||||||
|
bias_g,
|
||||||
|
bias_a,
|
||||||
|
)
|
||||||
|
r_a = preint.delta_R
|
||||||
|
r_b = reg.transform[:3, :3]
|
||||||
|
t_b = reg.transform[:3, 3]
|
||||||
|
rot_a = rotation_angle_deg(r_a)
|
||||||
|
if abs(rot_a - reg.rotation_deg) > max(15.0, 1.0 * max(rot_a, reg.rotation_deg)):
|
||||||
|
continue
|
||||||
|
|
||||||
|
pairs.append(
|
||||||
|
MotionPair(
|
||||||
|
session_id=session_id,
|
||||||
|
i=int(keyframe_indices[i]),
|
||||||
|
j=int(keyframe_indices[j]),
|
||||||
|
t_i_s=frame_i.t_mid_s,
|
||||||
|
t_j_s=frame_j.t_mid_s,
|
||||||
|
R_A=r_a,
|
||||||
|
R_B=r_b,
|
||||||
|
t_A_m=np.asarray(preint.delta_p, dtype=float),
|
||||||
|
t_B_m=np.asarray(t_b, dtype=float),
|
||||||
|
fitness=reg.fitness,
|
||||||
|
metadata={
|
||||||
|
"backend": reg.backend,
|
||||||
|
"rotation_deg_B": reg.rotation_deg,
|
||||||
|
"translation_m_B": reg.translation_m,
|
||||||
|
"rotation_deg_A": rot_a,
|
||||||
|
"weight": preint.weight,
|
||||||
|
"duration_s": preint.duration_s,
|
||||||
|
"mean_gyro_norm": preint.mean_gyro_norm,
|
||||||
|
"preint_sigma_rad": preint.sigma_rad,
|
||||||
|
"cov": preint.cov[0:3, 0:3].tolist(),
|
||||||
|
"cov9": preint.cov.tolist(),
|
||||||
|
"J_bg": preint.J_bg[0:3, 0:3].tolist(),
|
||||||
|
"J_bg9": preint.J_bg.tolist(),
|
||||||
|
"J_ba": preint.J_ba.tolist(),
|
||||||
|
"delta_v": preint.delta_v.tolist(),
|
||||||
|
"delta_p": preint.delta_p.tolist(),
|
||||||
|
"t_i_imu_s": t_i_imu,
|
||||||
|
"t_j_imu_s": t_j_imu,
|
||||||
|
"gyro_bias0_rad_s": bias_g.tolist(),
|
||||||
|
"accel_bias0_m_s2": bias_a.tolist(),
|
||||||
|
"time_offset_s": float(delta_t_s),
|
||||||
|
"keyframe_span": int(span),
|
||||||
|
"is_consecutive": bool(span == 1),
|
||||||
|
"modeling": "imu_preintegration_factor_phase_c",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
report_progress(event="complete", span=max_index_span, force=True)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)"
|
||||||
|
)
|
||||||
|
notes.append(
|
||||||
|
"quality rejects: "
|
||||||
|
f"fitness<{min_registration_fitness:.2f}: {rejected_fitness}, "
|
||||||
|
f"IMU gap>{max_imu_gap_s:.3f}s: {rejected_imu_gap}, "
|
||||||
|
f"LiDAR gap>{max_lidar_gap_s:.3f}s: {rejected_lidar_gap}"
|
||||||
|
)
|
||||||
|
return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes))
|
||||||
|
|
||||||
|
|
||||||
|
def pairs_to_transforms(pairs: tuple[MotionPair, ...]) -> tuple[list[np.ndarray], list[np.ndarray]]:
|
||||||
|
"""Helper returning SE(3) lists when translations are present."""
|
||||||
|
|
||||||
|
a_list: list[np.ndarray] = []
|
||||||
|
b_list: list[np.ndarray] = []
|
||||||
|
for pair in pairs:
|
||||||
|
if pair.t_B_m is None:
|
||||||
|
continue
|
||||||
|
t_a = np.zeros(3) if pair.t_A_m is None else pair.t_A_m
|
||||||
|
a_list.append(make_transform(t_a, pair.R_A))
|
||||||
|
b_list.append(make_transform(pair.t_B_m, pair.R_B))
|
||||||
|
return a_list, b_list
|
||||||
@@ -0,0 +1,154 @@
|
|||||||
|
"""Serialize / deserialize motion pairs for fast visualization."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import MotionPair
|
||||||
|
|
||||||
|
SCHEMA_VERSION = 2
|
||||||
|
|
||||||
|
# Keep visualization fields plus the compact 3x3 rotation metadata needed to
|
||||||
|
# rerun Phase-A without repeating LiDAR registration. Full 9x9 Phase-C matrices
|
||||||
|
# remain excluded from this cache.
|
||||||
|
_METADATA_KEEP = frozenset(
|
||||||
|
{
|
||||||
|
"backend",
|
||||||
|
"rotation_deg_A",
|
||||||
|
"rotation_deg_B",
|
||||||
|
"translation_m_B",
|
||||||
|
"weight",
|
||||||
|
"duration_s",
|
||||||
|
"mean_gyro_norm",
|
||||||
|
"preint_sigma_rad",
|
||||||
|
"cov",
|
||||||
|
"J_bg",
|
||||||
|
"phase_a_metadata_rehydrated",
|
||||||
|
"rehydrated_R_A_error_deg",
|
||||||
|
"t_i_imu_s",
|
||||||
|
"t_j_imu_s",
|
||||||
|
"gyro_bias0_rad_s",
|
||||||
|
"accel_bias0_m_s2",
|
||||||
|
"time_offset_s",
|
||||||
|
"keyframe_span",
|
||||||
|
"is_consecutive",
|
||||||
|
"modeling",
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _to_list(value: Any) -> Any:
|
||||||
|
if isinstance(value, np.ndarray):
|
||||||
|
return value.tolist()
|
||||||
|
if isinstance(value, (np.floating, np.integer, np.bool_)):
|
||||||
|
return value.item()
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def pair_to_dict(pair: MotionPair) -> dict[str, Any]:
|
||||||
|
meta = {
|
||||||
|
str(k): _to_list(v)
|
||||||
|
for k, v in (pair.metadata or {}).items()
|
||||||
|
if str(k) in _METADATA_KEEP
|
||||||
|
}
|
||||||
|
return {
|
||||||
|
"session_id": pair.session_id,
|
||||||
|
"i": int(pair.i),
|
||||||
|
"j": int(pair.j),
|
||||||
|
"t_i_s": float(pair.t_i_s),
|
||||||
|
"t_j_s": float(pair.t_j_s),
|
||||||
|
"R_A": np.asarray(pair.R_A, dtype=float).reshape(3, 3).tolist(),
|
||||||
|
"R_B": np.asarray(pair.R_B, dtype=float).reshape(3, 3).tolist(),
|
||||||
|
"t_A_m": None if pair.t_A_m is None else np.asarray(pair.t_A_m, dtype=float).reshape(3).tolist(),
|
||||||
|
"t_B_m": None if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float).reshape(3).tolist(),
|
||||||
|
"fitness": float(pair.fitness),
|
||||||
|
"metadata": meta,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def pair_from_dict(payload: dict[str, Any]) -> MotionPair:
|
||||||
|
t_a = payload.get("t_A_m")
|
||||||
|
t_b = payload.get("t_B_m")
|
||||||
|
return MotionPair(
|
||||||
|
session_id=str(payload.get("session_id", "")),
|
||||||
|
i=int(payload["i"]),
|
||||||
|
j=int(payload["j"]),
|
||||||
|
t_i_s=float(payload["t_i_s"]),
|
||||||
|
t_j_s=float(payload["t_j_s"]),
|
||||||
|
R_A=np.asarray(payload["R_A"], dtype=float).reshape(3, 3),
|
||||||
|
R_B=np.asarray(payload["R_B"], dtype=float).reshape(3, 3),
|
||||||
|
t_A_m=None if t_a is None else np.asarray(t_a, dtype=float).reshape(3),
|
||||||
|
t_B_m=None if t_b is None else np.asarray(t_b, dtype=float).reshape(3),
|
||||||
|
fitness=float(payload.get("fitness", 0.0)),
|
||||||
|
metadata=dict(payload.get("metadata") or {}),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def build_motion_pairs_payload(
|
||||||
|
*,
|
||||||
|
prepared_sessions: list[dict[str, Any]],
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Build a JSON-serializable cache from pipeline ``prepared`` session dicts."""
|
||||||
|
|
||||||
|
sessions_out: list[dict[str, Any]] = []
|
||||||
|
for prep in prepared_sessions:
|
||||||
|
pairs = prep.get("pairs") or ()
|
||||||
|
sessions_out.append(
|
||||||
|
{
|
||||||
|
"session_id": prep.get("session_id"),
|
||||||
|
"delta_t_s": float(prep.get("time_offset_s", 0.0)),
|
||||||
|
"gyro_bias_rad_s": np.asarray(prep.get("gyro_bias_rad_s", np.zeros(3)), dtype=float)
|
||||||
|
.reshape(3)
|
||||||
|
.tolist(),
|
||||||
|
"pair_count": len(pairs),
|
||||||
|
"pairs": [pair_to_dict(pair) for pair in pairs],
|
||||||
|
}
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"schema_version": SCHEMA_VERSION,
|
||||||
|
"sessions": sessions_out,
|
||||||
|
"note": "Cached motion pairs for visualization; A=IMU preintegration, B=LiDAR registration",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def save_motion_pairs(path: Path | str, payload: dict[str, Any]) -> Path:
|
||||||
|
destination = Path(path)
|
||||||
|
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
destination.write_text(json.dumps(payload, indent=2), encoding="utf-8")
|
||||||
|
return destination
|
||||||
|
|
||||||
|
|
||||||
|
def load_motion_pairs(path: Path | str) -> dict[str, Any]:
|
||||||
|
payload = json.loads(Path(path).read_text(encoding="utf-8"))
|
||||||
|
version = int(payload.get("schema_version", 0))
|
||||||
|
if version not in {1, SCHEMA_VERSION}:
|
||||||
|
raise ValueError(
|
||||||
|
f"unsupported motion_pairs schema_version={version}; "
|
||||||
|
f"expected 1 or {SCHEMA_VERSION}"
|
||||||
|
)
|
||||||
|
return payload
|
||||||
|
|
||||||
|
|
||||||
|
def pairs_for_session(payload: dict[str, Any], session_id: str | None = None) -> list[MotionPair]:
|
||||||
|
sessions = payload.get("sessions") or []
|
||||||
|
if not sessions:
|
||||||
|
return []
|
||||||
|
if session_id is None:
|
||||||
|
chosen = sessions[0]
|
||||||
|
else:
|
||||||
|
chosen = next((s for s in sessions if s.get("session_id") == session_id), None)
|
||||||
|
if chosen is None:
|
||||||
|
raise KeyError(f"session_id {session_id!r} not found in motion_pairs cache")
|
||||||
|
return [pair_from_dict(item) for item in chosen.get("pairs") or []]
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_motion_pairs_path(summary_path: Path | str) -> Path | None:
|
||||||
|
"""Return ``motion_pairs.json`` next to a summary if it exists."""
|
||||||
|
|
||||||
|
summary = Path(summary_path)
|
||||||
|
candidate = summary.parent / "motion_pairs.json"
|
||||||
|
return candidate if candidate.is_file() else None
|
||||||
@@ -0,0 +1,114 @@
|
|||||||
|
"""Normalized-Jacobian observability analysis for rotation / SE(3) gates."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import MotionPair
|
||||||
|
from .geometry import skew, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ObservabilityReport:
|
||||||
|
rotation_observable: bool
|
||||||
|
translation_observable: bool
|
||||||
|
condition_rotation: float
|
||||||
|
condition_translation: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_jacobian(pairs: list[MotionPair], r_x: np.ndarray) -> np.ndarray:
|
||||||
|
rows = []
|
||||||
|
for pair in pairs:
|
||||||
|
# Residual r = log(R_x^T R_A R_x R_B^T); approximate J w.r.t. left perturbation of R_x.
|
||||||
|
# Use finite-difference columns for robustness in V1.
|
||||||
|
base = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||||
|
cols = []
|
||||||
|
eps = 1e-5
|
||||||
|
for axis in range(3):
|
||||||
|
delta = np.zeros(3)
|
||||||
|
delta[axis] = eps
|
||||||
|
r_pert = r_x @ (np.eye(3) + skew(delta))
|
||||||
|
# Orthonormalize lightly
|
||||||
|
u, _, vt = np.linalg.svd(r_pert)
|
||||||
|
r_pert = u @ vt
|
||||||
|
pert = so3_log(r_pert.T @ pair.R_A @ r_pert @ pair.R_B.T)
|
||||||
|
cols.append((pert - base) / eps)
|
||||||
|
rows.append(np.column_stack(cols))
|
||||||
|
return np.vstack(rows) if rows else np.zeros((0, 3))
|
||||||
|
|
||||||
|
|
||||||
|
def analyze_observability(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
r_x: np.ndarray,
|
||||||
|
*,
|
||||||
|
condition_threshold: float = 100.0,
|
||||||
|
) -> ObservabilityReport:
|
||||||
|
"""Gate whether rotation-only or full SE(3) should be accepted."""
|
||||||
|
|
||||||
|
usable = list(pairs)
|
||||||
|
notes: list[str] = []
|
||||||
|
if len(usable) < 3:
|
||||||
|
return ObservabilityReport(False, False, 1e9, 1e9, ("insufficient pairs",))
|
||||||
|
|
||||||
|
j_r = _rotation_jacobian(usable, np.asarray(r_x, dtype=float))
|
||||||
|
if j_r.size == 0:
|
||||||
|
return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
|
||||||
|
|
||||||
|
singular = np.linalg.svd(j_r, compute_uv=False)
|
||||||
|
cond_r = float(singular[0] / max(singular[-1], 1e-12))
|
||||||
|
rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1)))
|
||||||
|
rotation_ok = (
|
||||||
|
cond_r < condition_threshold
|
||||||
|
and rotation_information > 1e-3
|
||||||
|
and singular[-1] > 1e-6
|
||||||
|
)
|
||||||
|
|
||||||
|
# Translation lever arm is observable through stacked (R_A - I). Pure
|
||||||
|
# planar yaw leaves its vertical column in the nullspace and must fail.
|
||||||
|
translation_rows = [
|
||||||
|
np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
|
||||||
|
for pair in usable
|
||||||
|
if pair.t_B_m is not None
|
||||||
|
]
|
||||||
|
if translation_rows:
|
||||||
|
j_t = np.vstack(translation_rows)
|
||||||
|
singular_t = np.linalg.svd(j_t, compute_uv=False)
|
||||||
|
cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
|
||||||
|
translation_information = float(
|
||||||
|
singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
cond_t = 1e9
|
||||||
|
translation_information = 0.0
|
||||||
|
translation_ok = (
|
||||||
|
len(translation_rows) >= 5
|
||||||
|
and cond_t < condition_threshold
|
||||||
|
and translation_information > 0.02
|
||||||
|
)
|
||||||
|
|
||||||
|
if not rotation_ok:
|
||||||
|
notes.append(
|
||||||
|
f"rotation not observable: condition={cond_r:.1f}, "
|
||||||
|
f"min_information={rotation_information:.3e}"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append(
|
||||||
|
f"rotation observable: condition={cond_r:.1f}, "
|
||||||
|
f"min_information={rotation_information:.3e}"
|
||||||
|
)
|
||||||
|
if not translation_ok:
|
||||||
|
notes.append(
|
||||||
|
f"translation not observable: condition={cond_t:.1f}, "
|
||||||
|
f"min_information={translation_information:.3e}; "
|
||||||
|
"full SE3 will be rejected"
|
||||||
|
)
|
||||||
|
return ObservabilityReport(
|
||||||
|
rotation_observable=rotation_ok,
|
||||||
|
translation_observable=translation_ok,
|
||||||
|
condition_rotation=cond_r,
|
||||||
|
condition_translation=cond_t,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,290 @@
|
|||||||
|
"""Cached Phase-A replay: rehydrate gyro factors, compare variants, write reports."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
from collections import defaultdict
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .imu_io import load_imu_samples
|
||||||
|
from .motion_pairs_io import (
|
||||||
|
build_motion_pairs_payload,
|
||||||
|
load_motion_pairs,
|
||||||
|
pair_from_dict,
|
||||||
|
save_motion_pairs,
|
||||||
|
)
|
||||||
|
from .phase_a import (
|
||||||
|
ProgressCallback,
|
||||||
|
phase_a_comparison_to_dict,
|
||||||
|
phase_a_metadata_complete,
|
||||||
|
rehydrate_phase_a_pairs,
|
||||||
|
solve_phase_a_comparison,
|
||||||
|
)
|
||||||
|
from .vehicle_config import load_vehicle_config, prior_enabled
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_prior(
|
||||||
|
vehicle_config_path: Path,
|
||||||
|
) -> tuple[np.ndarray | None, float]:
|
||||||
|
config = load_vehicle_config(vehicle_config_path)
|
||||||
|
if not prior_enabled(config, "rotation_prior"):
|
||||||
|
return None, 15.0
|
||||||
|
prior = (config.get("initialization") or {}).get("rotation_prior") or {}
|
||||||
|
matrix = prior.get("R_IMU_lidar")
|
||||||
|
if matrix is None:
|
||||||
|
return None, float(prior.get("sigma_deg", 15.0))
|
||||||
|
return (
|
||||||
|
np.asarray(matrix, dtype=float).reshape(3, 3),
|
||||||
|
float(prior.get("sigma_deg", 15.0)),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _sanitize_json(value: Any) -> Any:
|
||||||
|
if isinstance(value, dict):
|
||||||
|
return {str(key): _sanitize_json(item) for key, item in value.items()}
|
||||||
|
if isinstance(value, (list, tuple)):
|
||||||
|
return [_sanitize_json(item) for item in value]
|
||||||
|
if isinstance(value, np.ndarray):
|
||||||
|
return _sanitize_json(value.tolist())
|
||||||
|
if isinstance(value, (np.floating, float)):
|
||||||
|
number = float(value)
|
||||||
|
return number if np.isfinite(number) else None
|
||||||
|
if isinstance(value, (np.integer, np.bool_)):
|
||||||
|
return value.item()
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def _write_json(path: Path, payload: Any) -> None:
|
||||||
|
path.write_text(
|
||||||
|
json.dumps(_sanitize_json(payload), indent=2, ensure_ascii=False) + "\n",
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _load_cached_sessions(
|
||||||
|
motion_pairs_path: Path,
|
||||||
|
) -> tuple[
|
||||||
|
dict[str, Any],
|
||||||
|
list,
|
||||||
|
dict[str, np.ndarray],
|
||||||
|
dict[str, float],
|
||||||
|
]:
|
||||||
|
payload = load_motion_pairs(motion_pairs_path)
|
||||||
|
pairs = []
|
||||||
|
biases: dict[str, np.ndarray] = {}
|
||||||
|
offsets: dict[str, float] = {}
|
||||||
|
for session in payload.get("sessions") or []:
|
||||||
|
session_id = str(session["session_id"])
|
||||||
|
biases[session_id] = np.asarray(
|
||||||
|
session.get("gyro_bias_rad_s", np.zeros(3)),
|
||||||
|
dtype=float,
|
||||||
|
).reshape(3)
|
||||||
|
offsets[session_id] = float(session.get("delta_t_s", 0.0))
|
||||||
|
pairs.extend(
|
||||||
|
pair_from_dict(item)
|
||||||
|
for item in session.get("pairs") or []
|
||||||
|
)
|
||||||
|
if not pairs:
|
||||||
|
raise ValueError(f"motion-pair cache is empty: {motion_pairs_path}")
|
||||||
|
return payload, pairs, biases, offsets
|
||||||
|
|
||||||
|
|
||||||
|
def run_phase_a_replay(
|
||||||
|
*,
|
||||||
|
motion_pairs_path: Path,
|
||||||
|
vehicle_config_path: Path,
|
||||||
|
output_directory: Path,
|
||||||
|
imu_paths_by_session: dict[str, Path] | None = None,
|
||||||
|
excluded_sessions: set[str] | None = None,
|
||||||
|
strong_rotation_min_deg: float = 1.0,
|
||||||
|
decorrelation_block_s: float = 3.0,
|
||||||
|
max_pairs_per_block: int = 1,
|
||||||
|
bias_prior_sigma_rad_s: float = 0.002,
|
||||||
|
yaw_std_max_deg: float = 0.5,
|
||||||
|
leave_one_out_yaw_range_max_deg: float = 1.0,
|
||||||
|
data_prior_difference_max_deg: float = 1.0,
|
||||||
|
max_nfev: int = 200,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Run Phase-A only. Existing LiDAR relative motions are never recomputed."""
|
||||||
|
|
||||||
|
output_directory.mkdir(parents=True, exist_ok=True)
|
||||||
|
source_payload, pairs, bias0, offsets = _load_cached_sessions(
|
||||||
|
motion_pairs_path
|
||||||
|
)
|
||||||
|
session_ids = sorted(bias0)
|
||||||
|
if progress_callback is not None:
|
||||||
|
progress_callback(
|
||||||
|
"cache_loaded",
|
||||||
|
{
|
||||||
|
"schema_version": source_payload.get("schema_version"),
|
||||||
|
"sessions": len(session_ids),
|
||||||
|
"pairs": len(pairs),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
rehydration_report: dict[str, Any] = {
|
||||||
|
"required": not phase_a_metadata_complete(pairs),
|
||||||
|
"pair_count": len(pairs),
|
||||||
|
}
|
||||||
|
if not phase_a_metadata_complete(pairs):
|
||||||
|
supplied_paths = {} if imu_paths_by_session is None else imu_paths_by_session
|
||||||
|
missing = [sid for sid in session_ids if sid not in supplied_paths]
|
||||||
|
if missing:
|
||||||
|
raise ValueError(
|
||||||
|
"v1 cache lacks J_bg/cov; provide --session-imu for: "
|
||||||
|
+ ", ".join(missing)
|
||||||
|
)
|
||||||
|
imu_by_session = {
|
||||||
|
sid: load_imu_samples(supplied_paths[sid])
|
||||||
|
for sid in session_ids
|
||||||
|
}
|
||||||
|
pairs, details = rehydrate_phase_a_pairs(
|
||||||
|
pairs,
|
||||||
|
imu_by_session=imu_by_session,
|
||||||
|
bias0_by_session=bias0,
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
)
|
||||||
|
rehydration_report.update(details)
|
||||||
|
if float(details["max_R_A_error_deg"]) > 0.05:
|
||||||
|
raise ValueError(
|
||||||
|
"rehydrated IMU rotations do not match cached R_A: "
|
||||||
|
f"max error={details['max_R_A_error_deg']:.6f} deg; "
|
||||||
|
"check session-to-IMU path mapping"
|
||||||
|
)
|
||||||
|
|
||||||
|
grouped: dict[str, list] = defaultdict(list)
|
||||||
|
for pair in pairs:
|
||||||
|
grouped[pair.session_id].append(pair)
|
||||||
|
enriched_payload = build_motion_pairs_payload(
|
||||||
|
prepared_sessions=[
|
||||||
|
{
|
||||||
|
"session_id": sid,
|
||||||
|
"time_offset_s": offsets[sid],
|
||||||
|
"gyro_bias_rad_s": bias0[sid],
|
||||||
|
"pairs": tuple(grouped[sid]),
|
||||||
|
}
|
||||||
|
for sid in session_ids
|
||||||
|
]
|
||||||
|
)
|
||||||
|
enriched_cache_path = save_motion_pairs(
|
||||||
|
output_directory / "motion_pairs_phase_a_v2.json",
|
||||||
|
enriched_payload,
|
||||||
|
)
|
||||||
|
|
||||||
|
rotation_prior, rotation_prior_sigma_deg = _rotation_prior(
|
||||||
|
vehicle_config_path
|
||||||
|
)
|
||||||
|
comparison = solve_phase_a_comparison(
|
||||||
|
pairs,
|
||||||
|
gyro_bias_rad_s_by_session=bias0,
|
||||||
|
rotation_prior=rotation_prior,
|
||||||
|
rotation_prior_sigma_deg=rotation_prior_sigma_deg,
|
||||||
|
preexcluded_session_ids=excluded_sessions,
|
||||||
|
strong_rotation_min_deg=strong_rotation_min_deg,
|
||||||
|
decorrelation_block_s=decorrelation_block_s,
|
||||||
|
max_pairs_per_block=max_pairs_per_block,
|
||||||
|
bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
|
||||||
|
yaw_std_max_deg=yaw_std_max_deg,
|
||||||
|
leave_one_out_yaw_range_max_deg=(
|
||||||
|
leave_one_out_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
data_prior_difference_max_deg=data_prior_difference_max_deg,
|
||||||
|
run_leave_one_out=True,
|
||||||
|
max_nfev=max_nfev,
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
)
|
||||||
|
full = phase_a_comparison_to_dict(comparison)
|
||||||
|
full["input"] = {
|
||||||
|
"motion_pairs": str(motion_pairs_path),
|
||||||
|
"source_schema_version": source_payload.get("schema_version"),
|
||||||
|
"vehicle_config": str(vehicle_config_path),
|
||||||
|
"session_imu_paths": {
|
||||||
|
sid: str(path)
|
||||||
|
for sid, path in (imu_paths_by_session or {}).items()
|
||||||
|
},
|
||||||
|
"excluded_sessions": sorted(excluded_sessions or set()),
|
||||||
|
}
|
||||||
|
full["rehydration"] = rehydration_report
|
||||||
|
full["enriched_cache"] = str(enriched_cache_path)
|
||||||
|
full["parameters"] = {
|
||||||
|
"strong_rotation_min_deg": strong_rotation_min_deg,
|
||||||
|
"decorrelation_block_s": decorrelation_block_s,
|
||||||
|
"max_pairs_per_block": max_pairs_per_block,
|
||||||
|
"bias_prior_sigma_rad_s": bias_prior_sigma_rad_s,
|
||||||
|
"rotation_prior_sigma_deg": rotation_prior_sigma_deg,
|
||||||
|
"yaw_std_max_deg": yaw_std_max_deg,
|
||||||
|
"leave_one_out_yaw_range_max_deg": (
|
||||||
|
leave_one_out_yaw_range_max_deg
|
||||||
|
),
|
||||||
|
"data_prior_difference_max_deg": (
|
||||||
|
data_prior_difference_max_deg
|
||||||
|
),
|
||||||
|
"max_nfev": max_nfev,
|
||||||
|
}
|
||||||
|
|
||||||
|
variants = full["variants"]
|
||||||
|
summary = {
|
||||||
|
"status": comparison.solution_status,
|
||||||
|
"accepted": comparison.accepted,
|
||||||
|
"partial_accepted": comparison.partial_accepted,
|
||||||
|
"acceptance_checks": comparison.acceptance_checks,
|
||||||
|
"primary_result": comparison.recommended_result,
|
||||||
|
"variants": {
|
||||||
|
name: {
|
||||||
|
"rpy_deg_xyz": item["rpy_deg_xyz"],
|
||||||
|
"R_IMU_lidar": item["R_IMU_lidar"],
|
||||||
|
"residual_rms_deg": item["residual_rms_deg"],
|
||||||
|
"residual_p95_deg": item["residual_p95_deg"],
|
||||||
|
"accepted": item["accepted"],
|
||||||
|
"gyro_bias_rad_s_per_session": item[
|
||||||
|
"gyro_bias_rad_s_per_session"
|
||||||
|
],
|
||||||
|
}
|
||||||
|
for name, item in variants.items()
|
||||||
|
if item is not None
|
||||||
|
},
|
||||||
|
"marginal_observability_A1": full[
|
||||||
|
"marginal_observability_A1"
|
||||||
|
],
|
||||||
|
"data_vs_prior_yaw_diff_deg": (
|
||||||
|
comparison.data_vs_prior_yaw_diff_deg
|
||||||
|
),
|
||||||
|
"data_vs_prior_geodesic_deg": (
|
||||||
|
comparison.data_vs_prior_geodesic_deg
|
||||||
|
),
|
||||||
|
"leave_one_out_yaw_range_deg": (
|
||||||
|
comparison.leave_one_out_yaw_range_deg
|
||||||
|
),
|
||||||
|
"leave_one_out_observable_max_deg": (
|
||||||
|
comparison.leave_one_out_observable_max_deg
|
||||||
|
),
|
||||||
|
"strong_pair_candidate_count": (
|
||||||
|
comparison.strong_pair_candidate_count
|
||||||
|
),
|
||||||
|
"decorrelated_pair_count": comparison.decorrelated_pair_count,
|
||||||
|
"strong_pair_counts_per_session": (
|
||||||
|
comparison.strong_pair_counts_per_session
|
||||||
|
),
|
||||||
|
"excluded_sessions": list(comparison.excluded_sessions),
|
||||||
|
"rehydration": rehydration_report,
|
||||||
|
"comparison_file": "phase_a_comparison.json",
|
||||||
|
"observability_file": "phase_a_observability.json",
|
||||||
|
"leave_one_out_file": "phase_a_leave_one_out.json",
|
||||||
|
"enriched_cache_file": enriched_cache_path.name,
|
||||||
|
}
|
||||||
|
|
||||||
|
_write_json(output_directory / "phase_a_comparison.json", full)
|
||||||
|
_write_json(
|
||||||
|
output_directory / "phase_a_observability.json",
|
||||||
|
full["marginal_observability_A1"],
|
||||||
|
)
|
||||||
|
_write_json(
|
||||||
|
output_directory / "phase_a_leave_one_out.json",
|
||||||
|
full["leave_one_out"],
|
||||||
|
)
|
||||||
|
_write_json(output_directory / "phase_a_summary.json", summary)
|
||||||
|
return summary
|
||||||
@@ -0,0 +1,941 @@
|
|||||||
|
"""Executable LiDAR–IMU calibration pipeline (V1)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Callable
|
||||||
|
from dataclasses import asdict, dataclass, replace
|
||||||
|
from pathlib import Path
|
||||||
|
from time import perf_counter
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import (
|
||||||
|
CalibrationMode,
|
||||||
|
CalibrationRequest,
|
||||||
|
CalibrationResult,
|
||||||
|
CalibrationStatus,
|
||||||
|
MotionPair,
|
||||||
|
SessionInput,
|
||||||
|
)
|
||||||
|
from .finalize import finalize_result
|
||||||
|
from .imu_audit import audit_imu
|
||||||
|
from .imu_io import load_imu_samples
|
||||||
|
from .joint_optimizer import solve_joint_extrinsic
|
||||||
|
from .keyframes import build_keyframes
|
||||||
|
from .lidar_deskew import deskew_lidar_frames
|
||||||
|
from .lidar_io import load_lidar_frames
|
||||||
|
from .motion_pairs import build_motion_pairs
|
||||||
|
from .motion_pairs_io import build_motion_pairs_payload
|
||||||
|
from .rotation_handeye import solve_rotation_handeye
|
||||||
|
from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
|
||||||
|
from .timestamp_audit import audit_timestamps
|
||||||
|
from .vehicle_config import load_vehicle_config, prior_enabled
|
||||||
|
|
||||||
|
# Remap keyframe indices so multi-session Phase-C graphs do not collide.
|
||||||
|
_SESSION_INDEX_OFFSET = 1_000_000
|
||||||
|
|
||||||
|
|
||||||
|
def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult:
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=refined.delta_t_s,
|
||||||
|
correlation_peak=refined.correlation_peak,
|
||||||
|
search_s=previous.search_s,
|
||||||
|
notes=tuple(list(previous.notes) + list(refined.notes)),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class PipelineStage:
|
||||||
|
name: str
|
||||||
|
responsibility: str
|
||||||
|
|
||||||
|
|
||||||
|
STAGES = (
|
||||||
|
PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"),
|
||||||
|
PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"),
|
||||||
|
PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"),
|
||||||
|
PipelineStage("time_offset", "各会话独立粗估/精修 δt"),
|
||||||
|
PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"),
|
||||||
|
PipelineStage("motion_pairs", "各会话构造运动对，再合并"),
|
||||||
|
PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"),
|
||||||
|
PipelineStage("joint_optimizer", "Phase-A 会话级零偏联合精修；Phase-B/C 暂时门控"),
|
||||||
|
PipelineStage("finalize", "写出结果与质量报告"),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
ProgressCallback = Callable[[dict[str, Any]], None]
|
||||||
|
|
||||||
|
|
||||||
|
def _emit_progress(
|
||||||
|
callback: ProgressCallback | None,
|
||||||
|
stage_index: int,
|
||||||
|
event: str,
|
||||||
|
**fields: Any,
|
||||||
|
) -> None:
|
||||||
|
if callback is None:
|
||||||
|
return
|
||||||
|
callback(
|
||||||
|
{
|
||||||
|
"stage_index": stage_index,
|
||||||
|
"stage_total": len(STAGES),
|
||||||
|
"stage": STAGES[stage_index - 1].name,
|
||||||
|
"event": event,
|
||||||
|
**fields,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def describe_pipeline(_: CalibrationRequest) -> tuple[PipelineStage, ...]:
|
||||||
|
"""Return the planned stages."""
|
||||||
|
|
||||||
|
return STAGES
|
||||||
|
|
||||||
|
|
||||||
|
def _build_pairs_and_handeye(
|
||||||
|
*,
|
||||||
|
session_id: str,
|
||||||
|
working_frames,
|
||||||
|
imu,
|
||||||
|
delta_t_s: float,
|
||||||
|
gyro_bias_rad_s: np.ndarray,
|
||||||
|
request: CalibrationRequest,
|
||||||
|
R_prior: np.ndarray | None = None,
|
||||||
|
prior_sigma_deg: float | None = None,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
):
|
||||||
|
keyframes = build_keyframes(
|
||||||
|
working_frames,
|
||||||
|
min_translation_m=request.min_pair_translation_m,
|
||||||
|
min_rotation_deg=request.min_pair_rotation_deg,
|
||||||
|
min_registration_fitness=request.min_registration_fitness,
|
||||||
|
)
|
||||||
|
if progress_callback is not None:
|
||||||
|
progress_callback(
|
||||||
|
{
|
||||||
|
"event": "keyframes_ready",
|
||||||
|
"keyframe_count": len(keyframes.indices),
|
||||||
|
"lidar_frame_count": len(working_frames),
|
||||||
|
}
|
||||||
|
)
|
||||||
|
pair_set = build_motion_pairs(
|
||||||
|
session_id=session_id,
|
||||||
|
keyframes=list(keyframes.frames),
|
||||||
|
keyframe_indices=keyframes.indices,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=delta_t_s,
|
||||||
|
gyro_bias_rad_s=gyro_bias_rad_s,
|
||||||
|
min_rotation_deg=request.min_pair_rotation_deg,
|
||||||
|
min_translation_m=request.min_pair_translation_m,
|
||||||
|
min_registration_fitness=request.min_registration_fitness,
|
||||||
|
max_imu_gap_s=request.max_imu_gap_s,
|
||||||
|
max_lidar_gap_s=request.max_lidar_gap_s,
|
||||||
|
all_frame_times_s=np.asarray([frame.t_mid_s for frame in working_frames], dtype=float),
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
)
|
||||||
|
handeye = solve_rotation_handeye(
|
||||||
|
pair_set.pairs,
|
||||||
|
R_prior=R_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
)
|
||||||
|
return keyframes, pair_set, handeye
|
||||||
|
|
||||||
|
|
||||||
|
def _translation_prior_from_config(
|
||||||
|
vehicle_config: dict[str, Any] | None,
|
||||||
|
) -> tuple[np.ndarray | None, np.ndarray | float | None]:
|
||||||
|
if vehicle_config is None or not prior_enabled(vehicle_config, "translation_prior"):
|
||||||
|
return None, None
|
||||||
|
init_cfg = vehicle_config.get("initialization") or {}
|
||||||
|
tp = init_cfg.get("translation_prior") or {}
|
||||||
|
if tp.get("t_IMU_lidar_m") is None:
|
||||||
|
return None, None
|
||||||
|
return np.asarray(tp["t_IMU_lidar_m"], dtype=float).reshape(3), tp.get("sigma_m", [0.05, 0.05, 0.05])
|
||||||
|
|
||||||
|
|
||||||
|
def _rotation_prior_from_config(
|
||||||
|
vehicle_config: dict[str, Any] | None,
|
||||||
|
) -> tuple[np.ndarray | None, float | None]:
|
||||||
|
if vehicle_config is None or not prior_enabled(vehicle_config, "rotation_prior"):
|
||||||
|
return None, None
|
||||||
|
init_cfg = vehicle_config.get("initialization") or {}
|
||||||
|
rp = init_cfg.get("rotation_prior") or {}
|
||||||
|
if rp.get("R_IMU_lidar") is None:
|
||||||
|
return None, None
|
||||||
|
return np.asarray(rp["R_IMU_lidar"], dtype=float).reshape(3, 3), float(rp.get("sigma_deg", 15.0))
|
||||||
|
|
||||||
|
|
||||||
|
def _prepare_session_pairs(
|
||||||
|
session: SessionInput,
|
||||||
|
request: CalibrationRequest,
|
||||||
|
*,
|
||||||
|
R_prior: np.ndarray | None = None,
|
||||||
|
prior_sigma_deg: float | None = None,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
session_index: int = 1,
|
||||||
|
session_total: int = 1,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet."""
|
||||||
|
|
||||||
|
started_at = perf_counter()
|
||||||
|
|
||||||
|
def emit(stage_index: int, event: str, **fields: Any) -> None:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
stage_index,
|
||||||
|
event,
|
||||||
|
session=session.session_id,
|
||||||
|
session_index=session_index,
|
||||||
|
session_total=session_total,
|
||||||
|
**fields,
|
||||||
|
)
|
||||||
|
|
||||||
|
emit(
|
||||||
|
2,
|
||||||
|
"session_start",
|
||||||
|
imu_source=str(session.imu_source),
|
||||||
|
lidar_source=str(session.lidar_source),
|
||||||
|
)
|
||||||
|
imu = load_imu_samples(session.imu_source)
|
||||||
|
frames = load_lidar_frames(session.lidar_source)
|
||||||
|
emit(
|
||||||
|
2,
|
||||||
|
"data_loaded",
|
||||||
|
imu_samples=int(imu.t_s.size),
|
||||||
|
lidar_frames=len(frames),
|
||||||
|
imu_span_s=float(imu.t_s[-1] - imu.t_s[0]) if imu.t_s.size >= 2 else 0.0,
|
||||||
|
lidar_span_s=(
|
||||||
|
float(frames[-1].t_mid_s - frames[0].t_mid_s) if len(frames) >= 2 else 0.0
|
||||||
|
),
|
||||||
|
elapsed_s=perf_counter() - started_at,
|
||||||
|
)
|
||||||
|
|
||||||
|
ts = audit_timestamps(imu, frames)
|
||||||
|
emit(2, "audit_complete", ok=ts.ok)
|
||||||
|
if not ts.ok:
|
||||||
|
emit(2, "blocked", reason="timestamp_audit")
|
||||||
|
return {"ok": False, "stage": "timestamp_audit", "session_id": session.session_id, "report": asdict(ts)}
|
||||||
|
|
||||||
|
imu_report = audit_imu(imu)
|
||||||
|
emit(
|
||||||
|
3,
|
||||||
|
"audit_complete",
|
||||||
|
ok=imu_report.ok,
|
||||||
|
gyro_bias_norm_rad_s=float(np.linalg.norm(imu_report.gyro_bias_rad_s)),
|
||||||
|
)
|
||||||
|
if not imu_report.ok:
|
||||||
|
emit(3, "blocked", reason="imu_audit")
|
||||||
|
return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)}
|
||||||
|
|
||||||
|
fixed_time_offset_s = (
|
||||||
|
session.fixed_time_offset_s
|
||||||
|
if session.fixed_time_offset_s is not None
|
||||||
|
else request.fixed_time_offset_s
|
||||||
|
)
|
||||||
|
if fixed_time_offset_s is not None:
|
||||||
|
offset_source = "fixed"
|
||||||
|
offset = TimeOffsetResult(
|
||||||
|
delta_t_s=float(fixed_time_offset_s),
|
||||||
|
correlation_peak=1.0,
|
||||||
|
search_s=0.0,
|
||||||
|
notes=(
|
||||||
|
f"fixed_time_offset_s={float(fixed_time_offset_s):.6f} "
|
||||||
|
"(skip |ω| search; intended for host-UTC-bridged sessions)",
|
||||||
|
),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
offset_source = "estimated"
|
||||||
|
offset = estimate_time_offset(
|
||||||
|
imu,
|
||||||
|
frames,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
search_s=request.time_offset_search_s,
|
||||||
|
)
|
||||||
|
if not offset.ok:
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"blocked",
|
||||||
|
reason="time_offset",
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
correlation_peak=float(offset.correlation_peak),
|
||||||
|
)
|
||||||
|
return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}
|
||||||
|
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"offset_ready",
|
||||||
|
source=offset_source,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
correlation_peak=float(offset.correlation_peak),
|
||||||
|
)
|
||||||
|
coarse_delta_t = float(offset.delta_t_s)
|
||||||
|
working_frames = frames
|
||||||
|
r_x = np.eye(3) if R_prior is None else np.asarray(R_prior, dtype=float).reshape(3, 3)
|
||||||
|
handeye = None
|
||||||
|
pair_set = None
|
||||||
|
keyframes = None
|
||||||
|
pairs_notes: list[str] = []
|
||||||
|
pair_count = 0
|
||||||
|
|
||||||
|
iterations_total = max(1, request.max_iterations)
|
||||||
|
build_pass = "outer"
|
||||||
|
|
||||||
|
def on_build_progress(payload: dict[str, Any]) -> None:
|
||||||
|
event = str(payload.get("event", "running"))
|
||||||
|
stage_index = 5 if event == "keyframes_ready" else 6
|
||||||
|
fields = {key: value for key, value in payload.items() if key != "event"}
|
||||||
|
emit(
|
||||||
|
stage_index,
|
||||||
|
event,
|
||||||
|
iteration=iteration + 1,
|
||||||
|
iterations_total=iterations_total,
|
||||||
|
build_pass=build_pass,
|
||||||
|
**fields,
|
||||||
|
)
|
||||||
|
|
||||||
|
for iteration in range(iterations_total):
|
||||||
|
build_pass = "outer"
|
||||||
|
emit(
|
||||||
|
5,
|
||||||
|
"iteration_start",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
iterations_total=iterations_total,
|
||||||
|
deskew=iteration > 0,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
)
|
||||||
|
if iteration > 0:
|
||||||
|
deskew_started_at = perf_counter()
|
||||||
|
emit(5, "deskew_start", iteration=iteration + 1)
|
||||||
|
working_frames = deskew_lidar_frames(
|
||||||
|
frames,
|
||||||
|
imu,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
R_IMU_lidar=r_x,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
)
|
||||||
|
emit(
|
||||||
|
5,
|
||||||
|
"deskew_complete",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
lidar_frames=len(working_frames),
|
||||||
|
elapsed_s=perf_counter() - deskew_started_at,
|
||||||
|
)
|
||||||
|
keyframes, pair_set, handeye = _build_pairs_and_handeye(
|
||||||
|
session_id=session.session_id,
|
||||||
|
working_frames=working_frames,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
request=request,
|
||||||
|
R_prior=R_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
progress_callback=on_build_progress,
|
||||||
|
)
|
||||||
|
pairs_notes = list(pair_set.notes)
|
||||||
|
pair_count = len(pair_set.pairs)
|
||||||
|
emit(
|
||||||
|
7,
|
||||||
|
"local_handeye",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
build_pass=build_pass,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
rms_deg=float(handeye.residual_rms_deg),
|
||||||
|
p95_deg=float(handeye.residual_p95_deg),
|
||||||
|
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
|
||||||
|
ok=handeye.ok,
|
||||||
|
)
|
||||||
|
if pair_count < 3:
|
||||||
|
emit(
|
||||||
|
6,
|
||||||
|
"blocked",
|
||||||
|
reason="insufficient_motion_pairs",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"ok": False,
|
||||||
|
"stage": "motion_pairs",
|
||||||
|
"session_id": session.session_id,
|
||||||
|
"iteration": iteration,
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"imu_audit": asdict(imu_report),
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"keyframes": 0 if keyframes is None else len(keyframes.indices),
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": asdict(handeye),
|
||||||
|
}
|
||||||
|
r_x = handeye.R_IMU_lidar
|
||||||
|
|
||||||
|
if not request.enable_signed_time_refine:
|
||||||
|
continue
|
||||||
|
|
||||||
|
for refine_step in range(1, 3):
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"signed_refine_start",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
refine_step=refine_step,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
)
|
||||||
|
refined = refine_time_offset_signed(
|
||||||
|
imu,
|
||||||
|
frames,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
R_IMU_lidar=r_x,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
|
||||||
|
max_shift_s=request.max_signed_refine_shift_s,
|
||||||
|
)
|
||||||
|
# Also bound total walk away from the original coarse estimate.
|
||||||
|
if abs(refined.delta_t_s - coarse_delta_t) > request.max_signed_refine_shift_s:
|
||||||
|
refined = TimeOffsetResult(
|
||||||
|
delta_t_s=float(offset.delta_t_s),
|
||||||
|
correlation_peak=refined.correlation_peak,
|
||||||
|
search_s=refined.search_s,
|
||||||
|
notes=tuple(
|
||||||
|
list(refined.notes)
|
||||||
|
+ [
|
||||||
|
f"signed refine clamped: |δt-coarse| would exceed "
|
||||||
|
f"{request.max_signed_refine_shift_s:.3f}s"
|
||||||
|
]
|
||||||
|
),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
|
||||||
|
offset = _merge_time_offset(offset, refined)
|
||||||
|
emit(
|
||||||
|
4,
|
||||||
|
"signed_refine_complete",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
refine_step=refine_step,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
shift_s=float(delta_shift),
|
||||||
|
correlation_peak=float(refined.correlation_peak),
|
||||||
|
)
|
||||||
|
if delta_shift < 1e-3:
|
||||||
|
break
|
||||||
|
build_pass = f"signed_refine_{refine_step}"
|
||||||
|
keyframes, pair_set, handeye = _build_pairs_and_handeye(
|
||||||
|
session_id=session.session_id,
|
||||||
|
working_frames=working_frames,
|
||||||
|
imu=imu,
|
||||||
|
delta_t_s=offset.delta_t_s,
|
||||||
|
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||||
|
request=request,
|
||||||
|
R_prior=R_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
progress_callback=on_build_progress,
|
||||||
|
)
|
||||||
|
pairs_notes = list(pair_set.notes)
|
||||||
|
pair_count = len(pair_set.pairs)
|
||||||
|
emit(
|
||||||
|
7,
|
||||||
|
"local_handeye",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
build_pass=build_pass,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
rms_deg=float(handeye.residual_rms_deg),
|
||||||
|
p95_deg=float(handeye.residual_p95_deg),
|
||||||
|
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
|
||||||
|
ok=handeye.ok,
|
||||||
|
)
|
||||||
|
if pair_count < 3:
|
||||||
|
emit(
|
||||||
|
6,
|
||||||
|
"blocked",
|
||||||
|
reason="insufficient_motion_pairs_after_signed_refine",
|
||||||
|
iteration=iteration + 1,
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"ok": False,
|
||||||
|
"stage": "motion_pairs",
|
||||||
|
"session_id": session.session_id,
|
||||||
|
"iteration": iteration,
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"imu_audit": asdict(imu_report),
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"keyframes": 0 if keyframes is None else len(keyframes.indices),
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye": asdict(handeye),
|
||||||
|
}
|
||||||
|
r_x = handeye.R_IMU_lidar
|
||||||
|
assert handeye is not None and pair_set is not None and keyframes is not None
|
||||||
|
acc_mean = np.asarray(imu_report.static_acc_mean_m_s2, dtype=float).reshape(3)
|
||||||
|
acc_n = float(np.linalg.norm(acc_mean))
|
||||||
|
if acc_n > 1e-6:
|
||||||
|
gravity_init = -acc_mean * (9.80665 / acc_n)
|
||||||
|
else:
|
||||||
|
gravity_init = np.array([0.0, 0.0, -9.80665])
|
||||||
|
|
||||||
|
emit(
|
||||||
|
7,
|
||||||
|
"session_complete",
|
||||||
|
keyframes=len(keyframes.indices),
|
||||||
|
pair_count=pair_count,
|
||||||
|
time_offset_s=float(offset.delta_t_s),
|
||||||
|
local_handeye_ok=handeye.ok,
|
||||||
|
elapsed_s=perf_counter() - started_at,
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"ok": True,
|
||||||
|
"session_id": session.session_id,
|
||||||
|
"pairs": tuple(pair_set.pairs),
|
||||||
|
"gyro_bias_rad_s": np.asarray(imu_report.gyro_bias_rad_s, dtype=float).reshape(3),
|
||||||
|
"gravity_init_m_s2": gravity_init,
|
||||||
|
"timestamp_audit": asdict(ts),
|
||||||
|
"imu_audit": {
|
||||||
|
**asdict(imu_report),
|
||||||
|
"gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(),
|
||||||
|
"static_acc_mean_m_s2": imu_report.static_acc_mean_m_s2.tolist(),
|
||||||
|
},
|
||||||
|
"time_offset": asdict(offset),
|
||||||
|
"time_offset_s": float(offset.delta_t_s),
|
||||||
|
"keyframes": len(keyframes.indices),
|
||||||
|
"pair_count": pair_count,
|
||||||
|
"pair_notes": pairs_notes,
|
||||||
|
"handeye_local": {
|
||||||
|
"residual_rms_deg": handeye.residual_rms_deg,
|
||||||
|
"residual_median_deg": handeye.residual_median_deg,
|
||||||
|
"residual_p95_deg": handeye.residual_p95_deg,
|
||||||
|
"outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
|
||||||
|
"pair_count": handeye.pair_count,
|
||||||
|
"ok": handeye.ok,
|
||||||
|
"notes": handeye.notes,
|
||||||
|
"R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]:
|
||||||
|
merged: list[MotionPair] = []
|
||||||
|
for index, prep in enumerate(prepared):
|
||||||
|
id_offset = (index + 1) * _SESSION_INDEX_OFFSET
|
||||||
|
for pair in prep["pairs"]:
|
||||||
|
merged.append(
|
||||||
|
replace(
|
||||||
|
pair,
|
||||||
|
i=int(pair.i) + id_offset,
|
||||||
|
j=int(pair.j) + id_offset,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return merged
|
||||||
|
|
||||||
|
|
||||||
|
def run_calibration(
|
||||||
|
request: CalibrationRequest,
|
||||||
|
*,
|
||||||
|
progress_callback: ProgressCallback | None = None,
|
||||||
|
) -> CalibrationResult:
|
||||||
|
"""Run the V1 calibration pipeline for one or more sessions.
|
||||||
|
|
||||||
|
Multi-session: each session estimates its own δt and builds motion pairs;
|
||||||
|
rotation hand-eye and joint SE3 are solved once on the merged pair set.
|
||||||
|
"""
|
||||||
|
|
||||||
|
overall_started_at = perf_counter()
|
||||||
|
|
||||||
|
def finish(
|
||||||
|
*,
|
||||||
|
status: CalibrationStatus,
|
||||||
|
message: str,
|
||||||
|
details: dict[str, Any],
|
||||||
|
T_IMU_lidar: np.ndarray | None = None,
|
||||||
|
time_offset_s: float | None = None,
|
||||||
|
motion_pairs_payload: dict[str, Any] | None = None,
|
||||||
|
) -> CalibrationResult:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
9,
|
||||||
|
"writing_result",
|
||||||
|
status=status.value,
|
||||||
|
output_directory=str(request.output_directory),
|
||||||
|
)
|
||||||
|
result = finalize_result(
|
||||||
|
status=status,
|
||||||
|
message=message,
|
||||||
|
details=details,
|
||||||
|
T_IMU_lidar=T_IMU_lidar,
|
||||||
|
time_offset_s=time_offset_s,
|
||||||
|
output_directory=request.output_directory,
|
||||||
|
motion_pairs_payload=motion_pairs_payload,
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
9,
|
||||||
|
"complete",
|
||||||
|
status=result.status.value,
|
||||||
|
elapsed_s=perf_counter() - overall_started_at,
|
||||||
|
)
|
||||||
|
return result
|
||||||
|
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"pipeline_start",
|
||||||
|
mode=request.requested_mode.value,
|
||||||
|
session_count=len(request.sessions),
|
||||||
|
max_iterations=max(1, request.max_iterations),
|
||||||
|
output_directory=str(request.output_directory),
|
||||||
|
)
|
||||||
|
if not request.sessions:
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message="no sessions provided",
|
||||||
|
details={},
|
||||||
|
)
|
||||||
|
|
||||||
|
vehicle_config = None
|
||||||
|
if request.vehicle_config is not None:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"loading_vehicle_config",
|
||||||
|
path=str(request.vehicle_config),
|
||||||
|
)
|
||||||
|
try:
|
||||||
|
vehicle_config = load_vehicle_config(request.vehicle_config)
|
||||||
|
except Exception as exc: # noqa: BLE001 - surface config problems as blocked
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"blocked",
|
||||||
|
reason="vehicle_config",
|
||||||
|
error=str(exc),
|
||||||
|
)
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=f"vehicle config failed: {exc}",
|
||||||
|
details={},
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
1,
|
||||||
|
"vehicle_config_ready",
|
||||||
|
loaded=vehicle_config is not None,
|
||||||
|
)
|
||||||
|
|
||||||
|
r_prior, prior_sigma_deg = _rotation_prior_from_config(vehicle_config)
|
||||||
|
|
||||||
|
prepared: list[dict[str, Any]] = []
|
||||||
|
session_total = len(request.sessions)
|
||||||
|
for session_index, session in enumerate(request.sessions, start=1):
|
||||||
|
prep = _prepare_session_pairs(
|
||||||
|
session,
|
||||||
|
request,
|
||||||
|
R_prior=r_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
progress_callback=progress_callback,
|
||||||
|
session_index=session_index,
|
||||||
|
session_total=session_total,
|
||||||
|
)
|
||||||
|
if not prep.get("ok"):
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})",
|
||||||
|
details={"sessions": [prep]},
|
||||||
|
)
|
||||||
|
prepared.append(prep)
|
||||||
|
|
||||||
|
all_pairs = _remap_pairs_for_joint(prepared)
|
||||||
|
pair_counts_per_session = {
|
||||||
|
p["session_id"]: int(p["pair_count"]) for p in prepared
|
||||||
|
}
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
7,
|
||||||
|
"joint_handeye_start",
|
||||||
|
session_count=len(prepared),
|
||||||
|
merged_pair_count=len(all_pairs),
|
||||||
|
pair_counts_per_session=pair_counts_per_session,
|
||||||
|
)
|
||||||
|
handeye_started_at = perf_counter()
|
||||||
|
handeye = solve_rotation_handeye(
|
||||||
|
all_pairs,
|
||||||
|
R_prior=r_prior,
|
||||||
|
prior_sigma_deg=prior_sigma_deg,
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
7,
|
||||||
|
"joint_handeye_complete",
|
||||||
|
pair_count=handeye.pair_count,
|
||||||
|
rms_deg=float(handeye.residual_rms_deg),
|
||||||
|
p95_deg=float(handeye.residual_p95_deg),
|
||||||
|
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
|
||||||
|
ok=handeye.ok,
|
||||||
|
elapsed_s=perf_counter() - handeye_started_at,
|
||||||
|
)
|
||||||
|
if handeye.pair_count < 3:
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message="blocked at stage rotation_handeye (joint)",
|
||||||
|
details={
|
||||||
|
"sessions": [_public_session(p) for p in prepared],
|
||||||
|
"joint_handeye": asdict(handeye),
|
||||||
|
"merged_pair_count": len(all_pairs),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
|
||||||
|
t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
|
||||||
|
gyro_bias_by_session = {
|
||||||
|
p["session_id"]: np.asarray(p["gyro_bias_rad_s"], dtype=float) for p in prepared
|
||||||
|
}
|
||||||
|
time_offset_by_session = {
|
||||||
|
p["session_id"]: float(p["time_offset_s"]) for p in prepared
|
||||||
|
}
|
||||||
|
|
||||||
|
preexcluded_session_ids = {
|
||||||
|
p["session_id"] for p in prepared if not p["handeye_local"]["ok"]
|
||||||
|
}
|
||||||
|
if len(preexcluded_session_ids) == len(prepared):
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_complete",
|
||||||
|
accepted=False,
|
||||||
|
reason="all_sessions_failed_local_handeye_gate",
|
||||||
|
excluded_sessions=sorted(preexcluded_session_ids),
|
||||||
|
)
|
||||||
|
return finish(
|
||||||
|
status=CalibrationStatus.BLOCKED,
|
||||||
|
message=(
|
||||||
|
"Phase-A blocked: all sessions failed the local "
|
||||||
|
"rotation residual gate"
|
||||||
|
),
|
||||||
|
details={
|
||||||
|
"sessions": [_public_session(p) for p in prepared],
|
||||||
|
"joint_handeye": asdict(handeye),
|
||||||
|
"merged_pair_count": len(all_pairs),
|
||||||
|
"excluded_sessions": sorted(
|
||||||
|
preexcluded_session_ids
|
||||||
|
),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_start",
|
||||||
|
session_count=len(prepared),
|
||||||
|
merged_pair_count=len(all_pairs),
|
||||||
|
preexcluded_sessions=sorted(preexcluded_session_ids),
|
||||||
|
)
|
||||||
|
phase_a_started_at = perf_counter()
|
||||||
|
|
||||||
|
def on_phase_a_progress(
|
||||||
|
event: str,
|
||||||
|
fields: dict[str, Any],
|
||||||
|
) -> None:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
event,
|
||||||
|
**fields,
|
||||||
|
)
|
||||||
|
|
||||||
|
joint = solve_joint_extrinsic(
|
||||||
|
all_pairs,
|
||||||
|
handeye.R_IMU_lidar,
|
||||||
|
force_rotation_only=force_rotation_only,
|
||||||
|
imu=None,
|
||||||
|
gyro_bias_rad_s_by_session=gyro_bias_by_session,
|
||||||
|
time_offset_s_by_session=time_offset_by_session,
|
||||||
|
preexcluded_session_ids=preexcluded_session_ids,
|
||||||
|
rotation_prior=r_prior,
|
||||||
|
rotation_prior_sigma_deg=(
|
||||||
|
15.0 if prior_sigma_deg is None else prior_sigma_deg
|
||||||
|
),
|
||||||
|
phase_a_progress_callback=on_phase_a_progress,
|
||||||
|
enable_phase_c=not force_rotation_only,
|
||||||
|
t_init_m=t_prior,
|
||||||
|
t_prior_m=t_prior,
|
||||||
|
t_prior_sigma_m=t_prior_sigma,
|
||||||
|
)
|
||||||
|
included_sessions = [
|
||||||
|
item.session_id for item in joint.phase_a_sessions if item.included_in_final
|
||||||
|
]
|
||||||
|
excluded_sessions = [
|
||||||
|
item.session_id for item in joint.phase_a_sessions if not item.included_in_final
|
||||||
|
]
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_complete",
|
||||||
|
accepted=joint.phase_a_accepted,
|
||||||
|
joint_rms_deg=float(joint.residual_rms_rot_deg),
|
||||||
|
rotation_observable=joint.observability.rotation_observable,
|
||||||
|
included_sessions=included_sessions,
|
||||||
|
excluded_sessions=excluded_sessions,
|
||||||
|
elapsed_s=perf_counter() - phase_a_started_at,
|
||||||
|
)
|
||||||
|
for item in joint.phase_a_sessions:
|
||||||
|
_emit_progress(
|
||||||
|
progress_callback,
|
||||||
|
8,
|
||||||
|
"phase_a_session",
|
||||||
|
session=item.session_id,
|
||||||
|
included=item.included_in_final,
|
||||||
|
accepted=item.accepted,
|
||||||
|
pair_count=item.pair_count,
|
||||||
|
rms_deg=float(item.residual_rms_deg),
|
||||||
|
p95_deg=float(item.residual_p95_deg),
|
||||||
|
bias_delta_norm_rad_s=float(
|
||||||
|
np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)
|
||||||
|
),
|
||||||
|
gyro_bias_rad_s=np.asarray(item.gyro_bias_rad_s, dtype=float).round(8).tolist(),
|
||||||
|
)
|
||||||
|
|
||||||
|
phase_a_by_session = {
|
||||||
|
item.session_id: item for item in joint.phase_a_sessions
|
||||||
|
}
|
||||||
|
session_results = []
|
||||||
|
for prep in prepared:
|
||||||
|
phase_a = phase_a_by_session.get(prep["session_id"])
|
||||||
|
session_bias = joint.gyro_bias_rad_s_per_session.get(prep["session_id"])
|
||||||
|
session_results.append(
|
||||||
|
{
|
||||||
|
**_public_session(prep),
|
||||||
|
"vehicle_config_loaded": vehicle_config is not None,
|
||||||
|
"handeye": {
|
||||||
|
"residual_rms_deg": handeye.residual_rms_deg,
|
||||||
|
"residual_median_deg": handeye.residual_median_deg,
|
||||||
|
"residual_p95_deg": handeye.residual_p95_deg,
|
||||||
|
"outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
|
||||||
|
"pair_count": handeye.pair_count,
|
||||||
|
"ok": handeye.ok,
|
||||||
|
"notes": tuple(list(handeye.notes) + [f"joint over {len(request.sessions)} sessions"]),
|
||||||
|
"R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
|
||||||
|
},
|
||||||
|
"joint": {
|
||||||
|
"translation_accepted": joint.translation_accepted,
|
||||||
|
"residual_rms_rot_deg": joint.residual_rms_rot_deg,
|
||||||
|
"residual_rms_trans_m": joint.residual_rms_trans_m,
|
||||||
|
"observability": asdict(joint.observability),
|
||||||
|
"notes": joint.notes,
|
||||||
|
"T_IMU_lidar": joint.T_IMU_lidar.tolist(),
|
||||||
|
"phase_a": None if phase_a is None else asdict(phase_a),
|
||||||
|
"gyro_bias_rad_s": None
|
||||||
|
if session_bias is None
|
||||||
|
else np.asarray(session_bias, dtype=float).tolist(),
|
||||||
|
"accel_bias_m_s2": None
|
||||||
|
if joint.accel_bias_m_s2 is None
|
||||||
|
else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(),
|
||||||
|
"gravity_m_s2": None
|
||||||
|
if joint.gravity_m_s2 is None
|
||||||
|
else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
|
||||||
|
},
|
||||||
|
"translation_accepted": joint.translation_accepted,
|
||||||
|
"rotation_ok": (
|
||||||
|
phase_a is not None
|
||||||
|
and phase_a.included_in_final
|
||||||
|
and phase_a.accepted
|
||||||
|
and joint.phase_a_accepted
|
||||||
|
and joint.observability.rotation_observable
|
||||||
|
),
|
||||||
|
"rotation_prior_constrained": (
|
||||||
|
phase_a is not None
|
||||||
|
and phase_a.included_in_final
|
||||||
|
and phase_a.accepted
|
||||||
|
and joint.phase_a_accepted
|
||||||
|
and not joint.observability.rotation_observable
|
||||||
|
and r_prior is not None
|
||||||
|
),
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
T = np.asarray(joint.T_IMU_lidar, dtype=float)
|
||||||
|
if request.requested_mode == CalibrationMode.ROTATION_ONLY:
|
||||||
|
# A rotation-only result must never expose a seed/prior translation,
|
||||||
|
# including when the rotation itself is rejected by a later gate.
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
# Multi-session offsets stay in details; the legacy scalar is single-session only.
|
||||||
|
delta_t = float(prepared[0]["time_offset_s"]) if len(prepared) == 1 else None
|
||||||
|
joint_rotation_ok = joint.phase_a_accepted
|
||||||
|
if not joint_rotation_ok:
|
||||||
|
status = CalibrationStatus.BLOCKED
|
||||||
|
message = (
|
||||||
|
f"joint rotation rejected: RMS={joint.residual_rms_rot_deg:.3f} deg "
|
||||||
|
"or a retained session failed the Phase-A residual gates"
|
||||||
|
)
|
||||||
|
elif request.requested_mode == CalibrationMode.FULL_SE3:
|
||||||
|
if joint.translation_accepted:
|
||||||
|
status = CalibrationStatus.FULL_SE3_ACCEPTED
|
||||||
|
message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
|
||||||
|
else:
|
||||||
|
status = CalibrationStatus.FULL_SE3_REJECTED
|
||||||
|
message = (
|
||||||
|
f"rotation accepted jointly ({len(prepared)} sessions); "
|
||||||
|
"translation deferred until Phase-B/C session-state redesign"
|
||||||
|
)
|
||||||
|
elif joint.observability.rotation_observable:
|
||||||
|
status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
|
||||||
|
message = (
|
||||||
|
f"rotation-only calibration accepted "
|
||||||
|
f"(joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
|
||||||
|
)
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
elif r_prior is not None:
|
||||||
|
status = CalibrationStatus.ROTATION_ONLY_PRIOR_CONSTRAINED
|
||||||
|
message = (
|
||||||
|
"rotation residuals passed, but motion does not independently observe all "
|
||||||
|
"rotation axes; result remains constrained by the installation prior"
|
||||||
|
)
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
else:
|
||||||
|
status = CalibrationStatus.BLOCKED
|
||||||
|
message = "rotation residuals passed but rotation observability failed without a prior"
|
||||||
|
T = T.copy()
|
||||||
|
T[:3, 3] = 0.0
|
||||||
|
|
||||||
|
return finish(
|
||||||
|
status=status,
|
||||||
|
message=message,
|
||||||
|
details={
|
||||||
|
"sessions": session_results,
|
||||||
|
"joint": {
|
||||||
|
"session_count": len(prepared),
|
||||||
|
"merged_pair_count": len(all_pairs),
|
||||||
|
"pair_counts_per_session": {p["session_id"]: p["pair_count"] for p in prepared},
|
||||||
|
"time_offset_s_per_session": {p["session_id"]: p["time_offset_s"] for p in prepared},
|
||||||
|
"handeye_rms_deg": handeye.residual_rms_deg,
|
||||||
|
"handeye_p95_deg": handeye.residual_p95_deg,
|
||||||
|
"handeye_outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
|
||||||
|
"phase_a_accepted": joint.phase_a_accepted,
|
||||||
|
"phase_a_comparison": joint.phase_a_comparison,
|
||||||
|
"phase_a_sessions": [asdict(item) for item in joint.phase_a_sessions],
|
||||||
|
"gyro_bias_rad_s_per_session": {
|
||||||
|
sid: np.asarray(value, dtype=float).tolist()
|
||||||
|
for sid, value in joint.gyro_bias_rad_s_per_session.items()
|
||||||
|
},
|
||||||
|
"excluded_sessions": [
|
||||||
|
item.session_id for item in joint.phase_a_sessions if not item.included_in_final
|
||||||
|
],
|
||||||
|
"joint_rotation_rms_deg": joint.residual_rms_rot_deg,
|
||||||
|
"rotation_observable": joint.observability.rotation_observable,
|
||||||
|
"translation_accepted": joint.translation_accepted,
|
||||||
|
},
|
||||||
|
"joint_handeye": asdict(handeye),
|
||||||
|
},
|
||||||
|
T_IMU_lidar=None if status == CalibrationStatus.BLOCKED else T,
|
||||||
|
time_offset_s=delta_t,
|
||||||
|
motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
|
||||||
|
payload = dict(session_result)
|
||||||
|
payload.pop("T_IMU_lidar", None)
|
||||||
|
payload.pop("pairs", None)
|
||||||
|
payload.pop("gyro_bias_rad_s", None)
|
||||||
|
payload.pop("gravity_init_m_s2", None)
|
||||||
|
return payload
|
||||||
@@ -0,0 +1,159 @@
|
|||||||
|
"""LiDAR relative-motion registration.
|
||||||
|
|
||||||
|
Uses Open3D Generalized ICP when available; otherwise a NumPy point-to-point ICP.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import LidarFrame
|
||||||
|
from .geometry import make_transform, orthonormalize_rotation, rotation_angle_deg, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RegistrationResult:
|
||||||
|
transform: np.ndarray
|
||||||
|
fitness: float
|
||||||
|
rotation_deg: float
|
||||||
|
translation_m: float
|
||||||
|
backend: str
|
||||||
|
ok: bool
|
||||||
|
|
||||||
|
|
||||||
|
def _voxel_downsample(points: np.ndarray, voxel: float) -> np.ndarray:
|
||||||
|
if points.shape[0] == 0:
|
||||||
|
return points
|
||||||
|
quantized = np.floor(points / voxel).astype(np.int64)
|
||||||
|
_, unique_indices = np.unique(quantized, axis=0, return_index=True)
|
||||||
|
return points[np.sort(unique_indices)]
|
||||||
|
|
||||||
|
|
||||||
|
def _numpy_icp(
|
||||||
|
source: np.ndarray,
|
||||||
|
target: np.ndarray,
|
||||||
|
*,
|
||||||
|
max_iterations: int = 30,
|
||||||
|
max_correspondence: float = 1.0,
|
||||||
|
) -> RegistrationResult:
|
||||||
|
src = _voxel_downsample(source, 0.2)
|
||||||
|
tgt = _voxel_downsample(target, 0.2)
|
||||||
|
if src.shape[0] < 50 or tgt.shape[0] < 50:
|
||||||
|
return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "numpy_icp", False)
|
||||||
|
|
||||||
|
# Subsample for speed.
|
||||||
|
rng = np.random.default_rng(0)
|
||||||
|
if src.shape[0] > 4000:
|
||||||
|
src = src[rng.choice(src.shape[0], 4000, replace=False)]
|
||||||
|
if tgt.shape[0] > 8000:
|
||||||
|
tgt = tgt[rng.choice(tgt.shape[0], 8000, replace=False)]
|
||||||
|
|
||||||
|
r = np.eye(3)
|
||||||
|
t = np.zeros(3)
|
||||||
|
last_error = 1e9
|
||||||
|
inlier_ratio = 0.0
|
||||||
|
for _ in range(max_iterations):
|
||||||
|
transformed = src @ r.T + t
|
||||||
|
# Nearest neighbour in target via brute force on chunks.
|
||||||
|
diff = transformed[:, None, :] - tgt[None, :, :]
|
||||||
|
dist2 = np.sum(diff * diff, axis=2)
|
||||||
|
nn = np.argmin(dist2, axis=1)
|
||||||
|
dist = np.sqrt(dist2[np.arange(src.shape[0]), nn])
|
||||||
|
mask = dist < max_correspondence
|
||||||
|
inlier_ratio = float(np.mean(mask))
|
||||||
|
if np.count_nonzero(mask) < 30:
|
||||||
|
break
|
||||||
|
p = transformed[mask]
|
||||||
|
q = tgt[nn[mask]]
|
||||||
|
mu_p = p.mean(axis=0)
|
||||||
|
mu_q = q.mean(axis=0)
|
||||||
|
h = (p - mu_p).T @ (q - mu_q)
|
||||||
|
u, _, vt = np.linalg.svd(h)
|
||||||
|
r_delta = vt.T @ u.T
|
||||||
|
if np.linalg.det(r_delta) < 0:
|
||||||
|
vt[-1, :] *= -1
|
||||||
|
r_delta = vt.T @ u.T
|
||||||
|
t_delta = mu_q - r_delta @ mu_p
|
||||||
|
# Update global transform: x' = r_delta (r x + t) + t_delta
|
||||||
|
r = orthonormalize_rotation(r_delta @ r)
|
||||||
|
t = r_delta @ t + t_delta
|
||||||
|
mean_err = float(np.mean(dist[mask]))
|
||||||
|
if abs(last_error - mean_err) < 1e-4:
|
||||||
|
break
|
||||||
|
last_error = mean_err
|
||||||
|
|
||||||
|
transform = make_transform(t, r)
|
||||||
|
return RegistrationResult(
|
||||||
|
transform=transform,
|
||||||
|
fitness=inlier_ratio,
|
||||||
|
rotation_deg=rotation_angle_deg(r),
|
||||||
|
translation_m=float(np.linalg.norm(t)),
|
||||||
|
backend="numpy_icp",
|
||||||
|
ok=inlier_ratio > 0.15,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _open3d_gicp(source: np.ndarray, target: np.ndarray) -> RegistrationResult | None:
|
||||||
|
try:
|
||||||
|
import open3d as o3d
|
||||||
|
except ImportError:
|
||||||
|
return None
|
||||||
|
|
||||||
|
src = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(source))
|
||||||
|
tgt = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(target))
|
||||||
|
src = src.voxel_down_sample(0.2)
|
||||||
|
tgt = tgt.voxel_down_sample(0.2)
|
||||||
|
if len(src.points) < 50 or len(tgt.points) < 50:
|
||||||
|
return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "open3d_gicp", False)
|
||||||
|
src.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
|
||||||
|
tgt.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
|
||||||
|
result = o3d.pipelines.registration.registration_generalized_icp(
|
||||||
|
src,
|
||||||
|
tgt,
|
||||||
|
1.0,
|
||||||
|
np.eye(4),
|
||||||
|
o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),
|
||||||
|
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50),
|
||||||
|
)
|
||||||
|
transform = np.asarray(result.transformation, dtype=float)
|
||||||
|
return RegistrationResult(
|
||||||
|
transform=transform,
|
||||||
|
fitness=float(result.fitness),
|
||||||
|
rotation_deg=rotation_angle_deg(transform[:3, :3]),
|
||||||
|
translation_m=float(np.linalg.norm(transform[:3, 3])),
|
||||||
|
backend="open3d_gicp",
|
||||||
|
ok=float(result.fitness) > 0.15,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def register_lidar_pair(source_points: np.ndarray, target_points: np.ndarray) -> RegistrationResult:
|
||||||
|
"""Register source -> target and return ``T_target_source``."""
|
||||||
|
|
||||||
|
source = np.asarray(source_points, dtype=float).reshape(-1, 3)
|
||||||
|
target = np.asarray(target_points, dtype=float).reshape(-1, 3)
|
||||||
|
open3d_result = _open3d_gicp(source, target)
|
||||||
|
if open3d_result is not None:
|
||||||
|
return open3d_result
|
||||||
|
return _numpy_icp(source, target)
|
||||||
|
|
||||||
|
|
||||||
|
def estimate_frame_rotations(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
stride: int = 1,
|
||||||
|
) -> tuple[list[np.ndarray], list[tuple[float, float]]]:
|
||||||
|
"""Estimate consecutive (or strided) LiDAR relative rotations for time sync."""
|
||||||
|
|
||||||
|
rotations: list[np.ndarray] = []
|
||||||
|
pair_times: list[tuple[float, float]] = []
|
||||||
|
for index in range(0, len(frames) - stride, max(stride, 1)):
|
||||||
|
a = frames[index]
|
||||||
|
b = frames[index + stride]
|
||||||
|
result = register_lidar_pair(b.points_xyz, a.points_xyz)
|
||||||
|
if not result.ok:
|
||||||
|
continue
|
||||||
|
rotations.append(result.transform[:3, :3])
|
||||||
|
pair_times.append((a.t_mid_s, b.t_mid_s))
|
||||||
|
return rotations, pair_times
|
||||||
@@ -0,0 +1,217 @@
|
|||||||
|
"""SO(3) rotation hand-eye solver for ``R_A R_X = R_X R_B``."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
|
||||||
|
from .contracts import MotionPair
|
||||||
|
from .geometry import orthonormalize_rotation, rotation_angle_deg, skew, so3_exp, so3_log
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RotationHandeyeResult:
|
||||||
|
R_IMU_lidar: np.ndarray
|
||||||
|
residual_rms_deg: float
|
||||||
|
residual_median_deg: float
|
||||||
|
residual_p95_deg: float
|
||||||
|
outlier_fraction_gt_5deg: float
|
||||||
|
pair_count: int
|
||||||
|
ok: bool
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_weight(pair: MotionPair) -> float:
|
||||||
|
weight = float(pair.metadata.get("weight", 1.0))
|
||||||
|
if not np.isfinite(weight) or weight <= 0:
|
||||||
|
return 1.0
|
||||||
|
return weight
|
||||||
|
|
||||||
|
|
||||||
|
def _tsai_rotation_initial(
|
||||||
|
pairs: list[MotionPair],
|
||||||
|
pair_weights: np.ndarray | None = None,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Closed-form rotation hand-eye initial guess (Tsai-style linear solve)."""
|
||||||
|
|
||||||
|
rows: list[np.ndarray] = []
|
||||||
|
rhs: list[np.ndarray] = []
|
||||||
|
weights = np.ones(len(pairs)) if pair_weights is None else np.asarray(pair_weights, dtype=float)
|
||||||
|
for pair, pair_weight in zip(pairs, weights):
|
||||||
|
alpha = so3_log(pair.R_A)
|
||||||
|
beta = so3_log(pair.R_B)
|
||||||
|
if np.linalg.norm(alpha) < 1e-6 or np.linalg.norm(beta) < 1e-6:
|
||||||
|
continue
|
||||||
|
w = np.sqrt(float(pair_weight))
|
||||||
|
rows.append(w * skew(alpha + beta))
|
||||||
|
rhs.append(w * (beta - alpha))
|
||||||
|
if len(rows) < 2:
|
||||||
|
return np.eye(3)
|
||||||
|
a = np.vstack(rows)
|
||||||
|
b = np.concatenate(rhs)
|
||||||
|
try:
|
||||||
|
rotvec, *_ = np.linalg.lstsq(a, b, rcond=None)
|
||||||
|
except np.linalg.LinAlgError:
|
||||||
|
return np.eye(3)
|
||||||
|
return orthonormalize_rotation(so3_exp(rotvec))
|
||||||
|
|
||||||
|
|
||||||
|
def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
|
||||||
|
err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||||
|
return float(np.degrees(np.linalg.norm(err)))
|
||||||
|
|
||||||
|
|
||||||
|
def _rms_deg(r_x: np.ndarray, pairs: list[MotionPair]) -> float:
|
||||||
|
if not pairs:
|
||||||
|
return 1e9
|
||||||
|
errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in pairs], dtype=float)
|
||||||
|
return float(np.sqrt(np.mean(errs**2)))
|
||||||
|
|
||||||
|
|
||||||
|
def select_strong_rotation_pairs(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
*,
|
||||||
|
min_rotation_deg: float = 1.0,
|
||||||
|
) -> list[MotionPair]:
|
||||||
|
"""Return pairs that independently excite rotation on both sensor sides."""
|
||||||
|
|
||||||
|
threshold = float(min_rotation_deg)
|
||||||
|
return [
|
||||||
|
pair
|
||||||
|
for pair in pairs
|
||||||
|
if rotation_angle_deg(pair.R_A) > threshold
|
||||||
|
and rotation_angle_deg(pair.R_B) > threshold
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def estimate_rotation_handeye_initial(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
*,
|
||||||
|
min_rotation_deg: float = 1.0,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Return the fast data-only Tsai initialization without nonlinear refine."""
|
||||||
|
|
||||||
|
usable = select_strong_rotation_pairs(
|
||||||
|
pairs,
|
||||||
|
min_rotation_deg=min_rotation_deg,
|
||||||
|
)
|
||||||
|
if not usable:
|
||||||
|
return np.eye(3)
|
||||||
|
raw_weights = np.asarray(
|
||||||
|
[_pair_weight(pair) for pair in usable],
|
||||||
|
dtype=float,
|
||||||
|
)
|
||||||
|
median = max(float(np.median(raw_weights)), 1e-12)
|
||||||
|
weights = np.clip(raw_weights / median, 0.1, 10.0)
|
||||||
|
return _tsai_rotation_initial(usable, weights)
|
||||||
|
|
||||||
|
|
||||||
|
def solve_rotation_handeye(
|
||||||
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||||
|
*,
|
||||||
|
R_prior: np.ndarray | None = None,
|
||||||
|
prior_sigma_deg: float | None = None,
|
||||||
|
) -> RotationHandeyeResult:
|
||||||
|
"""Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement.
|
||||||
|
|
||||||
|
Optional CAD / installation ``R_prior`` soft-constrains the extrinsic yaw that
|
||||||
|
is weakly observable under near-planar motion.
|
||||||
|
"""
|
||||||
|
|
||||||
|
usable = select_strong_rotation_pairs(pairs)
|
||||||
|
notes: list[str] = []
|
||||||
|
if len(usable) < 3:
|
||||||
|
return RotationHandeyeResult(
|
||||||
|
R_IMU_lidar=np.eye(3),
|
||||||
|
residual_rms_deg=1e9,
|
||||||
|
residual_median_deg=1e9,
|
||||||
|
residual_p95_deg=1e9,
|
||||||
|
outlier_fraction_gt_5deg=1.0,
|
||||||
|
pair_count=len(usable),
|
||||||
|
ok=False,
|
||||||
|
notes=("need at least 3 motion pairs with meaningful rotation",),
|
||||||
|
)
|
||||||
|
|
||||||
|
raw_weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
|
||||||
|
median_raw_weight = max(float(np.median(raw_weights)), 1e-12)
|
||||||
|
weights = np.clip(raw_weights / median_raw_weight, 0.1, 10.0)
|
||||||
|
r0 = _tsai_rotation_initial(usable, weights)
|
||||||
|
r_prior = None
|
||||||
|
if R_prior is not None:
|
||||||
|
r_prior = orthonormalize_rotation(np.asarray(R_prior, dtype=float).reshape(3, 3))
|
||||||
|
rms_tsai = _rms_deg(r0, usable)
|
||||||
|
rms_prior = _rms_deg(r_prior, usable)
|
||||||
|
if rms_prior <= rms_tsai * 1.25:
|
||||||
|
r0 = r_prior
|
||||||
|
notes.append(
|
||||||
|
f"init from rotation prior (rms={rms_prior:.3f} deg vs Tsai {rms_tsai:.3f} deg)"
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
notes.append(
|
||||||
|
f"init from Tsai (rms={rms_tsai:.3f} deg; prior {rms_prior:.3f} deg kept as soft constraint)"
|
||||||
|
)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
"weighted hand-eye: normalized/clipped IMU confidence "
|
||||||
|
f"raw_median={median_raw_weight:.3g}, "
|
||||||
|
f"normalized_min={float(np.min(weights)):.3g}, "
|
||||||
|
f"normalized_max={float(np.max(weights)):.3g}"
|
||||||
|
)
|
||||||
|
|
||||||
|
def pack(r: np.ndarray) -> np.ndarray:
|
||||||
|
return so3_log(r)
|
||||||
|
|
||||||
|
def unpack(vec: np.ndarray) -> np.ndarray:
|
||||||
|
return orthonormalize_rotation(so3_exp(vec))
|
||||||
|
|
||||||
|
sigma = 15.0 if prior_sigma_deg is None else float(prior_sigma_deg)
|
||||||
|
prior_w = 0.0
|
||||||
|
if r_prior is not None and sigma > 1e-6:
|
||||||
|
# Scale prior to a few strong pairs so it regularizes yaw without dominating.
|
||||||
|
prior_w = float(np.sqrt(np.median(weights)) / np.deg2rad(sigma))
|
||||||
|
notes.append(f"rotation prior soft constraint sigma={sigma:.1f} deg, weight={prior_w:.3g}")
|
||||||
|
|
||||||
|
def residual(vec: np.ndarray) -> np.ndarray:
|
||||||
|
r_x = unpack(vec)
|
||||||
|
residuals = []
|
||||||
|
for pair, weight in zip(usable, weights):
|
||||||
|
err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||||
|
residuals.append(np.sqrt(weight) * err)
|
||||||
|
if r_prior is not None and prior_w > 0:
|
||||||
|
residuals.append(prior_w * so3_log(r_prior.T @ r_x))
|
||||||
|
return np.concatenate(residuals)
|
||||||
|
|
||||||
|
opt = least_squares(residual, pack(r0), loss="huber", f_scale=np.deg2rad(1.0), max_nfev=200)
|
||||||
|
r_x = unpack(opt.x)
|
||||||
|
errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in usable], dtype=float)
|
||||||
|
# Report unweighted RMS/median for interpretability.
|
||||||
|
rms = float(np.sqrt(np.mean(errs**2)))
|
||||||
|
med = float(np.median(errs))
|
||||||
|
p95 = float(np.percentile(errs, 95.0))
|
||||||
|
outlier_fraction = float(np.mean(errs > 5.0))
|
||||||
|
notes.append(f"optimized over {len(usable)} pairs")
|
||||||
|
notes.append(
|
||||||
|
f"rotation residual quality: rms={rms:.3f} deg, median={med:.3f} deg, "
|
||||||
|
f"p95={p95:.3f} deg, >5deg={100.0 * outlier_fraction:.2f}%"
|
||||||
|
)
|
||||||
|
ok = (
|
||||||
|
len(usable) >= 3
|
||||||
|
and rms < 1.5
|
||||||
|
and med < 0.5
|
||||||
|
and p95 < 1.5
|
||||||
|
and outlier_fraction <= 0.005
|
||||||
|
)
|
||||||
|
if not ok:
|
||||||
|
notes.append("rotation residual distribution failed acceptance gates")
|
||||||
|
return RotationHandeyeResult(
|
||||||
|
R_IMU_lidar=r_x,
|
||||||
|
residual_rms_deg=rms,
|
||||||
|
residual_median_deg=med,
|
||||||
|
residual_p95_deg=p95,
|
||||||
|
outlier_fraction_gt_5deg=outlier_fraction,
|
||||||
|
pair_count=len(usable),
|
||||||
|
ok=ok,
|
||||||
|
notes=tuple(notes),
|
||||||
|
)
|
||||||
@@ -0,0 +1,332 @@
|
|||||||
|
"""Constant IMU-to-LiDAR clock-offset estimation via angular-rate correlation."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy import signal
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
from .geometry import rotation_angle_deg, so3_log
|
||||||
|
from .registration import estimate_frame_rotations
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class TimeOffsetResult:
|
||||||
|
delta_t_s: float
|
||||||
|
correlation_peak: float
|
||||||
|
search_s: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
ok: bool = True
|
||||||
|
|
||||||
|
|
||||||
|
def _magnitude_series(times: np.ndarray, values: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||||
|
mag = np.linalg.norm(values, axis=1) if values.ndim == 2 else np.asarray(values, dtype=float)
|
||||||
|
return np.asarray(times, dtype=float), np.asarray(mag, dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def _correlate_offset(
|
||||||
|
imu_t: np.ndarray,
|
||||||
|
imu_mag: np.ndarray,
|
||||||
|
lidar_t: np.ndarray,
|
||||||
|
lidar_mag: np.ndarray,
|
||||||
|
*,
|
||||||
|
search_s: float,
|
||||||
|
sample_hz: float,
|
||||||
|
) -> tuple[float, float]:
|
||||||
|
"""Return ``(delta_t, peak)`` for ``t_imu = t_lidar + delta_t``.
|
||||||
|
|
||||||
|
Implementation: resample both on LiDAR-relative grid, shift IMU by candidate
|
||||||
|
offsets, maximize normalized correlation. This avoids ambiguous lag signs.
|
||||||
|
"""
|
||||||
|
|
||||||
|
t_start = float(lidar_t[0])
|
||||||
|
t_end = float(lidar_t[-1])
|
||||||
|
if t_end - t_start < 0.5:
|
||||||
|
return 0.0, 0.0
|
||||||
|
dt = 1.0 / sample_hz
|
||||||
|
grid = np.arange(t_start, t_end, dt)
|
||||||
|
lidar_sig = np.interp(grid, lidar_t, lidar_mag, left=0.0, right=0.0)
|
||||||
|
lidar_sig = lidar_sig - np.mean(lidar_sig)
|
||||||
|
lidar_norm = float(np.linalg.norm(lidar_sig)) + 1e-12
|
||||||
|
|
||||||
|
best_delta = 0.0
|
||||||
|
best_peak = -1.0
|
||||||
|
for delta in np.arange(-search_s, search_s + 1e-12, dt):
|
||||||
|
imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
|
||||||
|
imu_sig = imu_sig - np.mean(imu_sig)
|
||||||
|
denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
|
||||||
|
peak = float(np.dot(imu_sig, lidar_sig) / denom)
|
||||||
|
if peak > best_peak:
|
||||||
|
best_peak = peak
|
||||||
|
best_delta = float(delta)
|
||||||
|
|
||||||
|
# Local parabolic refinement.
|
||||||
|
deltas = np.array([best_delta - dt, best_delta, best_delta + dt], dtype=float)
|
||||||
|
peaks = []
|
||||||
|
for delta in deltas:
|
||||||
|
imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
|
||||||
|
imu_sig = imu_sig - np.mean(imu_sig)
|
||||||
|
denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
|
||||||
|
peaks.append(float(np.dot(imu_sig, lidar_sig) / denom))
|
||||||
|
y0, y1, y2 = peaks
|
||||||
|
denom = y0 - 2 * y1 + y2
|
||||||
|
if abs(denom) > 1e-12:
|
||||||
|
refined = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
|
||||||
|
# Parabola can jump outside the searched window; keep it clamped.
|
||||||
|
if abs(refined) <= search_s + dt:
|
||||||
|
best_delta = refined
|
||||||
|
best_peak = float(y1)
|
||||||
|
return best_delta, best_peak
|
||||||
|
|
||||||
|
|
||||||
|
def estimate_time_offset(
|
||||||
|
imu: ImuSeries,
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
search_s: float = 1.0,
|
||||||
|
sample_hz: float = 50.0,
|
||||||
|
) -> TimeOffsetResult:
|
||||||
|
"""Estimate ``t_imu = t_lidar + delta_t``.
|
||||||
|
|
||||||
|
Positive ``delta_t`` means the IMU clock reading is ahead of the LiDAR clock
|
||||||
|
for the same physical instant (IMU timestamps are larger).
|
||||||
|
"""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
if len(frames) < 5:
|
||||||
|
return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR frames",), False)
|
||||||
|
|
||||||
|
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
gyro = imu.gyro_rad_s - bias
|
||||||
|
|
||||||
|
# Use short consecutive (or near-consecutive) pairs. A large stride (e.g.
|
||||||
|
# len//20) averages over many seconds and destroys |ω| correlation even when
|
||||||
|
# host/device clocks are already aligned.
|
||||||
|
stride = 1 if len(frames) < 80 else 2
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
|
||||||
|
if len(rotations) < 8:
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=1)
|
||||||
|
if len(rotations) < 4:
|
||||||
|
return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR relative rotations",), False)
|
||||||
|
|
||||||
|
lidar_t = []
|
||||||
|
lidar_w = []
|
||||||
|
for (t_a, t_b), rotation in zip(pair_times, rotations):
|
||||||
|
dt_pair = max(t_b - t_a, 1e-3)
|
||||||
|
omega = so3_log(rotation) / dt_pair
|
||||||
|
lidar_t.append(0.5 * (t_a + t_b))
|
||||||
|
lidar_w.append(omega)
|
||||||
|
lidar_t_arr = np.asarray(lidar_t, dtype=float)
|
||||||
|
lidar_w_arr = np.asarray(lidar_w, dtype=float)
|
||||||
|
|
||||||
|
imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
|
||||||
|
lidar_t_mag, lidar_mag = _magnitude_series(lidar_t_arr, lidar_w_arr)
|
||||||
|
delta, peak = _correlate_offset(
|
||||||
|
imu_t,
|
||||||
|
imu_mag,
|
||||||
|
lidar_t_mag,
|
||||||
|
lidar_mag,
|
||||||
|
search_s=search_s,
|
||||||
|
sample_hz=sample_hz,
|
||||||
|
)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
|
||||||
|
)
|
||||||
|
notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
|
||||||
|
# Host-UTC-bridged sessions are already on one timeline; |ω| peak can stay
|
||||||
|
# weak even at the correct lag (ICP rate vs gyro scale). Accept near-zero δt.
|
||||||
|
near_zero = abs(float(delta)) <= min(0.05, 0.25 * float(search_s))
|
||||||
|
ok = peak > 0.15 or near_zero
|
||||||
|
if peak <= 0.15 and near_zero:
|
||||||
|
notes.append(
|
||||||
|
f"correlation peak weak ({peak:.3f}) but |delta_t|={abs(delta):.4f}s ~0; "
|
||||||
|
"accepting as already-aligned (e.g. host UTC bridge)"
|
||||||
|
)
|
||||||
|
elif not ok:
|
||||||
|
notes.append("correlation peak is weak; check overlapping motion and axis units")
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=delta,
|
||||||
|
correlation_peak=peak,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=ok,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def lidar_time_to_imu_time(t_lidar_s: float, delta_t_s: float) -> float:
|
||||||
|
"""Convert a LiDAR timestamp to the IMU clock using ``t_imu = t_lidar + delta_t``."""
|
||||||
|
|
||||||
|
return float(t_lidar_s + delta_t_s)
|
||||||
|
|
||||||
|
|
||||||
|
def _lidar_omega_series(
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
stride: int,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray]:
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
|
||||||
|
if len(rotations) < 4:
|
||||||
|
rotations, pair_times = estimate_frame_rotations(frames, stride=1)
|
||||||
|
lidar_t: list[float] = []
|
||||||
|
lidar_w: list[np.ndarray] = []
|
||||||
|
for (t_a, t_b), rotation in zip(pair_times, rotations):
|
||||||
|
dt_pair = max(t_b - t_a, 1e-3)
|
||||||
|
omega = so3_log(rotation) / dt_pair
|
||||||
|
lidar_t.append(0.5 * (t_a + t_b))
|
||||||
|
lidar_w.append(omega)
|
||||||
|
return np.asarray(lidar_t, dtype=float), np.asarray(lidar_w, dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def refine_time_offset_signed(
|
||||||
|
imu: ImuSeries,
|
||||||
|
frames: list[LidarFrame],
|
||||||
|
*,
|
||||||
|
delta_t_s: float,
|
||||||
|
R_IMU_lidar: np.ndarray,
|
||||||
|
gyro_bias_rad_s: np.ndarray | None = None,
|
||||||
|
search_s: float = 0.08,
|
||||||
|
sample_hz: float = 50.0,
|
||||||
|
max_shift_s: float | None = 0.05,
|
||||||
|
) -> TimeOffsetResult:
|
||||||
|
"""Refine ``δt`` with signed 3-axis rates using a known ``R_IMU_lidar``.
|
||||||
|
|
||||||
|
Cost: mean squared error between ``gyro_imu(t_lidar+δt)`` and
|
||||||
|
``R_IMU_lidar @ omega_lidar(t_lidar)`` on a common grid around the coarse ``δt``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
notes: list[str] = [f"signed refine around coarse delta_t={delta_t_s:.6f}s"]
|
||||||
|
if len(frames) < 5:
|
||||||
|
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR frames",), False)
|
||||||
|
|
||||||
|
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||||
|
gyro = imu.gyro_rad_s - bias
|
||||||
|
r_x = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3)
|
||||||
|
|
||||||
|
stride = max(1, len(frames) // 20)
|
||||||
|
lidar_t, lidar_w = _lidar_omega_series(frames, stride=stride)
|
||||||
|
if lidar_t.size < 4:
|
||||||
|
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR omega samples",), False)
|
||||||
|
|
||||||
|
# Predicted IMU-frame angular rate from LiDAR relative rotations.
|
||||||
|
pred = (r_x @ lidar_w.T).T
|
||||||
|
t_start = float(lidar_t[0])
|
||||||
|
t_end = float(lidar_t[-1])
|
||||||
|
if t_end - t_start < 0.5:
|
||||||
|
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("LiDAR span too short for signed refine",), False)
|
||||||
|
|
||||||
|
dt = 1.0 / sample_hz
|
||||||
|
grid = np.arange(t_start, t_end, dt)
|
||||||
|
pred_grid = np.column_stack(
|
||||||
|
[np.interp(grid, lidar_t, pred[:, axis], left=np.nan, right=np.nan) for axis in range(3)]
|
||||||
|
)
|
||||||
|
|
||||||
|
def _cost_and_corr(delta: float) -> tuple[float, float]:
|
||||||
|
meas = np.column_stack(
|
||||||
|
[
|
||||||
|
np.interp(grid + delta, imu.t_s, gyro[:, axis], left=np.nan, right=np.nan)
|
||||||
|
for axis in range(3)
|
||||||
|
]
|
||||||
|
)
|
||||||
|
mask = np.isfinite(pred_grid).all(axis=1) & np.isfinite(meas).all(axis=1)
|
||||||
|
if int(np.count_nonzero(mask)) < 10:
|
||||||
|
return float("inf"), -1.0
|
||||||
|
err = meas[mask] - pred_grid[mask]
|
||||||
|
cost = float(np.mean(np.sum(err * err, axis=1)))
|
||||||
|
a = meas[mask].reshape(-1)
|
||||||
|
b = pred_grid[mask].reshape(-1)
|
||||||
|
a = a - np.mean(a)
|
||||||
|
b = b - np.mean(b)
|
||||||
|
corr = float(np.dot(a, b) / ((np.linalg.norm(a) + 1e-12) * (np.linalg.norm(b) + 1e-12)))
|
||||||
|
return cost, corr
|
||||||
|
|
||||||
|
coarse_cost, coarse_corr = _cost_and_corr(float(delta_t_s))
|
||||||
|
best_delta = float(delta_t_s)
|
||||||
|
best_cost = coarse_cost
|
||||||
|
best_corr = coarse_corr
|
||||||
|
half = abs(float(search_s))
|
||||||
|
for delta in np.arange(delta_t_s - half, delta_t_s + half + 1e-12, dt):
|
||||||
|
cost, corr = _cost_and_corr(float(delta))
|
||||||
|
if cost < best_cost:
|
||||||
|
best_cost = cost
|
||||||
|
best_delta = float(delta)
|
||||||
|
best_corr = corr
|
||||||
|
|
||||||
|
# Parabolic refine on cost around the best discrete delta.
|
||||||
|
samples = []
|
||||||
|
for delta in (best_delta - dt, best_delta, best_delta + dt):
|
||||||
|
cost, _ = _cost_and_corr(float(delta))
|
||||||
|
samples.append(cost if np.isfinite(cost) else best_cost)
|
||||||
|
y0, y1, y2 = samples
|
||||||
|
denom = y0 - 2 * y1 + y2
|
||||||
|
if abs(denom) > 1e-12 and y1 <= y0 and y1 <= y2:
|
||||||
|
candidate = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
|
||||||
|
cand_cost, cand_corr = _cost_and_corr(candidate)
|
||||||
|
if cand_cost < best_cost:
|
||||||
|
best_delta = candidate
|
||||||
|
best_cost = cand_cost
|
||||||
|
best_corr = cand_corr
|
||||||
|
|
||||||
|
# Guard with magnitude correlation so ICP-biased signed minima cannot wander.
|
||||||
|
imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
|
||||||
|
lidar_t_mag, lidar_mag = _magnitude_series(lidar_t, lidar_w)
|
||||||
|
|
||||||
|
def _mag_score(delta: float) -> float:
|
||||||
|
t_start_l = float(lidar_t_mag[0])
|
||||||
|
t_end_l = float(lidar_t_mag[-1])
|
||||||
|
grid_m = np.arange(t_start_l, t_end_l, dt)
|
||||||
|
lidar_sig = np.interp(grid_m, lidar_t_mag, lidar_mag, left=0.0, right=0.0)
|
||||||
|
lidar_sig = lidar_sig - np.mean(lidar_sig)
|
||||||
|
imu_sig = np.interp(grid_m + delta, imu_t, imu_mag, left=0.0, right=0.0)
|
||||||
|
imu_sig = imu_sig - np.mean(imu_sig)
|
||||||
|
denom = (float(np.linalg.norm(lidar_sig)) + 1e-12) * (float(np.linalg.norm(imu_sig)) + 1e-12)
|
||||||
|
return float(np.dot(imu_sig, lidar_sig) / denom)
|
||||||
|
|
||||||
|
mag_at_coarse = _mag_score(float(delta_t_s))
|
||||||
|
mag_at_best = _mag_score(best_delta)
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
f"signed 3-axis refine: delta_t={best_delta:.6f}s, "
|
||||||
|
f"mse={best_cost:.4g} (coarse_mse={coarse_cost:.4g}), "
|
||||||
|
f"corr={best_corr:.3f}, mag_corr={mag_at_best:.3f} (coarse_mag={mag_at_coarse:.3f}), "
|
||||||
|
f"search=±{half:.3f}s"
|
||||||
|
)
|
||||||
|
shift = abs(best_delta - float(delta_t_s))
|
||||||
|
if max_shift_s is not None and shift > float(max_shift_s):
|
||||||
|
notes.append(
|
||||||
|
f"signed refine rejected: |Δδt|={shift:.4f}s exceeds max_shift={float(max_shift_s):.4f}s; "
|
||||||
|
"keeping previous delta_t"
|
||||||
|
)
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=float(delta_t_s),
|
||||||
|
correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
# Require a meaningful MSE drop so tiny downhill noise cannot walk δt across iterations.
|
||||||
|
improved = (
|
||||||
|
np.isfinite(best_cost)
|
||||||
|
and best_cost < coarse_cost * 0.98
|
||||||
|
# Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
|
||||||
|
and mag_at_best + 1e-4 >= mag_at_coarse
|
||||||
|
)
|
||||||
|
if not improved:
|
||||||
|
notes.append("signed refine rejected by MSE/mag-consistency; keeping previous delta_t")
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=float(delta_t_s),
|
||||||
|
correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
|
return TimeOffsetResult(
|
||||||
|
delta_t_s=best_delta,
|
||||||
|
correlation_peak=mag_at_best,
|
||||||
|
search_s=search_s,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=True,
|
||||||
|
)
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
"""Timestamp audit for IMU and LiDAR streams."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .contracts import ImuSeries, LidarFrame
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class TimestampAuditReport:
|
||||||
|
monotonic: bool
|
||||||
|
epoch_count: int
|
||||||
|
imu_rate_hz: float
|
||||||
|
lidar_rate_hz: float
|
||||||
|
imu_duration_s: float
|
||||||
|
lidar_duration_s: float
|
||||||
|
max_imu_gap_s: float
|
||||||
|
max_lidar_gap_s: float
|
||||||
|
notes: tuple[str, ...] = ()
|
||||||
|
ok: bool = True
|
||||||
|
|
||||||
|
|
||||||
|
def _rate_and_gaps(times: np.ndarray) -> tuple[float, float]:
|
||||||
|
if times.size < 2:
|
||||||
|
return 0.0, 0.0
|
||||||
|
dt = np.diff(times)
|
||||||
|
positive = dt[dt > 0]
|
||||||
|
if positive.size == 0:
|
||||||
|
return 0.0, float("inf")
|
||||||
|
rate = float(1.0 / np.median(positive))
|
||||||
|
return rate, float(np.max(dt))
|
||||||
|
|
||||||
|
|
||||||
|
def audit_timestamps(imu: ImuSeries, frames: list[LidarFrame]) -> TimestampAuditReport:
|
||||||
|
"""Audit native timestamps without assuming the two clocks share an epoch."""
|
||||||
|
|
||||||
|
notes: list[str] = []
|
||||||
|
imu_t = imu.t_s
|
||||||
|
lidar_t = np.asarray([frame.t_mid_s for frame in frames], dtype=float)
|
||||||
|
|
||||||
|
imu_mono = bool(np.all(np.diff(imu_t) >= 0)) if imu_t.size > 1 else False
|
||||||
|
lidar_mono = bool(np.all(np.diff(lidar_t) >= 0)) if lidar_t.size > 1 else False
|
||||||
|
if not imu_mono:
|
||||||
|
notes.append("IMU timestamps are not monotonic")
|
||||||
|
if not lidar_mono:
|
||||||
|
notes.append("LiDAR timestamps are not monotonic")
|
||||||
|
|
||||||
|
imu_rate, imu_gap = _rate_and_gaps(imu_t)
|
||||||
|
lidar_rate, lidar_gap = _rate_and_gaps(lidar_t)
|
||||||
|
if imu_t.size < 50:
|
||||||
|
notes.append(f"IMU sample count is low ({imu_t.size})")
|
||||||
|
if len(frames) < 5:
|
||||||
|
notes.append(f"LiDAR frame count is low ({len(frames)})")
|
||||||
|
if imu_gap > 0.05:
|
||||||
|
notes.append(f"large IMU gap detected: {imu_gap:.3f}s")
|
||||||
|
if lidar_gap > 1.0:
|
||||||
|
notes.append(f"large LiDAR gap detected: {lidar_gap:.3f}s")
|
||||||
|
|
||||||
|
notes.append(
|
||||||
|
"IMU and LiDAR clocks are treated as independent; constant offset is estimated later."
|
||||||
|
)
|
||||||
|
|
||||||
|
ok = imu_mono and lidar_mono and imu_t.size >= 50 and len(frames) >= 5
|
||||||
|
return TimestampAuditReport(
|
||||||
|
monotonic=imu_mono and lidar_mono,
|
||||||
|
epoch_count=2,
|
||||||
|
imu_rate_hz=imu_rate,
|
||||||
|
lidar_rate_hz=lidar_rate,
|
||||||
|
imu_duration_s=float(imu_t[-1] - imu_t[0]) if imu_t.size else 0.0,
|
||||||
|
lidar_duration_s=float(lidar_t[-1] - lidar_t[0]) if lidar_t.size else 0.0,
|
||||||
|
max_imu_gap_s=imu_gap,
|
||||||
|
max_lidar_gap_s=lidar_gap,
|
||||||
|
notes=tuple(notes),
|
||||||
|
ok=ok,
|
||||||
|
)
|
||||||
@@ -0,0 +1,83 @@
|
|||||||
|
"""Vehicle-installation configuration loading and light validation."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Mapping
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
REQUIRED_TOP_LEVEL_KEYS = frozenset({"schema_version", "vehicle", "installation", "sensors", "time"})
|
||||||
|
|
||||||
|
|
||||||
|
def validate_config_shape(config: Mapping[str, object]) -> list[str]:
|
||||||
|
"""Return missing top-level keys without inventing default values."""
|
||||||
|
|
||||||
|
return sorted(REQUIRED_TOP_LEVEL_KEYS.difference(config))
|
||||||
|
|
||||||
|
|
||||||
|
def validate_config_semantics(config: Mapping[str, Any]) -> list[str]:
|
||||||
|
"""Return semantic issues that block calibration interpretation."""
|
||||||
|
|
||||||
|
issues: list[str] = []
|
||||||
|
sensors = config.get("sensors")
|
||||||
|
if not isinstance(sensors, Mapping):
|
||||||
|
return ["sensors must be a mapping"]
|
||||||
|
imu = sensors.get("imu")
|
||||||
|
lidar = sensors.get("lidar")
|
||||||
|
if not isinstance(imu, Mapping):
|
||||||
|
issues.append("sensors.imu missing")
|
||||||
|
else:
|
||||||
|
axes = ((imu.get("raw_frame") or {}) if isinstance(imu.get("raw_frame"), Mapping) else {}).get("axes")
|
||||||
|
if not axes:
|
||||||
|
issues.append("sensors.imu.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||||
|
if not isinstance(lidar, Mapping):
|
||||||
|
issues.append("sensors.lidar missing")
|
||||||
|
else:
|
||||||
|
axes = ((lidar.get("raw_frame") or {}) if isinstance(lidar.get("raw_frame"), Mapping) else {}).get("axes")
|
||||||
|
if not axes:
|
||||||
|
issues.append("sensors.lidar.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||||
|
|
||||||
|
time_cfg = config.get("time")
|
||||||
|
if not isinstance(time_cfg, Mapping):
|
||||||
|
issues.append("time missing")
|
||||||
|
else:
|
||||||
|
for key in ("imu_timestamp_source", "lidar_timestamp_source", "lidar_frame_time_definition"):
|
||||||
|
if not time_cfg.get(key):
|
||||||
|
issues.append(f"time.{key} is empty")
|
||||||
|
return issues
|
||||||
|
|
||||||
|
|
||||||
|
def load_vehicle_config(path: str | Path) -> dict[str, Any]:
|
||||||
|
"""Load and lightly validate a YAML vehicle configuration."""
|
||||||
|
|
||||||
|
try:
|
||||||
|
import yaml
|
||||||
|
except ImportError as exc: # pragma: no cover
|
||||||
|
raise ImportError("PyYAML is required to load vehicle configuration files") from exc
|
||||||
|
|
||||||
|
config_path = Path(path)
|
||||||
|
with config_path.open("r", encoding="utf-8") as handle:
|
||||||
|
loaded = yaml.safe_load(handle)
|
||||||
|
if not isinstance(loaded, dict):
|
||||||
|
raise ValueError(f"vehicle config must be a mapping: {config_path}")
|
||||||
|
|
||||||
|
missing = validate_config_shape(loaded)
|
||||||
|
if missing:
|
||||||
|
raise ValueError(f"vehicle config missing keys {missing}: {config_path}")
|
||||||
|
|
||||||
|
semantic = validate_config_semantics(loaded)
|
||||||
|
if semantic:
|
||||||
|
raise ValueError("vehicle config semantic issues:\n- " + "\n- ".join(semantic))
|
||||||
|
return loaded
|
||||||
|
|
||||||
|
|
||||||
|
def prior_enabled(config: Mapping[str, Any], name: str) -> bool:
|
||||||
|
"""Return whether an optional prior is enabled."""
|
||||||
|
|
||||||
|
init = config.get("initialization")
|
||||||
|
if not isinstance(init, Mapping):
|
||||||
|
return False
|
||||||
|
prior = init.get(name)
|
||||||
|
if not isinstance(prior, Mapping):
|
||||||
|
return False
|
||||||
|
return bool(prior.get("enabled", False))
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
# `imu_lidar` 模块说明
|
||||||
|
|
||||||
|
**用途：** 改 `imu_lidar/` 源码时查阅。对外用法见根目录 [`README.md`](../README.md)。
|
||||||
|
|
||||||
|
本包实现 LiDAR–IMU 外参标定：在连续行驶数据上选取关键帧，用 IMU 预积分与雷达配准构造相对运动对，求解安装外参。
|
||||||
|
|
||||||
|
```text
|
||||||
|
A ≈ 关键帧间 IMU 相对运动（预积分：旋转 / 速度增量 / 位移增量）
|
||||||
|
B ≈ 关键帧间雷达配准
|
||||||
|
解 R_A R_X = R_X R_B → 旋转外参（手眼阶段只用旋转）
|
||||||
|
再精修旋转与陀螺零偏；在完整六自由度模式下，可观时再估计平移等
|
||||||
|
```
|
||||||
|
|
||||||
|
入口：
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m imu_lidar.cli plan
|
||||||
|
python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output ...
|
||||||
|
```
|
||||||
|
|
||||||
|
整体流程由 `pipeline.py` 串联。
|
||||||
|
修改本目录代码时，请同步更新本说明，并在 [`CHANGELOG.md`](CHANGELOG.md) 追加「时间戳 + 原本 → 改成」。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 流水线顺序与文件
|
||||||
|
|
||||||
|
|
||||||
|
| 顺序 | 文件 | 作用 |
|
||||||
|
| --- | ----------------------- | ----------------------------- |
|
||||||
|
| 0 | `contracts.py` | 公共数据类型与状态枚举 |
|
||||||
|
| 0 | `geometry.py` | 刚体变换与旋转工具 |
|
||||||
|
| 0 | `vehicle_config.py` | 读取并校验车辆 YAML |
|
||||||
|
| 1 | `imu_io.py` | 读标准 IMU 中间格式 |
|
||||||
|
| 1 | `lidar_io.py` | 读标准雷达会话目录 |
|
||||||
|
| 2 | `timestamp_audit.py` | 时间单调 / 频率 / 空洞检查 |
|
||||||
|
| 3 | `imu_audit.py` | 静止零偏、加速度模长检查、建议竖直轴 |
|
||||||
|
| 4 | `time_offset.py` | 粗估时间偏置 δt，并用旋转外参精修 |
|
||||||
|
| 5 | `registration.py` | 帧间点云配准 |
|
||||||
|
| 5 | `keyframes.py` | 按运动量抽取关键帧 |
|
||||||
|
| 5 | `lidar_deskew.py` | 可选点云去畸变（低速可关） |
|
||||||
|
| 6 | `imu_preintegration.py` | IMU 预积分（旋转及速度/位移增量、协方差、零偏雅可比） |
|
||||||
|
| 6 | `motion_pairs.py` | 构造运动对；手眼使用其中的旋转 |
|
||||||
|
| 6 | `motion_pairs_io.py` | 运动对 JSON 缓存读写（供可视化直读） |
|
||||||
|
| 7 | `rotation_handeye.py` | 加权旋转手眼 |
|
||||||
|
| 8 | `observability.py` | 旋转 / 平移可观性检查 |
|
||||||
|
| 8 | `joint_optimizer.py` | 联合精修；完整模式下可估计平移、重力、速度与时变零偏 |
|
||||||
|
| 9 | `finalize.py` | 写出结果 JSON（含 `motion_pairs.json`） |
|
||||||
|
| — | `pipeline.py` | 编排全流程 |
|
||||||
|
| — | `cli.py` | 命令行入口 |
|
||||||
|
| — | `CHANGELOG.md` | 改动记录 |
|
||||||
|
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 运行模式要点
|
||||||
|
|
||||||
|
- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。
|
||||||
|
- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。
|
||||||
|
- `--mode full_se3`：当前完成 Phase-A 后明确拒绝平移；待 Phase-B/C 会话状态重构完成后再恢复完整 SE(3) 交付。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 输入格式
|
||||||
|
|
||||||
|
```text
|
||||||
|
imu.csv # t,gx,gy,gz,ax,ay,az（建议设备时间）
|
||||||
|
lidar_session/
|
||||||
|
frames_index.csv # frame_id,filename,t_start,t_end
|
||||||
|
frames/frame_XXXXX.npz # points: (N,3) 米
|
||||||
|
```
|
||||||
|
|
||||||
|
原始 N300 `.rscap` + H32 dlog（或旧 MSOP `.rscap`）用仓库工具导出：`python tools/export_rscap_to_v1.py ...`（见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md)）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 当前能力
|
||||||
|
|
||||||
|
- 本包是仓库**唯一**标定路径：质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →（可选）完整六自由度 → 报告
|
||||||
|
- 点云去畸变：可选
|
||||||
|
- 阶段与用法见根目录 [`README.md`](../README.md)
|
||||||
|
- 改动史：[`CHANGELOG.md`](CHANGELOG.md)
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
[project]
|
||||||
|
name = "lidar-imu-calibration"
|
||||||
|
version = "0.3.0"
|
||||||
|
description = "LiDAR-IMU and RTK-IMU extrinsic calibration algorithms"
|
||||||
|
requires-python = ">=3.10"
|
||||||
|
dependencies = [
|
||||||
|
"numpy>=1.26",
|
||||||
|
"scipy>=1.11",
|
||||||
|
"pyyaml>=6.0",
|
||||||
|
]
|
||||||
|
|
||||||
|
[project.optional-dependencies]
|
||||||
|
open3d = ["open3d>=0.17"]
|
||||||
|
dev = ["pytest>=7.4"]
|
||||||
|
|
||||||
|
[project.scripts]
|
||||||
|
lidar-imu-calibration = "imu_lidar.cli:main"
|
||||||
|
|
||||||
|
[build-system]
|
||||||
|
requires = ["setuptools>=68", "wheel"]
|
||||||
|
build-backend = "setuptools.build_meta"
|
||||||
|
|
||||||
|
[tool.setuptools]
|
||||||
|
packages = ["imu_lidar", "rtk_imu", "tools"]
|
||||||
|
|
||||||
|
[tool.pytest.ini_options]
|
||||||
|
testpaths = ["tests"]
|
||||||
|
pythonpath = ["."]
|
||||||
@@ -1,4 +0,0 @@
|
|||||||
numpy>=1.26
|
|
||||||
scipy>=1.11
|
|
||||||
open3d>=0.18
|
|
||||||
small-gicp==1.0.1
|
|
||||||
@@ -1,10 +0,0 @@
|
|||||||
# 历史两批结果
|
|
||||||
|
|
||||||
第二批 38 站的密集 RTK 数据用于求解；第一批 38 站因 RTK 约 10 秒一条，仅作辅助检查。最终部署值：
|
|
||||||
|
|
||||||
```text
|
|
||||||
translation_m = [1.297759692, -0.000067331, 0.720497835]
|
|
||||||
RPY_deg_xyz = [-0.785151146, 1.202660822, -0.835510053]
|
|
||||||
```
|
|
||||||
|
|
||||||
共识估计使用 39 个运动对，AX RMS 约 `0.07985 m / 0.96118°`。第一批辅助检查 22 对约 `0.06067 m / 1.00298°`。`final_extrinsic_deployment.json` 是唯一建议直接交给下游的部署 JSON；其余文件用于审计和复现。
|
|
||||||
@@ -1,39 +0,0 @@
|
|||||||
time,nx,ny,nz,d,inliers,rms_m,frame_counter
|
|
||||||
1784279335.9505181,-0.05361540190749781,-0.028612066826359885,0.9981516609765378,0.9970438972044271,1413,0.014218123000381944,190
|
|
||||||
1784279429.4466305,-0.020242124527498212,-0.021859763099327908,0.999556105054566,0.9527601103226121,1403,0.012533257614588953,1125
|
|
||||||
1784279517.5427606,-0.025255763366701406,-0.019499239847777874,0.9994908334057517,0.9627800728846587,1377,0.013484126577744504,2006
|
|
||||||
1784279605.2394407,-0.01384716564355861,-0.022470612380854543,0.9996516031012351,0.9522772824995122,1451,0.012466425277627606,2883
|
|
||||||
1784279701.2360666,-0.01555857544224011,-0.0217686215576034,0.999641964828253,0.9563364194973247,1538,0.012300341594741273,3843
|
|
||||||
1784279817.2306573,-0.011087527873671243,-0.020924555502825015,0.9997195755323889,0.9578164603891901,1449,0.011989950419186364,5003
|
|
||||||
1784280797.6884267,-0.02596647147549029,-0.017803282998918958,0.999504269862602,0.9413409925183257,1070,0.01285544119037058,1175
|
|
||||||
1784280931.5831878,-0.015374876237213071,-0.006972433892068898,0.9998574890184657,0.9529924610673387,1231,0.012386720372038787,2514
|
|
||||||
1784281027.8795433,-0.018194511282557984,-0.007436316920549397,0.9998068118140855,0.9473391457135313,994,0.013690484345080097,3477
|
|
||||||
1784281119.375541,-0.031030924482729524,-0.005127874296086657,0.9995052709370525,0.9424388693848053,1050,0.012540091408749102,4392
|
|
||||||
1784281219.2718523,0.010887801537886658,0.029801594631508527,0.9994965336283518,1.0385701101629785,1314,0.012779819581803468,5391
|
|
||||||
1784281326.268232,-0.0096362623873578,0.012819044946118371,0.9998713989978268,0.9312224966475169,1290,0.014104946182144612,6461
|
|
||||||
1784281406.9641902,0.0003902542885943621,0.08032748256370953,0.9967684501661191,1.1034060039993756,1292,0.010758148594815282,7268
|
|
||||||
1784281474.3615055,-0.03105566487178763,0.07564589746567012,0.9966510140846617,1.0327196422481995,1815,0.011041719572931458,7942
|
|
||||||
1784281630.3553114,-0.08580403119897857,-0.004531705446499283,0.9963017273274953,0.9450482405407393,2217,0.013149265194066527,9502
|
|
||||||
1784281794.7491786,-0.0338374639041254,-0.01961931634157733,0.9992347614363835,0.9452222296717823,1260,0.01201022023689137,11146
|
|
||||||
1784281908.3446162,-0.021893307599930106,-0.01424087221046337,0.9996588821398128,0.9576567371739861,1494,0.01236191966907317,12282
|
|
||||||
1784282032.5400162,-0.0158344231842381,-0.012467864421327742,0.9997968910729789,0.9463865980851461,1636,0.010787373185908479,13524
|
|
||||||
1784282152.7353525,-0.018682494816813326,-0.008766064442460298,0.9997870375743079,0.9483840866729998,2240,0.01283360827008306,14726
|
|
||||||
1784282248.831164,-0.023968085422919984,0.0010098337757014242,0.9997122141481043,0.9229733966918622,2063,0.01198166200322995,15687
|
|
||||||
1784282392.325837,-0.03491854849921791,-0.007495342405346995,0.999362053918866,0.9445161886424389,2063,0.012487104595673213,17122
|
|
||||||
1784282521.1221898,-0.033279977268402205,-0.021263918239006526,0.9992198401223525,0.9241543379566599,1765,0.012696796680589562,18410
|
|
||||||
1784282614.418045,-0.02206065096269162,-0.02672916533917316,0.9993992592549654,0.9465522302118663,1961,0.013512740981540523,19343
|
|
||||||
1784282682.8141525,-0.02081301783029168,-0.01579943764964285,0.9996585397318182,0.9449041973558906,1859,0.012396974052191898,20027
|
|
||||||
1784282765.6112185,-0.004508281075411156,-0.014008591413946636,0.9998917115209738,0.950099620414229,2036,0.011293042628447103,20855
|
|
||||||
1784282827.209564,-0.01736207184562764,-0.0031377437424165654,0.999844344398384,0.9426007877012084,1748,0.010719044572619517,21471
|
|
||||||
1784282910.2059953,-0.021125469928628078,-0.014448226693694841,0.9996724279811374,0.9542841605935083,2168,0.013339960186967582,22301
|
|
||||||
1784282963.3037353,-0.006457463553009421,-0.02032265008628933,0.9997726196780605,0.963795839330157,2091,0.012167595789322305,22832
|
|
||||||
1784283066.8004546,-0.025064234285691236,-0.029087411217067063,0.9992625814411152,0.9318734965466619,1864,0.012746549021992943,23867
|
|
||||||
1784283133.8969557,-0.016700078063969132,-0.04176715532503475,0.9989877937836437,0.9264060770827569,1908,0.01109353135053672,24538
|
|
||||||
1784283183.2952216,-0.01653688679373809,-0.017415882722786116,0.9997115676054555,0.9575547004867051,2007,0.012534093025107626,25032
|
|
||||||
1784283245.892713,-0.025847980338270828,-0.0153447484376825,0.9995481082008092,0.9578045061243852,1561,0.012070094021521557,25658
|
|
||||||
1784283298.8906527,-0.021273542761096498,-0.005573033066936611,0.9997581595970231,0.9467349945273356,2032,0.012236068644419621,26188
|
|
||||||
1784283360.3892086,-0.039281329759085375,-0.01627781426001628,0.9990955959743164,0.9222774871766527,1808,0.013038192231074228,26803
|
|
||||||
1784283421.0858324,-0.03317220277295649,-0.00229990918740887,0.9994470047886079,0.920634362486867,1708,0.012670933440544421,27410
|
|
||||||
1784283483.1839027,-0.025661097999728377,-0.014377994766167732,0.9995672970420513,0.9594288765988624,2042,0.012797258981502222,28031
|
|
||||||
1784283558.880709,-0.02901405273913087,-0.038144166153403075,0.998850943500637,0.9554734954630025,1768,0.012270678438066111,28788
|
|
||||||
1784283636.4777331,-0.03570548009769577,-0.0034801149872434093,0.9993562965682804,0.9414430863966106,1632,0.012966623651022222,29564
|
|
||||||
|
@@ -1,79 +0,0 @@
|
|||||||
{
|
|
||||||
"recommended_backend": "open3d_gicp",
|
|
||||||
"selection_reason": "The two X estimates agree closely; Open3D has lower second-batch AX residual, better B loop closure, and lower first-batch auxiliary residual.",
|
|
||||||
"coordinate_convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
|
|
||||||
"measured_extrinsic_used_as_initial": false,
|
|
||||||
"second_batch_role": "estimation (dense RTK)",
|
|
||||||
"first_batch_role": "auxiliary check only (sparse RTK)",
|
|
||||||
"backend_difference": {
|
|
||||||
"translation_m": 0.00181035475331947,
|
|
||||||
"rotation_deg": 0.06312194455789993
|
|
||||||
},
|
|
||||||
"open3d_gicp": {
|
|
||||||
"translation_m": [
|
|
||||||
1.2978831677200011,
|
|
||||||
-0.0030997734098957433,
|
|
||||||
0.7217892226151688
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.7572070226288585,
|
|
||||||
1.1460424368859559,
|
|
||||||
-0.8797812288284186
|
|
||||||
],
|
|
||||||
"estimation_pairs": 66,
|
|
||||||
"estimation_translation_rms_m": 0.10017183831403174,
|
|
||||||
"estimation_rotation_rms_deg": 1.011529772202344,
|
|
||||||
"bootstrap_std": [
|
|
||||||
0.0020832827985508263,
|
|
||||||
0.002063603632322515,
|
|
||||||
0.001427746471392659,
|
|
||||||
0.07576290622767821,
|
|
||||||
0.06304875844333606,
|
|
||||||
0.07843172452171372
|
|
||||||
],
|
|
||||||
"initial_B_loop_closure": {
|
|
||||||
"count": 106,
|
|
||||||
"translation_rms_m": 0.020350848901853437,
|
|
||||||
"translation_p95_m": 0.04464367942375473,
|
|
||||||
"rotation_rms_deg": 0.2674546762953808,
|
|
||||||
"rotation_p95_deg": 0.5761692075739222
|
|
||||||
},
|
|
||||||
"batch1_auxiliary_pairs": 41,
|
|
||||||
"batch1_auxiliary_translation_rms_m": 0.06627076507951374,
|
|
||||||
"batch1_auxiliary_rotation_rms_deg": 1.0783402768758907
|
|
||||||
},
|
|
||||||
"small_gicp": {
|
|
||||||
"translation_m": [
|
|
||||||
1.2996301155160686,
|
|
||||||
-0.0035647011612625953,
|
|
||||||
0.7218861757826791
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.7871563544979889,
|
|
||||||
1.1414557494228486,
|
|
||||||
-0.9357571721886107
|
|
||||||
],
|
|
||||||
"estimation_pairs": 80,
|
|
||||||
"estimation_translation_rms_m": 0.11870623426762615,
|
|
||||||
"estimation_rotation_rms_deg": 1.135091439337232,
|
|
||||||
"bootstrap_std": [
|
|
||||||
0.0017068433472663032,
|
|
||||||
0.0019214288447692426,
|
|
||||||
0.0013180801706094514,
|
|
||||||
0.06849719260762097,
|
|
||||||
0.05792635838163369,
|
|
||||||
0.062093058432709465
|
|
||||||
],
|
|
||||||
"initial_B_loop_closure": {
|
|
||||||
"count": 252,
|
|
||||||
"translation_rms_m": 0.06356464456110195,
|
|
||||||
"translation_p95_m": 0.12253135158224471,
|
|
||||||
"rotation_rms_deg": 0.7286536542725655,
|
|
||||||
"rotation_p95_deg": 1.4911215410143956
|
|
||||||
},
|
|
||||||
"batch1_auxiliary_pairs": 36,
|
|
||||||
"batch1_auxiliary_translation_rms_m": 0.07402020402512643,
|
|
||||||
"batch1_auxiliary_rotation_rms_deg": 1.1492603033421918
|
|
||||||
},
|
|
||||||
"important_limit": "Backend agreement is strong, but AX rotation RMS remains about one degree. This is not a centimetre-grade absolute certification."
|
|
||||||
}
|
|
||||||
@@ -1,328 +0,0 @@
|
|||||||
{
|
|
||||||
"selection_is_X_independent": true,
|
|
||||||
"B_source": "Open3D; small_gicp is used only as an agreement gate",
|
|
||||||
"max_translation_m": 0.05,
|
|
||||||
"max_rotation_deg": 0.5,
|
|
||||||
"input_open3d_pairs": 41,
|
|
||||||
"accepted_pairs": 22,
|
|
||||||
"pairs": [
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 1,
|
|
||||||
"open3d_small_translation_m": 0.04084575096493436,
|
|
||||||
"open3d_small_rotation_deg": 0.8091245488258404,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 3,
|
|
||||||
"open3d_small_translation_m": 0.007440836728606337,
|
|
||||||
"open3d_small_rotation_deg": 0.14377171116926005,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 2,
|
|
||||||
"open3d_small_translation_m": 0.012040931709501762,
|
|
||||||
"open3d_small_rotation_deg": 0.09198739647809913,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 3,
|
|
||||||
"open3d_small_translation_m": 0.020167802344555865,
|
|
||||||
"open3d_small_rotation_deg": 0.10088741778579047,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.05126263142175136,
|
|
||||||
"open3d_small_rotation_deg": 1.129023564517054,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 4,
|
|
||||||
"open3d_small_translation_m": 0.008340648580577372,
|
|
||||||
"open3d_small_rotation_deg": 0.48893802037297335,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.03255543023061846,
|
|
||||||
"open3d_small_rotation_deg": 0.04924738744939371,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 4,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.01717130804559543,
|
|
||||||
"open3d_small_rotation_deg": 0.18948013657322885,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 5,
|
|
||||||
"j": 6,
|
|
||||||
"open3d_small_translation_m": 0.00790599819083906,
|
|
||||||
"open3d_small_rotation_deg": 0.1334214273599749,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 7,
|
|
||||||
"open3d_small_translation_m": 0.02785621409489221,
|
|
||||||
"open3d_small_rotation_deg": 0.4203987393515618,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 9,
|
|
||||||
"open3d_small_translation_m": 0.012940492880990915,
|
|
||||||
"open3d_small_rotation_deg": 0.16985420735800236,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 11,
|
|
||||||
"j": 12,
|
|
||||||
"open3d_small_translation_m": 0.016762449071916253,
|
|
||||||
"open3d_small_rotation_deg": 0.43689758943546,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 16,
|
|
||||||
"j": 17,
|
|
||||||
"open3d_small_translation_m": 0.038512603544217365,
|
|
||||||
"open3d_small_rotation_deg": 1.3958733432618273,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 16,
|
|
||||||
"j": 18,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 17,
|
|
||||||
"j": 18,
|
|
||||||
"open3d_small_translation_m": 0.04386909360158492,
|
|
||||||
"open3d_small_rotation_deg": 0.49818041971185584,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 18,
|
|
||||||
"j": 19,
|
|
||||||
"open3d_small_translation_m": 0.02904786593904068,
|
|
||||||
"open3d_small_rotation_deg": 0.6456312776257432,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 18,
|
|
||||||
"j": 20,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 18,
|
|
||||||
"j": 21,
|
|
||||||
"open3d_small_translation_m": 0.06690824468510452,
|
|
||||||
"open3d_small_rotation_deg": 0.5787554058174607,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 19,
|
|
||||||
"j": 20,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 20,
|
|
||||||
"j": 21,
|
|
||||||
"open3d_small_translation_m": 0.02975935281075696,
|
|
||||||
"open3d_small_rotation_deg": 0.36957453812106744,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 20,
|
|
||||||
"j": 22,
|
|
||||||
"open3d_small_translation_m": 0.01187767303223306,
|
|
||||||
"open3d_small_rotation_deg": 0.11119975547992203,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 20,
|
|
||||||
"j": 23,
|
|
||||||
"open3d_small_translation_m": 0.02085962034691778,
|
|
||||||
"open3d_small_rotation_deg": 0.14906239098917923,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 22,
|
|
||||||
"open3d_small_translation_m": 0.07344207054217174,
|
|
||||||
"open3d_small_rotation_deg": 0.4720715999477229,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 24,
|
|
||||||
"open3d_small_translation_m": 0.003446026826977358,
|
|
||||||
"open3d_small_rotation_deg": 0.06718280580638468,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 23,
|
|
||||||
"open3d_small_translation_m": 0.0038695246290457396,
|
|
||||||
"open3d_small_rotation_deg": 0.19583405427516354,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 24,
|
|
||||||
"open3d_small_translation_m": 0.008952357752471345,
|
|
||||||
"open3d_small_rotation_deg": 0.2129847177606736,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 23,
|
|
||||||
"j": 24,
|
|
||||||
"open3d_small_translation_m": 0.0569515146956438,
|
|
||||||
"open3d_small_rotation_deg": 0.6801187045572974,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 25,
|
|
||||||
"open3d_small_translation_m": 0.025823784600755004,
|
|
||||||
"open3d_small_rotation_deg": 0.7451009005368011,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 26,
|
|
||||||
"open3d_small_translation_m": 0.005434928313511662,
|
|
||||||
"open3d_small_rotation_deg": 0.38110259637848837,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 26,
|
|
||||||
"open3d_small_translation_m": 0.032394639661318274,
|
|
||||||
"open3d_small_rotation_deg": 0.6672581993183133,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 28,
|
|
||||||
"open3d_small_translation_m": 0.03319908369315011,
|
|
||||||
"open3d_small_rotation_deg": 0.3374096789165904,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 29,
|
|
||||||
"open3d_small_translation_m": 0.064122224643194,
|
|
||||||
"open3d_small_rotation_deg": 0.3369642063017735,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 28,
|
|
||||||
"j": 29,
|
|
||||||
"open3d_small_translation_m": 0.04054098962864038,
|
|
||||||
"open3d_small_rotation_deg": 0.6304817774750828,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 31,
|
|
||||||
"open3d_small_translation_m": 0.06429006702815496,
|
|
||||||
"open3d_small_rotation_deg": 0.6364945448655926,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 33,
|
|
||||||
"open3d_small_translation_m": 0.02846544971259277,
|
|
||||||
"open3d_small_rotation_deg": 0.5396634629075986,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 31,
|
|
||||||
"j": 33,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 33,
|
|
||||||
"j": 34,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 33,
|
|
||||||
"j": 35,
|
|
||||||
"open3d_small_translation_m": 0.012003505646551895,
|
|
||||||
"open3d_small_rotation_deg": 0.2900810668840737,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 34,
|
|
||||||
"j": 35,
|
|
||||||
"open3d_small_translation_m": 0.034420937161804874,
|
|
||||||
"open3d_small_rotation_deg": 0.3929515224293036,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 34,
|
|
||||||
"j": 37,
|
|
||||||
"open3d_small_translation_m": 0.017073125816226477,
|
|
||||||
"open3d_small_rotation_deg": 0.5761893617071098,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 35,
|
|
||||||
"j": 36,
|
|
||||||
"open3d_small_translation_m": 0.009761763696948316,
|
|
||||||
"open3d_small_rotation_deg": 0.47958651768297733,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
Binary file not shown.
@@ -1,530 +0,0 @@
|
|||||||
{
|
|
||||||
"selection_is_X_independent": true,
|
|
||||||
"B_source": "Open3D; small_gicp is used only as an agreement gate",
|
|
||||||
"max_translation_m": 0.05,
|
|
||||||
"max_rotation_deg": 0.5,
|
|
||||||
"input_open3d_pairs": 66,
|
|
||||||
"accepted_pairs": 39,
|
|
||||||
"pairs": [
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 1,
|
|
||||||
"open3d_small_translation_m": 0.014112857127352833,
|
|
||||||
"open3d_small_rotation_deg": 0.9322795088817045,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 2,
|
|
||||||
"open3d_small_translation_m": 0.04048559177128157,
|
|
||||||
"open3d_small_rotation_deg": 0.13828514886662066,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 0,
|
|
||||||
"j": 3,
|
|
||||||
"open3d_small_translation_m": 0.043319730028129194,
|
|
||||||
"open3d_small_rotation_deg": 0.18606836567929377,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 2,
|
|
||||||
"open3d_small_translation_m": 0.027224756971621137,
|
|
||||||
"open3d_small_rotation_deg": 0.11107273153325639,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 3,
|
|
||||||
"open3d_small_translation_m": 0.04006763116408136,
|
|
||||||
"open3d_small_rotation_deg": 0.3402294071905194,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 1,
|
|
||||||
"j": 4,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 3,
|
|
||||||
"open3d_small_translation_m": 0.01124849125851201,
|
|
||||||
"open3d_small_rotation_deg": 0.08934477097506471,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 4,
|
|
||||||
"open3d_small_translation_m": 0.03581057712566857,
|
|
||||||
"open3d_small_rotation_deg": 0.47251701196497253,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 2,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.08528072944363045,
|
|
||||||
"open3d_small_rotation_deg": 1.2141759174965312,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 4,
|
|
||||||
"open3d_small_translation_m": 0.0075101104952604815,
|
|
||||||
"open3d_small_rotation_deg": 0.18179062115813846,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 3,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.024397939276152487,
|
|
||||||
"open3d_small_rotation_deg": 0.3105330721864524,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 4,
|
|
||||||
"j": 5,
|
|
||||||
"open3d_small_translation_m": 0.020705651033865183,
|
|
||||||
"open3d_small_rotation_deg": 0.1233418901694322,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 4,
|
|
||||||
"j": 7,
|
|
||||||
"open3d_small_translation_m": 0.020920545183303704,
|
|
||||||
"open3d_small_rotation_deg": 0.7722300773846386,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 5,
|
|
||||||
"j": 7,
|
|
||||||
"open3d_small_translation_m": 0.04282503694424416,
|
|
||||||
"open3d_small_rotation_deg": 0.5633825712787606,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 5,
|
|
||||||
"j": 8,
|
|
||||||
"open3d_small_translation_m": 0.11029404203405435,
|
|
||||||
"open3d_small_rotation_deg": 0.5325792081092059,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 7,
|
|
||||||
"open3d_small_translation_m": 0.03271497478931517,
|
|
||||||
"open3d_small_rotation_deg": 0.31987628752295666,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 6,
|
|
||||||
"j": 8,
|
|
||||||
"open3d_small_translation_m": 0.07284151111110872,
|
|
||||||
"open3d_small_rotation_deg": 1.4450826858938794,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 7,
|
|
||||||
"j": 8,
|
|
||||||
"open3d_small_translation_m": 0.09279358525836445,
|
|
||||||
"open3d_small_rotation_deg": 0.19167684909579139,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 7,
|
|
||||||
"j": 9,
|
|
||||||
"open3d_small_translation_m": 0.012050834357664957,
|
|
||||||
"open3d_small_rotation_deg": 0.4077277697131033,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 7,
|
|
||||||
"j": 10,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 8,
|
|
||||||
"j": 9,
|
|
||||||
"open3d_small_translation_m": 0.026603663511016604,
|
|
||||||
"open3d_small_rotation_deg": 0.3213901608387603,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 8,
|
|
||||||
"j": 10,
|
|
||||||
"open3d_small_translation_m": 0.029568992143279488,
|
|
||||||
"open3d_small_rotation_deg": 0.42606536158015423,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 9,
|
|
||||||
"j": 10,
|
|
||||||
"open3d_small_translation_m": 0.03641531538812563,
|
|
||||||
"open3d_small_rotation_deg": 0.20836252860025806,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 10,
|
|
||||||
"j": 11,
|
|
||||||
"open3d_small_translation_m": 0.013655861114026318,
|
|
||||||
"open3d_small_rotation_deg": 0.7320732922437044,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 11,
|
|
||||||
"j": 12,
|
|
||||||
"open3d_small_translation_m": 0.015611342863539268,
|
|
||||||
"open3d_small_rotation_deg": 0.5694244139356097,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 11,
|
|
||||||
"j": 13,
|
|
||||||
"open3d_small_translation_m": 0.08667727215031616,
|
|
||||||
"open3d_small_rotation_deg": 0.8576430944387251,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 11,
|
|
||||||
"j": 14,
|
|
||||||
"open3d_small_translation_m": 0.013212394743280101,
|
|
||||||
"open3d_small_rotation_deg": 0.5395038717718552,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 13,
|
|
||||||
"open3d_small_translation_m": 0.05511305334837847,
|
|
||||||
"open3d_small_rotation_deg": 1.2808633685978204,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 14,
|
|
||||||
"open3d_small_translation_m": 0.01673887890907281,
|
|
||||||
"open3d_small_rotation_deg": 0.4590349395304042,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 12,
|
|
||||||
"j": 15,
|
|
||||||
"open3d_small_translation_m": 0.09777961576971127,
|
|
||||||
"open3d_small_rotation_deg": 0.6299487235133375,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 14,
|
|
||||||
"open3d_small_translation_m": 0.03968493416626958,
|
|
||||||
"open3d_small_rotation_deg": 1.39604879980664,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 15,
|
|
||||||
"open3d_small_translation_m": 0.04654254454408651,
|
|
||||||
"open3d_small_rotation_deg": 0.19549226846654652,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 13,
|
|
||||||
"j": 16,
|
|
||||||
"open3d_small_translation_m": 0.016539968897939728,
|
|
||||||
"open3d_small_rotation_deg": 0.5036637170277819,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 14,
|
|
||||||
"j": 15,
|
|
||||||
"open3d_small_translation_m": 0.08740655127874042,
|
|
||||||
"open3d_small_rotation_deg": 0.9024809168777701,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 14,
|
|
||||||
"j": 16,
|
|
||||||
"open3d_small_translation_m": 0.007961162148769676,
|
|
||||||
"open3d_small_rotation_deg": 0.3128279997254769,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 15,
|
|
||||||
"j": 16,
|
|
||||||
"open3d_small_translation_m": 0.05660525597010978,
|
|
||||||
"open3d_small_rotation_deg": 0.3498659583909866,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 17,
|
|
||||||
"j": 18,
|
|
||||||
"open3d_small_translation_m": 0.012632921988089662,
|
|
||||||
"open3d_small_rotation_deg": 0.23041873752448738,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 18,
|
|
||||||
"j": 19,
|
|
||||||
"open3d_small_translation_m": 0.02031854418240709,
|
|
||||||
"open3d_small_rotation_deg": 0.10632729898220249,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 20,
|
|
||||||
"j": 21,
|
|
||||||
"open3d_small_translation_m": 0.013503153993963056,
|
|
||||||
"open3d_small_rotation_deg": 0.17004359316703027,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 21,
|
|
||||||
"j": 22,
|
|
||||||
"open3d_small_translation_m": 0.06340422221619042,
|
|
||||||
"open3d_small_rotation_deg": 0.5794375484544898,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 23,
|
|
||||||
"open3d_small_translation_m": 0.0058706045875484576,
|
|
||||||
"open3d_small_rotation_deg": 0.09261171568609482,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 24,
|
|
||||||
"open3d_small_translation_m": 0.035452027277204735,
|
|
||||||
"open3d_small_rotation_deg": 0.344137196022356,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 22,
|
|
||||||
"j": 25,
|
|
||||||
"open3d_small_translation_m": 0.016505964730377052,
|
|
||||||
"open3d_small_rotation_deg": 0.2843410430726553,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 23,
|
|
||||||
"j": 24,
|
|
||||||
"open3d_small_translation_m": 0.00794618091675699,
|
|
||||||
"open3d_small_rotation_deg": 0.3391363354920984,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 25,
|
|
||||||
"open3d_small_translation_m": 0.005640140207369355,
|
|
||||||
"open3d_small_rotation_deg": 0.40156498371331323,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 26,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 24,
|
|
||||||
"j": 27,
|
|
||||||
"open3d_small_translation_m": 0.0748996904677792,
|
|
||||||
"open3d_small_rotation_deg": 0.13727707305657308,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 26,
|
|
||||||
"open3d_small_translation_m": 0.03081891880529517,
|
|
||||||
"open3d_small_rotation_deg": 0.36020242073965086,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 25,
|
|
||||||
"j": 27,
|
|
||||||
"open3d_small_translation_m": 0.021168896509812032,
|
|
||||||
"open3d_small_rotation_deg": 0.14597845760153952,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 27,
|
|
||||||
"open3d_small_translation_m": 0.01536711683556009,
|
|
||||||
"open3d_small_rotation_deg": 0.4140655889201126,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 28,
|
|
||||||
"open3d_small_translation_m": 0.00692948322087535,
|
|
||||||
"open3d_small_rotation_deg": 0.2580451959432504,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 26,
|
|
||||||
"j": 29,
|
|
||||||
"open3d_small_translation_m": 0.004434517854459698,
|
|
||||||
"open3d_small_rotation_deg": 0.3814089109711929,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 28,
|
|
||||||
"open3d_small_translation_m": 0.050137370980335605,
|
|
||||||
"open3d_small_rotation_deg": 0.3289989587699738,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 29,
|
|
||||||
"open3d_small_translation_m": 0.05401741972476896,
|
|
||||||
"open3d_small_rotation_deg": 0.5027869137341908,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 27,
|
|
||||||
"j": 30,
|
|
||||||
"open3d_small_translation_m": 0.014426259697867812,
|
|
||||||
"open3d_small_rotation_deg": 0.10299393098756303,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 29,
|
|
||||||
"j": 30,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "not_in_small_gicp_refined"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 31,
|
|
||||||
"open3d_small_translation_m": 0.017329917379458395,
|
|
||||||
"open3d_small_rotation_deg": 0.22398877826738903,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 30,
|
|
||||||
"j": 32,
|
|
||||||
"open3d_small_translation_m": 0.014715435206511102,
|
|
||||||
"open3d_small_rotation_deg": 0.11629038752134123,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 31,
|
|
||||||
"j": 32,
|
|
||||||
"open3d_small_translation_m": 0.015582546369975235,
|
|
||||||
"open3d_small_rotation_deg": 0.5827409440555306,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 32,
|
|
||||||
"j": 33,
|
|
||||||
"open3d_small_translation_m": 0.008274791505352475,
|
|
||||||
"open3d_small_rotation_deg": 0.16710510386507704,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 32,
|
|
||||||
"j": 34,
|
|
||||||
"open3d_small_translation_m": 0.009965535682972116,
|
|
||||||
"open3d_small_rotation_deg": 0.25506349516996085,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 32,
|
|
||||||
"j": 35,
|
|
||||||
"open3d_small_translation_m": 0.013983188091914709,
|
|
||||||
"open3d_small_rotation_deg": 0.4717278366783201,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 33,
|
|
||||||
"j": 34,
|
|
||||||
"open3d_small_translation_m": 0.01464477642231769,
|
|
||||||
"open3d_small_rotation_deg": 0.23054883080903335,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 33,
|
|
||||||
"j": 35,
|
|
||||||
"open3d_small_translation_m": 0.02477884255472743,
|
|
||||||
"open3d_small_rotation_deg": 0.21737771891740004,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 34,
|
|
||||||
"j": 35,
|
|
||||||
"open3d_small_translation_m": 0.05732289121946012,
|
|
||||||
"open3d_small_rotation_deg": 0.16151350666473244,
|
|
||||||
"accepted": false,
|
|
||||||
"reason": "backend_disagreement"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"i": 35,
|
|
||||||
"j": 36,
|
|
||||||
"open3d_small_translation_m": 0.01157700515581757,
|
|
||||||
"open3d_small_rotation_deg": 0.49328772540040833,
|
|
||||||
"accepted": true,
|
|
||||||
"reason": ""
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
Binary file not shown.
@@ -1,179 +0,0 @@
|
|||||||
{
|
|
||||||
"role": "auxiliary check only; first-batch RTK is sparse",
|
|
||||||
"blind_with_respect_to_X": true,
|
|
||||||
"note": "No AX residual was used to select these pairs",
|
|
||||||
"stations": 38,
|
|
||||||
"metrics": {
|
|
||||||
"pairs": 22,
|
|
||||||
"translation_m": {
|
|
||||||
"rms": 0.06067269746244947,
|
|
||||||
"median": 0.042680679229823305,
|
|
||||||
"p90": 0.090942947259565,
|
|
||||||
"p95": 0.10135707648063859,
|
|
||||||
"max": 0.14643003383767556
|
|
||||||
},
|
|
||||||
"rotation_deg": {
|
|
||||||
"rms": 1.002984942285067,
|
|
||||||
"median": 0.707406938077333,
|
|
||||||
"p90": 1.573355332434981,
|
|
||||||
"p95": 1.7757584051966382,
|
|
||||||
"max": 1.8710372365314285
|
|
||||||
},
|
|
||||||
"per_pair": [
|
|
||||||
{
|
|
||||||
"pair_index": 0,
|
|
||||||
"translation_m": 0.048343805321803004,
|
|
||||||
"rotation_deg": 0.6668376546913104,
|
|
||||||
"i": 0,
|
|
||||||
"j": 3
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 1,
|
|
||||||
"translation_m": 0.0423483041362295,
|
|
||||||
"rotation_deg": 1.7859688557628564,
|
|
||||||
"i": 1,
|
|
||||||
"j": 2
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 2,
|
|
||||||
"translation_m": 0.02300615185240312,
|
|
||||||
"rotation_deg": 1.8710372365314285,
|
|
||||||
"i": 2,
|
|
||||||
"j": 3
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 3,
|
|
||||||
"translation_m": 0.09155252198785023,
|
|
||||||
"rotation_deg": 0.6733340854508988,
|
|
||||||
"i": 3,
|
|
||||||
"j": 4
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 4,
|
|
||||||
"translation_m": 0.03158045213093479,
|
|
||||||
"rotation_deg": 0.2310116592109763,
|
|
||||||
"i": 3,
|
|
||||||
"j": 5
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 5,
|
|
||||||
"translation_m": 0.07825296974670955,
|
|
||||||
"rotation_deg": 0.7403656481818025,
|
|
||||||
"i": 4,
|
|
||||||
"j": 5
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 6,
|
|
||||||
"translation_m": 0.03623050776977002,
|
|
||||||
"rotation_deg": 0.29976499205053747,
|
|
||||||
"i": 5,
|
|
||||||
"j": 6
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 7,
|
|
||||||
"translation_m": 0.017322045818086197,
|
|
||||||
"rotation_deg": 1.0428116209013618,
|
|
||||||
"i": 6,
|
|
||||||
"j": 7
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 8,
|
|
||||||
"translation_m": 0.037890995883582496,
|
|
||||||
"rotation_deg": 0.5003524050137443,
|
|
||||||
"i": 6,
|
|
||||||
"j": 9
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 9,
|
|
||||||
"translation_m": 0.055811972906268326,
|
|
||||||
"rotation_deg": 0.9711899476368479,
|
|
||||||
"i": 11,
|
|
||||||
"j": 12
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 10,
|
|
||||||
"translation_m": 0.10187310566446957,
|
|
||||||
"rotation_deg": 0.48973592009907335,
|
|
||||||
"i": 17,
|
|
||||||
"j": 18
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 11,
|
|
||||||
"translation_m": 0.0304808468879537,
|
|
||||||
"rotation_deg": 1.3239848803794854,
|
|
||||||
"i": 20,
|
|
||||||
"j": 21
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 12,
|
|
||||||
"translation_m": 0.04462456608967399,
|
|
||||||
"rotation_deg": 0.2812977599385038,
|
|
||||||
"i": 20,
|
|
||||||
"j": 22
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 13,
|
|
||||||
"translation_m": 0.03611683783839059,
|
|
||||||
"rotation_deg": 0.7096084761069892,
|
|
||||||
"i": 20,
|
|
||||||
"j": 23
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 14,
|
|
||||||
"translation_m": 0.07267248449673575,
|
|
||||||
"rotation_deg": 0.6614151319251916,
|
|
||||||
"i": 21,
|
|
||||||
"j": 24
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 15,
|
|
||||||
"translation_m": 0.04417889214251473,
|
|
||||||
"rotation_deg": 0.537164719319421,
|
|
||||||
"i": 22,
|
|
||||||
"j": 23
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 16,
|
|
||||||
"translation_m": 0.020100630082582033,
|
|
||||||
"rotation_deg": 1.4866116021028042,
|
|
||||||
"i": 22,
|
|
||||||
"j": 24
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 17,
|
|
||||||
"translation_m": 0.04301305432341711,
|
|
||||||
"rotation_deg": 0.7052054000476768,
|
|
||||||
"i": 24,
|
|
||||||
"j": 26
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 18,
|
|
||||||
"translation_m": 0.0344886352240195,
|
|
||||||
"rotation_deg": 1.4977147244033493,
|
|
||||||
"i": 27,
|
|
||||||
"j": 28
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 19,
|
|
||||||
"translation_m": 0.14643003383767556,
|
|
||||||
"rotation_deg": 0.527768319167481,
|
|
||||||
"i": 33,
|
|
||||||
"j": 35
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 20,
|
|
||||||
"translation_m": 0.08545677470499786,
|
|
||||||
"rotation_deg": 0.7336105280022196,
|
|
||||||
"i": 34,
|
|
||||||
"j": 35
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 21,
|
|
||||||
"translation_m": 0.025212589301108413,
|
|
||||||
"rotation_deg": 1.5817598444384953,
|
|
||||||
"i": 35,
|
|
||||||
"j": 36
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,399 +0,0 @@
|
|||||||
{
|
|
||||||
"schema_version": 2,
|
|
||||||
"success": true,
|
|
||||||
"message": "`ftol` termination condition is satisfied.",
|
|
||||||
"convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
|
|
||||||
"equation": "A_ij X = X B_ij",
|
|
||||||
"measured_extrinsic_used_as_initial": false,
|
|
||||||
"translation_m": [
|
|
||||||
1.2977596922478565,
|
|
||||||
-6.733076264476427e-05,
|
|
||||||
0.7204978345266887
|
|
||||||
],
|
|
||||||
"rotation_rpy_deg_xyz": [
|
|
||||||
-0.7851511464150316,
|
|
||||||
1.2026608220810453,
|
|
||||||
-0.835510052785851
|
|
||||||
],
|
|
||||||
"quaternion_xyzw": [
|
|
||||||
-0.0067746045444860015,
|
|
||||||
0.010544431385358818,
|
|
||||||
-0.0072186568687935965,
|
|
||||||
0.9998953998759249
|
|
||||||
],
|
|
||||||
"matrix_4x4": [
|
|
||||||
[
|
|
||||||
0.9996734119195402,
|
|
||||||
0.014292934887214408,
|
|
||||||
0.021184463964312037,
|
|
||||||
1.2977596922478565
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.01457867229834346,
|
|
||||||
0.9998039914525529,
|
|
||||||
0.013395558575925315,
|
|
||||||
-6.733076264476427e-05
|
|
||||||
],
|
|
||||||
[
|
|
||||||
-0.020988849781798404,
|
|
||||||
-0.013700025104115046,
|
|
||||||
0.9996858394000506,
|
|
||||||
0.7204978345266887
|
|
||||||
],
|
|
||||||
[
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
1.0
|
|
||||||
]
|
|
||||||
],
|
|
||||||
"estimation": {
|
|
||||||
"stations": 38,
|
|
||||||
"pairs": 39,
|
|
||||||
"residuals": {
|
|
||||||
"pairs": 39,
|
|
||||||
"translation_m": {
|
|
||||||
"rms": 0.07984780651249465,
|
|
||||||
"median": 0.05392926910429046,
|
|
||||||
"p90": 0.11822807898946641,
|
|
||||||
"p95": 0.14420733112402187,
|
|
||||||
"max": 0.2330103519747209
|
|
||||||
},
|
|
||||||
"rotation_deg": {
|
|
||||||
"rms": 0.961176171904301,
|
|
||||||
"median": 0.8002960897011372,
|
|
||||||
"p90": 1.422624767604584,
|
|
||||||
"p95": 1.4793113785217964,
|
|
||||||
"max": 1.7237415928660533
|
|
||||||
},
|
|
||||||
"per_pair": [
|
|
||||||
{
|
|
||||||
"pair_index": 0,
|
|
||||||
"translation_m": 0.1353760421849012,
|
|
||||||
"rotation_deg": 0.5166365681634405
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 1,
|
|
||||||
"translation_m": 0.11487543918383647,
|
|
||||||
"rotation_deg": 0.5717221816616095
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 2,
|
|
||||||
"translation_m": 0.06210064976174764,
|
|
||||||
"rotation_deg": 0.4402983357286552
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 3,
|
|
||||||
"translation_m": 0.054355548674489614,
|
|
||||||
"rotation_deg": 0.5821611725015269
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 4,
|
|
||||||
"translation_m": 0.02391657882546618,
|
|
||||||
"rotation_deg": 0.32738975448002894
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 5,
|
|
||||||
"translation_m": 0.07775220171630512,
|
|
||||||
"rotation_deg": 0.8706732532310371
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 6,
|
|
||||||
"translation_m": 0.06265840922697155,
|
|
||||||
"rotation_deg": 0.8002960897011372
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 7,
|
|
||||||
"translation_m": 0.09282561040685418,
|
|
||||||
"rotation_deg": 1.0484294819479303
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 8,
|
|
||||||
"translation_m": 0.034474807960716856,
|
|
||||||
"rotation_deg": 0.40941928556312257
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 9,
|
|
||||||
"translation_m": 0.13163863821198593,
|
|
||||||
"rotation_deg": 1.1065820279922887
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 10,
|
|
||||||
"translation_m": 0.0018609877360485794,
|
|
||||||
"rotation_deg": 0.6081452332544832
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 11,
|
|
||||||
"translation_m": 0.06488404028756532,
|
|
||||||
"rotation_deg": 0.05192051717362457
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 12,
|
|
||||||
"translation_m": 0.1147488321233841,
|
|
||||||
"rotation_deg": 0.9937108406424444
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 13,
|
|
||||||
"translation_m": 0.05392926910429046,
|
|
||||||
"rotation_deg": 0.9700546737541248
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 14,
|
|
||||||
"translation_m": 0.02302185790244543,
|
|
||||||
"rotation_deg": 1.6149869587730064
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 15,
|
|
||||||
"translation_m": 0.2330103519747209,
|
|
||||||
"rotation_deg": 0.5939965715201481
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 16,
|
|
||||||
"translation_m": 0.2236889315761065,
|
|
||||||
"rotation_deg": 1.1151964340680336
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 17,
|
|
||||||
"translation_m": 0.04090994021171706,
|
|
||||||
"rotation_deg": 0.7785618518380144
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 18,
|
|
||||||
"translation_m": 0.054652520003134704,
|
|
||||||
"rotation_deg": 1.124375763416717
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 19,
|
|
||||||
"translation_m": 0.04547500551567543,
|
|
||||||
"rotation_deg": 0.6556954445211121
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 20,
|
|
||||||
"translation_m": 0.02484690940972022,
|
|
||||||
"rotation_deg": 1.7237415928660533
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 21,
|
|
||||||
"translation_m": 0.056832852413462566,
|
|
||||||
"rotation_deg": 0.5831473606925555
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 22,
|
|
||||||
"translation_m": 0.03998842216950218,
|
|
||||||
"rotation_deg": 1.124691549503354
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 23,
|
|
||||||
"translation_m": 0.052224143195848054,
|
|
||||||
"rotation_deg": 1.1184574012171749
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 24,
|
|
||||||
"translation_m": 0.06305629062353875,
|
|
||||||
"rotation_deg": 0.5252019728243343
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 25,
|
|
||||||
"translation_m": 0.0552093328283033,
|
|
||||||
"rotation_deg": 0.08705258513790126
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 26,
|
|
||||||
"translation_m": 0.08502281683725502,
|
|
||||||
"rotation_deg": 0.6459827373704314
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 27,
|
|
||||||
"translation_m": 0.03727269737723399,
|
|
||||||
"rotation_deg": 0.7631660358041192
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 28,
|
|
||||||
"translation_m": 0.04543722819346571,
|
|
||||||
"rotation_deg": 1.4218618784137362
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 29,
|
|
||||||
"translation_m": 0.02559982081388642,
|
|
||||||
"rotation_deg": 1.388947601285772
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 30,
|
|
||||||
"translation_m": 0.01827234061769129,
|
|
||||||
"rotation_deg": 1.206866980510853
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 31,
|
|
||||||
"translation_m": 0.06863732394647468,
|
|
||||||
"rotation_deg": 1.4642363140494394
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 32,
|
|
||||||
"translation_m": 0.04770629220585725,
|
|
||||||
"rotation_deg": 0.6244903967618446
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 33,
|
|
||||||
"translation_m": 0.027074935362624366,
|
|
||||||
"rotation_deg": 1.3688430767407767
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 34,
|
|
||||||
"translation_m": 0.03301156138156032,
|
|
||||||
"rotation_deg": 1.4256763243679755
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 35,
|
|
||||||
"translation_m": 0.054240160043693975,
|
|
||||||
"rotation_deg": 0.6486412679040612
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 36,
|
|
||||||
"translation_m": 0.021992835671895014,
|
|
||||||
"rotation_deg": 0.23769553016909395
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 37,
|
|
||||||
"translation_m": 0.031890552923517286,
|
|
||||||
"rotation_deg": 1.0832171246083164
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"pair_index": 38,
|
|
||||||
"translation_m": 0.025286131886091672,
|
|
||||||
"rotation_deg": 1.2215923174512224
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"ground": {
|
|
||||||
"planes": 38,
|
|
||||||
"body_origin_height_above_ground_m": 0.2335,
|
|
||||||
"formula": "d_lidar - (R_X n_lidar)^T t_X - body_height"
|
|
||||||
},
|
|
||||||
"linearized_one_sigma": {
|
|
||||||
"translation_m": [
|
|
||||||
0.0061384486036208784,
|
|
||||||
0.006616582082990767,
|
|
||||||
0.004372364137893693
|
|
||||||
],
|
|
||||||
"rotation_deg": [
|
|
||||||
0.06746979098802125,
|
|
||||||
0.05919345389075773,
|
|
||||||
0.16157711056486687
|
|
||||||
],
|
|
||||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
|
||||||
},
|
|
||||||
"weighted_jacobian_condition_number": 6.965138763120991,
|
|
||||||
"solver_multistart": {
|
|
||||||
"runs": 12,
|
|
||||||
"candidates_relative_to_best": [
|
|
||||||
{
|
|
||||||
"cost": 152.7941369486616,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 5.01999140559543e-10,
|
|
||||||
"rotation_deg": 7.693408441139155e-09
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866156,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 1.7788160768171148e-10,
|
|
||||||
"rotation_deg": 3.2213546311649044e-09
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.7941369486616,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 2.04052729806573e-10,
|
|
||||||
"rotation_deg": 5.128825141169912e-09
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866287,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 3.6987521890593636e-09,
|
|
||||||
"rotation_deg": 1.471926437983498e-07
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866153,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 3.0367491381779985e-10,
|
|
||||||
"rotation_deg": 3.27791256123688e-09
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866173,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 6.928454212106221e-10,
|
|
||||||
"rotation_deg": 2.8875952395155353e-08
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866153,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 1.9628898247072475e-10,
|
|
||||||
"rotation_deg": 3.769277991759282e-09
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866167,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 1.8108640246434077e-09,
|
|
||||||
"rotation_deg": 5.6964069970580913e-08
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866145,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 0.0,
|
|
||||||
"rotation_deg": 0.0
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.7941369486631,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 3.8878373558477815e-09,
|
|
||||||
"rotation_deg": 1.567977862312646e-07
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866162,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 1.928688225276107e-09,
|
|
||||||
"rotation_deg": 6.007537452298471e-08
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"cost": 152.79413694866165,
|
|
||||||
"success": true,
|
|
||||||
"translation_m": 6.380398519737792e-10,
|
|
||||||
"rotation_deg": 2.6596239917974983e-08
|
|
||||||
}
|
|
||||||
]
|
|
||||||
},
|
|
||||||
"bootstrap": {
|
|
||||||
"runs": 100,
|
|
||||||
"order": [
|
|
||||||
"x_m",
|
|
||||||
"y_m",
|
|
||||||
"z_m",
|
|
||||||
"roll_deg",
|
|
||||||
"pitch_deg",
|
|
||||||
"yaw_deg"
|
|
||||||
],
|
|
||||||
"std": [
|
|
||||||
0.003223574650236103,
|
|
||||||
0.003334649335095665,
|
|
||||||
0.0016858977028131694,
|
|
||||||
0.0980155224808408,
|
|
||||||
0.07461769310738842,
|
|
||||||
0.11886090357304822
|
|
||||||
],
|
|
||||||
"p025": [
|
|
||||||
1.2916920074190312,
|
|
||||||
-0.005413227092587022,
|
|
||||||
0.7171569502776788,
|
|
||||||
-0.9695959273616203,
|
|
||||||
1.066246526462818,
|
|
||||||
-1.100751980508544
|
|
||||||
],
|
|
||||||
"p975": [
|
|
||||||
1.3034038047298853,
|
|
||||||
0.007138524793626496,
|
|
||||||
0.7235834879340349,
|
|
||||||
-0.6076030002143074,
|
|
||||||
1.3517461178267727,
|
|
||||||
-0.6468477685453492
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,19 +0,0 @@
|
|||||||
label,roll_correction_deg,pitch_correction_deg,yaw_correction_deg,selected_pair_translation_cm,selected_pair_rotation_deg,all_pair_translation_rms_m,all_pair_rotation_rms_deg,normalized_global_rms,normalized_global_rms_change,improved_pairs,worsened_pairs,global_consistency_signal,ground_normal_tilt_rms_deg,ground_height_rms_m
|
|
||||||
baseline,0.0,0.0,0.0,13.53760421849012,0.5166365681634405,0.07984780651249465,0.961176171904301,2.4991413356769443,0.0,0,0,False,1.6662748432297785,0.03471396517121729
|
|
||||||
pitch_+0.100,0.0,0.1,0.0,13.322368374959654,0.4851311566747633,0.07937622701088871,0.9709178397492695,2.508178616440067,0.009037280763122713,17,22,False,1.661255702204205,0.03471396517121729
|
|
||||||
pitch_+0.200,0.0,0.2,0.0,13.107741962217032,0.45594025406310634,0.07905894857986535,0.991843015836615,2.536757223795175,0.03761588811823069,17,22,False,1.6622470214176333,0.03471396517121729
|
|
||||||
pitch_+0.300,0.0,0.3,0.0,12.89375489636804,0.4295360884271374,0.07889765995849754,1.0232658257505307,2.5842267894606974,0.0850854537837531,16,23,False,1.669238092496052,0.034713965171217276
|
|
||||||
pitch_+0.000_roll_-0.200,-0.2,0.0,0.0,13.395035432680281,0.501501180601739,0.08136434456273667,0.9671396098919116,2.5277457796497282,0.028604443972783944,12,27,False,1.7107124278398285,0.034713965171217304
|
|
||||||
pitch_+0.000_roll_-0.100,-0.1,0.0,0.0,13.466127879921094,0.5071126466746416,0.08054864611850543,0.958376027630505,2.503831503507573,0.004690167830628589,13,26,False,1.685676496509504,0.034713965171217304
|
|
||||||
pitch_+0.000_roll_+0.100,0.1,0.0,0.0,13.609457775458583,0.5298619890141043,0.07926481666577584,0.9754404878748625,2.5137822192795105,0.014640883602566213,23,16,False,1.652705907802981,0.034713965171217304
|
|
||||||
pitch_+0.100_roll_-0.200,-0.2,0.1,0.0,13.180263465884618,0.4689801268726104,0.08100204146829142,0.9768217811391146,2.5379628557651293,0.03882152008818496,14,25,False,1.7058242505297496,0.034713965171217304
|
|
||||||
pitch_+0.100_roll_-0.100,-0.1,0.1,0.0,13.251119920699841,0.47497594758710626,0.08013195956996022,0.9681458624297539,2.5134991588556583,0.014357823178714035,17,22,False,1.6807154082401394,0.03471396517121729
|
|
||||||
pitch_+0.100_roll_+0.100,0.1,0.1,0.0,13.394001942784334,0.4991919434350986,0.07873808044179886,0.9850410981186302,2.5221217227950565,0.022980387118112233,19,20,False,1.6476453039285432,0.0347139651712173
|
|
||||||
pitch_+0.200_roll_-0.200,-0.2,0.2,0.0,12.966120616418092,0.43871598020210434,0.08079188106279227,0.9976230703529106,2.567477045713792,0.0683357100368478,15,24,False,1.7067898021773054,0.034713965171217304
|
|
||||||
pitch_+0.200_roll_-0.100,-0.1,0.2,0.0,13.036731063691272,0.44511961856905025,0.07986863254178682,0.989129669887256,2.5426579402355864,0.04351660455864215,19,20,False,1.6816953294526065,0.03471396517121731
|
|
||||||
pitch_+0.200_roll_+0.100,0.1,0.2,0.0,13.179146202690733,0.4708734484159498,0.07836629319834118,1.0056724288764685,2.5499055866185576,0.0507642509416133,19,20,False,1.648644705864238,0.034713965171217304
|
|
||||||
pitch_+0.300_roll_-0.200,-0.2,0.3,0.0,12.752638042306833,0.4112074527490782,0.08073488194995813,1.0288692761236073,2.6156330810711834,0.11649174539423912,15,24,False,1.7135991876939975,0.03471396517121731
|
|
||||||
pitch_+0.300_roll_-0.100,-0.1,0.3,0.0,12.822991835578721,0.41803263983275235,0.07976001272785865,1.020636005744607,2.5906476559016074,0.0915063202246631,16,23,False,1.6886059172123598,0.0347139651712173
|
|
||||||
pitch_+0.300_roll_+0.100,0.1,0.3,0.0,12.964919880829425,0.44535539842024163,0.07815149226179761,1.036676092226784,2.596507573236926,0.09736623755998153,18,21,False,1.6556931398905776,0.034713965171217304
|
|
||||||
yaw_-0.200_diagnostic,0.0,0.0,-0.2,13.607478813867315,0.5166365681634414,0.08024771376258277,0.961176171904301,2.5042597228312786,0.005118387154334325,18,21,False,1.6662748432297787,0.0347139651712173
|
|
||||||
yaw_+0.200_diagnostic,0.0,0.0,0.2,13.479715314434312,0.5166365681634427,0.08028135387591602,0.961176171904301,2.5046909681249,0.005549632447955588,13,26,False,1.6662748432297787,0.0347139651712173
|
|
||||||
|
File diff suppressed because it is too large
Load Diff
@@ -1,19 +0,0 @@
|
|||||||
label,roll_correction_deg,pitch_correction_deg,yaw_correction_deg,selected_pair_translation_cm,selected_pair_rotation_deg,all_pair_translation_rms_m,all_pair_rotation_rms_deg,normalized_global_rms,normalized_global_rms_change,improved_pairs,worsened_pairs,global_consistency_signal,ground_normal_tilt_rms_deg,ground_height_rms_m
|
|
||||||
baseline,0.0,0.0,0.0,7.9716047501124,0.5471090190705759,0.10039155672864886,1.0152957563636194,2.855641391442648,0.0,0,0,False,1.6662748432297785,0.03471396517121729
|
|
||||||
pitch_+0.100,0.0,0.1,0.0,7.877722926634115,0.5242415332114175,0.10031155812336309,1.0345690614552345,2.8820645462820917,0.026423154839443797,27,39,False,1.661255702204205,0.03471396517121729
|
|
||||||
pitch_+0.200,0.0,0.2,0.0,7.783902534642328,0.5013877557627758,0.10034501725742666,1.0647782869175946,2.9262023505769332,0.07056095913428528,26,40,False,1.6622470214176333,0.03471396517121729
|
|
||||||
pitch_+0.300,0.0,0.3,0.0,7.69014529426882,0.4785497243321324,0.1004916878666913,1.1050268752403856,2.9872678674890754,0.1316264760464274,25,41,False,1.669238092496052,0.034713965171217276
|
|
||||||
pitch_+0.000_roll_-0.200,-0.2,0.0,0.0,7.822046878606513,0.5457706857344996,0.10257010426899679,1.0267538544989139,2.9026099336312234,0.04696854218857549,22,44,False,1.7107124278398285,0.034713965171217304
|
|
||||||
pitch_+0.000_roll_-0.100,-0.1,0.0,0.0,7.896797905766369,0.545955313109902,0.10143505999644054,1.0151691591975842,2.870174601960012,0.014533210517364115,25,41,False,1.685676496509504,0.034713965171217304
|
|
||||||
pitch_+0.000_roll_+0.100,0.1,0.0,0.0,8.046465471725153,0.5492256932471735,0.09944242267101946,1.0271293323765827,2.859282573021794,0.0036411815791459468,38,28,False,1.652705907802981,0.034713965171217304
|
|
||||||
pitch_+0.100_roll_-0.200,-0.2,0.1,0.0,7.728092854641688,0.5228446714705329,0.10256719458254226,1.0458160126880083,2.929664876976855,0.07402348553420701,24,42,False,1.7058242505297496,0.034713965171217304
|
|
||||||
pitch_+0.100_roll_-0.100,-0.1,0.1,0.0,7.802880223772661,0.5230373912267472,0.10139401948155134,1.0344448200154543,2.896999001946205,0.04135761050355713,31,35,False,1.6807154082401394,0.03471396517121729
|
|
||||||
pitch_+0.100_roll_+0.100,0.1,0.1,0.0,7.952619015892104,0.5264501536349752,0.09932272291285332,1.0461846579706375,2.885136161262053,0.029494769819405242,30,36,False,1.6476453039285432,0.0347139651712173
|
|
||||||
pitch_+0.200_roll_-0.200,-0.2,0.2,0.0,7.634199171873722,0.49992704876652105,0.10267526429269669,1.0757093910702569,2.9741363007059745,0.11849490926332651,27,39,False,1.7067898021773054,0.034713965171217304
|
|
||||||
pitch_+0.200_roll_-0.100,-0.1,0.2,0.0,7.709023440769963,0.5001285987059726,0.10146524807256484,1.0646575627535269,2.9414386837539155,0.08579729231126754,30,36,False,1.6816953294526065,0.03471396517121731
|
|
||||||
pitch_+0.200_roll_+0.100,0.1,0.2,0.0,7.8588344993060915,0.5036965847149959,0.09931755660454081,1.0760678191977628,2.928699135454269,0.07305774401162113,29,37,False,1.648644705864238,0.034713965171217304
|
|
||||||
pitch_+0.300_roll_-0.200,-0.2,0.3,0.0,7.540367539154748,0.47701910095086286,0.102893833307342,1.1155635852142416,3.035256961724302,0.17961557028165398,25,41,False,1.7135991876939975,0.03471396517121731
|
|
||||||
pitch_+0.300_roll_-0.100,-0.1,0.3,0.0,7.615229271779279,0.47723032409407057,0.10164837804314657,1.1049105360171487,3.0027098217989345,0.14706843035628658,25,41,False,1.6886059172123598,0.0347139651712173
|
|
||||||
pitch_+0.300_roll_+0.100,0.1,0.3,0.0,7.7651136462045685,0.48096817789925783,0.09942680753866891,1.1159092500059349,2.989195517179804,0.13355412573715597,28,38,False,1.6556931398905776,0.034713965171217304
|
|
||||||
yaw_-0.200_diagnostic,0.0,0.0,-0.2,7.9342751812671235,0.5471090190705757,0.10053142254152782,1.0152957563636194,2.8576089058597884,0.0019675144171404924,35,31,False,1.6662748432297787,0.0347139651712173
|
|
||||||
yaw_+0.200_diagnostic,0.0,0.0,0.2,8.012589151848083,0.547109019070576,0.10091495789660097,1.0152957563636194,2.8630112438600364,0.007369852417388412,18,48,False,1.6662748432297787,0.0347139651712173
|
|
||||||
|
-28131
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user