Compare commits
7
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
  lichun.quandCursor
|
c2f99b94a2 | ||
|
lichun.quandCursor
|
2237be77a4 | ||
|
lichun.quandCursor
|
30f7e66db3 | ||
|
lichun.quandCursor
|
4ff176d184 | ||
|
lichun.quandCursor
|
ea06a3a523 | ||
|
lichun.quandCursor
|
e50a79b114 | ||
|
lichun.quandCursor
|
cf1fad7594 |
+8
-1
@@ -1,3 +1,10 @@
|
||||
__pycache__/
|
||||
*.pyc
|
||||
*.py[cod]
|
||||
.pytest_cache/
|
||||
*.egg-info/
|
||||
.eggs/
|
||||
dist/
|
||||
build/
|
||||
examples/synthetic_session/
|
||||
.venv/
|
||||
venv/
|
||||
|
||||
@@ -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,175 @@
|
||||
# LiDAR–双天线 RTK 手眼标定
|
||||
# LiDAR–IMU 外参标定
|
||||
|
||||
本仓库提供一套可从原始 Medulla 记录复现的静态站点标定流程，求解三维激光雷达到后轮轴中心车体系的外参
|
||||
用连续行驶中的 LiDAR 与 IMU 相对运动，估计安装外参与时间偏置：
|
||||
|
||||
```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/`。叠点查看：
|
||||
|
||||
```powershell
|
||||
python tools\visualize_pair_3d.py `
|
||||
--lidar examples\synthetic_session\lidar `
|
||||
--imu examples\synthetic_session\imu.csv `
|
||||
--summary examples\synthetic_session\out\summary.json `
|
||||
--pair-index 0
|
||||
```
|
||||
|
||||
键 `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
|
||||
A_ij = T_W_Bi^-1 T_W_Bj # RTK 给出的车体相对运动
|
||||
B_ij = T_Li_Lj # GICP 给出的雷达相对运动
|
||||
A_ij X = X B_ij
|
||||
```
|
||||
|
||||
当前部署建议仍采用 [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 做数值和三维检查
|
||||
imu_lidar/ 算法与 CLI
|
||||
config/ 车辆配置模板
|
||||
docs/ 采集清单、数据格式、方法细述
|
||||
tools/ 导出、合成复现、可视化
|
||||
tests/ 自动化测试
|
||||
```
|
||||
|
||||
|
||||
## 重要限制
|
||||
| 文档 | 何时看 |
|
||||
| ------------------------------------------------ | ---------------- |
|
||||
| [docs/IMU-LiDAR标定.md](docs/IMU-LiDAR标定.md) | 要看方法约定与实现状态表 |
|
||||
| [tests/README.md](tests/README.md) | 要看合成用例 / S2 烟测记录 |
|
||||
| [imu_lidar/文件职责说明.md](imu_lidar/文件职责说明.md) | 要改源码 |
|
||||
| [imu_lidar/CHANGELOG.md](imu_lidar/CHANGELOG.md) | 要查改动史 |
|
||||
|
||||
- RTK 车体姿态当前是双天线 heading 构造的 yaw-only 轨迹；没有用 RTK pitch/roll 构造 A。
|
||||
- 新式解析器保存 IMU 与 RTK pitch 等原始字段，但当前手眼方程未融合 IMU。
|
||||
- 静止站点法不估计 LiDAR–RTK 时间偏移；时间戳关联必须在导出阶段通过审计。
|
||||
- 地面约束负责 roll、pitch 和 z 的补充可观性，不会独立求出另一套六自由度外参。
|
||||
- 仓库归档的是结果和轻量 B 文件，不包含云盘中的原始点云数据。
|
||||
|
||||
## 专题说明
|
||||
|
||||
- [运动对诊断](PAIR_DIAGNOSTICS.md)
|
||||
- [双后端共识筛选](CONSENSUS_SELECTION.md)
|
||||
- [结果文件索引](results/README.md)
|
||||
改算法请同步职责说明与 CHANGELOG；改对外用法请更新本 README。
|
||||
@@ -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,84 @@
|
||||
schema_version: 1
|
||||
|
||||
vehicle:
|
||||
vehicle_id: "outdoor_usable_20260808"
|
||||
body_frame:
|
||||
name: "base_link"
|
||||
# CAD / 后轮轴中心测量系（与安装图 dX/dY/dZ 一致）
|
||||
axes: "X forward, Y left, Z up"
|
||||
unit: m
|
||||
reference_point: "rear_axle_center"
|
||||
|
||||
installation:
|
||||
installation_id: "20260808_priority_windows"
|
||||
installed_at: "2026-08-08"
|
||||
notes: >
|
||||
HI13R4 + H32 DLogCapture. CAD mounts are origins vs rear axle center
|
||||
(translation only). LiDAR phase-center Z = CAD dZ + 63.5 mm.
|
||||
IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up).
|
||||
LiDAR Cartesian assumed body-aligned.
|
||||
|
||||
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:
|
||||
# CAD 图二：后轮轴中心 → IMU，单位 m（mm/1000）
|
||||
# dX=2574.126255, dY=36.5, dZ=892.5
|
||||
translation_m: [2.574126255, 0.0365, 0.8925]
|
||||
# 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 dX/dY/dZ + HI13R4 manual 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:
|
||||
# CAD 图一：后轮轴中心 → 雷达安装点，再加相位中心 +63.5 mm（仅 Z）
|
||||
# dX=2522.276859, dY=0.020526, dZ=1637.499879+63.5=1700.999879
|
||||
translation_m: [2.522276859, 0.000020526, 1.700999879]
|
||||
# 假设雷达系与车体 CAD 轴一致（导出 XYZ 已按此约定）
|
||||
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 dX/dY/dZ + phase-center +63.5mm on Z; attitude assumed = 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 is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd).
|
||||
Translation prior from CAD + LiDAR phase-center offset; use for full_se3 /
|
||||
sanity, not as hard lock for rotation_only.
|
||||
|
||||
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,195 @@
|
||||
# LiDAR–IMU 外参标定说明
|
||||
|
||||
**用途：** 方法约定与实现状态（深入阅读）。日常使用请先看根目录 [`README.md`](../README.md)。
|
||||
|
||||
本文说明标定目标、约定、流水线与实现状态。代码在 `imu_lidar/`。场地采集见 [`标定流程与采集清单.md`](标定流程与采集清单.md)。
|
||||
|
||||
---
|
||||
|
||||
## 1. 目标与约定
|
||||
|
||||
估计安装外参：
|
||||
|
||||
```text
|
||||
p_IMU = T_IMU_lidar · p_lidar
|
||||
```
|
||||
|
||||
约定：`T_A_B` 表示把 **B 系点**变换到 **A 系**。
|
||||
|
||||
相对运动手眼模型：
|
||||
|
||||
```text
|
||||
A_ij ≈ IMU 在 [t_i, t_j] 的相对运动（预积分）
|
||||
B_ij ≈ LiDAR 在同时间段的相对运动（关键帧配准）
|
||||
A X ≈ X B
|
||||
X = T_IMU_lidar
|
||||
```
|
||||
|
||||
旋转子问题（常规主交付）：
|
||||
|
||||
```text
|
||||
R_A R_X = R_X R_B
|
||||
→ R_IMU_lidar
|
||||
```
|
||||
|
||||
若已有完整六自由度外参，且另有 `T_RTK_lidar`，可链式得到：
|
||||
|
||||
```text
|
||||
T_lidar_IMU = inverse(T_IMU_lidar)
|
||||
T_RTK_IMU = T_RTK_lidar @ T_lidar_IMU
|
||||
```
|
||||
|
||||
**不要**把 IMU 加速度二次积分成轨迹，再当作绝对位姿去做完整六自由度手眼。
|
||||
|
||||
---
|
||||
|
||||
## 2. 交付分层
|
||||
|
||||
| 层级 | 交付 | 数据最低要求 |
|
||||
|---|---|---|
|
||||
| 第一步 | 旋转 + 时间偏置 δt | **设备时间戳**；静止 + 低速转弯 /「8」字；结构化场景 |
|
||||
| 第二步 | 上一步 + 可观的水平平移 | 更多转弯半径与加减速 |
|
||||
| 第三步 | 完整六自由度（含可靠竖直分量） | 缓坡俯仰激励，或外测垂直杆臂先验 |
|
||||
|
||||
可观性不过关 → 只交旋转，不强交“假精确”六自由度。
|
||||
仅主机接收时间、或残差未过门控的结果 → **不要当作正式安装参数**。
|
||||
|
||||
---
|
||||
|
||||
## 3. 总体原则
|
||||
|
||||
1. **时间同步优先于外参**：δt 未对齐时，旋转与平移都不可信。使用设备时间戳。
|
||||
2. **先求旋转，再求平移**：旋转通常更稳；平面运动下竖直平移常常不可观。
|
||||
3. **用连续运动标定**：停车多站、再靠站间长积分，不适合作为纯 IMU 外参主流程。
|
||||
4. **可观性门控**：过不了就降级交付。
|
||||
5. **残差小 ≠ 标定对**：需叠点云 / 跨会话等独立验证。
|
||||
6. **首轮建议低速**：先保证配准与时间对齐；点云去畸变可选。
|
||||
7. **安装参数不写死在源码**：轴向与时间语义进 YAML；机械尺寸可作检查，不能伪装成已标定平移。
|
||||
8. **注意耦合**：时间相关峰很弱时，δt、航向角与陀螺零偏可能互相补偿，结果不可当真。
|
||||
|
||||
---
|
||||
|
||||
## 4. 流水线（现行实现）
|
||||
|
||||
```text
|
||||
vehicle_config
|
||||
→ timestamp_audit
|
||||
→ imu_audit（静止零偏等）
|
||||
→ time_offset：粗估 δt
|
||||
→ keyframes / [可选] deskew
|
||||
→ motion_pairs：完整 IMU 预积分 + 配准 B
|
||||
→ rotation_handeye：加权求解 R
|
||||
→ （有候选 R 时）精修 δt，必要时交替重建运动对
|
||||
→ joint_optimizer：精修旋转与陀螺零偏；可观且 full_se3 时再估平移等
|
||||
→ finalize
|
||||
```
|
||||
|
||||
| 模块 | 文件 | 职责 |
|
||||
|---|---|---|
|
||||
| 配置 | `vehicle_config.py` | 读安装 YAML |
|
||||
| IO | `imu_io.py` / `lidar_io.py` | 标准 CSV / 帧目录 |
|
||||
| 质检 | `timestamp_audit.py` / `imu_audit.py` | 时间域、静止零偏 |
|
||||
| δt | `time_offset.py` | 粗估 + 有符号精修 |
|
||||
| 运动 | `keyframes.py` / `registration.py` / `lidar_deskew.py` | 关键帧、配准、可选去畸变 |
|
||||
| IMU 侧 | `imu_preintegration.py` / `motion_pairs.py` | 预积分与运动对 |
|
||||
| 求解 | `rotation_handeye.py` / `joint_optimizer.py` / `observability.py` | 手眼、联合精修、门控 |
|
||||
| 编排 | `pipeline.py` / `cli.py` / `finalize.py` | 入口与落盘 |
|
||||
|
||||
输入中间格式见 [V1_数据格式.md](V1_数据格式.md)。改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||
|
||||
### 运行示例
|
||||
|
||||
```powershell
|
||||
python -m pip install -e ".[dev]"
|
||||
python -m pip install -e ".[open3d]" # 可选
|
||||
|
||||
python -m imu_lidar.cli plan --mode rotation_only
|
||||
|
||||
python -m imu_lidar.cli run `
|
||||
--vehicle-config config\vehicle_installation.template.yaml `
|
||||
--imu path\to\imu.csv `
|
||||
--lidar path\to\lidar_session `
|
||||
--output path\to\output `
|
||||
--mode rotation_only `
|
||||
--time-offset-search-s 2.0
|
||||
```
|
||||
|
||||
合成自检：`python tools\generate_synthetic_session.py` 后跑 CLI，再 `python -m pytest -q`。
|
||||
|
||||
### 结果状态
|
||||
|
||||
| status | 含义 |
|
||||
|---|---|
|
||||
| `rotation_only_accepted` | 旋转过门，可交旋转与报告中的 δt |
|
||||
| `full_se3_accepted` | 可观且联合优化通过，可交完整 `T` |
|
||||
| `full_se3_rejected_due_to_observability` | 旋转可用，平移未接受 |
|
||||
| `blocked` | 质检 / δt / 手眼残差等硬门失败，**不交付** |
|
||||
|
||||
`rotation_only` 模式下：不要把未标定的机械平移拼进 4×4 伪装成完整标定。
|
||||
|
||||
---
|
||||
|
||||
## 5. 采集要点
|
||||
|
||||
单趟动态会话：
|
||||
|
||||
```text
|
||||
[静止 20–30 s] → [低速激励 3–8 min] → [再静止 10–20 s]
|
||||
```
|
||||
|
||||
优先激励：
|
||||
|
||||
- 低速「8」字 / 左右圆（旋转主激励）
|
||||
- 直线加减速（有助于时间对齐与水平平移）
|
||||
- 缓坡（仅完整六自由度需要）：约 3°～8°，连续长度优先 ≥ 20～30 m
|
||||
|
||||
硬条件：IMU / LiDAR **设备时间戳**；结构化场景；标定全程安装不得改动。
|
||||
|
||||
更完整的现场清单见 [标定流程与采集清单.md](标定流程与采集清单.md)。
|
||||
|
||||
---
|
||||
|
||||
## 6. 数学上允许与禁止
|
||||
|
||||
禁止：加速度二次积分当真值轨迹；把只有旋转的相对运动硬补成完整六自由度；用最终外参反向筛边掩盖失败。
|
||||
|
||||
允许：预积分旋转手眼求旋转；在可观时用预积分残差联合估计平移，并精修零偏等辅助量。
|
||||
|
||||
---
|
||||
|
||||
## 7. 车辆配置
|
||||
|
||||
使用 `--vehicle-config vehicle_installation.yaml`。
|
||||
模板中安装平移/旋转保持未标定状态，直至实测确认；禁止写死某车杆臂冒充结果。
|
||||
|
||||
---
|
||||
|
||||
## 8. 实现状态（当前阶段）
|
||||
|
||||
本仓库**仅此一条**标定路径：连续运动关键帧 + IMU 预积分。对外总览与「合成 / 旧车 / 合格数据预期」见根目录 [`README.md`](../README.md) §0。
|
||||
|
||||
| 项 | 状态 |
|
||||
|---|---|
|
||||
| 连续运动关键帧标定流水线 | 已实现（现行唯一路径） |
|
||||
| 加权预积分 / 加权手眼 | 已实现 |
|
||||
| 旋转预积分因子 + 有符号 δt 精修 | 已实现 |
|
||||
| 完整预积分（含速度/位移增量）与可观时的平移优化 | 已实现 |
|
||||
| 可观性门控 / `rotation_only` | 已实现 |
|
||||
| 合成数据 pytest / 一键复现 | 已实现（证明链路与已知 yaw/δt，不证明实车精度） |
|
||||
| 旧车主机时间烟测（S2） | 线下可跑；预期 `blocked`，不当交付 |
|
||||
| 设备时间新车数据正式验收 | **待做** |
|
||||
| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`（N300 rscap + H32 dlog/MSOP → V1；dlog 用 DIFOP 通道角） |
|
||||
|
||||
细项见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||
|
||||
---
|
||||
|
||||
## 9. 相关文档
|
||||
|
||||
| 内容 | 路径 | 备注 |
|
||||
|---|---|---|
|
||||
| 对外总览（优先） | 根目录 [`README.md`](../README.md) | 日常入口 |
|
||||
| 采集清单 | [`标定流程与采集清单.md`](标定流程与采集清单.md) | 现场 |
|
||||
| 数据格式 / 导出 | [`V1_数据格式.md`](V1_数据格式.md) | 中间格式 |
|
||||
| 文件职责说明 | [`imu_lidar/文件职责说明.md`](../imu_lidar/文件职责说明.md) | 改代码 |
|
||||
| 改动史 | [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md) | 改代码 |
|
||||
+109
@@ -0,0 +1,109 @@
|
||||
# V1 标准中间数据格式
|
||||
|
||||
**用途：** 标定程序读入的 CSV/NPZ 约定，以及新车原始数据如何导出。总览见根目录 [README](../README.md)。
|
||||
|
||||
## 从原始数据导出
|
||||
|
||||
**推荐（新 H32 + HI13）：** HI13 `.rscap` + 雷达 Medulla dlog / recovered zip（raw MSOP + DIFOP）。
|
||||
|
||||
```powershell
|
||||
python tools\export_rscap_to_v1.py `
|
||||
--imu-rscap path\to\hi13r4-imu.rscap `
|
||||
--imu-kind hi13 `
|
||||
--lidar-dlog path\to\session_or_dlog_or_recovered.zip `
|
||||
--host-start 2026-08-08T17:40:05 `
|
||||
--host-end 2026-08-08T17:45:15 `
|
||||
--out path\to\session_v1 `
|
||||
--frame-stride 5 `
|
||||
--require-difop
|
||||
```
|
||||
|
||||
`--lidar-dlog` 可为：标准 `dobject/`+`dobject_recording/` 目录，或 recovered zip（`indices.log` + `data.bin`）。
|
||||
`--imu-kind`：`hi13` / `n300` / `auto`（默认按文件名推断）。
|
||||
`--host-start/end`：按本地墙钟切窗（仅裁剪；标定主轴仍是设备时间）。
|
||||
默认 DObject：`frontlidar-msop-raw`、`frontlidar-difop-raw`。
|
||||
|
||||
**兼容旧 MSOP-only `.rscap`：**
|
||||
|
||||
```powershell
|
||||
python tools\export_rscap_to_v1.py `
|
||||
--imu-rscap path\to\n300.rscap `
|
||||
--lidar-rscap path\to\h32_msop.rscap `
|
||||
--out path\to\session_v1 `
|
||||
--frame-stride 1
|
||||
```
|
||||
|
||||
产出：`imu.csv`、`lidar/`（含 `frames_index.csv`）、`export_summary.json`。
|
||||
标定主轴仍是**设备时间**；同时写出**主机 UTC 接收时间**，用于把雷达帧桥接到 IMU 设备钟（禁止把两边设备时间第一帧强行重合）。
|
||||
|
||||
## IMU
|
||||
|
||||
文件：`imu.csv` 或 `imu.npz`
|
||||
|
||||
### CSV
|
||||
|
||||
```text
|
||||
t,gx,gy,gz,ax,ay,az,t_host_utc_s,receive_utc_ticks
|
||||
0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81,1754646005.123,6389...
|
||||
...
|
||||
```
|
||||
|
||||
| 列 | 含义 | 单位 |
|
||||
|---|---|---|
|
||||
| t | IMU 设备时钟时间 | s |
|
||||
| gx,gy,gz | 角速度 | rad/s |
|
||||
| ax,ay,az | 比力/加速度 | m/s² |
|
||||
| t_host_utc_s | 主机 UTC 接收时间（Unix） | s |
|
||||
| receive_utc_ticks | 同上，.NET UTC ticks | — |
|
||||
|
||||
### NPZ
|
||||
|
||||
数组：`t (N,)`, `gyro (N,3)`, `acc (N,3)`，含义同上。
|
||||
|
||||
> IMU 与 LiDAR 的时间原点可以不同。流水线会估计常值偏置：`t_imu = t_lidar + delta_t`。
|
||||
|
||||
## LiDAR
|
||||
|
||||
目录结构：
|
||||
|
||||
```text
|
||||
lidar_session/
|
||||
├── frames_index.csv
|
||||
└── frames/
|
||||
├── frame_00000.npz
|
||||
├── frame_00001.npz
|
||||
└── ...
|
||||
```
|
||||
|
||||
### frames_index.csv
|
||||
|
||||
```text
|
||||
frame_id,filename,t_start,t_end,host_receive_utc_ticks,t_host_utc_s,host_receive_utc_end_ticks,t_host_utc_end_s
|
||||
0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22
|
||||
```
|
||||
|
||||
| 列 | 含义 |
|
||||
|---|---|
|
||||
| t_start / t_end | H32 MSOP **设备时间**（秒） |
|
||||
| t_host_utc_s / t_host_utc_end_s | 帧首/末包 **HostReceiveUtcTicks** → Unix 秒 |
|
||||
|
||||
也兼容旧列名 `file`。对齐脚本用主机 UTC 把 `t_*` 重写到 IMU 设备钟后再跑标定。
|
||||
|
||||
### 每帧 NPZ
|
||||
|
||||
- `points`: `float64/float32`，形状 `(N, 3)`，LiDAR 直角坐标系，单位米
|
||||
|
||||
## 时间不同步能不能用？
|
||||
|
||||
可以，前提是：
|
||||
|
||||
1. 两边都覆盖同一段**有角速度激励**的物理运动（尤其是转弯）；
|
||||
2. 偏置近似为**常数**（短会话）；
|
||||
3. `--time-offset-search-s` 足够覆盖可能的偏移（默认 ±1 s，可加大）。
|
||||
|
||||
若两段数据完全不是同一趟行驶，或只有静止 IMU、没有重叠运动，则无法估 δt，标定会被 `blocked`。
|
||||
|
||||
## 最小可用会话
|
||||
|
||||
- IMU：建议含静止段 + 运动段，采样率稳定
|
||||
- LiDAR：建议 ≥ 20 帧，场景有墙/柱等结构，含转弯
|
||||
@@ -0,0 +1,157 @@
|
||||
# 纯 LiDAR–IMU：标定流程与采集清单
|
||||
|
||||
**用途：** 现场怎么采合格数据（看完根目录 [README](../README.md) 后再看本文即可）。
|
||||
算法命令与结果判读以 README 为准；改动史见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。
|
||||
|
||||
仅有激光雷达与 IMU、无 RTK/绝对位姿时的推荐采集与流程要点。
|
||||
目标外参：`p_IMU = T_IMU_lidar · p_lidar`（`T_A_B` 表示把 B 系点变到 A 系）。
|
||||
|
||||
---
|
||||
|
||||
## 1. 原则（先读）
|
||||
|
||||
1. **时间对齐优先**：时钟偏置未对准时，旋转与平移都不可信；正式数据用**设备时间戳**。
|
||||
2. **先旋转，再平移**：旋转通常更稳；平面低速时竖直方向平移常常不可观。
|
||||
3. **用连续运动**：停车多站适合 RTK 手眼，不适合作为纯 IMU 外参主流程。
|
||||
4. **可观才交平移**：激励不够就只交旋转，不强交“假精确”六自由度。
|
||||
5. **残差小 ≠ 标定对**：需叠点云、跨会话等独立验证。
|
||||
|
||||
相对运动模型：
|
||||
|
||||
```text
|
||||
A ≈ IMU 预积分相对运动（关键帧区间）
|
||||
B ≈ 雷达关键帧配准相对运动
|
||||
R_A · R_X ≈ R_X · R_B → 先求旋转
|
||||
完整模式且可观时再求平移 t
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 2. 端到端流程（与现行代码一致）
|
||||
|
||||
```text
|
||||
确认轴向 / 单位 / 时间语义
|
||||
→ 现场采集（首尾静止 + 低速多转弯；建议 ≥2 段独立会话）
|
||||
→ 导出标准中间格式（imu.csv + lidar 会话目录）
|
||||
→ 质检（时间 / IMU）不通过则停
|
||||
→ 粗估时间偏置 δt
|
||||
→ 关键帧 → 配准得 B；完整 IMU 预积分得 A（旋转/速度/位移增量）
|
||||
→ 加权旋转手眼得 R
|
||||
→ 用 R 精修 δt，必要时重新组对再解 R（可交替数轮）
|
||||
→ 联合精修 R 与常值陀螺零偏
|
||||
→ full_se3 且可观：再估重力、关键帧速度、时变零偏与平移 t
|
||||
→ 写出 T / δt / summary → 叠点云 / 跨会话验证后交付
|
||||
```
|
||||
|
||||
|
||||
| 步骤 | 现行模块 | 说明 |
|
||||
| --- | -------------------------------------------------------------------- | --------------------------------------- |
|
||||
| 质检 | `timestamp_audit` / `imu_audit` | 含静止段陀螺零偏初值 |
|
||||
| 时间 | `time_offset` | 模长相关粗估 + 有符号三轴精修 |
|
||||
| 运动对 | `keyframes` / `registration` / `imu_preintegration` / `motion_pairs` | 预积分始终算满；手眼先用旋转 |
|
||||
| 旋转 | `rotation_handeye` | 加权手眼 |
|
||||
| 精修 | `joint_optimizer` / `observability` | `rotation_only` 到旋转为止；`full_se3` 可观才碰平移 |
|
||||
|
||||
|
||||
点云去畸变（`lidar_deskew`）可选；低速首轮可不依赖。
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
## 3. 采集设计
|
||||
|
||||
|
||||
|
||||
### 3.1 单趟会话结构
|
||||
|
||||
```text
|
||||
静止 20–30 s → 连续运动 3–8 min → 再静止 10–20 s
|
||||
```
|
||||
|
||||
运动优先：低速「8」字 / 左右圆；再补加减速直线。
|
||||
要可靠竖直方向外参时，另加缓坡（约 3°～8°，有效长度优先 ≥20～30 m），或改用外测垂直尺寸先验。
|
||||
|
||||
### 3.2 会话安排
|
||||
|
||||
|
||||
| 会话 | 作用 |
|
||||
| ----- | ---------------------------------- |
|
||||
| A | 主标定 |
|
||||
| B | 独立验证（**同一场地**换一条不完全相同的路线即可，不参与求外参） |
|
||||
| C（可选） | 不同速度/路线，测稳定性 |
|
||||
|
||||
|
||||
安装全程不得改动。多会话不要跨会话拼运动对。
|
||||
|
||||
### 3.3 录制字段（原始，勿先做姿态融合）
|
||||
|
||||
- IMU：设备时间、陀螺、加速度（建议同时留主机接收时间便于排查）
|
||||
- LiDAR：每帧起止时间（最好有包级/逐点时间）、原始点云
|
||||
|
||||
中间格式见 `[V1_数据格式.md](V1_数据格式.md)`。
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
## 4. 现场 Checklist
|
||||
|
||||
**出发前**
|
||||
|
||||
- [ ] 安装固定；草图/卷尺粗测仅作参考，不当真值
|
||||
- [ ] 单位与轴向确认；设备时间可写盘
|
||||
- [ ] 结构化路线（墙/杆/路缘），避开空旷无特征区
|
||||
- [ ] 存储与供电充足
|
||||
|
||||
**录制中**
|
||||
|
||||
- [ ] 首尾静止；中间有明显左右转与加减速
|
||||
- [ ] 不改安装、不切换时间源
|
||||
- [ ] 记录会话 ID、天气、异常（急刹、掉包等）
|
||||
|
||||
**当场快查**
|
||||
|
||||
- [ ] IMU 静止段平稳，转弯时角速度明显
|
||||
- [ ] 点云帧数/点数正常，无明显大面积丢帧
|
||||
- [ ] 雷达与 IMU 时间覆盖同一时段
|
||||
|
||||
**回实验室**
|
||||
|
||||
- [ ] 已导出中间格式并通过质检
|
||||
- [ ] 本次目标：`rotation_only` 还是尝试 `full_se3`
|
||||
- [ ] 若要竖直方向：确认真有俯仰/高度激励，否则降级交付
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
## 5. 精度预期
|
||||
|
||||
|
||||
| 量 | 较现实范围 | 说明 |
|
||||
| ---------- | --------- | ---------------- |
|
||||
| 旋转 | 约 0.5°–2° | 最精确部分 |
|
||||
| 水平平移 | 数厘米～十几厘米 | 强依赖配准、激励与同步 |
|
||||
| 竖直 / 部分杠杆臂 | 往往更差甚至不可观 | 无高度激励时无法得出“精确 z” |
|
||||
|
||||
|
||||
如果条件有限，优先交付：**可靠旋转 + δt + 可观的平移分量（若有）+ 明确限制说明**。
|
||||
|
||||
满足条件后的模式与成功标志见根目录 [`README.md`](../README.md) §0。
|
||||
|
||||
---
|
||||
|
||||
|
||||
|
||||
## 6. 交付物建议
|
||||
|
||||
程序默认写出：`T_IMU_lidar.json`、`time_offset.json`、`summary.json`。
|
||||
完整报告目录还可补充：可观性结论、跨会话对比、运动对质量表、限制说明（尤其竖直方向与时间同步方式）。
|
||||
|
||||
```text
|
||||
静止 + 激励录制（多会话）
|
||||
→ 质检 → 估 δt → 关键帧 A/B → 先解 R
|
||||
→ 精修 δt 与 R → 可观则求 t → 验证后交付
|
||||
```
|
||||
|
||||
@@ -0,0 +1,245 @@
|
||||
# `imu_lidar` 改动记录
|
||||
|
||||
本文件专门记录 `imu_lidar` 目录内的实现改动。
|
||||
每条包含：**时间戳**、**改动内容**（以「原本怎么做 → 改成怎么做」书写）。
|
||||
|
||||
---
|
||||
|
||||
## 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,126 @@
|
||||
"""Command-line entry point for LiDAR–IMU calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from .contracts import CalibrationMode, CalibrationRequest, CalibrationStatus, SessionInput
|
||||
from .pipeline import describe_pipeline, run_calibration
|
||||
|
||||
|
||||
def build_parser() -> argparse.ArgumentParser:
|
||||
parser = argparse.ArgumentParser(description="LiDAR–IMU extrinsic calibration (V1)")
|
||||
subcommands = parser.add_subparsers(dest="command", required=True)
|
||||
|
||||
plan = subcommands.add_parser("plan", help="显示标定阶段，不读取数据")
|
||||
plan.add_argument("--vehicle-config", help="车辆配置路径（仅展示，plan 不读取）")
|
||||
plan.add_argument(
|
||||
"--mode",
|
||||
choices=[mode.value for mode in CalibrationMode],
|
||||
default=CalibrationMode.ROTATION_ONLY.value,
|
||||
)
|
||||
|
||||
run = subcommands.add_parser(
|
||||
"run",
|
||||
help="执行 V1 标定流水线（可重复 --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("--min-pair-rotation-deg", type=float, default=3.0)
|
||||
run.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
||||
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)})"
|
||||
)
|
||||
return tuple(
|
||||
SessionInput(session_id=sid, imu_source=imu, lidar_source=lidar)
|
||||
for sid, imu, lidar in zip(session_ids, imus, lidars)
|
||||
)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = build_parser()
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
if args.command == "plan":
|
||||
request = CalibrationRequest(
|
||||
vehicle_config=Path(args.vehicle_config) if args.vehicle_config else None,
|
||||
requested_mode=CalibrationMode(args.mode),
|
||||
)
|
||||
print("LiDAR–IMU calibration stages:")
|
||||
print(f"requested mode: {request.requested_mode.value}")
|
||||
for index, stage in enumerate(describe_pipeline(request), start=1):
|
||||
print(f"{index}. {stage.name}: {stage.responsibility}")
|
||||
return 0
|
||||
|
||||
if args.command == "run":
|
||||
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,
|
||||
time_offset_search_s=args.time_offset_search_s,
|
||||
)
|
||||
result = run_calibration(request)
|
||||
print(f"status: {result.status.value}")
|
||||
print(f"message: {result.message}")
|
||||
if result.time_offset_s is not None:
|
||||
print(f"time_offset_s (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,117 @@
|
||||
"""Shared contracts for the LiDAR–IMU calibration pipeline."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class TransformConvention(str, Enum):
|
||||
"""The only transform convention used by this project."""
|
||||
|
||||
T_A_B = "T_A_B maps points from frame B into frame A"
|
||||
|
||||
|
||||
class CalibrationMode(str, Enum):
|
||||
ROTATION_ONLY = "rotation_only"
|
||||
FULL_SE3 = "full_se3"
|
||||
|
||||
|
||||
class CalibrationStatus(str, Enum):
|
||||
NOT_RUN = "not_run"
|
||||
BLOCKED = "blocked"
|
||||
ROTATION_ONLY_ACCEPTED = "rotation_only_accepted"
|
||||
FULL_SE3_ACCEPTED = "full_se3_accepted"
|
||||
FULL_SE3_REJECTED = "full_se3_rejected_due_to_observability"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SessionInput:
|
||||
"""Input paths for one independently recorded session."""
|
||||
|
||||
session_id: str
|
||||
imu_source: Path
|
||||
lidar_source: Path
|
||||
board_configuration_id: str | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CalibrationRequest:
|
||||
"""Top-level calibration request."""
|
||||
|
||||
vehicle_config: Path | None
|
||||
sessions: tuple[SessionInput, ...] = ()
|
||||
requested_mode: CalibrationMode = CalibrationMode.ROTATION_ONLY
|
||||
output_directory: Path | None = None
|
||||
max_iterations: int = 2
|
||||
min_pair_rotation_deg: float = 3.0
|
||||
min_pair_translation_m: float = 0.3
|
||||
time_offset_search_s: float = 1.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class CalibrationResult:
|
||||
"""Result envelope written by finalize after pipeline gates."""
|
||||
|
||||
status: CalibrationStatus = CalibrationStatus.NOT_RUN
|
||||
message: str = "Calibration has not been executed."
|
||||
details: dict[str, Any] = field(default_factory=dict)
|
||||
T_IMU_lidar: np.ndarray | None = None
|
||||
time_offset_s: float | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ImuSeries:
|
||||
"""Normalized IMU samples.
|
||||
|
||||
``t_s`` is the native IMU clock in seconds (need not match LiDAR epoch).
|
||||
Gyro must be rad/s; accelerometer must be m/s^2.
|
||||
"""
|
||||
|
||||
t_s: np.ndarray
|
||||
gyro_rad_s: np.ndarray
|
||||
acc_m_s2: np.ndarray
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
object.__setattr__(self, "t_s", np.asarray(self.t_s, dtype=float).reshape(-1))
|
||||
object.__setattr__(self, "gyro_rad_s", np.asarray(self.gyro_rad_s, dtype=float).reshape(-1, 3))
|
||||
object.__setattr__(self, "acc_m_s2", np.asarray(self.acc_m_s2, dtype=float).reshape(-1, 3))
|
||||
n = self.t_s.size
|
||||
if self.gyro_rad_s.shape != (n, 3) or self.acc_m_s2.shape != (n, 3):
|
||||
raise ValueError("IMU arrays must share the same length and have shape (N, 3)")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LidarFrame:
|
||||
"""One LiDAR sweep in Cartesian sensor coordinates."""
|
||||
|
||||
frame_id: str
|
||||
t_start_s: float
|
||||
t_end_s: float
|
||||
points_xyz: np.ndarray
|
||||
path: Path | None = None
|
||||
|
||||
@property
|
||||
def t_mid_s(self) -> float:
|
||||
return 0.5 * (self.t_start_s + self.t_end_s)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MotionPair:
|
||||
"""One relative-motion observation between keyframes i and j."""
|
||||
|
||||
session_id: str
|
||||
i: int
|
||||
j: int
|
||||
t_i_s: float
|
||||
t_j_s: float
|
||||
R_A: np.ndarray
|
||||
R_B: np.ndarray
|
||||
t_A_m: np.ndarray | None = None
|
||||
t_B_m: np.ndarray | None = None
|
||||
fitness: float = 0.0
|
||||
metadata: dict[str, Any] = field(default_factory=dict)
|
||||
@@ -0,0 +1,76 @@
|
||||
"""Package calibration outputs as JSON-friendly artifacts."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import CalibrationResult, CalibrationStatus
|
||||
from .geometry import rotation_matrix_to_quaternion_xyzw, rpy_deg_xyz
|
||||
|
||||
|
||||
def _to_serializable(value: Any) -> Any:
|
||||
if isinstance(value, np.ndarray):
|
||||
return value.tolist()
|
||||
if isinstance(value, (np.floating, np.integer, np.bool_)):
|
||||
return value.item()
|
||||
if isinstance(value, Path):
|
||||
return str(value)
|
||||
if isinstance(value, dict):
|
||||
return {str(k): _to_serializable(v) for k, v in value.items()}
|
||||
if isinstance(value, (list, tuple)):
|
||||
return [_to_serializable(v) for v in value]
|
||||
return value
|
||||
|
||||
|
||||
def finalize_result(
|
||||
*,
|
||||
status: CalibrationStatus,
|
||||
message: str,
|
||||
details: dict[str, Any],
|
||||
T_IMU_lidar: np.ndarray | None = None,
|
||||
time_offset_s: float | None = None,
|
||||
output_directory: Path | None = None,
|
||||
) -> CalibrationResult:
|
||||
"""Build the result envelope and optionally write report files."""
|
||||
|
||||
result = CalibrationResult(
|
||||
status=status,
|
||||
message=message,
|
||||
details=_to_serializable(details),
|
||||
T_IMU_lidar=None if T_IMU_lidar is None else np.asarray(T_IMU_lidar, dtype=float),
|
||||
time_offset_s=time_offset_s,
|
||||
)
|
||||
|
||||
if output_directory is not None:
|
||||
output_directory = Path(output_directory)
|
||||
output_directory.mkdir(parents=True, exist_ok=True)
|
||||
summary = {
|
||||
"status": status.value,
|
||||
"message": message,
|
||||
"time_offset_s": time_offset_s,
|
||||
"details": result.details,
|
||||
}
|
||||
if result.T_IMU_lidar is not None:
|
||||
t = result.T_IMU_lidar
|
||||
summary["T_IMU_lidar"] = {
|
||||
"matrix": t.tolist(),
|
||||
"translation_m": t[:3, 3].tolist(),
|
||||
"rotation_quaternion_xyzw": rotation_matrix_to_quaternion_xyzw(t[:3, :3]).tolist(),
|
||||
"rpy_deg_xyz": rpy_deg_xyz(t[:3, :3]).tolist(),
|
||||
"convention": "p_IMU = T_IMU_lidar * p_lidar",
|
||||
}
|
||||
(output_directory / "T_IMU_lidar.json").write_text(
|
||||
json.dumps(summary["T_IMU_lidar"], indent=2),
|
||||
encoding="utf-8",
|
||||
)
|
||||
if time_offset_s is not None:
|
||||
(output_directory / "time_offset.json").write_text(
|
||||
json.dumps({"delta_t_s": time_offset_s, "definition": "t_imu = t_lidar + delta_t"}, indent=2),
|
||||
encoding="utf-8",
|
||||
)
|
||||
(output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
|
||||
return result
|
||||
@@ -0,0 +1,207 @@
|
||||
"""SE(3)/SO(3) utilities for LiDAR–IMU calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def skew(vector: np.ndarray) -> np.ndarray:
|
||||
"""Return the skew-symmetric matrix such that ``skew(v) @ w == v x w``."""
|
||||
|
||||
x, y, z = np.asarray(vector, dtype=float).reshape(3)
|
||||
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]], dtype=float)
|
||||
|
||||
|
||||
def so3_exp(rotation_vector: np.ndarray) -> np.ndarray:
|
||||
"""Map a rotation vector in radians onto SO(3)."""
|
||||
|
||||
vector = np.asarray(rotation_vector, dtype=float).reshape(3)
|
||||
angle = float(np.linalg.norm(vector))
|
||||
if angle < 1e-12:
|
||||
return np.eye(3) + skew(vector)
|
||||
axis_cross = skew(vector / angle)
|
||||
return np.eye(3) + math.sin(angle) * axis_cross + (1.0 - math.cos(angle)) * axis_cross @ axis_cross
|
||||
|
||||
|
||||
def so3_log(rotation: np.ndarray) -> np.ndarray:
|
||||
"""Map an SO(3) matrix to a rotation vector in radians."""
|
||||
|
||||
rotation = np.asarray(rotation, dtype=float).reshape(3, 3)
|
||||
cos_angle = float(np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0))
|
||||
angle = math.acos(cos_angle)
|
||||
if angle < 1e-12:
|
||||
return 0.5 * np.array(
|
||||
[
|
||||
rotation[2, 1] - rotation[1, 2],
|
||||
rotation[0, 2] - rotation[2, 0],
|
||||
rotation[1, 0] - rotation[0, 1],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if abs(angle - math.pi) < 1e-6:
|
||||
# Near 180°: use eigenvector of the +1 eigenvalue.
|
||||
eigvals, eigvecs = np.linalg.eigh(0.5 * (rotation + rotation.T))
|
||||
axis = eigvecs[:, int(np.argmax(eigvals))]
|
||||
return axis * angle
|
||||
return (
|
||||
0.5
|
||||
* angle
|
||||
/ math.sin(angle)
|
||||
* np.array(
|
||||
[
|
||||
rotation[2, 1] - rotation[1, 2],
|
||||
rotation[0, 2] - rotation[2, 0],
|
||||
rotation[1, 0] - rotation[0, 1],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def rotation_angle_deg(rotation: np.ndarray) -> float:
|
||||
"""Return the rotation angle in degrees."""
|
||||
|
||||
return float(np.degrees(np.linalg.norm(so3_log(rotation))))
|
||||
|
||||
|
||||
def inverse_transform(transform: np.ndarray) -> np.ndarray:
|
||||
"""Return the inverse of a rigid 4x4 transform."""
|
||||
|
||||
transform = np.asarray(transform, dtype=float)
|
||||
if transform.shape != (4, 4):
|
||||
raise ValueError("a rigid transform must have shape (4, 4)")
|
||||
result = np.eye(4)
|
||||
result[:3, :3] = transform[:3, :3].T
|
||||
result[:3, 3] = -result[:3, :3] @ transform[:3, 3]
|
||||
return result
|
||||
|
||||
|
||||
def make_transform(translation_m: np.ndarray, rotation: np.ndarray) -> np.ndarray:
|
||||
"""Build ``T_A_B`` from its translation and rotation components."""
|
||||
|
||||
translation_m = np.asarray(translation_m, dtype=float).reshape(3)
|
||||
rotation = np.asarray(rotation, dtype=float)
|
||||
if rotation.shape != (3, 3):
|
||||
raise ValueError("a rotation matrix must have shape (3, 3)")
|
||||
result = np.eye(4)
|
||||
result[:3, :3] = rotation
|
||||
result[:3, 3] = translation_m
|
||||
return result
|
||||
|
||||
|
||||
def transform_points(points: np.ndarray, transform: np.ndarray) -> np.ndarray:
|
||||
"""Apply ``T_A_B`` to an ``(N, 3)`` point array expressed in frame B."""
|
||||
|
||||
points = np.asarray(points, dtype=float)
|
||||
if points.ndim != 2 or points.shape[1] != 3:
|
||||
raise ValueError("points must have shape (N, 3)")
|
||||
return points @ transform[:3, :3].T + transform[:3, 3]
|
||||
|
||||
|
||||
def orthonormalize_rotation(rotation: np.ndarray) -> np.ndarray:
|
||||
"""Project a near-rotation matrix onto SO(3)."""
|
||||
|
||||
u, _, vt = np.linalg.svd(np.asarray(rotation, dtype=float).reshape(3, 3))
|
||||
result = u @ vt
|
||||
if np.linalg.det(result) < 0:
|
||||
u[:, -1] *= -1
|
||||
result = u @ vt
|
||||
return result
|
||||
|
||||
|
||||
def integrate_gyro_rotation(
|
||||
times_s: np.ndarray,
|
||||
gyro_rad_s: np.ndarray,
|
||||
t0: float,
|
||||
t1: float,
|
||||
bias_rad_s: np.ndarray | None = None,
|
||||
) -> np.ndarray:
|
||||
"""Integrate gyroscope samples on ``[t0, t1]`` and return ``R(t0<-t1)`` wait.
|
||||
|
||||
Returns ``R_i_j`` that maps vectors from the IMU frame at ``t1`` into the
|
||||
IMU frame at ``t0`` using right-invariant discrete integration:
|
||||
|
||||
R <- R @ Exp(omega * dt)
|
||||
"""
|
||||
|
||||
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||
if times_s.size < 2:
|
||||
return np.eye(3)
|
||||
bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
|
||||
|
||||
if t1 < t0:
|
||||
raise ValueError("t1 must be >= t0")
|
||||
|
||||
# Include one sample before t0 and after t1 when possible for interpolation.
|
||||
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||
left = max(left, 0)
|
||||
right = min(right, times_s.size - 1)
|
||||
if right <= left:
|
||||
return np.eye(3)
|
||||
|
||||
rotation = np.eye(3)
|
||||
for index in range(left, right):
|
||||
t_a = float(times_s[index])
|
||||
t_b = float(times_s[index + 1])
|
||||
if t_b <= t0 or t_a >= t1:
|
||||
continue
|
||||
seg0 = max(t_a, t0)
|
||||
seg1 = min(t_b, t1)
|
||||
dt = seg1 - seg0
|
||||
if dt <= 0:
|
||||
continue
|
||||
omega = 0.5 * (gyro_rad_s[index] + gyro_rad_s[index + 1]) - bias
|
||||
rotation = rotation @ so3_exp(omega * dt)
|
||||
return orthonormalize_rotation(rotation)
|
||||
|
||||
|
||||
def rotation_matrix_to_quaternion_xyzw(rotation: np.ndarray) -> np.ndarray:
|
||||
"""Convert SO(3) to quaternion ``[x, y, z, w]``."""
|
||||
|
||||
rotation = orthonormalize_rotation(rotation)
|
||||
trace = float(np.trace(rotation))
|
||||
if trace > 0:
|
||||
s = math.sqrt(trace + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (rotation[2, 1] - rotation[1, 2]) / s
|
||||
y = (rotation[0, 2] - rotation[2, 0]) / s
|
||||
z = (rotation[1, 0] - rotation[0, 1]) / s
|
||||
elif rotation[0, 0] > rotation[1, 1] and rotation[0, 0] > rotation[2, 2]:
|
||||
s = math.sqrt(1.0 + rotation[0, 0] - rotation[1, 1] - rotation[2, 2]) * 2.0
|
||||
w = (rotation[2, 1] - rotation[1, 2]) / s
|
||||
x = 0.25 * s
|
||||
y = (rotation[0, 1] + rotation[1, 0]) / s
|
||||
z = (rotation[0, 2] + rotation[2, 0]) / s
|
||||
elif rotation[1, 1] > rotation[2, 2]:
|
||||
s = math.sqrt(1.0 + rotation[1, 1] - rotation[0, 0] - rotation[2, 2]) * 2.0
|
||||
w = (rotation[0, 2] - rotation[2, 0]) / s
|
||||
x = (rotation[0, 1] + rotation[1, 0]) / s
|
||||
y = 0.25 * s
|
||||
z = (rotation[1, 2] + rotation[2, 1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + rotation[2, 2] - rotation[0, 0] - rotation[1, 1]) * 2.0
|
||||
w = (rotation[1, 0] - rotation[0, 1]) / s
|
||||
x = (rotation[0, 2] + rotation[2, 0]) / s
|
||||
y = (rotation[1, 2] + rotation[2, 1]) / s
|
||||
z = 0.25 * s
|
||||
return np.array([x, y, z, w], dtype=float)
|
||||
|
||||
|
||||
def rpy_deg_xyz(rotation: np.ndarray) -> np.ndarray:
|
||||
"""Intrinsic XYZ Euler angles in degrees from a rotation matrix."""
|
||||
|
||||
rotation = orthonormalize_rotation(rotation)
|
||||
sy = math.sqrt(rotation[0, 0] ** 2 + rotation[1, 0] ** 2)
|
||||
if sy > 1e-8:
|
||||
roll = math.atan2(rotation[2, 1], rotation[2, 2])
|
||||
pitch = math.atan2(-rotation[2, 0], sy)
|
||||
yaw = math.atan2(rotation[1, 0], rotation[0, 0])
|
||||
else:
|
||||
roll = math.atan2(-rotation[1, 2], rotation[1, 1])
|
||||
pitch = math.atan2(-rotation[2, 0], sy)
|
||||
yaw = 0.0
|
||||
return np.degrees(np.array([roll, pitch, yaw], dtype=float))
|
||||
@@ -0,0 +1,86 @@
|
||||
"""IMU unit, axis, bias, and saturation audit."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import ImuSeries
|
||||
|
||||
G = 9.80665
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ImuAuditReport:
|
||||
ok: bool
|
||||
gyro_bias_rad_s: np.ndarray
|
||||
static_acc_mean_m_s2: np.ndarray
|
||||
static_acc_norm_m_s2: float
|
||||
suggested_up_axis: int
|
||||
suggested_up_sign: float
|
||||
static_ratio: float
|
||||
notes: tuple[str, ...] = ()
|
||||
|
||||
|
||||
def _static_mask(gyro: np.ndarray, acc: np.ndarray) -> np.ndarray:
|
||||
gyro_norm = np.linalg.norm(gyro, axis=1)
|
||||
acc_norm = np.linalg.norm(acc, axis=1)
|
||||
gyro_thr = max(0.02, float(np.percentile(gyro_norm, 20)) * 1.5)
|
||||
acc_thr_low = 0.7 * G
|
||||
acc_thr_high = 1.3 * G
|
||||
return (gyro_norm < gyro_thr) & (acc_norm > acc_thr_low) & (acc_norm < acc_thr_high)
|
||||
|
||||
|
||||
def audit_imu(imu: ImuSeries) -> ImuAuditReport:
|
||||
"""Audit normalized IMU samples and estimate a static gyro bias."""
|
||||
|
||||
notes: list[str] = []
|
||||
mask = _static_mask(imu.gyro_rad_s, imu.acc_m_s2)
|
||||
static_ratio = float(np.mean(mask)) if mask.size else 0.0
|
||||
if static_ratio < 0.02:
|
||||
# Fall back to lowest-gyro percentile window.
|
||||
gyro_norm = np.linalg.norm(imu.gyro_rad_s, axis=1)
|
||||
cutoff = float(np.percentile(gyro_norm, 10))
|
||||
mask = gyro_norm <= cutoff
|
||||
notes.append("few gravity-consistent static samples; using lowest-gyro percentile")
|
||||
static_ratio = float(np.mean(mask))
|
||||
|
||||
if not np.any(mask):
|
||||
notes.append("no static samples found")
|
||||
bias = np.zeros(3)
|
||||
acc_mean = np.zeros(3)
|
||||
acc_norm = 0.0
|
||||
up_axis = 2
|
||||
up_sign = 1.0
|
||||
ok = False
|
||||
else:
|
||||
bias = np.mean(imu.gyro_rad_s[mask], axis=0)
|
||||
acc_mean = np.mean(imu.acc_m_s2[mask], axis=0)
|
||||
acc_norm = float(np.linalg.norm(acc_mean))
|
||||
up_axis = int(np.argmax(np.abs(acc_mean)))
|
||||
up_sign = float(np.sign(acc_mean[up_axis]) or 1.0)
|
||||
if abs(acc_norm - G) > 2.5:
|
||||
notes.append(
|
||||
f"static |acc|={acc_norm:.3f} differs from g={G}; check units (expect m/s^2)"
|
||||
)
|
||||
gyro_peak = float(np.max(np.linalg.norm(imu.gyro_rad_s, axis=1)))
|
||||
if gyro_peak > 20.0:
|
||||
notes.append(
|
||||
f"peak |gyro|={gyro_peak:.1f} rad/s looks extreme; check whether data is deg/s"
|
||||
)
|
||||
ok = abs(acc_norm - G) < 3.5 or static_ratio > 0.05
|
||||
|
||||
notes.append(
|
||||
f"suggested up axis index={up_axis} sign={up_sign:+.0f} (0=x,1=y,2=z)"
|
||||
)
|
||||
return ImuAuditReport(
|
||||
ok=ok,
|
||||
gyro_bias_rad_s=np.asarray(bias, dtype=float),
|
||||
static_acc_mean_m_s2=np.asarray(acc_mean, dtype=float),
|
||||
static_acc_norm_m_s2=float(acc_norm),
|
||||
suggested_up_axis=up_axis,
|
||||
suggested_up_sign=up_sign,
|
||||
static_ratio=static_ratio,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
@@ -0,0 +1,72 @@
|
||||
"""IMU adapters for the V1 standard intermediate format.
|
||||
|
||||
Accepted inputs
|
||||
---------------
|
||||
1. CSV with header:
|
||||
t,gx,gy,gz,ax,ay,az
|
||||
- ``t`` in seconds on the IMU clock
|
||||
- gyro in rad/s
|
||||
- accel in m/s^2
|
||||
|
||||
2. NPZ with arrays:
|
||||
t, gyro, acc
|
||||
shapes: (N,), (N,3), (N,3)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import ImuSeries
|
||||
|
||||
|
||||
def load_imu_samples(path: Path | str) -> ImuSeries:
|
||||
"""Load normalized IMU samples from CSV or NPZ."""
|
||||
|
||||
source = Path(path)
|
||||
if not source.exists():
|
||||
raise FileNotFoundError(source)
|
||||
if source.suffix.lower() == ".csv":
|
||||
return _load_imu_csv(source)
|
||||
if source.suffix.lower() == ".npz":
|
||||
return _load_imu_npz(source)
|
||||
raise ValueError(f"unsupported IMU format '{source.suffix}' (use .csv or .npz)")
|
||||
|
||||
|
||||
def _load_imu_csv(path: Path) -> ImuSeries:
|
||||
data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
|
||||
if data.ndim == 0:
|
||||
data = np.array([data])
|
||||
names = set(data.dtype.names or ())
|
||||
required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
|
||||
if not required.issubset(names):
|
||||
raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
|
||||
t = np.asarray(data["t"], dtype=float).reshape(-1)
|
||||
gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
|
||||
acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
|
||||
order = np.argsort(t)
|
||||
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||
|
||||
|
||||
def _load_imu_npz(path: Path) -> ImuSeries:
|
||||
with np.load(path) as payload:
|
||||
keys = set(payload.files)
|
||||
if not {"t", "gyro", "acc"}.issubset(keys):
|
||||
raise ValueError(f"IMU NPZ must contain t, gyro, acc; got {sorted(keys)}")
|
||||
t = np.asarray(payload["t"], dtype=float).reshape(-1)
|
||||
gyro = np.asarray(payload["gyro"], dtype=float).reshape(-1, 3)
|
||||
acc = np.asarray(payload["acc"], dtype=float).reshape(-1, 3)
|
||||
order = np.argsort(t)
|
||||
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||
|
||||
|
||||
def save_imu_csv(path: Path | str, imu: ImuSeries) -> None:
|
||||
"""Write IMU samples to the standard CSV format."""
|
||||
|
||||
destination = Path(path)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
array = np.column_stack([imu.t_s, imu.gyro_rad_s, imu.acc_m_s2])
|
||||
header = "t,gx,gy,gz,ax,ay,az"
|
||||
np.savetxt(destination, array, delimiter=",", header=header, comments="")
|
||||
@@ -0,0 +1,452 @@
|
||||
"""Frame-to-frame IMU preintegration (Phase-A rotation + Phase-C full factor).
|
||||
|
||||
Phase-A: ``ΔR``, 3×3 ``Σ``, ``J_bg``.
|
||||
Phase-C: ``ΔR/Δv/Δp``, 9×9 ``Σ`` (with bias RW process noise), ``J_bg``/``J_ba``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .geometry import orthonormalize_rotation, so3_exp, so3_log, skew
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GyroPreintegration:
|
||||
"""Rotation-only preintegration on ``[t0, t1]`` (IMU clock)."""
|
||||
|
||||
delta_R: np.ndarray
|
||||
duration_s: float
|
||||
mean_gyro_norm: float
|
||||
sigma_rad: float
|
||||
weight: float
|
||||
bias_rad_s: np.ndarray
|
||||
cov: np.ndarray
|
||||
J_bg: np.ndarray
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ImuPreintegration:
|
||||
"""Full IMU preintegration on ``[t0, t1]`` (IMU clock).
|
||||
|
||||
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``.
|
||||
``delta_v`` / ``delta_p`` are body-frame increments (no gravity).
|
||||
Error-state order in ``cov`` / Jacobians: ``[δθ, δv, δp]`` (9).
|
||||
``J_bg`` / ``J_ba`` are 9×3: first-order correction w.r.t. constant bias deltas.
|
||||
"""
|
||||
|
||||
delta_R: np.ndarray
|
||||
delta_v: np.ndarray
|
||||
delta_p: np.ndarray
|
||||
duration_s: float
|
||||
mean_gyro_norm: float
|
||||
sigma_rad: float
|
||||
weight: float
|
||||
gyro_bias_rad_s: np.ndarray
|
||||
acc_bias_m_s2: np.ndarray
|
||||
cov: np.ndarray
|
||||
J_bg: np.ndarray
|
||||
J_ba: np.ndarray
|
||||
|
||||
|
||||
def _right_jacobian(phi: np.ndarray) -> np.ndarray:
|
||||
"""SO(3) right Jacobian ``Jr(φ)`` with ``Exp(φ+δ)≈Exp(φ)Exp(Jr δ)``."""
|
||||
|
||||
phi = np.asarray(phi, dtype=float).reshape(3)
|
||||
angle = float(np.linalg.norm(phi))
|
||||
if angle < 1e-8:
|
||||
return np.eye(3) - 0.5 * skew(phi)
|
||||
axis = phi / angle
|
||||
s = skew(axis)
|
||||
return (
|
||||
np.eye(3)
|
||||
- ((1.0 - np.cos(angle)) / angle) * s
|
||||
+ ((angle - np.sin(angle)) / angle) * (s @ s)
|
||||
)
|
||||
|
||||
|
||||
def _interp_vec(times_s: np.ndarray, values: np.ndarray, t: float) -> np.ndarray:
|
||||
"""Linear interpolate a 3-vector series at an exact time."""
|
||||
|
||||
return np.array(
|
||||
[float(np.interp(t, times_s, values[:, axis])) for axis in range(3)],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
def _interp_gyro(times_s: np.ndarray, gyro_rad_s: np.ndarray, t: float) -> np.ndarray:
|
||||
"""Linear interpolate gyro at an exact time."""
|
||||
|
||||
return _interp_vec(times_s, gyro_rad_s, t)
|
||||
|
||||
|
||||
def _pair_weight(duration_s: float, mean_gyro_norm: float, cov_trace: float) -> float:
|
||||
"""Larger weight for short, excited, low-covariance intervals."""
|
||||
|
||||
duration_term = 1.0 / max(duration_s, 0.05)
|
||||
excite_term = min(max(mean_gyro_norm, 1e-3), 1.0)
|
||||
avg_var = max(cov_trace / 3.0, 1e-8)
|
||||
return float(duration_term * excite_term / avg_var)
|
||||
|
||||
|
||||
def preintegrate_gyro(
|
||||
times_s: np.ndarray,
|
||||
gyro_rad_s: np.ndarray,
|
||||
t0: float,
|
||||
t1: float,
|
||||
bias_rad_s: np.ndarray | None = None,
|
||||
*,
|
||||
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||
) -> GyroPreintegration:
|
||||
"""Discrete mid-point gyro preintegration with exact endpoints.
|
||||
|
||||
``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``
|
||||
via right-invariant updates ``ΔR ← ΔR Exp((ω-b) dt)``.
|
||||
|
||||
Also returns:
|
||||
- ``cov``: 3×3 covariance of the right tangent noise on ``ΔR``
|
||||
- ``J_bg``: ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``
|
||||
"""
|
||||
|
||||
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||
bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
|
||||
duration = float(max(t1 - t0, 0.0))
|
||||
|
||||
empty = GyroPreintegration(
|
||||
delta_R=np.eye(3),
|
||||
duration_s=0.0,
|
||||
mean_gyro_norm=0.0,
|
||||
sigma_rad=1e3,
|
||||
weight=1e-6,
|
||||
bias_rad_s=bias.copy(),
|
||||
cov=np.eye(3) * 1e6,
|
||||
J_bg=np.zeros((3, 3)),
|
||||
)
|
||||
if times_s.size < 2 or duration <= 0:
|
||||
return empty
|
||||
|
||||
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||
duration = float(max(t1 - t0, 0.0))
|
||||
if duration <= 0:
|
||||
return empty
|
||||
|
||||
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||
left = max(left, 0)
|
||||
right = min(right, times_s.size - 1)
|
||||
if right <= left:
|
||||
return empty
|
||||
|
||||
delta_r = np.eye(3)
|
||||
j_bg = np.zeros((3, 3))
|
||||
cov = np.zeros((3, 3))
|
||||
sigma2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||
gyro_norms: list[float] = []
|
||||
|
||||
for index in range(left, right):
|
||||
t_a = float(times_s[index])
|
||||
t_b = float(times_s[index + 1])
|
||||
if t_b <= t0 or t_a >= t1:
|
||||
continue
|
||||
seg0 = max(t_a, t0)
|
||||
seg1 = min(t_b, t1)
|
||||
dt = seg1 - seg0
|
||||
if dt <= 0:
|
||||
continue
|
||||
|
||||
# Exact endpoint gyro via linear interpolation inside the sample interval.
|
||||
g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
|
||||
g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
|
||||
omega = 0.5 * (g_a + g_b) - bias
|
||||
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||
|
||||
theta = omega * dt
|
||||
jr = _right_jacobian(theta)
|
||||
a_mat = so3_exp(-theta)
|
||||
j_bg = a_mat @ j_bg - jr * dt
|
||||
cov = a_mat @ cov @ a_mat.T + jr @ (sigma2 * dt * np.eye(3)) @ jr.T
|
||||
delta_r = delta_r @ so3_exp(theta)
|
||||
|
||||
delta_r = orthonormalize_rotation(delta_r)
|
||||
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||
cov = 0.5 * (cov + cov.T)
|
||||
cov = cov + np.eye(3) * 1e-12
|
||||
if mean_gyro_norm < 0.02:
|
||||
cov = cov * 4.0
|
||||
cov_trace = float(np.trace(cov))
|
||||
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||
return GyroPreintegration(
|
||||
delta_R=delta_r,
|
||||
duration_s=duration,
|
||||
mean_gyro_norm=mean_gyro_norm,
|
||||
sigma_rad=sigma_rad,
|
||||
weight=weight,
|
||||
bias_rad_s=bias.copy(),
|
||||
cov=cov,
|
||||
J_bg=np.asarray(j_bg, dtype=float),
|
||||
)
|
||||
|
||||
|
||||
def preintegrate_imu(
|
||||
times_s: np.ndarray,
|
||||
gyro_rad_s: np.ndarray,
|
||||
acc_m_s2: np.ndarray,
|
||||
t0: float,
|
||||
t1: float,
|
||||
gyro_bias_rad_s: np.ndarray | None = None,
|
||||
acc_bias_m_s2: np.ndarray | None = None,
|
||||
*,
|
||||
sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
|
||||
sigma_a_m_s2_sqrt_hz: float = 2.0e-2,
|
||||
sigma_bg_rw_rad_s_sqrt_hz: float = 1.0e-5,
|
||||
sigma_ba_rw_m_s2_sqrt_hz: float = 1.0e-3,
|
||||
) -> ImuPreintegration:
|
||||
"""Mid-point IMU preintegration with exact endpoints and bias-RW noise.
|
||||
|
||||
Discrete updates (right-invariant)::
|
||||
|
||||
ΔR ← ΔR Exp((ω-bg) dt)
|
||||
Δv ← Δv + ΔR (a-ba) dt
|
||||
Δp ← Δp + Δv_old dt + 0.5 ΔR (a-ba) dt²
|
||||
|
||||
Propagates a 15-DoF error state ``[δθ, δv, δp, δbg, δba]`` then returns the
|
||||
top-left 9×9 covariance (bias RW already folded in) and 9×3 Jacobians.
|
||||
"""
|
||||
|
||||
times_s = np.asarray(times_s, dtype=float).reshape(-1)
|
||||
gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
|
||||
acc_m_s2 = np.asarray(acc_m_s2, dtype=float).reshape(-1, 3)
|
||||
bg = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||||
ba = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float).reshape(3)
|
||||
|
||||
empty = ImuPreintegration(
|
||||
delta_R=np.eye(3),
|
||||
delta_v=np.zeros(3),
|
||||
delta_p=np.zeros(3),
|
||||
duration_s=0.0,
|
||||
mean_gyro_norm=0.0,
|
||||
sigma_rad=1e3,
|
||||
weight=1e-6,
|
||||
gyro_bias_rad_s=bg.copy(),
|
||||
acc_bias_m_s2=ba.copy(),
|
||||
cov=np.eye(9) * 1e6,
|
||||
J_bg=np.zeros((9, 3)),
|
||||
J_ba=np.zeros((9, 3)),
|
||||
)
|
||||
if times_s.size < 2 or acc_m_s2.shape != gyro_rad_s.shape:
|
||||
return empty
|
||||
|
||||
t0 = float(np.clip(t0, times_s[0], times_s[-1]))
|
||||
t1 = float(np.clip(t1, times_s[0], times_s[-1]))
|
||||
duration = float(max(t1 - t0, 0.0))
|
||||
if duration <= 0:
|
||||
return empty
|
||||
|
||||
left = int(np.searchsorted(times_s, t0, side="left") - 1)
|
||||
right = int(np.searchsorted(times_s, t1, side="right"))
|
||||
left = max(left, 0)
|
||||
right = min(right, times_s.size - 1)
|
||||
if right <= left:
|
||||
return empty
|
||||
|
||||
delta_r = np.eye(3)
|
||||
delta_v = np.zeros(3)
|
||||
delta_p = np.zeros(3)
|
||||
# Jacobians of [δθ, δv, δp] w.r.t. constant bias (accumulated analytically).
|
||||
j_bg = np.zeros((9, 3))
|
||||
j_ba = np.zeros((9, 3))
|
||||
# 15×15 covariance: [θ, v, p, bg, ba]
|
||||
cov15 = np.zeros((15, 15))
|
||||
sg2 = float(sigma_g_rad_s_sqrt_hz) ** 2
|
||||
sa2 = float(sigma_a_m_s2_sqrt_hz) ** 2
|
||||
sbg2 = float(sigma_bg_rw_rad_s_sqrt_hz) ** 2
|
||||
sba2 = float(sigma_ba_rw_m_s2_sqrt_hz) ** 2
|
||||
gyro_norms: list[float] = []
|
||||
|
||||
for index in range(left, right):
|
||||
t_a = float(times_s[index])
|
||||
t_b = float(times_s[index + 1])
|
||||
if t_b <= t0 or t_a >= t1:
|
||||
continue
|
||||
seg0 = max(t_a, t0)
|
||||
seg1 = min(t_b, t1)
|
||||
dt = seg1 - seg0
|
||||
if dt <= 0:
|
||||
continue
|
||||
|
||||
g_a = _interp_vec(times_s, gyro_rad_s, seg0)
|
||||
g_b = _interp_vec(times_s, gyro_rad_s, seg1)
|
||||
a_a = _interp_vec(times_s, acc_m_s2, seg0)
|
||||
a_b = _interp_vec(times_s, acc_m_s2, seg1)
|
||||
omega = 0.5 * (g_a + g_b) - bg
|
||||
acc = 0.5 * (a_a + a_b) - ba
|
||||
gyro_norms.append(float(np.linalg.norm(omega)))
|
||||
|
||||
theta = omega * dt
|
||||
jr = _right_jacobian(theta)
|
||||
r_dt = so3_exp(theta)
|
||||
r_mid = delta_r # rotate body accel into i0 frame before update
|
||||
|
||||
# Bias Jacobians (Forster-style first-order recursion).
|
||||
j_r_bg = j_bg[0:3]
|
||||
j_v_bg = j_bg[3:6]
|
||||
j_p_bg = j_bg[6:9]
|
||||
j_r_ba = j_ba[0:3]
|
||||
j_v_ba = j_ba[3:6]
|
||||
j_p_ba = j_ba[6:9]
|
||||
|
||||
acc_skew = skew(acc)
|
||||
j_p_bg_new = j_p_bg + j_v_bg * dt - 0.5 * r_mid @ acc_skew @ j_r_bg * (dt**2)
|
||||
j_v_bg_new = j_v_bg - r_mid @ acc_skew @ j_r_bg * dt
|
||||
j_r_bg_new = r_dt.T @ j_r_bg - jr * dt
|
||||
|
||||
j_p_ba_new = j_p_ba + j_v_ba * dt - 0.5 * r_mid * (dt**2)
|
||||
j_v_ba_new = j_v_ba - r_mid * dt
|
||||
j_r_ba_new = r_dt.T @ j_r_ba
|
||||
|
||||
j_bg = np.vstack([j_r_bg_new, j_v_bg_new, j_p_bg_new])
|
||||
j_ba = np.vstack([j_r_ba_new, j_v_ba_new, j_p_ba_new])
|
||||
|
||||
# Nominal state update (use pre-update Δv in position).
|
||||
delta_p = delta_p + delta_v * dt + 0.5 * r_mid @ acc * (dt**2)
|
||||
delta_v = delta_v + r_mid @ acc * dt
|
||||
delta_r = orthonormalize_rotation(delta_r @ r_dt)
|
||||
|
||||
# Linearized error-state transition (15×15).
|
||||
f = np.eye(15)
|
||||
a_mat = so3_exp(-theta)
|
||||
f[0:3, 0:3] = a_mat
|
||||
f[0:3, 9:12] = -jr * dt
|
||||
f[3:6, 0:3] = -r_mid @ acc_skew * dt
|
||||
f[3:6, 12:15] = -r_mid * dt
|
||||
f[6:9, 0:3] = -0.5 * r_mid @ acc_skew * (dt**2)
|
||||
f[6:9, 3:6] = np.eye(3) * dt
|
||||
f[6:9, 12:15] = -0.5 * r_mid * (dt**2)
|
||||
|
||||
# Noise: continuous densities σ²; Var(∫n dt)=σ² dt. Columns: n_g, n_a, n_bg, n_ba.
|
||||
g_mat = np.zeros((15, 12))
|
||||
g_mat[0:3, 0:3] = jr
|
||||
g_mat[3:6, 3:6] = r_mid
|
||||
g_mat[6:9, 3:6] = 0.5 * r_mid * dt
|
||||
g_mat[9:12, 6:9] = np.eye(3)
|
||||
g_mat[12:15, 9:12] = np.eye(3)
|
||||
q = np.zeros((12, 12))
|
||||
q[0:3, 0:3] = sg2 * dt * np.eye(3)
|
||||
q[3:6, 3:6] = sa2 * dt * np.eye(3)
|
||||
q[6:9, 6:9] = sbg2 * dt * np.eye(3)
|
||||
q[9:12, 9:12] = sba2 * dt * np.eye(3)
|
||||
cov15 = f @ cov15 @ f.T + g_mat @ q @ g_mat.T
|
||||
|
||||
delta_r = orthonormalize_rotation(delta_r)
|
||||
mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
|
||||
cov9 = cov15[0:9, 0:9]
|
||||
cov9 = 0.5 * (cov9 + cov9.T) + np.eye(9) * 1e-12
|
||||
if mean_gyro_norm < 0.02:
|
||||
cov9 = cov9.copy()
|
||||
cov9[0:3, 0:3] = cov9[0:3, 0:3] * 4.0
|
||||
cov_trace = float(np.trace(cov9[0:3, 0:3]))
|
||||
sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
|
||||
weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
|
||||
return ImuPreintegration(
|
||||
delta_R=delta_r,
|
||||
delta_v=np.asarray(delta_v, dtype=float),
|
||||
delta_p=np.asarray(delta_p, dtype=float),
|
||||
duration_s=duration,
|
||||
mean_gyro_norm=mean_gyro_norm,
|
||||
sigma_rad=sigma_rad,
|
||||
weight=weight,
|
||||
gyro_bias_rad_s=bg.copy(),
|
||||
acc_bias_m_s2=ba.copy(),
|
||||
cov=np.asarray(cov9, dtype=float),
|
||||
J_bg=np.asarray(j_bg, dtype=float),
|
||||
J_ba=np.asarray(j_ba, dtype=float),
|
||||
)
|
||||
|
||||
|
||||
def apply_bias_correction_imu(
|
||||
preint: ImuPreintegration,
|
||||
delta_gyro_bias: np.ndarray | None = None,
|
||||
delta_acc_bias: np.ndarray | None = None,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""First-order bias correction of ``ΔR/Δv/Δp``.
|
||||
|
||||
Returns ``(delta_R, delta_v, delta_p)``.
|
||||
"""
|
||||
|
||||
dbg = np.zeros(3) if delta_gyro_bias is None else np.asarray(delta_gyro_bias, dtype=float).reshape(3)
|
||||
dba = np.zeros(3) if delta_acc_bias is None else np.asarray(delta_acc_bias, dtype=float).reshape(3)
|
||||
j_bg = np.asarray(preint.J_bg, dtype=float).reshape(9, 3)
|
||||
j_ba = np.asarray(preint.J_ba, dtype=float).reshape(9, 3)
|
||||
delta_r = orthonormalize_rotation(preint.delta_R @ so3_exp(j_bg[0:3] @ dbg))
|
||||
delta_v = preint.delta_v + j_bg[3:6] @ dbg + j_ba[3:6] @ dba
|
||||
delta_p = preint.delta_p + j_bg[6:9] @ dbg + j_ba[6:9] @ dba
|
||||
return delta_r, np.asarray(delta_v, dtype=float), np.asarray(delta_p, dtype=float)
|
||||
|
||||
|
||||
def relative_rotation_from_lidar(R_X: np.ndarray, R_B: np.ndarray) -> np.ndarray:
|
||||
"""Map LiDAR relative rotation into IMU frame: ``R_X R_B R_X^T``."""
|
||||
|
||||
r_x = orthonormalize_rotation(R_X)
|
||||
r_b = orthonormalize_rotation(R_B)
|
||||
return orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||||
|
||||
|
||||
def preintegration_rotation_residual(
|
||||
delta_R: np.ndarray,
|
||||
R_X: np.ndarray,
|
||||
R_B: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""``log( delta_R^T * R_X R_B R_X^T )`` in so(3)."""
|
||||
|
||||
predicted = relative_rotation_from_lidar(R_X, R_B)
|
||||
return so3_log(delta_R.T @ predicted)
|
||||
|
||||
|
||||
def apply_bias_jacobian_correction(
|
||||
delta_R: np.ndarray,
|
||||
J_bg: np.ndarray,
|
||||
delta_bias_rad_s: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""First-order update ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``."""
|
||||
|
||||
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||
j_bg = np.asarray(J_bg, dtype=float).reshape(3, 3)
|
||||
return orthonormalize_rotation(delta_R @ so3_exp(j_bg @ db))
|
||||
|
||||
|
||||
def apply_constant_bias_correction(
|
||||
delta_R: np.ndarray,
|
||||
duration_s: float,
|
||||
delta_bias_rad_s: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""Legacy first-order correction when ``J_bg`` is unavailable.
|
||||
|
||||
``ΔR(b+δb) ≈ ΔR Exp(-δb Δt)`` (identity Jacobian approximation).
|
||||
"""
|
||||
|
||||
db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
|
||||
return orthonormalize_rotation(delta_R @ so3_exp(-db * float(duration_s)))
|
||||
|
||||
|
||||
def residual_whiten_matrix(cov: np.ndarray) -> np.ndarray:
|
||||
"""Return ``W`` such that ``W @ e`` is approximately information-whitened.
|
||||
|
||||
Accepts square ``n×n`` covariances (3×3 rotation or 9×9 full IMU).
|
||||
"""
|
||||
|
||||
matrix = np.asarray(cov, dtype=float)
|
||||
if matrix.ndim != 2 or matrix.shape[0] != matrix.shape[1]:
|
||||
raise ValueError("cov must be square")
|
||||
n = matrix.shape[0]
|
||||
matrix = 0.5 * (matrix + matrix.T) + np.eye(n) * 1e-10
|
||||
try:
|
||||
info = np.linalg.inv(matrix)
|
||||
return np.linalg.cholesky(info).T
|
||||
except np.linalg.LinAlgError:
|
||||
scale = 1.0 / max(float(np.sqrt(np.trace(matrix) / n)), 1e-6)
|
||||
return np.eye(n) * scale
|
||||
@@ -0,0 +1,545 @@
|
||||
"""Joint extrinsic refinement: Phase-A rotation factors + Phase-C SE(3) IMU factors."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
|
||||
from .contracts import ImuSeries, MotionPair
|
||||
from .geometry import make_transform, orthonormalize_rotation, so3_exp, so3_log
|
||||
from .imu_preintegration import (
|
||||
apply_bias_jacobian_correction,
|
||||
apply_constant_bias_correction,
|
||||
preintegrate_gyro,
|
||||
preintegration_rotation_residual,
|
||||
residual_whiten_matrix,
|
||||
)
|
||||
from .observability import ObservabilityReport, analyze_observability
|
||||
|
||||
G_NORM = 9.80665
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointExtrinsicResult:
|
||||
T_IMU_lidar: np.ndarray
|
||||
translation_accepted: bool
|
||||
residual_rms_rot_deg: float
|
||||
residual_rms_trans_m: float
|
||||
observability: ObservabilityReport
|
||||
gyro_bias_rad_s: np.ndarray | None = None
|
||||
accel_bias_m_s2: np.ndarray | None = None
|
||||
gravity_m_s2: np.ndarray | None = None
|
||||
notes: tuple[str, ...] = ()
|
||||
|
||||
|
||||
def _pair_weight(pair: MotionPair) -> float:
|
||||
weight = float(pair.metadata.get("weight", 1.0))
|
||||
if not np.isfinite(weight) or weight <= 0:
|
||||
return 1.0
|
||||
return weight
|
||||
|
||||
|
||||
def _pair_j_bg(pair: MotionPair) -> np.ndarray | None:
|
||||
raw = pair.metadata.get("J_bg")
|
||||
if raw is None:
|
||||
return None
|
||||
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||||
|
||||
|
||||
def _pair_cov(pair: MotionPair) -> np.ndarray:
|
||||
raw = pair.metadata.get("cov")
|
||||
if raw is None:
|
||||
sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2))
|
||||
return np.eye(3) * max(sigma, 1e-4) ** 2
|
||||
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||||
|
||||
|
||||
def _corrected_delta_r(
|
||||
pair: MotionPair,
|
||||
delta_bias: np.ndarray,
|
||||
*,
|
||||
imu: ImuSeries | None,
|
||||
bias0: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
j_bg = _pair_j_bg(pair)
|
||||
if j_bg is not None:
|
||||
return apply_bias_jacobian_correction(pair.R_A, j_bg, delta_bias)
|
||||
if imu is not None and "t_i_imu_s" in pair.metadata and "t_j_imu_s" in pair.metadata:
|
||||
preint = preintegrate_gyro(
|
||||
imu.t_s,
|
||||
imu.gyro_rad_s,
|
||||
float(pair.metadata["t_i_imu_s"]),
|
||||
float(pair.metadata["t_j_imu_s"]),
|
||||
bias0 + delta_bias,
|
||||
)
|
||||
return preint.delta_R
|
||||
duration = float(pair.metadata.get("duration_s", max(pair.t_j_s - pair.t_i_s, 1e-3)))
|
||||
return apply_constant_bias_correction(pair.R_A, duration, delta_bias)
|
||||
|
||||
|
||||
def _gravity_basis(g0: np.ndarray) -> np.ndarray:
|
||||
"""Return 3×2 orthonormal basis spanning the plane orthogonal to ``g0``."""
|
||||
|
||||
g = np.asarray(g0, dtype=float).reshape(3)
|
||||
n = np.linalg.norm(g)
|
||||
if n < 1e-9:
|
||||
g = np.array([0.0, 0.0, -G_NORM])
|
||||
n = G_NORM
|
||||
g = g / n
|
||||
axis = np.array([1.0, 0.0, 0.0]) if abs(g[0]) < 0.9 else np.array([0.0, 1.0, 0.0])
|
||||
e1 = np.cross(g, axis)
|
||||
e1 /= max(np.linalg.norm(e1), 1e-12)
|
||||
e2 = np.cross(g, e1)
|
||||
return np.column_stack([e1, e2])
|
||||
|
||||
|
||||
def _gravity_from_params(xy: np.ndarray, g0: np.ndarray, basis: np.ndarray) -> np.ndarray:
|
||||
raw = np.asarray(g0, dtype=float).reshape(3) + basis @ np.asarray(xy, dtype=float).reshape(2)
|
||||
n = float(np.linalg.norm(raw))
|
||||
if n < 1e-9:
|
||||
return np.asarray(g0, dtype=float).reshape(3)
|
||||
return raw * (G_NORM / n)
|
||||
|
||||
|
||||
def _lidar_to_imu_relative(r_x: np.ndarray, t_x: np.ndarray, r_b: np.ndarray, t_b: np.ndarray):
|
||||
"""Map LiDAR relative pose to IMU: ``T_A = T_X T_B T_X^{-1}``."""
|
||||
|
||||
r_a = orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||||
t_a = (np.eye(3) - r_a) @ t_x + r_x @ t_b
|
||||
return r_a, t_a
|
||||
|
||||
|
||||
def _corrected_preint_quantities(
|
||||
pair: MotionPair,
|
||||
bg_i: np.ndarray,
|
||||
ba_i: np.ndarray,
|
||||
bg0: np.ndarray,
|
||||
ba0: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""First-order correct ΔR/Δv/Δp for keyframe biases vs preintegration biases."""
|
||||
|
||||
dbg = np.asarray(bg_i, dtype=float).reshape(3) - np.asarray(bg0, dtype=float).reshape(3)
|
||||
dba = np.asarray(ba_i, dtype=float).reshape(3) - np.asarray(ba0, dtype=float).reshape(3)
|
||||
j_bg = pair.metadata.get("J_bg9")
|
||||
j_ba = pair.metadata.get("J_ba")
|
||||
delta_v0 = np.asarray(pair.metadata.get("delta_v", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||||
delta_p0 = (
|
||||
np.asarray(pair.t_A_m, dtype=float).reshape(3)
|
||||
if pair.t_A_m is not None
|
||||
else np.asarray(pair.metadata.get("delta_p", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||||
)
|
||||
if j_bg is None or j_ba is None:
|
||||
delta_r = apply_bias_jacobian_correction(
|
||||
pair.R_A,
|
||||
_pair_j_bg(pair) if _pair_j_bg(pair) is not None else np.zeros((3, 3)),
|
||||
dbg,
|
||||
)
|
||||
return delta_r, delta_v0, delta_p0
|
||||
j_bg_m = np.asarray(j_bg, dtype=float).reshape(9, 3)
|
||||
j_ba_m = np.asarray(j_ba, dtype=float).reshape(9, 3)
|
||||
delta_r = orthonormalize_rotation(pair.R_A @ so3_exp(j_bg_m[0:3] @ dbg))
|
||||
delta_v = delta_v0 + j_bg_m[3:6] @ dbg + j_ba_m[3:6] @ dba
|
||||
delta_p = delta_p0 + j_bg_m[6:9] @ dbg + j_ba_m[6:9] @ dba
|
||||
return delta_r, delta_v, delta_p
|
||||
|
||||
|
||||
def _build_nav_rotations(
|
||||
keyframe_ids: list[int],
|
||||
id_to_idx: dict[int, int],
|
||||
consecutive_pairs: dict[tuple[int, int], MotionPair],
|
||||
r_x: np.ndarray,
|
||||
t_x: np.ndarray,
|
||||
) -> list[np.ndarray]:
|
||||
"""Chain IMU orientations; 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_bias0: 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_bias0, 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_bias0, dtype=float).reshape(3)
|
||||
|
||||
# State: dθ(3), t(3), g_xy(2), v(3K), bg(3K), ba(3K)
|
||||
n_v = 3 * k_count
|
||||
n_b = 3 * k_count
|
||||
dim = 3 + 3 + 2 + n_v + n_b + n_b
|
||||
x0 = np.zeros(dim)
|
||||
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] = bg0
|
||||
|
||||
whitened = []
|
||||
for pair in usable:
|
||||
cov9 = pair.metadata.get("cov9")
|
||||
if cov9 is None:
|
||||
cov = _pair_cov(pair)
|
||||
cov9_m = np.eye(9)
|
||||
cov9_m[0:3, 0:3] = cov
|
||||
cov9_m[3:6, 3:6] = np.eye(3) * 0.25
|
||||
cov9_m[6:9, 6:9] = np.eye(3) * 1.0
|
||||
else:
|
||||
cov9_m = np.asarray(cov9, dtype=float).reshape(9, 9)
|
||||
whitened.append(residual_whiten_matrix(cov9_m))
|
||||
|
||||
def unpack(vec: np.ndarray):
|
||||
r_opt = orthonormalize_rotation(so3_exp(vec[0:3]) @ r_x)
|
||||
t_opt = vec[3:6]
|
||||
g_opt = _gravity_from_params(vec[6:8], g0, basis)
|
||||
base = 8
|
||||
vels = vec[base : base + n_v].reshape(k_count, 3)
|
||||
base += n_v
|
||||
bgs = vec[base : base + n_b].reshape(k_count, 3)
|
||||
base += n_b
|
||||
bas = vec[base : base + n_b].reshape(k_count, 3)
|
||||
return r_opt, t_opt, g_opt, vels, bgs, bas
|
||||
|
||||
def residuals(vec: np.ndarray) -> np.ndarray:
|
||||
r_opt, t_opt, g_opt, vels, bgs, bas = unpack(vec)
|
||||
nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
|
||||
out: list[np.ndarray] = []
|
||||
|
||||
for pair, whiten in zip(usable, whitened):
|
||||
i_idx = id_to_idx[pair.i]
|
||||
j_idx = id_to_idx[pair.j]
|
||||
dt = float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s))
|
||||
dt = max(dt, 1e-3)
|
||||
delta_r, delta_v, delta_p = _corrected_preint_quantities(
|
||||
pair, bgs[i_idx], bas[i_idx], bg0, ba0
|
||||
)
|
||||
t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
|
||||
r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
|
||||
r_i = nav_r[i_idx]
|
||||
v_i = vels[i_idx]
|
||||
v_j = vels[j_idx]
|
||||
|
||||
err_r = so3_log(delta_r.T @ r_meas)
|
||||
err_v = v_j - v_i - g_opt * dt - r_i @ delta_v
|
||||
err_p = r_i @ (t_meas - delta_p) - v_i * dt - 0.5 * g_opt * (dt**2)
|
||||
err = np.concatenate([err_r, err_v, err_p])
|
||||
w = np.sqrt(_pair_weight(pair))
|
||||
out.append(w * (whiten @ err))
|
||||
|
||||
# Bias random-walk between consecutive keyframes (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] - bg0))
|
||||
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 solve_joint_extrinsic(
|
||||
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||
r_x: np.ndarray,
|
||||
*,
|
||||
force_rotation_only: bool = False,
|
||||
imu: ImuSeries | None = None,
|
||||
delta_t_s: float = 0.0,
|
||||
gyro_bias_rad_s: np.ndarray | None = None,
|
||||
gravity_init_m_s2: np.ndarray | None = None,
|
||||
bias_prior_sigma_rad_s: float = 0.02,
|
||||
enable_phase_c: bool | None = None,
|
||||
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, weight, whiten in zip(usable, weights, whitens):
|
||||
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||
err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||||
residuals.append(np.sqrt(weight) * (whiten @ err))
|
||||
residuals.append(prior_w * delta_bias)
|
||||
return np.concatenate(residuals) if residuals else np.zeros(0)
|
||||
|
||||
def residual_rot_bias(vec: np.ndarray) -> np.ndarray:
|
||||
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
|
||||
return rotation_residuals(r_opt, vec[3:])
|
||||
|
||||
if usable:
|
||||
opt = least_squares(
|
||||
residual_rot_bias,
|
||||
np.zeros(6),
|
||||
loss="huber",
|
||||
f_scale=np.deg2rad(1.0),
|
||||
max_nfev=200,
|
||||
)
|
||||
r = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r)
|
||||
delta_bias = opt.x[3:]
|
||||
bias_out = bias0 + delta_bias
|
||||
notes.append(
|
||||
"phase-A joint refine (Σ-whitened + J_bg): "
|
||||
f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
|
||||
f"weighted pairs={len(usable)}"
|
||||
)
|
||||
else:
|
||||
bias_out = bias0
|
||||
delta_bias = np.zeros(3)
|
||||
notes.append("no pairs for joint refine")
|
||||
|
||||
rot_errs = []
|
||||
for pair in usable:
|
||||
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||||
err = preintegration_rotation_residual(delta_r, r, pair.R_B)
|
||||
rot_errs.append(np.degrees(np.linalg.norm(err)))
|
||||
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
|
||||
|
||||
t = np.zeros(3) 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_bias0=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)
|
||||
translation_accepted = True
|
||||
notes.append(
|
||||
"phase-C translation residual/gate failed; keeping CAD translation prior"
|
||||
)
|
||||
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 or t_prior_m is not None
|
||||
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 = True
|
||||
notes.append("SE3 motion solve gated off; using CAD translation prior with refined rotation")
|
||||
else:
|
||||
notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")
|
||||
|
||||
return JointExtrinsicResult(
|
||||
T_IMU_lidar=make_transform(t, r),
|
||||
translation_accepted=bool(translation_accepted and np.linalg.norm(t) > 0),
|
||||
residual_rms_rot_deg=rot_rms,
|
||||
residual_rms_trans_m=0.0 if not translation_accepted else trans_rms,
|
||||
observability=observability,
|
||||
gyro_bias_rad_s=np.asarray(bias_out, dtype=float),
|
||||
accel_bias_m_s2=None if accel_bias_out is None else np.asarray(accel_bias_out, dtype=float),
|
||||
gravity_m_s2=None if gravity_out is None else np.asarray(gravity_out, dtype=float),
|
||||
notes=tuple(notes),
|
||||
)
|
||||
@@ -0,0 +1,49 @@
|
||||
"""LiDAR keyframe selection."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import LidarFrame
|
||||
from .registration import register_lidar_pair
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class KeyframeSet:
|
||||
indices: tuple[int, ...]
|
||||
frames: tuple[LidarFrame, ...]
|
||||
|
||||
|
||||
def build_keyframes(
|
||||
frames: list[LidarFrame],
|
||||
*,
|
||||
min_translation_m: float = 0.3,
|
||||
min_rotation_deg: float = 3.0,
|
||||
max_frame_gap: int = 8,
|
||||
) -> KeyframeSet:
|
||||
"""Select keyframes with enough relative motion for hand-eye pairs."""
|
||||
|
||||
if not frames:
|
||||
return KeyframeSet((), ())
|
||||
|
||||
selected = [0]
|
||||
last = 0
|
||||
for index in range(1, len(frames)):
|
||||
if index - last > max_frame_gap:
|
||||
selected.append(index)
|
||||
last = index
|
||||
continue
|
||||
result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
|
||||
if not result.ok:
|
||||
continue
|
||||
if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
|
||||
selected.append(index)
|
||||
last = index
|
||||
|
||||
if selected[-1] != len(frames) - 1 and len(frames) > 1:
|
||||
selected.append(len(frames) - 1)
|
||||
|
||||
unique = tuple(dict.fromkeys(selected))
|
||||
return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))
|
||||
@@ -0,0 +1,59 @@
|
||||
"""Coarse LiDAR deskew using a constant body rate over the sweep."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import ImuSeries, LidarFrame
|
||||
from .geometry import so3_exp
|
||||
from .time_offset import lidar_time_to_imu_time
|
||||
|
||||
|
||||
def deskew_lidar_frames(
|
||||
frames: list[LidarFrame],
|
||||
imu: ImuSeries,
|
||||
*,
|
||||
delta_t_s: float,
|
||||
R_IMU_lidar: np.ndarray | None = None,
|
||||
gyro_bias_rad_s: np.ndarray | None = None,
|
||||
) -> list[LidarFrame]:
|
||||
"""Return deskewed copies when extrinsic is known; otherwise return originals."""
|
||||
|
||||
if R_IMU_lidar is None:
|
||||
return frames
|
||||
|
||||
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||
r_li = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3).T
|
||||
output: list[LidarFrame] = []
|
||||
|
||||
for frame in frames:
|
||||
n = frame.points_xyz.shape[0]
|
||||
if n < 10:
|
||||
output.append(frame)
|
||||
continue
|
||||
t_mid_imu = lidar_time_to_imu_time(frame.t_mid_s, delta_t_s)
|
||||
index = int(np.clip(np.searchsorted(imu.t_s, t_mid_imu), 1, imu.t_s.size - 1))
|
||||
omega_lidar = r_li @ (imu.gyro_rad_s[index] - bias)
|
||||
duration = max(frame.t_end_s - frame.t_start_s, 1e-3)
|
||||
rel = np.linspace(-0.5, 0.5, n) * duration
|
||||
deskewed = np.empty_like(frame.points_xyz)
|
||||
# Piecewise-constant rotation over a few time bins.
|
||||
bins = 12
|
||||
edges = np.linspace(-0.5 * duration, 0.5 * duration, bins + 1)
|
||||
for b in range(bins):
|
||||
mask = (rel >= edges[b]) & (rel <= edges[b + 1] if b == bins - 1 else rel < edges[b + 1])
|
||||
if not np.any(mask):
|
||||
continue
|
||||
tau = 0.5 * (edges[b] + edges[b + 1])
|
||||
rot = so3_exp(omega_lidar * float(tau))
|
||||
deskewed[mask] = frame.points_xyz[mask] @ rot.T
|
||||
output.append(
|
||||
LidarFrame(
|
||||
frame_id=frame.frame_id,
|
||||
t_start_s=frame.t_start_s,
|
||||
t_end_s=frame.t_end_s,
|
||||
points_xyz=deskewed,
|
||||
path=frame.path,
|
||||
)
|
||||
)
|
||||
return output
|
||||
@@ -0,0 +1,82 @@
|
||||
"""LiDAR adapters for the V1 standard intermediate format.
|
||||
|
||||
Accepted input: a directory containing ``frames_index.csv`` and per-frame NPZ files.
|
||||
|
||||
frames_index.csv
|
||||
----------------
|
||||
frame_id,file,t_start,t_end
|
||||
|
||||
Each NPZ referenced by ``file`` must contain:
|
||||
- points: float array shaped (N, 3) in LiDAR Cartesian coordinates (metres)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import LidarFrame
|
||||
|
||||
|
||||
def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
|
||||
"""Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
|
||||
|
||||
root = Path(path)
|
||||
index_path = root / "frames_index.csv"
|
||||
if not index_path.exists():
|
||||
raise FileNotFoundError(f"missing frames_index.csv under {root}")
|
||||
|
||||
rows = np.genfromtxt(index_path, delimiter=",", names=True, dtype=None, encoding="utf-8")
|
||||
if rows.ndim == 0:
|
||||
rows = np.array([rows])
|
||||
names = set(rows.dtype.names or ())
|
||||
# NumPy may rename reserved name ``file`` to ``file_``.
|
||||
file_key = "filename" if "filename" in names else ("file_" if "file_" in names else "file")
|
||||
required = {"frame_id", "t_start", "t_end"}
|
||||
if not required.issubset(names) or file_key not in names:
|
||||
raise ValueError(
|
||||
f"frames_index.csv must contain frame_id,{file_key}/filename,t_start,t_end; got {sorted(names)}"
|
||||
)
|
||||
|
||||
frames: list[LidarFrame] = []
|
||||
for row in rows:
|
||||
frame_id = str(row["frame_id"])
|
||||
rel = str(row[file_key])
|
||||
npz_path = root / rel
|
||||
with np.load(npz_path) as payload:
|
||||
if "points" not in payload.files:
|
||||
raise ValueError(f"{npz_path} must contain array 'points'")
|
||||
points = np.asarray(payload["points"], dtype=float)
|
||||
if points.ndim != 2 or points.shape[1] < 3:
|
||||
raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
|
||||
frames.append(
|
||||
LidarFrame(
|
||||
frame_id=frame_id,
|
||||
t_start_s=float(row["t_start"]),
|
||||
t_end_s=float(row["t_end"]),
|
||||
points_xyz=points[:, :3],
|
||||
path=npz_path,
|
||||
)
|
||||
)
|
||||
frames.sort(key=lambda frame: frame.t_mid_s)
|
||||
return frames
|
||||
|
||||
|
||||
def save_lidar_session(
|
||||
root: Path | str,
|
||||
frames: list[LidarFrame],
|
||||
*,
|
||||
points_dirname: str = "frames",
|
||||
) -> None:
|
||||
"""Write a LiDAR session directory in the standard intermediate format."""
|
||||
|
||||
destination = Path(root)
|
||||
frames_dir = destination / points_dirname
|
||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||
index_rows: list[str] = ["frame_id,filename,t_start,t_end"]
|
||||
for index, frame in enumerate(frames):
|
||||
relative = f"{points_dirname}/frame_{index:05d}.npz"
|
||||
np.savez_compressed(destination / relative, points=np.asarray(frame.points_xyz, dtype=float))
|
||||
index_rows.append(f"{frame.frame_id},{relative},{frame.t_start_s:.9f},{frame.t_end_s:.9f}")
|
||||
(destination / "frames_index.csv").write_text("\n".join(index_rows) + "\n", encoding="utf-8")
|
||||
@@ -0,0 +1,136 @@
|
||||
"""Build IMU/LiDAR relative-motion pairs for hand-eye calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import ImuSeries, LidarFrame, MotionPair
|
||||
from .geometry import make_transform, rotation_angle_deg
|
||||
from .imu_preintegration import preintegrate_imu
|
||||
from .registration import register_lidar_pair
|
||||
from .time_offset import lidar_time_to_imu_time
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MotionPairSet:
|
||||
pairs: tuple[MotionPair, ...]
|
||||
notes: tuple[str, ...] = ()
|
||||
|
||||
|
||||
def build_motion_pairs(
|
||||
*,
|
||||
session_id: str,
|
||||
keyframes: list[LidarFrame],
|
||||
keyframe_indices: list[int] | tuple[int, ...],
|
||||
imu: ImuSeries,
|
||||
delta_t_s: float,
|
||||
gyro_bias_rad_s: np.ndarray | None = None,
|
||||
acc_bias_m_s2: np.ndarray | None = None,
|
||||
min_rotation_deg: float = 3.0,
|
||||
min_translation_m: float = 0.3,
|
||||
max_index_span: int = 4,
|
||||
) -> MotionPairSet:
|
||||
"""Create A/B motion pairs between nearby keyframes.
|
||||
|
||||
IMU side uses full Phase-C preintegration (``ΔR/Δv/Δp``, ``Σ9``, ``J_bg/J_ba``).
|
||||
Rotation hand-eye still consumes ``R_A = ΔR`` only.
|
||||
"""
|
||||
|
||||
notes: list[str] = []
|
||||
pairs: list[MotionPair] = []
|
||||
bias_g = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||
bias_a = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float)
|
||||
n = len(keyframes)
|
||||
if n < 2:
|
||||
return MotionPairSet((), ("need at least two keyframes",))
|
||||
|
||||
for span in range(1, max_index_span + 1):
|
||||
for start in range(0, n - span):
|
||||
i = start
|
||||
j = start + span
|
||||
frame_i = keyframes[i]
|
||||
frame_j = keyframes[j]
|
||||
reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
|
||||
if not reg.ok:
|
||||
continue
|
||||
if reg.rotation_deg < min_rotation_deg and reg.translation_m < min_translation_m:
|
||||
continue
|
||||
|
||||
t_i_imu = lidar_time_to_imu_time(frame_i.t_mid_s, delta_t_s)
|
||||
t_j_imu = lidar_time_to_imu_time(frame_j.t_mid_s, delta_t_s)
|
||||
if t_j_imu <= t_i_imu:
|
||||
continue
|
||||
if t_i_imu < imu.t_s[0] or t_j_imu > imu.t_s[-1]:
|
||||
continue
|
||||
|
||||
preint = preintegrate_imu(
|
||||
imu.t_s,
|
||||
imu.gyro_rad_s,
|
||||
imu.acc_m_s2,
|
||||
t_i_imu,
|
||||
t_j_imu,
|
||||
bias_g,
|
||||
bias_a,
|
||||
)
|
||||
r_a = preint.delta_R
|
||||
r_b = reg.transform[:3, :3]
|
||||
t_b = reg.transform[:3, 3]
|
||||
rot_a = rotation_angle_deg(r_a)
|
||||
if abs(rot_a - reg.rotation_deg) > max(15.0, 1.0 * max(rot_a, reg.rotation_deg)):
|
||||
continue
|
||||
|
||||
pairs.append(
|
||||
MotionPair(
|
||||
session_id=session_id,
|
||||
i=int(keyframe_indices[i]),
|
||||
j=int(keyframe_indices[j]),
|
||||
t_i_s=frame_i.t_mid_s,
|
||||
t_j_s=frame_j.t_mid_s,
|
||||
R_A=r_a,
|
||||
R_B=r_b,
|
||||
t_A_m=np.asarray(preint.delta_p, dtype=float),
|
||||
t_B_m=np.asarray(t_b, dtype=float),
|
||||
fitness=reg.fitness,
|
||||
metadata={
|
||||
"backend": reg.backend,
|
||||
"rotation_deg_B": reg.rotation_deg,
|
||||
"translation_m_B": reg.translation_m,
|
||||
"rotation_deg_A": rot_a,
|
||||
"weight": preint.weight,
|
||||
"duration_s": preint.duration_s,
|
||||
"mean_gyro_norm": preint.mean_gyro_norm,
|
||||
"preint_sigma_rad": preint.sigma_rad,
|
||||
"cov": preint.cov[0:3, 0:3].tolist(),
|
||||
"cov9": preint.cov.tolist(),
|
||||
"J_bg": preint.J_bg[0:3, 0:3].tolist(),
|
||||
"J_bg9": preint.J_bg.tolist(),
|
||||
"J_ba": preint.J_ba.tolist(),
|
||||
"delta_v": preint.delta_v.tolist(),
|
||||
"delta_p": preint.delta_p.tolist(),
|
||||
"t_i_imu_s": t_i_imu,
|
||||
"t_j_imu_s": t_j_imu,
|
||||
"modeling": "imu_preintegration_factor_phase_c",
|
||||
},
|
||||
)
|
||||
)
|
||||
|
||||
notes.append(
|
||||
f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)"
|
||||
)
|
||||
return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes))
|
||||
|
||||
|
||||
def pairs_to_transforms(pairs: tuple[MotionPair, ...]) -> tuple[list[np.ndarray], list[np.ndarray]]:
|
||||
"""Helper returning SE(3) lists when translations are present."""
|
||||
|
||||
a_list: list[np.ndarray] = []
|
||||
b_list: list[np.ndarray] = []
|
||||
for pair in pairs:
|
||||
if pair.t_B_m is None:
|
||||
continue
|
||||
t_a = np.zeros(3) if pair.t_A_m is None else pair.t_A_m
|
||||
a_list.append(make_transform(t_a, pair.R_A))
|
||||
b_list.append(make_transform(pair.t_B_m, pair.R_B))
|
||||
return a_list, b_list
|
||||
@@ -0,0 +1,104 @@
|
||||
"""Normalized-Jacobian observability analysis for rotation / SE(3) gates."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import MotionPair
|
||||
from .geometry import skew, so3_log
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ObservabilityReport:
|
||||
rotation_observable: bool
|
||||
translation_observable: bool
|
||||
condition_rotation: float
|
||||
condition_translation: float
|
||||
notes: tuple[str, ...] = ()
|
||||
|
||||
|
||||
def _rotation_jacobian(pairs: list[MotionPair], r_x: np.ndarray) -> np.ndarray:
|
||||
rows = []
|
||||
for pair in pairs:
|
||||
# Residual r = log(R_x^T R_A R_x R_B^T); approximate J w.r.t. left perturbation of R_x.
|
||||
# Use finite-difference columns for robustness in V1.
|
||||
base = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||
cols = []
|
||||
eps = 1e-5
|
||||
for axis in range(3):
|
||||
delta = np.zeros(3)
|
||||
delta[axis] = eps
|
||||
r_pert = r_x @ (np.eye(3) + skew(delta))
|
||||
# Orthonormalize lightly
|
||||
u, _, vt = np.linalg.svd(r_pert)
|
||||
r_pert = u @ vt
|
||||
pert = so3_log(r_pert.T @ pair.R_A @ r_pert @ pair.R_B.T)
|
||||
cols.append((pert - base) / eps)
|
||||
rows.append(np.column_stack(cols))
|
||||
return np.vstack(rows) if rows else np.zeros((0, 3))
|
||||
|
||||
|
||||
def analyze_observability(
|
||||
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||||
r_x: np.ndarray,
|
||||
*,
|
||||
condition_threshold: float = 100.0,
|
||||
) -> ObservabilityReport:
|
||||
"""Gate whether rotation-only or full SE(3) should be accepted."""
|
||||
|
||||
usable = list(pairs)
|
||||
notes: list[str] = []
|
||||
if len(usable) < 3:
|
||||
return ObservabilityReport(False, False, 1e9, 1e9, ("insufficient pairs",))
|
||||
|
||||
j_r = _rotation_jacobian(usable, np.asarray(r_x, dtype=float))
|
||||
if j_r.size == 0:
|
||||
return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
|
||||
|
||||
# Normalize columns.
|
||||
col_norm = np.linalg.norm(j_r, axis=0) + 1e-12
|
||||
j_r_n = j_r / col_norm
|
||||
singular = np.linalg.svd(j_r_n, compute_uv=False)
|
||||
cond_r = float(singular[0] / max(singular[-1], 1e-12))
|
||||
rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3
|
||||
|
||||
# Translation observability proxy: diversity of rotation axes and presence of translation in B.
|
||||
axes = []
|
||||
translations = []
|
||||
for pair in usable:
|
||||
axis = so3_log(pair.R_B)
|
||||
n = np.linalg.norm(axis)
|
||||
if n > 1e-8:
|
||||
axes.append(axis / n)
|
||||
if pair.t_B_m is not None:
|
||||
translations.append(pair.t_B_m)
|
||||
axis_rank = 0
|
||||
if axes:
|
||||
axis_mat = np.asarray(axes, dtype=float)
|
||||
axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
|
||||
trans_span = 0.0
|
||||
if translations:
|
||||
tmat = np.asarray(translations, dtype=float)
|
||||
trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
|
||||
# For planar yaw-mostly motion, translation z is typically weak.
|
||||
translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
|
||||
cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))
|
||||
|
||||
if not rotation_ok:
|
||||
notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
|
||||
else:
|
||||
notes.append(f"rotation condition {cond_r:.1f}")
|
||||
if not translation_ok:
|
||||
notes.append(
|
||||
f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); "
|
||||
"V1 will reject full SE3 without strong priors"
|
||||
)
|
||||
return ObservabilityReport(
|
||||
rotation_observable=rotation_ok,
|
||||
translation_observable=translation_ok,
|
||||
condition_rotation=cond_r,
|
||||
condition_translation=cond_t,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
@@ -0,0 +1,424 @@
|
||||
"""Executable LiDAR–IMU calibration pipeline (V1)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass, replace
|
||||
from pathlib import Path
|
||||
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 .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", "用全部会话运动对联合精修；完整模式估平移"),
|
||||
PipelineStage("finalize", "写出结果与质量报告"),
|
||||
)
|
||||
|
||||
|
||||
def describe_pipeline(_: CalibrationRequest) -> tuple[PipelineStage, ...]:
|
||||
"""Return the planned stages."""
|
||||
|
||||
return STAGES
|
||||
|
||||
|
||||
def _build_pairs_and_handeye(
|
||||
*,
|
||||
session_id: str,
|
||||
working_frames,
|
||||
imu,
|
||||
delta_t_s: float,
|
||||
gyro_bias_rad_s: np.ndarray,
|
||||
request: CalibrationRequest,
|
||||
):
|
||||
keyframes = build_keyframes(
|
||||
working_frames,
|
||||
min_translation_m=request.min_pair_translation_m,
|
||||
min_rotation_deg=request.min_pair_rotation_deg,
|
||||
)
|
||||
pair_set = build_motion_pairs(
|
||||
session_id=session_id,
|
||||
keyframes=list(keyframes.frames),
|
||||
keyframe_indices=keyframes.indices,
|
||||
imu=imu,
|
||||
delta_t_s=delta_t_s,
|
||||
gyro_bias_rad_s=gyro_bias_rad_s,
|
||||
min_rotation_deg=request.min_pair_rotation_deg,
|
||||
min_translation_m=request.min_pair_translation_m,
|
||||
)
|
||||
handeye = solve_rotation_handeye(pair_set.pairs)
|
||||
return keyframes, pair_set, handeye
|
||||
|
||||
|
||||
def _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 _prepare_session_pairs(
|
||||
session: SessionInput,
|
||||
request: CalibrationRequest,
|
||||
) -> dict[str, Any]:
|
||||
"""Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet."""
|
||||
|
||||
imu = load_imu_samples(session.imu_source)
|
||||
frames = load_lidar_frames(session.lidar_source)
|
||||
|
||||
ts = audit_timestamps(imu, frames)
|
||||
if not ts.ok:
|
||||
return {"ok": False, "stage": "timestamp_audit", "session_id": session.session_id, "report": asdict(ts)}
|
||||
|
||||
imu_report = audit_imu(imu)
|
||||
if not imu_report.ok:
|
||||
return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)}
|
||||
|
||||
offset = estimate_time_offset(
|
||||
imu,
|
||||
frames,
|
||||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||
search_s=request.time_offset_search_s,
|
||||
)
|
||||
if not offset.ok:
|
||||
return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}
|
||||
|
||||
working_frames = frames
|
||||
r_x = np.eye(3)
|
||||
handeye = None
|
||||
pair_set = None
|
||||
keyframes = None
|
||||
pairs_notes: list[str] = []
|
||||
pair_count = 0
|
||||
|
||||
for iteration in range(max(1, request.max_iterations)):
|
||||
if iteration > 0:
|
||||
working_frames = deskew_lidar_frames(
|
||||
frames,
|
||||
imu,
|
||||
delta_t_s=offset.delta_t_s,
|
||||
R_IMU_lidar=r_x,
|
||||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||
)
|
||||
keyframes, pair_set, handeye = _build_pairs_and_handeye(
|
||||
session_id=session.session_id,
|
||||
working_frames=working_frames,
|
||||
imu=imu,
|
||||
delta_t_s=offset.delta_t_s,
|
||||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||
request=request,
|
||||
)
|
||||
pairs_notes = list(pair_set.notes)
|
||||
pair_count = len(pair_set.pairs)
|
||||
if pair_count < 3:
|
||||
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
|
||||
|
||||
for _ in range(2):
|
||||
refined = refine_time_offset_signed(
|
||||
imu,
|
||||
frames,
|
||||
delta_t_s=offset.delta_t_s,
|
||||
R_IMU_lidar=r_x,
|
||||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||
search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
|
||||
)
|
||||
delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
|
||||
offset = _merge_time_offset(offset, refined)
|
||||
if delta_shift < 1e-3:
|
||||
break
|
||||
keyframes, pair_set, handeye = _build_pairs_and_handeye(
|
||||
session_id=session.session_id,
|
||||
working_frames=working_frames,
|
||||
imu=imu,
|
||||
delta_t_s=offset.delta_t_s,
|
||||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||||
request=request,
|
||||
)
|
||||
pairs_notes = list(pair_set.notes)
|
||||
pair_count = len(pair_set.pairs)
|
||||
if pair_count < 3:
|
||||
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])
|
||||
|
||||
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,
|
||||
"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) -> 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.
|
||||
"""
|
||||
|
||||
if not request.sessions:
|
||||
return finalize_result(
|
||||
status=CalibrationStatus.BLOCKED,
|
||||
message="no sessions provided",
|
||||
details={},
|
||||
output_directory=request.output_directory,
|
||||
)
|
||||
|
||||
vehicle_config = None
|
||||
if request.vehicle_config is not None:
|
||||
try:
|
||||
vehicle_config = load_vehicle_config(request.vehicle_config)
|
||||
except Exception as exc: # noqa: BLE001 - surface config problems as blocked
|
||||
return finalize_result(
|
||||
status=CalibrationStatus.BLOCKED,
|
||||
message=f"vehicle config failed: {exc}",
|
||||
details={},
|
||||
output_directory=request.output_directory,
|
||||
)
|
||||
|
||||
prepared: list[dict[str, Any]] = []
|
||||
for session in request.sessions:
|
||||
prep = _prepare_session_pairs(session, request)
|
||||
if not prep.get("ok"):
|
||||
return finalize_result(
|
||||
status=CalibrationStatus.BLOCKED,
|
||||
message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})",
|
||||
details={"sessions": [prep]},
|
||||
output_directory=request.output_directory,
|
||||
)
|
||||
prepared.append(prep)
|
||||
|
||||
all_pairs = _remap_pairs_for_joint(prepared)
|
||||
handeye = solve_rotation_handeye(all_pairs)
|
||||
if not handeye.ok:
|
||||
return finalize_result(
|
||||
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),
|
||||
},
|
||||
output_directory=request.output_directory,
|
||||
)
|
||||
|
||||
force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
|
||||
t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
|
||||
gyro_bias = np.mean(np.stack([p["gyro_bias_rad_s"] for p in prepared], axis=0), axis=0)
|
||||
gravity_init = np.mean(np.stack([p["gravity_init_m_s2"] for p in prepared], axis=0), axis=0)
|
||||
g_n = float(np.linalg.norm(gravity_init))
|
||||
if g_n > 1e-6:
|
||||
gravity_init = gravity_init * (9.80665 / g_n)
|
||||
|
||||
joint = solve_joint_extrinsic(
|
||||
all_pairs,
|
||||
handeye.R_IMU_lidar,
|
||||
force_rotation_only=force_rotation_only,
|
||||
imu=None,
|
||||
delta_t_s=0.0,
|
||||
gyro_bias_rad_s=gyro_bias,
|
||||
gravity_init_m_s2=gravity_init,
|
||||
enable_phase_c=not force_rotation_only,
|
||||
t_init_m=t_prior,
|
||||
t_prior_m=t_prior,
|
||||
t_prior_sigma_m=t_prior_sigma,
|
||||
)
|
||||
|
||||
session_results = []
|
||||
for prep in prepared:
|
||||
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,
|
||||
"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(),
|
||||
"gyro_bias_rad_s": None
|
||||
if joint.gyro_bias_rad_s is None
|
||||
else np.asarray(joint.gyro_bias_rad_s, dtype=float).tolist(),
|
||||
"accel_bias_m_s2": None
|
||||
if joint.accel_bias_m_s2 is None
|
||||
else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(),
|
||||
"gravity_m_s2": None
|
||||
if joint.gravity_m_s2 is None
|
||||
else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
|
||||
},
|
||||
"translation_accepted": joint.translation_accepted,
|
||||
"rotation_ok": handeye.ok and joint.observability.rotation_observable,
|
||||
}
|
||||
)
|
||||
|
||||
T = np.asarray(joint.T_IMU_lidar, dtype=float)
|
||||
# Report per-session δt list; keep first as scalar for backward-compatible field.
|
||||
delta_t = float(prepared[0]["time_offset_s"])
|
||||
if 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 rejected by observability/residual gates"
|
||||
)
|
||||
else:
|
||||
status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
|
||||
message = f"rotation-only calibration accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
|
||||
T = T.copy()
|
||||
T[:3, 3] = 0.0
|
||||
|
||||
return finalize_result(
|
||||
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,
|
||||
"translation_accepted": joint.translation_accepted,
|
||||
},
|
||||
},
|
||||
T_IMU_lidar=T,
|
||||
time_offset_s=delta_t,
|
||||
output_directory=request.output_directory,
|
||||
)
|
||||
|
||||
|
||||
def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
|
||||
payload = dict(session_result)
|
||||
payload.pop("T_IMU_lidar", None)
|
||||
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,113 @@
|
||||
"""SO(3) rotation hand-eye solver for ``R_A R_X = R_X R_B``."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
|
||||
from .contracts import MotionPair
|
||||
from .geometry import orthonormalize_rotation, rotation_angle_deg, skew, so3_exp, so3_log
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RotationHandeyeResult:
|
||||
R_IMU_lidar: np.ndarray
|
||||
residual_rms_deg: float
|
||||
residual_median_deg: float
|
||||
pair_count: int
|
||||
ok: bool
|
||||
notes: tuple[str, ...] = ()
|
||||
|
||||
|
||||
def _pair_weight(pair: MotionPair) -> float:
|
||||
weight = float(pair.metadata.get("weight", 1.0))
|
||||
if not np.isfinite(weight) or weight <= 0:
|
||||
return 1.0
|
||||
return weight
|
||||
|
||||
|
||||
def _tsai_rotation_initial(pairs: list[MotionPair]) -> np.ndarray:
|
||||
"""Closed-form rotation hand-eye initial guess (Tsai-style linear solve)."""
|
||||
|
||||
rows: list[np.ndarray] = []
|
||||
rhs: list[np.ndarray] = []
|
||||
for pair in pairs:
|
||||
alpha = so3_log(pair.R_A)
|
||||
beta = so3_log(pair.R_B)
|
||||
if np.linalg.norm(alpha) < 1e-6 or np.linalg.norm(beta) < 1e-6:
|
||||
continue
|
||||
w = np.sqrt(_pair_weight(pair))
|
||||
rows.append(w * skew(alpha + beta))
|
||||
rhs.append(w * (beta - alpha))
|
||||
if len(rows) < 2:
|
||||
return np.eye(3)
|
||||
a = np.vstack(rows)
|
||||
b = np.concatenate(rhs)
|
||||
try:
|
||||
rotvec, *_ = np.linalg.lstsq(a, b, rcond=None)
|
||||
except np.linalg.LinAlgError:
|
||||
return np.eye(3)
|
||||
return orthonormalize_rotation(so3_exp(rotvec))
|
||||
|
||||
|
||||
def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
|
||||
err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||
return float(np.degrees(np.linalg.norm(err)))
|
||||
|
||||
|
||||
def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> RotationHandeyeResult:
|
||||
"""Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement."""
|
||||
|
||||
usable = [pair for pair in pairs if rotation_angle_deg(pair.R_A) > 1.0 and rotation_angle_deg(pair.R_B) > 1.0]
|
||||
notes: list[str] = []
|
||||
if len(usable) < 3:
|
||||
return RotationHandeyeResult(
|
||||
R_IMU_lidar=np.eye(3),
|
||||
residual_rms_deg=1e9,
|
||||
residual_median_deg=1e9,
|
||||
pair_count=len(usable),
|
||||
ok=False,
|
||||
notes=("need at least 3 motion pairs with meaningful rotation",),
|
||||
)
|
||||
|
||||
r0 = _tsai_rotation_initial(usable)
|
||||
weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
|
||||
notes.append(
|
||||
f"weighted hand-eye: weight median={float(np.median(weights)):.3g}, "
|
||||
f"min={float(np.min(weights)):.3g}, max={float(np.max(weights)):.3g}"
|
||||
)
|
||||
|
||||
def pack(r: np.ndarray) -> np.ndarray:
|
||||
return so3_log(r)
|
||||
|
||||
def unpack(vec: np.ndarray) -> np.ndarray:
|
||||
return orthonormalize_rotation(so3_exp(vec))
|
||||
|
||||
def residual(vec: np.ndarray) -> np.ndarray:
|
||||
r_x = unpack(vec)
|
||||
residuals = []
|
||||
for pair, weight in zip(usable, weights):
|
||||
err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||||
residuals.append(np.sqrt(weight) * err)
|
||||
return np.concatenate(residuals)
|
||||
|
||||
opt = least_squares(residual, pack(r0), loss="huber", f_scale=np.deg2rad(1.0), max_nfev=200)
|
||||
r_x = unpack(opt.x)
|
||||
errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in usable], dtype=float)
|
||||
# Report unweighted RMS/median for interpretability.
|
||||
rms = float(np.sqrt(np.mean(errs**2)))
|
||||
med = float(np.median(errs))
|
||||
notes.append(f"optimized over {len(usable)} pairs")
|
||||
ok = rms < 5.0 and len(usable) >= 3
|
||||
if not ok:
|
||||
notes.append("rotation residual RMS too high or too few pairs")
|
||||
return RotationHandeyeResult(
|
||||
R_IMU_lidar=r_x,
|
||||
residual_rms_deg=rms,
|
||||
residual_median_deg=med,
|
||||
pair_count=len(usable),
|
||||
ok=ok,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
@@ -0,0 +1,317 @@
|
||||
"""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,
|
||||
) -> TimeOffsetResult:
|
||||
"""Refine ``δt`` with signed 3-axis rates using a known ``R_IMU_lidar``.
|
||||
|
||||
Cost: mean squared error between ``gyro_imu(t_lidar+δt)`` and
|
||||
``R_IMU_lidar @ omega_lidar(t_lidar)`` on a common grid around the coarse ``δt``.
|
||||
"""
|
||||
|
||||
notes: list[str] = [f"signed refine around coarse delta_t={delta_t_s:.6f}s"]
|
||||
if len(frames) < 5:
|
||||
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR frames",), False)
|
||||
|
||||
bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
|
||||
gyro = imu.gyro_rad_s - bias
|
||||
r_x = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3)
|
||||
|
||||
stride = max(1, len(frames) // 20)
|
||||
lidar_t, lidar_w = _lidar_omega_series(frames, stride=stride)
|
||||
if lidar_t.size < 4:
|
||||
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR omega samples",), False)
|
||||
|
||||
# Predicted IMU-frame angular rate from LiDAR relative rotations.
|
||||
pred = (r_x @ lidar_w.T).T
|
||||
t_start = float(lidar_t[0])
|
||||
t_end = float(lidar_t[-1])
|
||||
if t_end - t_start < 0.5:
|
||||
return TimeOffsetResult(delta_t_s, 0.0, search_s, ("LiDAR span too short for signed refine",), False)
|
||||
|
||||
dt = 1.0 / sample_hz
|
||||
grid = np.arange(t_start, t_end, dt)
|
||||
pred_grid = np.column_stack(
|
||||
[np.interp(grid, lidar_t, pred[:, axis], left=np.nan, right=np.nan) for axis in range(3)]
|
||||
)
|
||||
|
||||
def _cost_and_corr(delta: float) -> tuple[float, float]:
|
||||
meas = np.column_stack(
|
||||
[
|
||||
np.interp(grid + delta, imu.t_s, gyro[:, axis], left=np.nan, right=np.nan)
|
||||
for axis in range(3)
|
||||
]
|
||||
)
|
||||
mask = np.isfinite(pred_grid).all(axis=1) & np.isfinite(meas).all(axis=1)
|
||||
if int(np.count_nonzero(mask)) < 10:
|
||||
return float("inf"), -1.0
|
||||
err = meas[mask] - pred_grid[mask]
|
||||
cost = float(np.mean(np.sum(err * err, axis=1)))
|
||||
a = meas[mask].reshape(-1)
|
||||
b = pred_grid[mask].reshape(-1)
|
||||
a = a - np.mean(a)
|
||||
b = b - np.mean(b)
|
||||
corr = float(np.dot(a, b) / ((np.linalg.norm(a) + 1e-12) * (np.linalg.norm(b) + 1e-12)))
|
||||
return cost, corr
|
||||
|
||||
coarse_cost, coarse_corr = _cost_and_corr(float(delta_t_s))
|
||||
best_delta = float(delta_t_s)
|
||||
best_cost = coarse_cost
|
||||
best_corr = coarse_corr
|
||||
half = abs(float(search_s))
|
||||
for delta in np.arange(delta_t_s - half, delta_t_s + half + 1e-12, dt):
|
||||
cost, corr = _cost_and_corr(float(delta))
|
||||
if cost < best_cost:
|
||||
best_cost = cost
|
||||
best_delta = float(delta)
|
||||
best_corr = corr
|
||||
|
||||
# Parabolic refine on cost around the best discrete delta.
|
||||
samples = []
|
||||
for delta in (best_delta - dt, best_delta, best_delta + dt):
|
||||
cost, _ = _cost_and_corr(float(delta))
|
||||
samples.append(cost if np.isfinite(cost) else best_cost)
|
||||
y0, y1, y2 = samples
|
||||
denom = y0 - 2 * y1 + y2
|
||||
if abs(denom) > 1e-12 and y1 <= y0 and y1 <= y2:
|
||||
candidate = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
|
||||
cand_cost, cand_corr = _cost_and_corr(candidate)
|
||||
if cand_cost < best_cost:
|
||||
best_delta = candidate
|
||||
best_cost = cand_cost
|
||||
best_corr = cand_corr
|
||||
|
||||
# Guard with magnitude correlation so ICP-biased signed minima cannot wander.
|
||||
imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
|
||||
lidar_t_mag, lidar_mag = _magnitude_series(lidar_t, lidar_w)
|
||||
|
||||
def _mag_score(delta: float) -> float:
|
||||
t_start_l = float(lidar_t_mag[0])
|
||||
t_end_l = float(lidar_t_mag[-1])
|
||||
grid_m = np.arange(t_start_l, t_end_l, dt)
|
||||
lidar_sig = np.interp(grid_m, lidar_t_mag, lidar_mag, left=0.0, right=0.0)
|
||||
lidar_sig = lidar_sig - np.mean(lidar_sig)
|
||||
imu_sig = np.interp(grid_m + delta, imu_t, imu_mag, left=0.0, right=0.0)
|
||||
imu_sig = imu_sig - np.mean(imu_sig)
|
||||
denom = (float(np.linalg.norm(lidar_sig)) + 1e-12) * (float(np.linalg.norm(imu_sig)) + 1e-12)
|
||||
return float(np.dot(imu_sig, lidar_sig) / denom)
|
||||
|
||||
mag_at_coarse = _mag_score(float(delta_t_s))
|
||||
mag_at_best = _mag_score(best_delta)
|
||||
|
||||
notes.append(
|
||||
f"signed 3-axis refine: delta_t={best_delta:.6f}s, "
|
||||
f"mse={best_cost:.4g} (coarse_mse={coarse_cost:.4g}), "
|
||||
f"corr={best_corr:.3f}, mag_corr={mag_at_best:.3f} (coarse_mag={mag_at_coarse:.3f}), "
|
||||
f"search=±{half:.3f}s"
|
||||
)
|
||||
improved = (
|
||||
np.isfinite(best_cost)
|
||||
and best_cost < coarse_cost * 0.999
|
||||
# Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
|
||||
and mag_at_best + 1e-4 >= mag_at_coarse
|
||||
)
|
||||
if not improved:
|
||||
notes.append("signed refine rejected by MSE/mag-consistency; keeping previous delta_t")
|
||||
return TimeOffsetResult(
|
||||
delta_t_s=float(delta_t_s),
|
||||
correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
|
||||
search_s=search_s,
|
||||
notes=tuple(notes),
|
||||
ok=True,
|
||||
)
|
||||
return TimeOffsetResult(
|
||||
delta_t_s=best_delta,
|
||||
correlation_peak=mag_at_best,
|
||||
search_s=search_s,
|
||||
notes=tuple(notes),
|
||||
ok=True,
|
||||
)
|
||||
@@ -0,0 +1,78 @@
|
||||
"""Timestamp audit for IMU and LiDAR streams."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .contracts import ImuSeries, LidarFrame
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TimestampAuditReport:
|
||||
monotonic: bool
|
||||
epoch_count: int
|
||||
imu_rate_hz: float
|
||||
lidar_rate_hz: float
|
||||
imu_duration_s: float
|
||||
lidar_duration_s: float
|
||||
max_imu_gap_s: float
|
||||
max_lidar_gap_s: float
|
||||
notes: tuple[str, ...] = ()
|
||||
ok: bool = True
|
||||
|
||||
|
||||
def _rate_and_gaps(times: np.ndarray) -> tuple[float, float]:
|
||||
if times.size < 2:
|
||||
return 0.0, 0.0
|
||||
dt = np.diff(times)
|
||||
positive = dt[dt > 0]
|
||||
if positive.size == 0:
|
||||
return 0.0, float("inf")
|
||||
rate = float(1.0 / np.median(positive))
|
||||
return rate, float(np.max(dt))
|
||||
|
||||
|
||||
def audit_timestamps(imu: ImuSeries, frames: list[LidarFrame]) -> TimestampAuditReport:
|
||||
"""Audit native timestamps without assuming the two clocks share an epoch."""
|
||||
|
||||
notes: list[str] = []
|
||||
imu_t = imu.t_s
|
||||
lidar_t = np.asarray([frame.t_mid_s for frame in frames], dtype=float)
|
||||
|
||||
imu_mono = bool(np.all(np.diff(imu_t) >= 0)) if imu_t.size > 1 else False
|
||||
lidar_mono = bool(np.all(np.diff(lidar_t) >= 0)) if lidar_t.size > 1 else False
|
||||
if not imu_mono:
|
||||
notes.append("IMU timestamps are not monotonic")
|
||||
if not lidar_mono:
|
||||
notes.append("LiDAR timestamps are not monotonic")
|
||||
|
||||
imu_rate, imu_gap = _rate_and_gaps(imu_t)
|
||||
lidar_rate, lidar_gap = _rate_and_gaps(lidar_t)
|
||||
if imu_t.size < 50:
|
||||
notes.append(f"IMU sample count is low ({imu_t.size})")
|
||||
if len(frames) < 5:
|
||||
notes.append(f"LiDAR frame count is low ({len(frames)})")
|
||||
if imu_gap > 0.05:
|
||||
notes.append(f"large IMU gap detected: {imu_gap:.3f}s")
|
||||
if lidar_gap > 1.0:
|
||||
notes.append(f"large LiDAR gap detected: {lidar_gap:.3f}s")
|
||||
|
||||
notes.append(
|
||||
"IMU and LiDAR clocks are treated as independent; constant offset is estimated later."
|
||||
)
|
||||
|
||||
ok = imu_mono and lidar_mono and imu_t.size >= 50 and len(frames) >= 5
|
||||
return TimestampAuditReport(
|
||||
monotonic=imu_mono and lidar_mono,
|
||||
epoch_count=2,
|
||||
imu_rate_hz=imu_rate,
|
||||
lidar_rate_hz=lidar_rate,
|
||||
imu_duration_s=float(imu_t[-1] - imu_t[0]) if imu_t.size else 0.0,
|
||||
lidar_duration_s=float(lidar_t[-1] - lidar_t[0]) if lidar_t.size else 0.0,
|
||||
max_imu_gap_s=imu_gap,
|
||||
max_lidar_gap_s=lidar_gap,
|
||||
notes=tuple(notes),
|
||||
ok=ok,
|
||||
)
|
||||
@@ -0,0 +1,83 @@
|
||||
"""Vehicle-installation configuration loading and light validation."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Mapping
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
REQUIRED_TOP_LEVEL_KEYS = frozenset({"schema_version", "vehicle", "installation", "sensors", "time"})
|
||||
|
||||
|
||||
def validate_config_shape(config: Mapping[str, object]) -> list[str]:
|
||||
"""Return missing top-level keys without inventing default values."""
|
||||
|
||||
return sorted(REQUIRED_TOP_LEVEL_KEYS.difference(config))
|
||||
|
||||
|
||||
def validate_config_semantics(config: Mapping[str, Any]) -> list[str]:
|
||||
"""Return semantic issues that block calibration interpretation."""
|
||||
|
||||
issues: list[str] = []
|
||||
sensors = config.get("sensors")
|
||||
if not isinstance(sensors, Mapping):
|
||||
return ["sensors must be a mapping"]
|
||||
imu = sensors.get("imu")
|
||||
lidar = sensors.get("lidar")
|
||||
if not isinstance(imu, Mapping):
|
||||
issues.append("sensors.imu missing")
|
||||
else:
|
||||
axes = ((imu.get("raw_frame") or {}) if isinstance(imu.get("raw_frame"), Mapping) else {}).get("axes")
|
||||
if not axes:
|
||||
issues.append("sensors.imu.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||
if not isinstance(lidar, Mapping):
|
||||
issues.append("sensors.lidar missing")
|
||||
else:
|
||||
axes = ((lidar.get("raw_frame") or {}) if isinstance(lidar.get("raw_frame"), Mapping) else {}).get("axes")
|
||||
if not axes:
|
||||
issues.append("sensors.lidar.raw_frame.axes is empty (declare axis meaning even if approximate)")
|
||||
|
||||
time_cfg = config.get("time")
|
||||
if not isinstance(time_cfg, Mapping):
|
||||
issues.append("time missing")
|
||||
else:
|
||||
for key in ("imu_timestamp_source", "lidar_timestamp_source", "lidar_frame_time_definition"):
|
||||
if not time_cfg.get(key):
|
||||
issues.append(f"time.{key} is empty")
|
||||
return issues
|
||||
|
||||
|
||||
def load_vehicle_config(path: str | Path) -> dict[str, Any]:
|
||||
"""Load and lightly validate a YAML vehicle configuration."""
|
||||
|
||||
try:
|
||||
import yaml
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise ImportError("PyYAML is required to load vehicle configuration files") from exc
|
||||
|
||||
config_path = Path(path)
|
||||
with config_path.open("r", encoding="utf-8") as handle:
|
||||
loaded = yaml.safe_load(handle)
|
||||
if not isinstance(loaded, dict):
|
||||
raise ValueError(f"vehicle config must be a mapping: {config_path}")
|
||||
|
||||
missing = validate_config_shape(loaded)
|
||||
if missing:
|
||||
raise ValueError(f"vehicle config missing keys {missing}: {config_path}")
|
||||
|
||||
semantic = validate_config_semantics(loaded)
|
||||
if semantic:
|
||||
raise ValueError("vehicle config semantic issues:\n- " + "\n- ".join(semantic))
|
||||
return loaded
|
||||
|
||||
|
||||
def prior_enabled(config: Mapping[str, Any], name: str) -> bool:
|
||||
"""Return whether an optional prior is enabled."""
|
||||
|
||||
init = config.get("initialization")
|
||||
if not isinstance(init, Mapping):
|
||||
return False
|
||||
prior = init.get(name)
|
||||
if not isinstance(prior, Mapping):
|
||||
return False
|
||||
return bool(prior.get("enabled", False))
|
||||
@@ -0,0 +1,81 @@
|
||||
# `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` | 构造运动对；手眼使用其中的旋转 |
|
||||
| 7 | `rotation_handeye.py` | 加权旋转手眼 |
|
||||
| 8 | `observability.py` | 旋转 / 平移可观性检查 |
|
||||
| 8 | `joint_optimizer.py` | 联合精修；完整模式下可估计平移、重力、速度与时变零偏 |
|
||||
| 9 | `finalize.py` | 写出结果 JSON |
|
||||
| — | `pipeline.py` | 编排全流程 |
|
||||
| — | `cli.py` | 命令行入口 |
|
||||
| — | `CHANGELOG.md` | 改动记录 |
|
||||
|
||||
|
||||
---
|
||||
|
||||
## 运行模式要点
|
||||
|
||||
- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。
|
||||
- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。
|
||||
- `--mode full_se3`：在可观时再估计重力、关键帧速度、时变零偏与平移；结果写入 `summary.json` 的 joint 字段。
|
||||
|
||||
---
|
||||
|
||||
## 输入格式
|
||||
|
||||
```text
|
||||
imu.csv # t,gx,gy,gz,ax,ay,az（建议设备时间）
|
||||
lidar_session/
|
||||
frames_index.csv # frame_id,filename,t_start,t_end
|
||||
frames/frame_XXXXX.npz # points: (N,3) 米
|
||||
```
|
||||
|
||||
原始 N300 `.rscap` + H32 dlog（或旧 MSOP `.rscap`）用仓库工具导出：`python tools/export_rscap_to_v1.py ...`（见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md)）。
|
||||
|
||||
---
|
||||
|
||||
## 当前能力
|
||||
|
||||
- 本包是仓库**唯一**标定路径：质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →（可选）完整六自由度 → 报告
|
||||
- 点云去畸变：可选
|
||||
- 阶段与用法见根目录 [`README.md`](../README.md)
|
||||
- 改动史：[`CHANGELOG.md`](CHANGELOG.md)
|
||||
@@ -0,0 +1,28 @@
|
||||
[project]
|
||||
name = "lidar-imu-calibration"
|
||||
version = "0.3.0"
|
||||
description = "LiDAR–IMU extrinsic calibration from continuous-motion keyframes (imu_lidar)"
|
||||
requires-python = ">=3.10"
|
||||
dependencies = [
|
||||
"numpy>=1.26",
|
||||
"scipy>=1.11",
|
||||
"pyyaml>=6.0",
|
||||
]
|
||||
|
||||
[project.optional-dependencies]
|
||||
open3d = ["open3d>=0.17"]
|
||||
dev = ["pytest>=7.4"]
|
||||
|
||||
[project.scripts]
|
||||
lidar-imu-calibration = "imu_lidar.cli:main"
|
||||
|
||||
[build-system]
|
||||
requires = ["setuptools>=68", "wheel"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
|
||||
[tool.setuptools]
|
||||
packages = ["imu_lidar", "tools"]
|
||||
|
||||
[tool.pytest.ini_options]
|
||||
testpaths = ["tests"]
|
||||
pythonpath = ["."]
|
||||
@@ -1,4 +0,0 @@
|
||||
numpy>=1.26
|
||||
scipy>=1.11
|
||||
open3d>=0.18
|
||||
small-gicp==1.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
-19
@@ -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,9.17040157943579,0.3948131792835417,0.11878243181951181,1.1425260172834293,3.296235617224735,0.0,0,0,False,1.6662748432297785,0.03471396517121729
|
||||
pitch_+0.100,0.0,0.1,0.0,9.076928136371041,0.4171107485761563,0.11876672804528438,1.1650719587800913,3.3274291807303316,0.031193563505596433,37,43,False,1.661255702204205,0.03471396517121729
|
||||
pitch_+0.200,0.0,0.2,0.0,8.983515248729091,0.4394867360130287,0.11886418170047355,1.2000618469690334,3.378175724075376,0.08194010685064068,33,47,False,1.6622470214176333,0.03471396517121729
|
||||
pitch_+0.300,0.0,0.3,0.0,8.890164410286015,0.4619296835950138,0.11907438084482405,1.246448116042569,3.4476100262900324,0.1513744090652973,29,51,False,1.669238092496052,0.034713965171217276
|
||||
pitch_+0.000_roll_-0.200,-0.2,0.0,0.0,9.01972941526644,0.4032977537513619,0.12043024872839399,1.1611526935950138,3.3458153318563415,0.049579714631606375,32,48,False,1.7107124278398285,0.034713965171217304
|
||||
pitch_+0.000_roll_-0.100,-0.1,0.0,0.0,9.095041148790378,0.3984113441028921,0.1195670451553767,1.1451882592248208,3.3112438616237037,0.015008244398968529,36,44,False,1.685676496509504,0.034713965171217304
|
||||
pitch_+0.000_roll_+0.100,0.1,0.0,0.0,9.245809148607497,0.39253869750368725,0.1180779565326178,1.153258101553409,3.3010571825949637,0.004821565370228598,39,41,False,1.652705907802981,0.034713965171217304
|
||||
pitch_+0.100_roll_-0.200,-0.2,0.1,0.0,8.926223519858327,0.4251505609368216,0.12047176074785593,1.1833438511380916,3.3773641697603143,0.08112855253557916,35,45,False,1.7058242505297496,0.034713965171217304
|
||||
pitch_+0.100_roll_-0.100,-0.1,0.1,0.0,9.001551548682645,0.4205181630564456,0.1195801596045202,1.1676828282839524,3.342708269995132,0.046472652770396916,34,46,False,1.6807154082401394,0.03471396517121729
|
||||
pitch_+0.100_roll_+0.100,0.1,0.1,0.0,9.152351713867697,0.41495850563128295,0.11803306342745587,1.175598202053685,3.331793234208173,0.035557616983437956,33,47,False,1.6476453039285432,0.0347139651712173
|
||||
pitch_+0.200_roll_-0.200,-0.2,0.2,0.0,8.832778133209484,0.4471243504044202,0.12062476910644687,1.2178088730560497,3.428172658132847,0.13193704090811176,30,50,False,1.7067898021773054,0.034713965171217304
|
||||
pitch_+0.200_roll_-0.100,-0.1,0.2,0.0,8.908122483450981,0.44272195299870193,0.11970563817510238,1.2025967761770837,3.3936310861155006,0.09739546889076545,33,47,False,1.6816953294526065,0.03471396517121731
|
||||
pitch_+0.200_roll_+0.100,0.1,0.2,0.0,9.058954849462527,0.4374446064319599,0.11810204126073977,1.2102837520093128,3.3820679918494174,0.0858323746246823,32,48,False,1.648644705864238,0.034713965171217304
|
||||
pitch_+0.300_roll_-0.200,-0.2,0.3,0.0,8.739394768298625,0.469202063453847,0.1208887180452031,1.263543756747655,3.4973996284538247,0.20116401122908956,29,51,False,1.7135991876939975,0.03471396517121731
|
||||
pitch_+0.300_roll_-0.100,-0.1,0.3,0.0,8.81475545648032,0.46500876133584307,0.11994299492844523,1.2488889095688314,3.4631521547194706,0.16691653749473545,30,50,False,1.6886059172123598,0.0347139651712173
|
||||
pitch_+0.300_roll_+0.100,0.1,0.3,0.0,8.96562003954328,0.45998721539756443,0.11828455597352863,1.2562925755921872,3.4510257337050003,0.1547901164802652,29,51,False,1.6556931398905776,0.034713965171217304
|
||||
yaw_-0.200_diagnostic,0.0,0.0,-0.2,9.118174501598277,0.39481317928354154,0.11873527555886121,1.142526017283429,3.295555956965929,-0.0006796602588061695,45,35,True,1.6662748432297787,0.0347139651712173
|
||||
yaw_+0.200_diagnostic,0.0,0.0,0.2,9.225647367504836,0.39481317928354204,0.1194719055604376,1.142526017283429,3.306187727273662,0.009952110048927043,27,53,False,1.6662748432297787,0.0347139651712173
|
||||
|
-33675
File diff suppressed because it is too large
Load Diff
@@ -1,46 +0,0 @@
|
||||
{
|
||||
"convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
|
||||
"translation_m": [
|
||||
1.2977596922478565,
|
||||
-0.00006733076264476427,
|
||||
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,
|
||||
-0.00006733076264476427
|
||||
],
|
||||
[
|
||||
-0.020988849781798404,
|
||||
-0.013700025104115046,
|
||||
0.9996858394000506,
|
||||
0.7204978345266887
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
]
|
||||
}
|
||||
|
||||
@@ -1,405 +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
|
||||
]
|
||||
},
|
||||
"selection": {
|
||||
"method": "Open3D B gated by Open3D-small_gicp cross-backend agreement",
|
||||
"selection_is_X_independent": true,
|
||||
"consensus_pair_threshold": "Open3D-small_gicp B delta <= 0.05 m and <= 0.50 deg",
|
||||
"warning": "AX rotation RMS remains about one degree; this is not centimetre-grade absolute certification."
|
||||
}
|
||||
}
|
||||
@@ -1,440 +0,0 @@
|
||||
{
|
||||
"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": [
|
||||
1.2977596922478565,
|
||||
-6.733076264476427e-05,
|
||||
0.7204978345266887
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7851511464150316,
|
||||
1.2026608220810453,
|
||||
-0.835510052785851
|
||||
],
|
||||
"estimation": {
|
||||
"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
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap_std": [
|
||||
0.003223574650236103,
|
||||
0.003334649335095665,
|
||||
0.0016858977028131694,
|
||||
0.0980155224808408,
|
||||
0.07461769310738842,
|
||||
0.11886090357304822
|
||||
],
|
||||
"batch1_auxiliary": {
|
||||
"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
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"separate_backend_results": {
|
||||
"open3d_gicp": {
|
||||
"translation_m": [
|
||||
1.2978831677200011,
|
||||
-0.0030997734098957433,
|
||||
0.7217892226151688
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7572070226288585,
|
||||
1.1460424368859559,
|
||||
-0.8797812288284186
|
||||
]
|
||||
},
|
||||
"small_gicp": {
|
||||
"translation_m": [
|
||||
1.2996301155160686,
|
||||
-0.0035647011612625953,
|
||||
0.7218861757826791
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7871563544979889,
|
||||
1.1414557494228486,
|
||||
-0.9357571721886107
|
||||
]
|
||||
}
|
||||
},
|
||||
"warning": "AX rotation RMS remains about one degree; this is not centimetre-grade absolute certification."
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
-1267
File diff suppressed because it is too large
Load Diff
@@ -1,109 +0,0 @@
|
||||
i,j,rtk_translation_m,rtk_rotation_deg,heldout_inlier_ratio,heldout_inlier_rmse_m,hessian_rank,hessian_condition,reverse_translation_m,reverse_rotation_deg,multistart_success_rate,accepted,rejection_reasons
|
||||
0,1,4.069460063439237,24.612641616688823,0.742135248947238,0.1176855237061111,6,4.407651482488117,0.013825313356667012,0.06962701621921227,1.0,True,
|
||||
0,2,8.883155687691525,14.186041609026283,0.6152139070398225,0.12748244203393638,6,7.377377816318336,0.010848197501521375,0.23936253539883626,1.0,True,
|
||||
0,3,5.800196618465403,50.885974946353855,0.7200942109830172,0.11742249635115339,6,5.031984522656203,0.010896220625647894,0.1047126090415015,1.0,True,
|
||||
1,2,4.892046307546544,10.426600007662538,0.7419273778112828,0.11333156226718596,6,4.459403177706152,0.011700301190659394,0.07531112640817904,1.0,True,
|
||||
1,3,1.7315947839093804,26.273333329665036,0.860897030953885,0.10772639754828932,6,3.19377614050938,0.0031419813068167823,0.07553165598013784,1.0,True,
|
||||
1,4,1.6946255625760025,60.40345832959281,0.881875,0.10577894988132426,6,3.092963722704653,0.004385743087150129,0.11984963227966941,1.0,True,
|
||||
2,3,3.2805755806518446,36.69993333732758,0.7933853118712274,0.10942029884827265,6,3.7004807304428255,0.008219359847113026,0.1277049389798412,1.0,True,
|
||||
2,4,4.866335020594746,70.83005833725535,0.7129525942515864,0.11823277244188567,6,3.5207832348575936,0.003926646926802803,0.12676782091003777,1.0,True,
|
||||
2,5,1.8274406190287722,30.448549979291712,0.7434186132740082,0.11591916677323456,6,3.2245913426430066,0.005762289421636631,0.0417195018069042,1.0,True,
|
||||
3,4,1.75881263274858,34.13012499992777,0.8393594395095709,0.11198421236679276,6,2.9955796261301395,0.016040499501420776,0.10475252258183747,1.0,True,
|
||||
3,5,2.1020200425132134,67.14848331661929,0.7942090219957748,0.1150960059583273,6,3.466731955810642,0.01716401750472473,0.19985666972488197,1.0,True,
|
||||
3,6,5.791805311102367,61.163322289150074,0.6534504391468005,0.12221285442048085,6,4.065547866736895,0.010758157432153631,0.2541461196933768,1.0,True,
|
||||
4,5,3.30592832159088,101.27860831654709,0.7519675356615839,0.11239744207499523,6,3.538427973689991,0.007054246334297592,0.24517862996973167,1.0,True,
|
||||
4,6,6.453995240105232,95.29344728907782,0.6228429546865301,0.12861898753238143,6,5.239918976750594,0.01896562608787198,0.25933537698897335,1.0,True,
|
||||
4,7,9.537609568572432,26.32130681988008,0.6193556570268899,0.13116150037214716,6,6.94052030016787,0.01746189088684816,0.36780633600082835,1.0,True,
|
||||
5,6,3.7769167422851373,5.985161027469212,0.7384387717351092,0.11889495868628594,6,3.6057257786167156,0.00882639593332281,0.0737386250122742,1.0,True,
|
||||
5,7,6.565348405821576,74.95730149666699,0.6792642983119174,0.11864042030155272,6,4.820407924638239,0.009720083307578809,0.20972540690307045,1.0,True,
|
||||
5,8,6.3343271669452825,107.99940292217995,0.6467507736253273,0.12542317047495377,6,3.0799731272083655,0.01002585992591628,0.05691592874139322,1.0,True,
|
||||
6,7,3.1558607172566147,68.97214046919777,0.7476148072764279,0.11288592031143596,6,3.5859463118826977,0.010814191328094169,0.0266481566439959,1.0,True,
|
||||
6,8,3.3979014114419868,102.01424189471071,0.6804804804804805,0.11508385505331041,6,2.2706877166387276,0.004482804469688638,0.1044466289150781,1.0,True,
|
||||
6,9,0.28270347875173524,124.11602215258995,0.7371697154471545,0.11682527448508172,6,2.8975705145594723,0.005203408768588573,0.04849411501759064,1.0,True,
|
||||
7,8,4.78580085090425,33.04210142551296,0.664336521952416,0.11766047279270792,6,2.929070995599858,0.0018478739597912201,0.09974755915747875,1.0,True,
|
||||
7,9,2.876695741106431,166.91183737821345,0.6077132839890923,0.1180552047396233,6,3.7184930305903685,0.010919730241803961,0.14576304064343415,1.0,True,
|
||||
7,10,3.5227760858025783,151.5297683494061,0.8481789137380191,0.10615102438506704,6,2.8662462784517695,0.006788694147854834,0.052872203183928275,1.0,True,
|
||||
8,9,3.469611483011297,133.86973595269956,0.5943661971830986,0.11920565901187036,6,3.7883774947006965,0.0042443197269814905,0.10102914613043958,1.0,True,
|
||||
8,10,8.226583602288494,175.42813022508852,0.5924719507781397,0.12046539737502422,6,6.6212639471399966,0.012068925048483609,0.06971570917645852,1.0,True,
|
||||
8,11,1.865267488778983,173.7180803410843,0.6789593030194534,0.1124672015619688,6,2.333957745729889,0.006563181150688944,0.09301162138898374,1.0,True,
|
||||
9,10,5.488028285806731,41.558394272381754,0.6015824400204186,0.11914584090399423,6,5.490861355062907,0.023677817470035802,0.12091279277864625,1.0,True,
|
||||
9,11,4.718056434111957,39.84834438838189,0.5936159475145092,0.12220038333634799,6,2.975477564952316,0.010739868846460122,0.11267510434066752,1.0,True,
|
||||
9,12,4.0664516687652705,11.300244475855449,0.6125753012048193,0.12248054470736866,6,2.850813791288553,0.006578056867618694,0.0648765356282168,1.0,True,
|
||||
10,11,8.48963239412477,1.7100498839998592,0.5922981366459628,0.1252332170275593,6,3.561970732702301,0.0101884878590875,0.04905586883665525,1.0,True,
|
||||
10,12,7.8510898708654135,30.258149796526293,0.5541275333662877,0.12481360204475032,6,3.7517795436266956,0.031261518480906504,0.2822575477830575,1.0,True,
|
||||
10,13,7.303425556850936,93.89697482620899,0.4898572501878287,0.1234709130125476,6,5.011926866876523,0.002887507644989318,0.12861291537137956,1.0,True,
|
||||
11,12,0.684356309096518,28.54809991252644,0.8033488144707895,0.10033649821782255,6,2.8495161791494152,0.0037957676909615035,0.030493386938618566,1.0,True,
|
||||
11,13,4.983180422136547,92.18692494220912,0.48959881129271915,0.13002768142853224,6,7.500744983348576,0.04820000634069387,0.21663658694591276,1.0,True,
|
||||
11,14,7.513058208926106,116.17598776756182,0.5511551155115512,0.1363490361205856,6,8.69179713026475,0.020420243910966678,0.3388561639372597,1.0,True,
|
||||
12,13,4.470221622128888,63.638825029682685,0.5484109386548411,0.12959344453923985,6,6.303133528073185,0.011857268436770598,0.07665557322523474,1.0,True,
|
||||
12,14,7.042432348069883,87.62788785503537,0.5823908549191292,0.13141832933236727,6,7.376965777627224,0.010833935971029728,0.22584716035991784,1.0,True,
|
||||
12,15,9.425648786798952,64.11974003309916,0.5143102812922246,0.13728792513153318,6,10.6657111200586,0.012453922151317217,0.21281971313986298,1.0,True,
|
||||
13,14,2.6059162757855927,23.989062825352686,0.6649008254281138,0.11829356218038511,6,4.46953470312236,0.010710695211339957,0.05103080008067046,1.0,True,
|
||||
13,15,4.985522367267706,0.4809150034164723,0.6433105164903546,0.12280456147298241,6,5.48212715248282,0.002079348825514708,0.08095419399489742,1.0,True,
|
||||
13,16,5.330126890587504,0.570225046299944,0.5092755076460266,0.12767535698436475,6,5.1227851245513,0.01771530501608281,0.1987385247368959,1.0,True,
|
||||
14,15,2.3845573265941185,23.508147821936216,0.7934889434889435,0.10996554201681306,6,3.5978390564938083,0.010537581931095797,0.18886023198851049,1.0,True,
|
||||
14,16,7.871327660405588,24.55928787165262,0.5124335024124707,0.1271130929550258,6,6.773046277884419,0.0367675981786339,0.17067950171934768,1.0,True,
|
||||
14,17,6.334147330965719,0.13339551798522414,0.5997034840622684,0.12123178848040567,6,5.0521521307734485,0.007039639082169618,0.1420706023439064,1.0,True,
|
||||
15,16,10.188241681839806,1.051140049716416,0.4525684289463817,0.1310762605599437,6,10.161233588287896,0.03028489261617412,0.2306307424740811,1.0,True,
|
||||
15,17,8.639041695194567,23.641543339921444,0.5419370943584623,0.12926666329467473,6,7.1208904116826295,0.016749087599431237,0.21516518274709787,1.0,True,
|
||||
15,18,9.620687246763701,76.25281000098852,0.5239486723415068,0.13468232645821807,6,7.765728655204814,0.015824236097669597,0.5418501890608881,1.0,False,forward_reverse_rotation
|
||||
16,17,1.5531467972267479,24.69268338963786,0.725455688246386,0.11626130134637655,6,2.5539312507809044,0.008173066808338859,0.1459219364983215,1.0,True,
|
||||
16,18,0.5677742310084086,77.30395005070493,0.7509598157153826,0.11215936550371307,6,2.72958456426664,0.01007065253007885,0.024451533638690782,1.0,True,
|
||||
16,19,2.9811390265794215,109.91770005034847,0.61725,0.11278472852394253,6,3.8445921585562353,0.013911959588582991,0.09838261866068182,1.0,True,
|
||||
17,18,0.98603936348044,52.61126666106708,0.7936102236421725,0.10947670477666799,6,2.9613430589728713,0.004348617987976676,0.08578297576648478,1.0,True,
|
||||
17,19,3.326868398717413,85.22501666071064,0.6638372238172513,0.11457050422944211,6,4.039052271877423,0.011384309901198326,0.08601325248059774,1.0,True,
|
||||
17,20,1.702964560097507,146.95247656520317,0.6755903523172118,0.11461754659819628,6,2.9771145257220444,0.003070224389531284,0.041777705584758074,1.0,True,
|
||||
18,19,3.0423123591285863,32.61374999964355,0.7679796696315121,0.10951225500902462,6,3.162387301290471,0.0016757410207260358,0.05240557176555545,1.0,True,
|
||||
18,20,1.0853574230860805,94.34120990413592,0.7909488300334276,0.11336174254244849,6,2.472783746978993,0.0011490974177271606,0.05798276029492007,1.0,True,
|
||||
18,21,0.7190584934612743,134.43032499953063,0.7659654868371332,0.11623488568922577,6,2.3334272634442037,0.0007397588392132655,0.049540488476333674,1.0,True,
|
||||
19,20,1.9962827272376449,61.72745990449234,0.7140883977900553,0.1080327967474786,6,2.8867642059111027,0.008102122424322853,0.0646114560547033,1.0,True,
|
||||
19,21,3.133843614009462,101.81657499988698,0.6584615384615384,0.10963878686899117,6,2.8691912248684632,0.012913229197399395,0.08080164801156113,1.0,True,
|
||||
19,22,3.558853688535236,131.87013333415302,0.6261591020009761,0.11484815169172223,6,3.1506020281084415,0.008079663853152794,0.06664730348633674,1.0,True,
|
||||
20,21,1.4312693216245356,40.08911509539463,0.8130212871903398,0.10717601941116739,6,2.598682401671686,0.003986207075465725,0.014468856174509968,1.0,True,
|
||||
20,22,2.8745434122269677,70.1426734296607,0.709279368213228,0.11654592484503022,6,3.3550200552976293,0.004732316748497569,0.04653511032479733,1.0,True,
|
||||
20,23,2.368927629183393,88.7954400941961,0.7151155604993202,0.11440209684434971,6,3.0862044513861178,0.013864191008669256,0.04741774806449891,1.0,True,
|
||||
21,22,1.9888588003695142,30.053558334266054,0.7434966727162734,0.11011755646689264,6,3.014221192692681,0.0010075959724342985,0.03301381213076696,1.0,True,
|
||||
21,23,1.9477165743251885,48.70632499880147,0.7529083858458555,0.11149633235838309,6,3.077926485095861,0.001989351505445817,0.010666288445208057,1.0,True,
|
||||
21,24,3.76075382790461,86.89547500633407,0.7106949236076885,0.11526418670282225,6,3.884216729530811,0.0033123302523526757,0.010450448797924646,1.0,True,
|
||||
22,23,0.8778876913955667,18.652766664535406,0.8800913132284032,0.0910569893232571,6,2.4100478670835583,0.0032245216994413483,0.016462830433921317,1.0,True,
|
||||
22,24,2.2510479983846268,56.84191667206801,0.746915842188836,0.11145740682737049,6,2.851886485949151,0.01259476909551429,0.07575320422516746,1.0,True,
|
||||
22,25,3.452614527841686,105.36038662410485,0.6202953787517865,0.10996904036547328,6,3.4469213215286616,0.01971125516503169,0.10319789629151777,1.0,True,
|
||||
23,24,1.8212773036309853,38.18915000753262,0.7859065329092244,0.11467536863831725,6,2.8422525641977647,0.011606021246419816,0.08140725284903579,1.0,True,
|
||||
23,25,2.999856552735995,86.70761995956946,0.6364070141953955,0.11293944470788622,6,3.2389862166172447,0.02314438992859391,0.07068788850704667,1.0,True,
|
||||
23,26,1.7088514993141615,157.34900000166795,0.6150898203592814,0.11134349524292353,6,3.3995614886239296,0.039322949699257106,0.11562859307450143,1.0,True,
|
||||
24,25,1.2059352225587747,48.51846995203683,0.7858615254031769,0.11149776506996652,6,2.476851261276463,0.011240124367435099,0.1049569848741807,1.0,True,
|
||||
24,26,1.1958827550030364,119.15984999413537,0.7456781105429754,0.10341842736085993,6,2.5300194557440525,0.01395883182919579,0.08780591339848752,1.0,True,
|
||||
24,27,3.786115539231191,149.59794999698258,0.621380846325167,0.10953325191126222,6,3.7363642522268807,0.016637332395867215,0.10353943868518889,1.0,True,
|
||||
25,26,1.8270495459501794,70.64138004209858,0.8347382167873679,0.10115747661888057,6,3.0333095788198037,0.01268722454395532,0.041403863092781025,1.0,True,
|
||||
25,27,4.741885618180033,101.07948004494568,0.624412013026173,0.11277808045896366,6,3.9078457957611743,0.025589655197266314,0.15422514597254583,1.0,True,
|
||||
25,28,3.8640843382258447,141.59343004213218,0.5869307400379506,0.11770827672499123,6,4.799123781945888,0.022027669812843296,0.2897817423195282,1.0,True,
|
||||
26,27,2.939444415707082,30.43810000284707,0.6580285783482683,0.1098010629341849,6,3.5110471043097586,0.0028795138573569803,0.007524827744647535,1.0,True,
|
||||
26,28,2.285770454773634,70.95205000003368,0.6673413501607334,0.11114983317987888,6,3.8873517833668108,0.008151782060846088,0.22733511766651973,1.0,True,
|
||||
26,29,3.6568094249627374,131.92299999999997,0.567654409139593,0.1129321723600836,6,3.929185583738771,0.018525590401292088,0.6411617076853331,1.0,False,forward_reverse_rotation
|
||||
27,28,2.3764785758716087,40.513949997186636,0.795572759162937,0.10258057277674534,6,3.138921602248635,0.0021193681800924964,0.11642323746608753,1.0,True,
|
||||
27,29,4.061689152213973,101.48489999715295,0.7440454600411075,0.10506613395932038,6,3.1227510205159508,0.010617830050775887,0.010252586949293494,1.0,True,
|
||||
27,30,5.033742765789926,178.61309998505533,0.5825184112036702,0.11453665822491028,6,7.650962341345717,2.5431128502617377,2.5742599673919253,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
28,29,1.7860052935263149,60.970949999966315,0.829172610556348,0.101007907265047,6,3.8068503338763042,0.002050657211153992,0.060432422726630436,1.0,True,
|
||||
28,30,7.378510482049102,138.09914998778677,0.5397673314339981,0.12193285407050827,6,9.554349687793176,2.3478431587463846,1.5786016001583607,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
28,31,8.396001985859645,161.15751666826222,0.5045164398410213,0.12642928247376978,6,9.31764609998303,0.020394024193417442,0.4096021180283358,1.0,True,
|
||||
29,30,8.925911734961222,77.12819998782052,0.5217133364973896,0.12674065647229513,6,8.522188842167411,0.029506475723043876,0.11389243918851435,1.0,True,
|
||||
29,31,9.936139535050964,100.18656666829587,0.49500421331407246,0.12930714939449198,6,8.914982386428042,0.09268316078142827,0.5769686714189283,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
29,32,9.719184595429152,159.1482027457047,0.4949034656433625,0.12826034683961704,6,9.48481031343127,0.046228293643484374,0.3497292020413176,0.0,False,multistart_instability
|
||||
30,31,1.0176267260390144,23.058366680475352,0.8362783988460152,0.09839553390448733,6,3.4866604582833345,0.00292801654357991,0.07029435398214784,1.0,True,
|
||||
30,32,0.8427696971904044,82.02000275788396,0.8588193030774758,0.09922846236206209,6,3.665055653691297,0.007868521232743847,0.0646663588791515,1.0,True,
|
||||
30,33,1.1320092061720382,147.1799498371321,0.7985030295829868,0.09950089003213429,6,4.012416511656465,0.002818384349407474,0.04751878128625789,1.0,True,
|
||||
31,32,0.2771314063233405,58.9616360774086,0.8562575941676792,0.09641015722415121,6,3.1475922536183365,0.00743603397476557,0.05749362415769439,1.0,True,
|
||||
31,33,0.5409306841643147,124.12158315665668,0.7594021215043394,0.09971190839470828,6,3.924941555652805,0.008658288176145966,0.04699249624411687,1.0,True,
|
||||
31,34,0.9901318542574941,155.5976733552555,0.7827868852459017,0.09387821156002771,6,3.677769944550314,0.001760438169327793,0.07141137968742438,0.0,False,multistart_instability
|
||||
32,33,0.38765596924480794,65.15994707924807,0.859390009606148,0.09596734045987164,6,3.3457711754162425,0.007599187780672587,0.08086834965344242,1.0,True,
|
||||
32,34,1.0327853226589305,96.63603727784675,0.8306916426512968,0.09663160733562838,6,3.561692133355112,0.008708468398558678,0.031578495955716095,1.0,True,
|
||||
32,35,1.1947451269258382,131.6892972449946,0.8055028462998103,0.09329597448562056,6,3.202004817860687,0.002684669665422271,0.0241000049832631,1.0,True,
|
||||
33,34,0.7453275493729801,31.47609019859864,0.8550154872527996,0.09764473733600805,6,3.153108280248987,0.005560284067502743,0.07666131775182662,1.0,True,
|
||||
33,35,1.0924186931915323,66.52935016574651,0.8344316309719935,0.08989959394950665,6,3.014281766333208,0.0013737288250339052,0.028590963377823297,1.0,True,
|
||||
33,36,3.335020341384374,123.21987515531416,0.7655961609449982,0.10046425727734096,6,3.222688396070449,0.012544124903927084,0.05831564134554671,1.0,True,
|
||||
34,35,0.6186130964444055,35.05325996714787,0.8692633560837845,0.08711586696140278,6,2.9228018947680083,0.006834102546614079,0.04740606842462777,1.0,True,
|
||||
34,36,3.114273556969299,91.74378495671547,0.771256306140027,0.09836949441930562,6,3.7389528980898077,0.004832468349243269,0.046439990812924645,1.0,True,
|
||||
34,37,2.8155528331227235,126.08390993626574,0.722881252293017,0.10001696647063055,6,3.8298681993003196,0.0035748664819913244,0.09211471145637762,1.0,True,
|
||||
35,36,2.4989423629184655,56.69052498956759,0.7697728101081278,0.09462474495647087,6,3.9667933176330195,0.004833519542267701,0.07989067847153554,1.0,True,
|
||||
35,37,2.2182956249874577,91.03064996911786,0.7351769110010868,0.09626480689856566,6,3.604883740945235,0.00826951342075784,0.05772015666601106,1.0,True,
|
||||
36,37,0.5129211474320938,34.34012497955026,0.8790139064475347,0.08686306972783016,6,2.86419132160936,0.0018743847744752261,0.0426177033120738,1.0,True,
|
||||
|
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -1,109 +0,0 @@
|
||||
i,j,rtk_translation_m,rtk_rotation_deg,heldout_inlier_ratio,heldout_inlier_rmse_m,hessian_rank,hessian_condition,reverse_translation_m,reverse_rotation_deg,multistart_success_rate,accepted,rejection_reasons
|
||||
0,1,0.503306788400079,13.198324485965863,0.7947725072604066,0.09714510386272111,6,3.1645559924308744,0.00040681040655899406,0.015836974306118006,1.0,True,
|
||||
0,2,1.1601020961002007,26.306295080591493,0.8196783588704571,0.10111548255452249,6,3.1496351272437915,0.0024786662018318395,0.04305117802148083,1.0,True,
|
||||
0,3,1.1328421148283978,41.74134818989305,0.8170202208162759,0.10132889132333661,6,2.9636847958606816,0.001763337668740155,0.0152261810177797,1.0,True,
|
||||
1,2,0.6927979865343085,13.107970594625625,0.7847728726807421,0.08513837010948266,6,2.6465075635845983,0.003621881376895638,0.011153493139183525,1.0,True,
|
||||
1,3,0.7002295454253883,28.543023703927183,0.7885699962401304,0.09713251502799032,6,2.6341764735488145,0.0014094469663425105,0.02902143941632062,1.0,True,
|
||||
1,4,0.8045641659338967,57.731023816949346,0.7363658899355479,0.09863727794945644,6,2.7377680197189993,0.011592408190980286,0.17084480097436408,1.0,True,
|
||||
2,3,0.13949776780243606,15.435053109301553,0.918286915396742,0.07288259780013887,6,2.6383381348446657,0.002273707715582919,0.0026923349652342365,1.0,True,
|
||||
2,4,0.6701726006967371,44.62305322232371,0.8370341514089079,0.09441127521663696,6,3.0939727970407334,0.0041401597740440875,0.03075174550917053,1.0,True,
|
||||
2,5,0.8004226998626774,62.96656300910673,0.8235371293623721,0.10626795357026747,6,2.8027866429960917,0.009148796674106823,0.05256412309062332,1.0,True,
|
||||
3,4,0.5366157827594368,29.188000113022152,0.8850603941513032,0.07613142655207121,6,2.6417590251999084,0.0013904645484531576,0.0259404694028417,1.0,True,
|
||||
3,5,0.6760093701980526,47.53150989980517,0.8740046236835346,0.09072202522560696,6,2.686157540984516,0.006509933107990933,0.02545212192512678,1.0,True,
|
||||
3,6,1.4361364806393455,80.09091141471853,0.6795928884308168,0.10271649349323747,6,3.008166890660092,0.00828154435242913,0.026486510290724834,1.0,True,
|
||||
4,5,0.17861948949299924,18.343509786783002,0.8684444444444445,0.0784387700526441,6,2.6861954442048104,0.0025072579888539386,0.021042298559759365,1.0,True,
|
||||
4,6,1.5807073315069207,50.90291130169636,0.6781301745000636,0.10313081772493418,6,2.8455217243212387,0.003271195715147848,0.14716032493381534,1.0,True,
|
||||
4,7,1.7804201387535592,75.18775779751417,0.7873548387096774,0.09891997778736471,6,3.1802938248442474,0.004633577062239209,0.04472721629193465,1.0,True,
|
||||
5,6,1.5316987521992935,32.55940151491337,0.6911991765311374,0.10152400788085783,6,3.2393513551418907,0.005288907067525738,0.05494235971756111,1.0,True,
|
||||
5,7,1.6091514152462776,56.844248010731185,0.7747360187719984,0.09121609616459708,6,3.123473300580222,0.00421464356720393,0.015634168997181876,1.0,True,
|
||||
5,8,1.1872485760158462,85.67585439000777,0.7829477514946712,0.10577559680144696,6,3.109219016133088,0.0033105282814613057,0.030813019102438145,1.0,True,
|
||||
6,7,1.5692385974903325,24.284846495817817,0.7347979599843075,0.10196810239533251,6,3.458846242963224,0.004020707752206965,0.039850323489805044,1.0,True,
|
||||
6,8,0.5035712140385419,53.11645287509441,0.7155149934810952,0.10460245111398395,6,3.0180670962260874,0.0072368624874882504,0.0055907688631446776,1.0,True,
|
||||
6,9,0.8508108569116883,66.44324365209327,0.6825438366919655,0.10495957402394829,6,3.140616320789838,0.00430894236478819,0.13911475434755866,1.0,True,
|
||||
7,8,1.1230406357307108,28.831606379276582,0.8127890077949531,0.09942756972076687,6,2.9436564519387325,0.008316114154115147,0.01814737917470093,1.0,True,
|
||||
7,9,0.7186795671679719,42.158397156275456,0.8193873491579367,0.09507291602212191,6,3.5012345051117295,0.006751593996528964,0.029293578235221863,1.0,True,
|
||||
7,10,0.39293571356254015,58.25702665650087,0.7940216681895313,0.10356080767641777,6,2.817840996766873,0.006883567504360684,0.016492683422577963,1.0,True,
|
||||
8,9,0.4407680716864947,13.326790776998866,0.8581438392384981,0.09484304022144129,6,3.219825075793889,0.007253035671159373,0.024252719250072975,1.0,True,
|
||||
8,10,0.968166048306331,29.425420277224287,0.8015356585111921,0.10152298131640612,6,3.7446585062478155,0.0017523919908150464,0.05042176209078739,1.0,True,
|
||||
8,11,3.0540124804140354,66.34511043540417,0.688687555052221,0.10536230082368817,6,4.696625415382456,0.0036281146181207165,0.013078005975049522,1.0,True,
|
||||
9,10,0.5276649044820116,16.098629500225417,0.8560606060606061,0.09594368034796862,6,3.2778548903058375,0.010491037599418197,0.0599937188173442,1.0,True,
|
||||
9,11,2.680593454060616,53.0183196584053,0.6966788735951509,0.10384409271666459,6,4.3419055531595605,0.009543265092514783,0.09977883713501384,1.0,True,
|
||||
9,12,3.2944321799753977,73.22024906015783,0.6579243162233046,0.10621203405076814,6,4.293885241325807,0.0028782078295197843,0.0743495992598884,1.0,True,
|
||||
10,11,2.249454986440387,36.91969015817989,0.729195173529046,0.10869447817964137,6,4.322382098282597,0.0028206125960216254,0.014140069566934874,1.0,True,
|
||||
10,12,2.8783779741216167,57.12161955993242,0.6945982527377876,0.10596349826421764,6,4.493186221276381,0.005660464094292755,0.08952914345108036,1.0,True,
|
||||
10,13,3.84803303865234,78.07081297783375,0.6530587074494326,0.1098871098886565,6,4.997105968288151,0.009441275754405344,0.06902225135435515,1.0,True,
|
||||
11,12,0.636223282239137,20.20192940175253,0.865792610250298,0.09248705530398661,6,3.4380496057859116,0.007654923324833218,0.03083951917063289,1.0,True,
|
||||
11,13,2.0145868546459504,41.151122819653864,0.8217656194003106,0.10443913508460159,6,3.957969004992005,0.0019958122994113434,0.04243931133970266,1.0,True,
|
||||
11,14,1.2405786051647103,116.67752660647822,0.7462918660287081,0.10754571546972236,6,4.619857754383166,0.0030226348073827927,0.05528144266002643,1.0,True,
|
||||
12,13,1.5928240921286525,20.949193417901323,0.8347322378531742,0.10791137431619166,6,3.358528082155117,0.0033753816186126483,0.015427430722607398,1.0,True,
|
||||
12,14,0.6774809171975742,96.47559720472566,0.7626657196969697,0.10926443071446633,6,3.847953795189004,0.0011083901763643633,0.03408686226954418,1.0,True,
|
||||
12,15,1.8892292086221258,131.14857460490867,0.7404945183111733,0.10470183850479874,6,3.1806996321008576,0.008442575945215317,0.02844707164454242,1.0,True,
|
||||
13,14,0.9721300146850552,75.52640378682433,0.8138569225293629,0.1125369920515294,6,3.2147734820704015,0.005457899756974233,0.04250371025579654,1.0,True,
|
||||
13,15,2.426469315960194,110.19938118700728,0.7853886616014026,0.10710393317615087,6,2.4365756135744383,0.0023363953219779758,0.003679886698275418,1.0,True,
|
||||
13,16,4.157137927296689,135.66277163100625,0.7461059190031153,0.10791684095774041,6,2.2141337125272007,0.0023169831632091926,0.02473862097872449,1.0,True,
|
||||
14,15,2.176233486689304,34.672977400182965,0.7456694756554307,0.11049010248131722,6,2.3376174680383377,0.0027632919724097076,0.05272762900968177,1.0,True,
|
||||
14,16,3.79947855245853,60.13636784418198,0.7114577084583084,0.11170286188791147,6,2.479974772421199,0.0043726035924820505,0.11525585711885118,1.0,True,
|
||||
14,17,6.190474035537447,15.261944565117341,0.6170986278878855,0.11803645680912848,6,5.206426513766648,0.02802927019183766,0.10048860643286558,1.0,True,
|
||||
15,16,1.7328875589135755,25.463390443999018,0.7687034629476421,0.09834033850996364,6,2.3729567416081196,0.004926598691892943,0.10049788043616295,1.0,True,
|
||||
15,17,8.352264787019024,19.411032835065622,0.5596902808274586,0.12817180913542037,6,8.442537153126064,0.04660873899639204,0.08128930035257874,1.0,True,
|
||||
15,18,6.346037230040679,25.592965647994788,0.6134687462217386,0.11861991782546771,6,5.933642175309751,0.006137337753783083,0.040498570689394164,1.0,True,
|
||||
16,17,9.849975560032544,44.87442327906463,0.589649455234486,0.12715782204597983,6,10.240964391202342,0.016491489842564398,0.29283865924681673,1.0,True,
|
||||
16,18,7.9099649541476005,0.12957520399577063,0.6441329694864798,0.11977657776801591,6,7.973620675877663,0.01581702082704062,0.07681086634765077,1.0,True,
|
||||
16,19,6.609737623005951,34.172763646089344,0.642055375405338,0.11922626532224312,6,5.690723701436521,0.01936736899083179,0.22155509936605944,1.0,True,
|
||||
17,18,2.081596655954183,45.003998483060414,0.761206687666586,0.10248796635270672,6,4.778521884225018,0.003602819302929225,0.04280787772842571,1.0,True,
|
||||
17,19,4.34928579963815,79.047186925154,0.6654080389768575,0.10560001932239894,6,4.647516519431804,0.015683211248127868,0.18051709633786264,1.0,True,
|
||||
17,20,4.4206846692505035,169.37257900760153,0.6377810007251632,0.11215519048187066,6,5.178731668470772,0.016926609942259613,0.20634229894075337,1.0,True,
|
||||
18,19,3.459222336215112,34.04318844209358,0.7556208482370976,0.10994458141969204,6,3.594858191146124,0.004654350735545165,0.12873243616635754,1.0,True,
|
||||
18,20,4.94806857498194,124.36858052454107,0.6686517992904207,0.11525187440612464,6,4.510107356424666,0.022993775653731053,0.1013964892640344,1.0,True,
|
||||
18,21,6.5847443121227816,175.25342056617163,0.5905541093343251,0.1262853354637165,6,5.942345924349615,0.021857815102479028,0.4259066161744867,1.0,True,
|
||||
19,20,3.031758915138065,90.32539208244746,0.7420998980632009,0.12052023632629244,6,3.3463968507926847,0.012566250876576632,0.08500209892702067,1.0,True,
|
||||
19,21,3.732423384308353,141.21023212407198,0.6515019319456562,0.12055764747498063,6,4.076270140910382,0.007120603737667027,0.05561876226347328,1.0,True,
|
||||
19,22,6.37255124464243,175.13200822775684,0.5905501782985226,0.12548545004843537,6,5.705777346141328,0.005772061596466365,0.10356450735473072,1.0,True,
|
||||
20,21,2.057726146362386,50.884840041624386,0.7791517249907258,0.1109933103332085,6,3.525554312141476,0.004882992437392809,0.09954673132586826,1.0,True,
|
||||
20,22,4.388457127616061,94.54259968980386,0.6575290550783224,0.11893104898070972,6,4.203674621017558,0.005511015274661758,0.13108161960065734,1.0,True,
|
||||
20,23,8.256548639868363,126.4903843444735,0.5799237611181702,0.12732395079162134,6,6.120182481367245,0.02421237470645105,0.2728497258921342,1.0,True,
|
||||
21,22,2.665930009787955,43.65775964817948,0.7379804721295267,0.11301282071283571,6,4.252070885311591,0.005342970038949662,0.023692959917636515,1.0,True,
|
||||
21,23,6.670349114267683,75.60554430284907,0.6154133001864512,0.11796945645750438,6,5.503558762374886,0.021862964848253088,0.18671115674675795,1.0,True,
|
||||
21,24,4.0761181240750135,118.87493116884215,0.6688601936925751,0.11271893154362743,6,4.6072424136923145,0.017882487871653564,0.11231417140662198,1.0,True,
|
||||
22,23,4.005228418650323,31.947784654669576,0.7269494538989077,0.11696827793053062,6,3.9354343956207307,0.036708381465019306,0.17149160359500737,1.0,True,
|
||||
22,24,1.4231800329066895,75.21717152066267,0.7625269567423569,0.11122908154278312,6,2.9923801902407035,0.033708002403797385,0.18898419347187523,1.0,True,
|
||||
22,25,0.9824487562406952,165.63918634111403,0.7063042657606644,0.1109686247290255,6,3.260862218157852,0.0237546710240931,0.20388046327310663,1.0,True,
|
||||
23,24,2.6253440212817054,43.26938686599312,0.785377057547531,0.11259637754562075,6,3.1689777161216344,0.008388924730327863,0.09824473511740778,1.0,True,
|
||||
23,25,4.30404511575867,133.69140168644446,0.682445260093659,0.11585305930039552,6,4.459805342354154,0.03262070035564156,0.26820774135203534,1.0,True,
|
||||
23,26,6.580252552934289,159.52196102005493,0.6190717727618564,0.1225437625254746,6,4.748664745468758,0.020450511219618477,0.11876511292532045,1.0,True,
|
||||
24,25,2.0155105379126432,90.4220148204513,0.7662452591656131,0.10751986617832841,6,3.2073569479179214,0.020801978066425954,0.11499836231449187,1.0,True,
|
||||
24,26,4.344602404326578,157.20865211395216,0.7051298290056998,0.11713046861462412,6,4.0423396253765445,0.02866879831073192,0.14020373652670878,1.0,True,
|
||||
24,27,4.194437499675352,171.65485957819877,0.7231216797369087,0.11332956637109663,6,3.5737137574022637,0.0391123663878616,0.31745415833557195,1.0,True,
|
||||
25,26,2.340922525153354,66.78663729350096,0.8146752104535746,0.1067005622933313,6,2.8555177924942416,0.015155094735859666,0.09937091691588736,1.0,True,
|
||||
25,27,2.187150933555911,97.92312560134954,0.8480552070263488,0.10282262567757873,6,2.8008487935140383,0.020178532297188766,0.14259764688967286,1.0,True,
|
||||
25,28,6.688406442287195,171.26720999836604,0.6516274978006786,0.11571138405667894,6,3.5101673009694134,0.012932261164889271,0.1340627397278959,1.0,True,
|
||||
26,27,0.6349061963265976,31.136488307848563,0.8667504714016342,0.10966038141955464,6,2.9297274048756186,0.0016799531047772863,0.021489450960699312,1.0,True,
|
||||
26,28,5.008419104260455,121.94615270813352,0.7044824981113069,0.11528623479803272,6,2.991781333569699,0.011526585390891731,0.12425176309998268,1.0,True,
|
||||
26,29,2.224643201350574,145.73622267800252,0.713654161930024,0.11136018082941974,6,3.2605126809731075,0.0026952303885011083,0.026655596724504224,1.0,True,
|
||||
27,28,5.622863551235285,90.80966440028486,0.7058971457311706,0.1123767383226778,6,3.2376046779603893,0.004853942846193662,0.04151426276015074,1.0,True,
|
||||
27,29,2.458639091699311,114.59973437015381,0.7564328960645812,0.11469589052522192,6,3.205131133212321,0.012664149059349924,0.06847473063899887,1.0,True,
|
||||
27,30,2.614522132166758,143.98503586800658,0.7694542698332492,0.10812761282109745,6,3.5636250205999853,0.029100131201629794,0.2714584305620497,1.0,True,
|
||||
28,29,5.5254251571777075,23.790069969868927,0.6751737207833228,0.11779777341629802,6,4.576892329120707,0.02178556321929983,0.16690946122682282,1.0,True,
|
||||
28,30,6.682895390289165,53.17537146772172,0.6491205871188156,0.12153627244194264,6,5.217063769204219,0.03128065038149053,0.11076032475862632,1.0,True,
|
||||
28,31,11.27338119162288,23.882243944837096,0.5547279383916173,0.13098667045941628,6,8.538477868201968,0.007811024872033181,0.25693805699014277,1.0,True,
|
||||
29,30,1.1998189814142899,29.3853014978528,0.8093424727088093,0.11457686827315132,6,2.757583581977887,0.011632920497220726,0.07792616982135933,1.0,True,
|
||||
29,31,5.755712819539279,0.09217397496817598,0.7190982776089159,0.1179079898277715,6,4.190353867263325,0.010102013331328315,0.05295397679545269,1.0,True,
|
||||
29,32,7.3497160995478215,44.7412737155501,0.683451384417257,0.12293657670032715,6,4.583568719289398,0.015683401555203327,0.1727804303986473,1.0,True,
|
||||
30,31,4.592128513798009,29.29312752288462,0.7552004058853374,0.1130974670000902,6,3.6871413543680527,0.010357085925134528,0.12088315402655661,1.0,True,
|
||||
30,32,6.2188831730504885,74.12657521340287,0.7014059073906874,0.11707558829895812,6,4.009905097582674,0.044382968752358344,0.22757601248033596,1.0,True,
|
||||
30,33,7.344850966128434,112.7430048900433,0.6023929471032745,0.12334051659083738,6,4.260065691019417,0.002643716461244746,0.05342399356841621,1.0,True,
|
||||
31,32,1.7043840283934533,44.83344769051828,0.785355810063055,0.11197405173054666,6,3.256332960817364,0.017947904056538493,0.11867936940390052,1.0,True,
|
||||
31,33,3.3472002644811774,83.44987736715868,0.6260366926363408,0.11401170513833872,6,3.204107721135536,0.005088227871807909,0.05996016929359153,1.0,True,
|
||||
31,34,3.311630046755356,133.49479301359594,0.6536974685122833,0.11166700866683012,6,3.289691648920709,0.01957685602042411,0.06586346108113965,1.0,True,
|
||||
32,33,1.9394369188755196,38.61642967664041,0.7442187300370512,0.11487243540634488,6,2.5670787340169654,0.00371510987119075,0.04386474659625542,1.0,True,
|
||||
32,34,2.1435446082722334,88.6613453230776,0.7408722109533469,0.1145103812359268,6,2.7396880211128143,0.0033854042879271646,0.04345206885157784,1.0,True,
|
||||
32,35,3.1027737257249126,119.84526039487966,0.8249047681597268,0.10651397196801952,6,2.6908216879663684,0.014229552741777998,0.06789311177543367,1.0,True,
|
||||
33,34,0.5704890065278277,50.0449156464372,0.8265015479876161,0.1079360614731508,6,3.0056080815747785,0.005517776656180037,0.04447304678975396,1.0,True,
|
||||
33,35,2.3843314966611664,81.22883071823928,0.7029487179487179,0.11079043369762126,6,2.804837502264682,0.0045321280438173584,0.02066446888667937,1.0,True,
|
||||
33,36,3.5605240220038255,106.66041349586102,0.612932138284251,0.11677728789370431,6,3.2326527799764184,0.011704878383874686,0.04254924292122366,1.0,True,
|
||||
34,35,2.9403442697824866,31.183915071802076,0.7198829665436968,0.11381984857774002,6,3.296185466097946,0.015242618456135898,0.1219677981121258,1.0,True,
|
||||
34,36,4.102710388907318,56.61549784942384,0.6450260449752255,0.11677131628998773,6,3.6854553885932066,0.0029445631740664846,0.0910859237401823,1.0,True,
|
||||
34,37,3.6865711881345757,30.12086856551724,0.6870731404445548,0.11846537329500156,6,4.5399964456315365,0.013941395463696922,0.12307688004234886,1.0,True,
|
||||
35,36,1.2045561534352058,25.431582777621774,0.811216429699842,0.11156358795429076,6,2.5821119008039335,0.003966448406861189,0.11486278330291146,1.0,True,
|
||||
35,37,6.084062615492111,1.063046506284832,0.6916945230136282,0.12226449151266647,6,3.9062079497745845,0.012490325532322776,0.1588294259454204,1.0,True,
|
||||
36,37,7.2781244258509386,26.494629283906598,0.6406009244992296,0.123972843683949,6,4.59789964279208,0.011995188628592031,0.19550165133048591,1.0,True,
|
||||
|
File diff suppressed because it is too large
Load Diff
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -1,312 +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": 41,
|
||||
"translation_m": {
|
||||
"rms": 0.06627076507951374,
|
||||
"median": 0.047988641017293804,
|
||||
"p90": 0.09181504914902948,
|
||||
"p95": 0.11277678323253028,
|
||||
"max": 0.15600463105844264
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.0783402768758907,
|
||||
"median": 0.7259689138952466,
|
||||
"p90": 1.6809461304799602,
|
||||
"p95": 1.7792880941356781,
|
||||
"max": 1.856511169904947
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.023735072067512103,
|
||||
"rotation_deg": 0.35856568622307705,
|
||||
"i": 0,
|
||||
"j": 1
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.05526576799076716,
|
||||
"rotation_deg": 0.688292682153714,
|
||||
"i": 0,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.04806529792477384,
|
||||
"rotation_deg": 1.7792880941356781,
|
||||
"i": 1,
|
||||
"j": 2
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.02732889363320134,
|
||||
"rotation_deg": 1.856511169904947,
|
||||
"i": 2,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.03927257848059381,
|
||||
"rotation_deg": 1.6809461304799602,
|
||||
"i": 2,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.09000097958526627,
|
||||
"rotation_deg": 0.6847419474141874,
|
||||
"i": 3,
|
||||
"j": 4
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.03101397287067455,
|
||||
"rotation_deg": 0.16914815750366516,
|
||||
"i": 3,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.07754221345029244,
|
||||
"rotation_deg": 0.7259689138952466,
|
||||
"i": 4,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.03330333298983308,
|
||||
"rotation_deg": 0.2953381460156314,
|
||||
"i": 5,
|
||||
"j": 6
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.013926178859499422,
|
||||
"rotation_deg": 1.0956547835384745,
|
||||
"i": 6,
|
||||
"j": 7
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.040796788287758956,
|
||||
"rotation_deg": 0.5732579539729787,
|
||||
"i": 6,
|
||||
"j": 9
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.05468973725915344,
|
||||
"rotation_deg": 1.0002199501064295,
|
||||
"i": 11,
|
||||
"j": 12
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.06427358796963496,
|
||||
"rotation_deg": 1.4794289625543822,
|
||||
"i": 16,
|
||||
"j": 17
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.15600463105844264,
|
||||
"rotation_deg": 1.480613890179137,
|
||||
"i": 16,
|
||||
"j": 18
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.10022423249183152,
|
||||
"rotation_deg": 0.4939634910387222,
|
||||
"i": 17,
|
||||
"j": 18
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.11277678323253028,
|
||||
"rotation_deg": 1.4534337020592556,
|
||||
"i": 18,
|
||||
"j": 19
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.09181504914902948,
|
||||
"rotation_deg": 0.5121047642461125,
|
||||
"i": 18,
|
||||
"j": 20
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.07621294129483619,
|
||||
"rotation_deg": 1.2114882514394736,
|
||||
"i": 18,
|
||||
"j": 21
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.05057962673548217,
|
||||
"rotation_deg": 1.7070235499578508,
|
||||
"i": 19,
|
||||
"j": 20
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.028485843207444878,
|
||||
"rotation_deg": 1.282910024621228,
|
||||
"i": 20,
|
||||
"j": 21
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.047988641017293804,
|
||||
"rotation_deg": 0.3479009355224502,
|
||||
"i": 20,
|
||||
"j": 22
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.036673661370321194,
|
||||
"rotation_deg": 0.7294078635231547,
|
||||
"i": 20,
|
||||
"j": 23
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.053305241810354,
|
||||
"rotation_deg": 1.5203537906970106,
|
||||
"i": 21,
|
||||
"j": 22
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.06724337658756505,
|
||||
"rotation_deg": 0.6388309960595039,
|
||||
"i": 21,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.04395944502343888,
|
||||
"rotation_deg": 0.532486384255132,
|
||||
"i": 22,
|
||||
"j": 23
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.02014164497275169,
|
||||
"rotation_deg": 1.454337113913715,
|
||||
"i": 22,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.015070014725991494,
|
||||
"rotation_deg": 1.3619366988381034,
|
||||
"i": 23,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.04337448537472198,
|
||||
"rotation_deg": 0.5946988587087709,
|
||||
"i": 24,
|
||||
"j": 25
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.04609106556910267,
|
||||
"rotation_deg": 0.8042445263370658,
|
||||
"i": 24,
|
||||
"j": 26
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.08550650491731009,
|
||||
"rotation_deg": 0.4123121681282486,
|
||||
"i": 25,
|
||||
"j": 26
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.03526150890834157,
|
||||
"rotation_deg": 1.4877458273245419,
|
||||
"i": 27,
|
||||
"j": 28
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.04728048748001428,
|
||||
"rotation_deg": 1.6462112767436325,
|
||||
"i": 27,
|
||||
"j": 29
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.038221731232831464,
|
||||
"rotation_deg": 0.32809158958487783,
|
||||
"i": 28,
|
||||
"j": 29
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.08450868583143198,
|
||||
"rotation_deg": 0.4200755376297717,
|
||||
"i": 30,
|
||||
"j": 31
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.08626234893146881,
|
||||
"rotation_deg": 0.4145763973148079,
|
||||
"i": 30,
|
||||
"j": 33
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.031883992409525753,
|
||||
"rotation_deg": 0.47209162292986384,
|
||||
"i": 31,
|
||||
"j": 33
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.07512585318274038,
|
||||
"rotation_deg": 0.4948719177387593,
|
||||
"i": 33,
|
||||
"j": 34
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.1468426442855739,
|
||||
"rotation_deg": 0.5255877510410011,
|
||||
"i": 33,
|
||||
"j": 35
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.0854721649952596,
|
||||
"rotation_deg": 0.7101299092541083,
|
||||
"i": 34,
|
||||
"j": 35
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.03486824711848649,
|
||||
"rotation_deg": 1.7800497000791062,
|
||||
"i": 34,
|
||||
"j": 37
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.028519367999920175,
|
||||
"rotation_deg": 1.540726633727796,
|
||||
"i": 35,
|
||||
"j": 36
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,744 +0,0 @@
|
||||
{
|
||||
"schema_version": 2,
|
||||
"success": true,
|
||||
"message": "Both `ftol` and `xtol` termination conditions are 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.2962115049352554,
|
||||
-0.0001653420733896053,
|
||||
0.7164468137861116
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7022421146530834,
|
||||
1.3831306856730625,
|
||||
-0.8286069697615047
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.006040299739348631,
|
||||
0.012113565983787752,
|
||||
-0.007156270443853885,
|
||||
0.9998827751749451
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9996040986249817,
|
||||
0.014164523963697197,
|
||||
0.024310743983262467,
|
||||
1.2962115049352554
|
||||
],
|
||||
[
|
||||
-0.01445720224151504,
|
||||
0.9998246051447864,
|
||||
0.011905807424097893,
|
||||
-0.0001653420733896053
|
||||
],
|
||||
[
|
||||
-0.02413783990927558,
|
||||
-0.012252559240975732,
|
||||
0.9996335525964305,
|
||||
0.7164468137861116
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 38,
|
||||
"pairs": 108,
|
||||
"residuals": {
|
||||
"pairs": 108,
|
||||
"translation_m": {
|
||||
"rms": 0.19743856479972394,
|
||||
"median": 0.06046352611504703,
|
||||
"p90": 0.21107717888307384,
|
||||
"p95": 0.4320413132492668,
|
||||
"max": 0.9185086085818974
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.594164834809931,
|
||||
"median": 0.9846062073478139,
|
||||
"p90": 2.2895871608918337,
|
||||
"p95": 2.7021815285034116,
|
||||
"max": 5.449412955100641
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.07883616778657004,
|
||||
"rotation_deg": 0.5080220774435835
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.13193158825620543,
|
||||
"rotation_deg": 0.4741606069012087
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.11129387971109267,
|
||||
"rotation_deg": 0.5206505897892112
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.06037317704760375,
|
||||
"rotation_deg": 0.4636666105342909
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.0532377491787842,
|
||||
"rotation_deg": 0.6385789459945687
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.036086228084217256,
|
||||
"rotation_deg": 0.7053466734460281
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.024212071864605337,
|
||||
"rotation_deg": 0.35966262441445085
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.07843107284074759,
|
||||
"rotation_deg": 0.9672771351659644
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.1331135483643765,
|
||||
"rotation_deg": 0.9356542123031394
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.06287483417453568,
|
||||
"rotation_deg": 0.8539179575478302
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.09307074193602183,
|
||||
"rotation_deg": 1.1452312070788857
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.10534310615277773,
|
||||
"rotation_deg": 1.2697416838407303
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.034156566922473784,
|
||||
"rotation_deg": 0.4470789762879368
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.05706608980344394,
|
||||
"rotation_deg": 1.4538259192907348
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.21156185043155915,
|
||||
"rotation_deg": 0.32326983959580347
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.04366721285272495,
|
||||
"rotation_deg": 1.1486889953780957
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.19784063737858906,
|
||||
"rotation_deg": 0.4945905742106831
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.13271207797084064,
|
||||
"rotation_deg": 0.8074538026104654
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.13649995064331025,
|
||||
"rotation_deg": 1.035099347913503
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.058892470655748055,
|
||||
"rotation_deg": 0.6808326191608242
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.12809115411885974,
|
||||
"rotation_deg": 0.8895134738281241
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.07473485983007296,
|
||||
"rotation_deg": 0.7244832869216128
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.003615738521241208,
|
||||
"rotation_deg": 0.6536825959057917
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.06957198571596916,
|
||||
"rotation_deg": 0.7883364856790823
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.06578636179058665,
|
||||
"rotation_deg": 0.09595187830987498
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.11708949328304538,
|
||||
"rotation_deg": 0.9037314334019761
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.2785740172886443,
|
||||
"rotation_deg": 1.2395284599701288
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.054803484360171,
|
||||
"rotation_deg": 0.9197207214648975
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.2066140285757045,
|
||||
"rotation_deg": 1.4700310072490443
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.21086946250515157,
|
||||
"rotation_deg": 1.0216432855984101
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.16273991885860312,
|
||||
"rotation_deg": 1.6658320411463268
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.17536744938199297,
|
||||
"rotation_deg": 1.603914654640798
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.12486444755757231,
|
||||
"rotation_deg": 1.72640237007742
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.03629298593419621,
|
||||
"rotation_deg": 0.7021350382463348
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.04096591124737788,
|
||||
"rotation_deg": 1.182229412077533
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.023876028562682318,
|
||||
"rotation_deg": 2.3067680414707423
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.0232665744848516,
|
||||
"rotation_deg": 0.45561721689220397
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.0202215693688227,
|
||||
"rotation_deg": 1.7396398605001189
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.2386974208323925,
|
||||
"rotation_deg": 0.3084186872525065
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.0320417923623188,
|
||||
"rotation_deg": 1.537812332531765
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.22893456979179405,
|
||||
"rotation_deg": 0.6120657430945021
|
||||
},
|
||||
{
|
||||
"pair_index": 41,
|
||||
"translation_m": 0.3264153842951392,
|
||||
"rotation_deg": 0.5281733672424023
|
||||
},
|
||||
{
|
||||
"pair_index": 42,
|
||||
"translation_m": 0.1540882733017021,
|
||||
"rotation_deg": 1.9432086107260433
|
||||
},
|
||||
{
|
||||
"pair_index": 43,
|
||||
"translation_m": 0.21054079914538157,
|
||||
"rotation_deg": 1.2442015725741578
|
||||
},
|
||||
{
|
||||
"pair_index": 44,
|
||||
"translation_m": 0.48891681345533766,
|
||||
"rotation_deg": 2.8273101876072357
|
||||
},
|
||||
{
|
||||
"pair_index": 45,
|
||||
"translation_m": 0.10733355603351397,
|
||||
"rotation_deg": 0.8721227552809412
|
||||
},
|
||||
{
|
||||
"pair_index": 46,
|
||||
"translation_m": 0.9185086085818974,
|
||||
"rotation_deg": 4.489220955656682
|
||||
},
|
||||
{
|
||||
"pair_index": 47,
|
||||
"translation_m": 0.7319199589340636,
|
||||
"rotation_deg": 5.449412955100641
|
||||
},
|
||||
{
|
||||
"pair_index": 48,
|
||||
"translation_m": 0.8659475201606082,
|
||||
"rotation_deg": 3.8214376146729045
|
||||
},
|
||||
{
|
||||
"pair_index": 49,
|
||||
"translation_m": 0.7210574374098978,
|
||||
"rotation_deg": 4.561541716053782
|
||||
},
|
||||
{
|
||||
"pair_index": 50,
|
||||
"translation_m": 0.5570099786748203,
|
||||
"rotation_deg": 5.373002500144395
|
||||
},
|
||||
{
|
||||
"pair_index": 51,
|
||||
"translation_m": 0.04582605589663051,
|
||||
"rotation_deg": 0.6822468845946373
|
||||
},
|
||||
{
|
||||
"pair_index": 52,
|
||||
"translation_m": 0.036857756558338325,
|
||||
"rotation_deg": 1.7515216368522777
|
||||
},
|
||||
{
|
||||
"pair_index": 53,
|
||||
"translation_m": 0.043496707068384696,
|
||||
"rotation_deg": 1.7461098288880414
|
||||
},
|
||||
{
|
||||
"pair_index": 54,
|
||||
"translation_m": 0.045832318963813956,
|
||||
"rotation_deg": 1.041278909174025
|
||||
},
|
||||
{
|
||||
"pair_index": 55,
|
||||
"translation_m": 0.060553875182490315,
|
||||
"rotation_deg": 0.8052115259091664
|
||||
},
|
||||
{
|
||||
"pair_index": 56,
|
||||
"translation_m": 0.06652308069145153,
|
||||
"rotation_deg": 0.9632642598400051
|
||||
},
|
||||
{
|
||||
"pair_index": 57,
|
||||
"translation_m": 0.035908234140899456,
|
||||
"rotation_deg": 1.2689384623057567
|
||||
},
|
||||
{
|
||||
"pair_index": 58,
|
||||
"translation_m": 0.060880526853900836,
|
||||
"rotation_deg": 0.5935557534854639
|
||||
},
|
||||
{
|
||||
"pair_index": 59,
|
||||
"translation_m": 0.11419626074713392,
|
||||
"rotation_deg": 0.7130165148085522
|
||||
},
|
||||
{
|
||||
"pair_index": 60,
|
||||
"translation_m": 0.04265037619565028,
|
||||
"rotation_deg": 0.699587177579278
|
||||
},
|
||||
{
|
||||
"pair_index": 61,
|
||||
"translation_m": 0.04378802459846447,
|
||||
"rotation_deg": 0.675205047149443
|
||||
},
|
||||
{
|
||||
"pair_index": 62,
|
||||
"translation_m": 0.08344520239044273,
|
||||
"rotation_deg": 1.3214027680911706
|
||||
},
|
||||
{
|
||||
"pair_index": 63,
|
||||
"translation_m": 0.017638868448433938,
|
||||
"rotation_deg": 0.17533894669557326
|
||||
},
|
||||
{
|
||||
"pair_index": 64,
|
||||
"translation_m": 0.03489149886931174,
|
||||
"rotation_deg": 1.664944154610128
|
||||
},
|
||||
{
|
||||
"pair_index": 65,
|
||||
"translation_m": 0.10079671035324393,
|
||||
"rotation_deg": 0.6171025033257196
|
||||
},
|
||||
{
|
||||
"pair_index": 66,
|
||||
"translation_m": 0.014396578516872966,
|
||||
"rotation_deg": 1.650080935912764
|
||||
},
|
||||
{
|
||||
"pair_index": 67,
|
||||
"translation_m": 0.06023554532048859,
|
||||
"rotation_deg": 0.42594747696355045
|
||||
},
|
||||
{
|
||||
"pair_index": 68,
|
||||
"translation_m": 0.03699195896487648,
|
||||
"rotation_deg": 1.2778861613746475
|
||||
},
|
||||
{
|
||||
"pair_index": 69,
|
||||
"translation_m": 0.046905032676790276,
|
||||
"rotation_deg": 1.132463118089895
|
||||
},
|
||||
{
|
||||
"pair_index": 70,
|
||||
"translation_m": 0.070685940640895,
|
||||
"rotation_deg": 0.6925680609090175
|
||||
},
|
||||
{
|
||||
"pair_index": 71,
|
||||
"translation_m": 0.05072278859454283,
|
||||
"rotation_deg": 0.40553765157890925
|
||||
},
|
||||
{
|
||||
"pair_index": 72,
|
||||
"translation_m": 0.061114192405838416,
|
||||
"rotation_deg": 0.8033058778952246
|
||||
},
|
||||
{
|
||||
"pair_index": 73,
|
||||
"translation_m": 0.12007843139860787,
|
||||
"rotation_deg": 0.9239227811720148
|
||||
},
|
||||
{
|
||||
"pair_index": 74,
|
||||
"translation_m": 0.12118933148474953,
|
||||
"rotation_deg": 1.305746559495919
|
||||
},
|
||||
{
|
||||
"pair_index": 75,
|
||||
"translation_m": 0.05055191131491414,
|
||||
"rotation_deg": 0.20965439069314443
|
||||
},
|
||||
{
|
||||
"pair_index": 76,
|
||||
"translation_m": 0.08360132660274813,
|
||||
"rotation_deg": 0.876573476333775
|
||||
},
|
||||
{
|
||||
"pair_index": 77,
|
||||
"translation_m": 0.02835721910581123,
|
||||
"rotation_deg": 1.5894295378250647
|
||||
},
|
||||
{
|
||||
"pair_index": 78,
|
||||
"translation_m": 0.03908539475543364,
|
||||
"rotation_deg": 0.8083289126351658
|
||||
},
|
||||
{
|
||||
"pair_index": 79,
|
||||
"translation_m": 0.054143804150054306,
|
||||
"rotation_deg": 1.6541418156239664
|
||||
},
|
||||
{
|
||||
"pair_index": 80,
|
||||
"translation_m": 0.026283210691822913,
|
||||
"rotation_deg": 1.6667348211605557
|
||||
},
|
||||
{
|
||||
"pair_index": 81,
|
||||
"translation_m": 0.089350850518513,
|
||||
"rotation_deg": 2.2825286794384145
|
||||
},
|
||||
{
|
||||
"pair_index": 82,
|
||||
"translation_m": 0.03347579217967635,
|
||||
"rotation_deg": 1.9977395380091536
|
||||
},
|
||||
{
|
||||
"pair_index": 83,
|
||||
"translation_m": 0.019000630740619542,
|
||||
"rotation_deg": 1.5515794014919868
|
||||
},
|
||||
{
|
||||
"pair_index": 84,
|
||||
"translation_m": 0.04109198168971092,
|
||||
"rotation_deg": 0.6855676498071515
|
||||
},
|
||||
{
|
||||
"pair_index": 85,
|
||||
"translation_m": 0.04412164134783469,
|
||||
"rotation_deg": 0.6915721855798296
|
||||
},
|
||||
{
|
||||
"pair_index": 86,
|
||||
"translation_m": 0.11294515765470313,
|
||||
"rotation_deg": 2.2385943138303417
|
||||
},
|
||||
{
|
||||
"pair_index": 87,
|
||||
"translation_m": 0.01925918248868863,
|
||||
"rotation_deg": 1.0019352795296634
|
||||
},
|
||||
{
|
||||
"pair_index": 88,
|
||||
"translation_m": 0.07700662983793743,
|
||||
"rotation_deg": 2.187099011126195
|
||||
},
|
||||
{
|
||||
"pair_index": 89,
|
||||
"translation_m": 0.081983290439235,
|
||||
"rotation_deg": 1.2833572216410762
|
||||
},
|
||||
{
|
||||
"pair_index": 90,
|
||||
"translation_m": 0.07413447803071471,
|
||||
"rotation_deg": 1.509039762712118
|
||||
},
|
||||
{
|
||||
"pair_index": 91,
|
||||
"translation_m": 0.05026833056909235,
|
||||
"rotation_deg": 0.6540616669180302
|
||||
},
|
||||
{
|
||||
"pair_index": 92,
|
||||
"translation_m": 0.07040328963228819,
|
||||
"rotation_deg": 1.1208457441248971
|
||||
},
|
||||
{
|
||||
"pair_index": 93,
|
||||
"translation_m": 0.035342214388759695,
|
||||
"rotation_deg": 0.932859896131992
|
||||
},
|
||||
{
|
||||
"pair_index": 94,
|
||||
"translation_m": 0.09059341699612283,
|
||||
"rotation_deg": 2.23170809346331
|
||||
},
|
||||
{
|
||||
"pair_index": 95,
|
||||
"translation_m": 0.0903976819000103,
|
||||
"rotation_deg": 2.3060569509498126
|
||||
},
|
||||
{
|
||||
"pair_index": 96,
|
||||
"translation_m": 0.029370567781526302,
|
||||
"rotation_deg": 1.2512304574145157
|
||||
},
|
||||
{
|
||||
"pair_index": 97,
|
||||
"translation_m": 0.031018252539194446,
|
||||
"rotation_deg": 1.2538724028818724
|
||||
},
|
||||
{
|
||||
"pair_index": 98,
|
||||
"translation_m": 0.05078748646827035,
|
||||
"rotation_deg": 0.3396814347935095
|
||||
},
|
||||
{
|
||||
"pair_index": 99,
|
||||
"translation_m": 0.024770901259540175,
|
||||
"rotation_deg": 0.09062392685294303
|
||||
},
|
||||
{
|
||||
"pair_index": 100,
|
||||
"translation_m": 0.03340161795186317,
|
||||
"rotation_deg": 1.0544497064860483
|
||||
},
|
||||
{
|
||||
"pair_index": 101,
|
||||
"translation_m": 0.032353919411514664,
|
||||
"rotation_deg": 2.4697997330248893
|
||||
},
|
||||
{
|
||||
"pair_index": 102,
|
||||
"translation_m": 0.0369893920228258,
|
||||
"rotation_deg": 0.9428098575535183
|
||||
},
|
||||
{
|
||||
"pair_index": 103,
|
||||
"translation_m": 0.03967084241987133,
|
||||
"rotation_deg": 2.4057498894359104
|
||||
},
|
||||
{
|
||||
"pair_index": 104,
|
||||
"translation_m": 0.032499330822438305,
|
||||
"rotation_deg": 0.5201344456818268
|
||||
},
|
||||
{
|
||||
"pair_index": 105,
|
||||
"translation_m": 0.020738575117572048,
|
||||
"rotation_deg": 1.147380363634985
|
||||
},
|
||||
{
|
||||
"pair_index": 106,
|
||||
"translation_m": 0.02864934739022941,
|
||||
"rotation_deg": 0.9393233353789158
|
||||
},
|
||||
{
|
||||
"pair_index": 107,
|
||||
"translation_m": 0.04427763743614998,
|
||||
"rotation_deg": 2.333163177357132
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"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.004408386781524471,
|
||||
0.004767641769352496,
|
||||
0.005598290825790153
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.048493732410523904,
|
||||
0.046658283462926005,
|
||||
0.07519664546145975
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 7.050612320852161,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 643.4104764663608,
|
||||
"success": true,
|
||||
"translation_m": 3.912474170044392e-10,
|
||||
"rotation_deg": 6.735629049437961e-10
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663626,
|
||||
"success": true,
|
||||
"translation_m": 5.541272376488872e-09,
|
||||
"rotation_deg": 8.145564755773614e-08
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663621,
|
||||
"success": true,
|
||||
"translation_m": 5.7457948050995e-09,
|
||||
"rotation_deg": 9.303346267795607e-08
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663617,
|
||||
"success": true,
|
||||
"translation_m": 2.194969543097371e-09,
|
||||
"rotation_deg": 6.831261261103626e-08
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663606,
|
||||
"success": true,
|
||||
"translation_m": 0.0,
|
||||
"rotation_deg": 0.0
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663606,
|
||||
"success": true,
|
||||
"translation_m": 2.0328177429825194e-10,
|
||||
"rotation_deg": 5.30535295392565e-10
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663606,
|
||||
"success": true,
|
||||
"translation_m": 2.942069407882596e-10,
|
||||
"rotation_deg": 1.1060405674229135e-09
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663616,
|
||||
"success": true,
|
||||
"translation_m": 2.0219843122070147e-09,
|
||||
"rotation_deg": 6.76470161118562e-08
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663626,
|
||||
"success": true,
|
||||
"translation_m": 4.562054371593163e-09,
|
||||
"rotation_deg": 1.0596545697409183e-07
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663607,
|
||||
"success": true,
|
||||
"translation_m": 1.928078172154913e-10,
|
||||
"rotation_deg": 2.1724633303911955e-09
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663614,
|
||||
"success": true,
|
||||
"translation_m": 4.546336113588724e-09,
|
||||
"rotation_deg": 6.943102831728965e-08
|
||||
},
|
||||
{
|
||||
"cost": 643.4104764663606,
|
||||
"success": true,
|
||||
"translation_m": 2.3977413195330228e-09,
|
||||
"rotation_deg": 3.302215565653965e-08
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.0019183316339420184,
|
||||
0.0018458296498062193,
|
||||
0.0015077091479455522,
|
||||
0.06309617953577086,
|
||||
0.06679628584461805,
|
||||
0.059116801250773655
|
||||
],
|
||||
"p025": [
|
||||
1.2931455727368442,
|
||||
-0.0034690771097207083,
|
||||
0.7130935831011376,
|
||||
-0.8251808751744363,
|
||||
1.2576102536763605,
|
||||
-0.9269499764475692
|
||||
],
|
||||
"p975": [
|
||||
1.3004042751519689,
|
||||
0.0031684595904240874,
|
||||
0.7192601795844623,
|
||||
-0.5861986801138164,
|
||||
1.5316240168054431,
|
||||
-0.7099716198219946
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,534 +0,0 @@
|
||||
{
|
||||
"schema_version": 2,
|
||||
"success": true,
|
||||
"message": "Both `ftol` and `xtol` termination conditions are 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.2978831677200011,
|
||||
-0.0030997734098957433,
|
||||
0.7217892226151688
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7572070226288585,
|
||||
1.1460424368859559,
|
||||
-0.8797812288284186
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.006530524361500762,
|
||||
0.01005115598038874,
|
||||
-0.007610830022211968,
|
||||
0.9998991958099549
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9996820990596618,
|
||||
0.015088846999329667,
|
||||
0.020199690985244836,
|
||||
1.2978831677200011
|
||||
],
|
||||
[
|
||||
-0.01535140427529437,
|
||||
0.9997988550358736,
|
||||
0.012906736835176902,
|
||||
-0.0030997734098957433
|
||||
],
|
||||
[
|
||||
-0.020000880141759644,
|
||||
-0.013212727393950824,
|
||||
0.9997126530300434,
|
||||
0.7217892226151688
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 38,
|
||||
"pairs": 66,
|
||||
"residuals": {
|
||||
"pairs": 66,
|
||||
"translation_m": {
|
||||
"rms": 0.10017183831403174,
|
||||
"median": 0.055359414994267414,
|
||||
"p90": 0.15842638830193137,
|
||||
"p95": 0.2228212283177476,
|
||||
"max": 0.3308905173432371
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.011529772202344,
|
||||
"median": 0.80406471347537,
|
||||
"p90": 1.4897684651925762,
|
||||
"p95": 1.743501596940947,
|
||||
"max": 2.0866296585079986
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.08037293687095128,
|
||||
"rotation_deg": 0.5606213555410943
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.13688783822315873,
|
||||
"rotation_deg": 0.5385985764755513
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.1161820533771873,
|
||||
"rotation_deg": 0.6051501049087227
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.06274731030725329,
|
||||
"rotation_deg": 0.43240094308199595
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.05401417992581746,
|
||||
"rotation_deg": 0.5598851592535787
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.031474943657373466,
|
||||
"rotation_deg": 0.4777215600802768
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.02358754528130127,
|
||||
"rotation_deg": 0.31065781541529597
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.07821720458579819,
|
||||
"rotation_deg": 0.8327360477136897
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.13242931760456017,
|
||||
"rotation_deg": 0.7104180024890483
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.06337205097891703,
|
||||
"rotation_deg": 0.7783419276431949
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.09374967709473093,
|
||||
"rotation_deg": 1.0032646553341338
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.03473040865260945,
|
||||
"rotation_deg": 0.38999250019936094
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.21059850723479812,
|
||||
"rotation_deg": 0.3446682830121482
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.19573146558983065,
|
||||
"rotation_deg": 0.48095280922563605
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.1323566970626505,
|
||||
"rotation_deg": 0.7201707943590688
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.12966351135913337,
|
||||
"rotation_deg": 1.112776940821149
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.05614712317985888,
|
||||
"rotation_deg": 0.875368334260281
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.07051356442391056,
|
||||
"rotation_deg": 0.6787378054467239
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.00234867626016906,
|
||||
"rotation_deg": 0.6239120141422153
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.06798198251653766,
|
||||
"rotation_deg": 1.002748878489142
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.06408705260318404,
|
||||
"rotation_deg": 0.03962823745578253
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.112944365872764,
|
||||
"rotation_deg": 1.0217086690617905
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.052992189070368054,
|
||||
"rotation_deg": 0.9852776192856918
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.15367415365164572,
|
||||
"rotation_deg": 1.6994783049152902
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.034245918976706625,
|
||||
"rotation_deg": 0.6364964475716594
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.032447856067877326,
|
||||
"rotation_deg": 1.0563370884814496
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.013423581617645036,
|
||||
"rotation_deg": 2.0866296585079986
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.020771076323520654,
|
||||
"rotation_deg": 0.38738877042810915
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.025006531842362605,
|
||||
"rotation_deg": 1.52954100014841
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.24776379251997158,
|
||||
"rotation_deg": 0.5631330497861866
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.03125984364955898,
|
||||
"rotation_deg": 1.3670046215070957
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.23303779095818136,
|
||||
"rotation_deg": 0.6816682778063402
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.3308905173432371,
|
||||
"rotation_deg": 0.35773823775063585
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.16317862295221702,
|
||||
"rotation_deg": 1.883393614450365
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.22689546867873073,
|
||||
"rotation_deg": 1.1173958780054432
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.11481327905632552,
|
||||
"rotation_deg": 0.9531859192751095
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.03799144229981835,
|
||||
"rotation_deg": 0.7744523894118182
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.05628293924136787,
|
||||
"rotation_deg": 1.14782377649109
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.044190715841938503,
|
||||
"rotation_deg": 0.6522697817009423
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.01566159386457093,
|
||||
"rotation_deg": 0.21165082671261798
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.025885925200718387,
|
||||
"rotation_deg": 1.758176027616166
|
||||
},
|
||||
{
|
||||
"pair_index": 41,
|
||||
"translation_m": 0.05672466093666094,
|
||||
"rotation_deg": 0.656889656854011
|
||||
},
|
||||
{
|
||||
"pair_index": 42,
|
||||
"translation_m": 0.04499649801025904,
|
||||
"rotation_deg": 1.202397214867585
|
||||
},
|
||||
{
|
||||
"pair_index": 43,
|
||||
"translation_m": 0.05457170680867596,
|
||||
"rotation_deg": 1.0831506943354967
|
||||
},
|
||||
{
|
||||
"pair_index": 44,
|
||||
"translation_m": 0.061259367696104086,
|
||||
"rotation_deg": 0.4788495792916977
|
||||
},
|
||||
{
|
||||
"pair_index": 45,
|
||||
"translation_m": 0.11550019477035034,
|
||||
"rotation_deg": 0.4484966984286178
|
||||
},
|
||||
{
|
||||
"pair_index": 46,
|
||||
"translation_m": 0.11728514823414202,
|
||||
"rotation_deg": 0.829787499307545
|
||||
},
|
||||
{
|
||||
"pair_index": 47,
|
||||
"translation_m": 0.05724708891657523,
|
||||
"rotation_deg": 0.13864261724965515
|
||||
},
|
||||
{
|
||||
"pair_index": 48,
|
||||
"translation_m": 0.08594624077085096,
|
||||
"rotation_deg": 0.5654259879584598
|
||||
},
|
||||
{
|
||||
"pair_index": 49,
|
||||
"translation_m": 0.03620565055727226,
|
||||
"rotation_deg": 0.7322933697368991
|
||||
},
|
||||
{
|
||||
"pair_index": 50,
|
||||
"translation_m": 0.04214151304995954,
|
||||
"rotation_deg": 1.4499959302367427
|
||||
},
|
||||
{
|
||||
"pair_index": 51,
|
||||
"translation_m": 0.028423032063821733,
|
||||
"rotation_deg": 1.4083884036448566
|
||||
},
|
||||
{
|
||||
"pair_index": 52,
|
||||
"translation_m": 0.07837351471415148,
|
||||
"rotation_deg": 2.059047791320455
|
||||
},
|
||||
{
|
||||
"pair_index": 53,
|
||||
"translation_m": 0.028769288969574303,
|
||||
"rotation_deg": 1.678230006332038
|
||||
},
|
||||
{
|
||||
"pair_index": 54,
|
||||
"translation_m": 0.017809796347028943,
|
||||
"rotation_deg": 1.156650317229778
|
||||
},
|
||||
{
|
||||
"pair_index": 55,
|
||||
"translation_m": 0.016503937937620585,
|
||||
"rotation_deg": 1.0883396698886323
|
||||
},
|
||||
{
|
||||
"pair_index": 56,
|
||||
"translation_m": 0.06654456279831698,
|
||||
"rotation_deg": 1.4361320254246386
|
||||
},
|
||||
{
|
||||
"pair_index": 57,
|
||||
"translation_m": 0.047435323978454284,
|
||||
"rotation_deg": 0.5969029690601318
|
||||
},
|
||||
{
|
||||
"pair_index": 58,
|
||||
"translation_m": 0.03126348405840702,
|
||||
"rotation_deg": 1.1168540542357488
|
||||
},
|
||||
{
|
||||
"pair_index": 59,
|
||||
"translation_m": 0.02474086331243135,
|
||||
"rotation_deg": 1.3807442010221649
|
||||
},
|
||||
{
|
||||
"pair_index": 60,
|
||||
"translation_m": 0.030093157854206205,
|
||||
"rotation_deg": 1.4167630603035772
|
||||
},
|
||||
{
|
||||
"pair_index": 61,
|
||||
"translation_m": 0.0541740507536005,
|
||||
"rotation_deg": 0.698365025436339
|
||||
},
|
||||
{
|
||||
"pair_index": 62,
|
||||
"translation_m": 0.022134680709029235,
|
||||
"rotation_deg": 0.2658404010324305
|
||||
},
|
||||
{
|
||||
"pair_index": 63,
|
||||
"translation_m": 0.029045352917775814,
|
||||
"rotation_deg": 1.1570185249880154
|
||||
},
|
||||
{
|
||||
"pair_index": 64,
|
||||
"translation_m": 0.027405495714480508,
|
||||
"rotation_deg": 1.03451592246849
|
||||
},
|
||||
{
|
||||
"pair_index": 65,
|
||||
"translation_m": 0.026462755506179093,
|
||||
"rotation_deg": 1.2477273813337815
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"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.004882663700530851,
|
||||
0.005223180397875539,
|
||||
0.004609821026450364
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.05337730845362286,
|
||||
0.05101978444118743,
|
||||
0.13461471349060455
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 6.280132943609949,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 257.9158465208353,
|
||||
"success": true,
|
||||
"translation_m": 1.5339146489710882e-09,
|
||||
"rotation_deg": 8.769019455279312e-08
|
||||
},
|
||||
{
|
||||
"cost": 257.915846520835,
|
||||
"success": true,
|
||||
"translation_m": 1.6096332607990603e-10,
|
||||
"rotation_deg": 3.2573047951731743e-09
|
||||
},
|
||||
{
|
||||
"cost": 257.915846520835,
|
||||
"success": true,
|
||||
"translation_m": 1.7670480374755598e-10,
|
||||
"rotation_deg": 6.1834270102153095e-09
|
||||
},
|
||||
{
|
||||
"cost": 257.915846520835,
|
||||
"success": true,
|
||||
"translation_m": 4.1405468581662865e-10,
|
||||
"rotation_deg": 3.068020689667501e-08
|
||||
},
|
||||
{
|
||||
"cost": 257.9158465208356,
|
||||
"success": true,
|
||||
"translation_m": 2.8335867565734006e-09,
|
||||
"rotation_deg": 1.5660913277854904e-07
|
||||
},
|
||||
{
|
||||
"cost": 257.915846520835,
|
||||
"success": true,
|
||||
"translation_m": 1.0113279249597898e-10,
|
||||
"rotation_deg": 1.1726291770865867e-09
|
||||
},
|
||||
{
|
||||
"cost": 257.91584652083503,
|
||||
"success": true,
|
||||
"translation_m": 8.073503715426879e-10,
|
||||
"rotation_deg": 4.276975901573228e-08
|
||||
},
|
||||
{
|
||||
"cost": 257.91584652083486,
|
||||
"success": true,
|
||||
"translation_m": 3.469446951953614e-18,
|
||||
"rotation_deg": 0.0
|
||||
},
|
||||
{
|
||||
"cost": 257.91584652083515,
|
||||
"success": true,
|
||||
"translation_m": 1.698393313088208e-09,
|
||||
"rotation_deg": 8.683351386297902e-08
|
||||
},
|
||||
{
|
||||
"cost": 257.91584652083503,
|
||||
"success": true,
|
||||
"translation_m": 1.831365187011624e-10,
|
||||
"rotation_deg": 1.6896050804471163e-09
|
||||
},
|
||||
{
|
||||
"cost": 257.91584652083503,
|
||||
"success": true,
|
||||
"translation_m": 1.2207922904627949e-09,
|
||||
"rotation_deg": 2.6207051319974274e-08
|
||||
},
|
||||
{
|
||||
"cost": 257.9158465208349,
|
||||
"success": true,
|
||||
"translation_m": 2.9844783853630704e-10,
|
||||
"rotation_deg": 1.1466486750928911e-08
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.0020832827985508263,
|
||||
0.002063603632322515,
|
||||
0.001427746471392659,
|
||||
0.07576290622767821,
|
||||
0.06304875844333606,
|
||||
0.07843172452171372
|
||||
],
|
||||
"p025": [
|
||||
1.2941517658812944,
|
||||
-0.0068139260683400035,
|
||||
0.7194879609149559,
|
||||
-0.932759562453139,
|
||||
1.0361273609600192,
|
||||
-1.0172648960899553
|
||||
],
|
||||
"p975": [
|
||||
1.303011833178259,
|
||||
0.0009615093863316334,
|
||||
0.7242819230641808,
|
||||
-0.6514715860744424,
|
||||
1.2471483979735007,
|
||||
-0.7328998428204786
|
||||
]
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
-1107
File diff suppressed because it is too large
Load Diff
@@ -1,109 +0,0 @@
|
||||
i,j,rtk_translation_m,rtk_rotation_deg,heldout_inlier_ratio,heldout_inlier_rmse_m,hessian_rank,hessian_condition,reverse_translation_m,reverse_rotation_deg,multistart_success_rate,accepted,rejection_reasons
|
||||
0,1,4.069460063439237,24.612641616688823,0.7376764924448849,0.11846276711125131,6,14.186383674198092,0.007238562624113292,0.02790414903884654,1.0,True,
|
||||
0,2,8.883155687691525,14.186041609026283,0.617186536801874,0.12801123590557925,6,36.79061260583155,0.006865219918284302,0.20055597955733165,1.0,True,
|
||||
0,3,5.800196618465403,50.885974946353855,0.7186066691459031,0.11701588647718943,6,18.363958192984455,0.008227849157773135,0.05354665376691754,1.0,True,
|
||||
1,2,4.892046307546544,10.426600007662538,0.7423043095866315,0.11327867264180574,6,26.05793878332379,0.01880830643908174,0.13698190350335646,1.0,True,
|
||||
1,3,1.7315947839093804,26.273333329665036,0.8389134554643083,0.10681097771213023,6,14.956194090958816,0.0035146019341874887,0.1349785173147079,1.0,True,
|
||||
1,4,1.6946255625760025,60.40345832959281,0.88,0.10289486740770283,6,11.829281279209892,0.006064649004018339,0.061683233541617866,1.0,True,
|
||||
2,3,3.2805755806518446,36.69993333732758,0.795020120724346,0.10968948046836192,6,23.96630628312886,0.006456546110122383,0.030970536985363332,1.0,True,
|
||||
2,4,4.866335020594746,70.83005833725535,0.7190493965409979,0.11680954298455397,6,23.730154915082874,0.005632827193528356,0.03284052493715785,1.0,True,
|
||||
2,5,1.8274406190287722,30.448549979291712,0.7598566308243727,0.11774293624986373,6,16.599379025063065,0.028141707120452414,0.19962462817569399,1.0,True,
|
||||
3,4,1.75881263274858,34.13012499992777,0.83898411109721,0.10512726552504312,6,15.664666117555983,0.006186049607122224,0.016880074405568043,1.0,True,
|
||||
3,5,2.1020200425132134,67.14848331661929,0.7968186901951038,0.11623997504357782,6,18.1548400244513,0.021983524103315014,0.3960603757299183,1.0,True,
|
||||
3,6,5.791805311102367,61.163322289150074,0.6568381430363864,0.12435814704811046,6,25.0151910725651,0.008738020425158864,0.1291569973379011,1.0,True,
|
||||
4,5,3.30592832159088,101.27860831654709,0.7543039842597147,0.11308907219490882,6,19.690455732375476,0.002243469564109645,0.06843841908142442,1.0,True,
|
||||
4,6,6.453995240105232,95.29344728907782,0.6218497827436374,0.12814112793605542,6,41.84651093530334,0.01600824572236542,0.4490447249637784,1.0,True,
|
||||
4,7,9.537609568572432,26.32130681988008,0.6169457128361238,0.13205154815512993,6,36.92786505190044,0.019160428104594212,0.2888349968548246,1.0,True,
|
||||
5,6,3.7769167422851373,5.985161027469212,0.7372055740535208,0.1175976253407229,6,19.32269960027907,0.013833903611932107,0.01952771029395653,1.0,True,
|
||||
5,7,6.565348405821576,74.95730149666699,0.6841773746535651,0.11964187966504931,6,27.33180748081407,0.003240486196092054,0.04738439683023512,1.0,True,
|
||||
5,8,6.3343271669452825,107.99940292217995,0.6521066412758867,0.12472719695199193,6,19.156993592870748,0.0278691016568626,0.41056933218195657,1.0,True,
|
||||
6,7,3.1558607172566147,68.97214046919777,0.7538481109273629,0.11470554120698542,6,17.160170663404163,0.03433121044907878,0.14981218442224864,1.0,True,
|
||||
6,8,3.3979014114419868,102.01424189471071,0.683003003003003,0.11564979549315224,6,20.447019845630425,0.008147035817338538,0.038229943243018406,1.0,True,
|
||||
6,9,0.28270347875173524,124.11602215258995,0.7360264227642277,0.11829700338913093,6,18.21588471622476,0.003942462178993821,0.004948404195791413,1.0,True,
|
||||
7,8,4.78580085090425,33.04210142551296,0.6644591611479028,0.12091344193997677,6,22.65051252921267,0.010087854632047892,0.1144705976875931,1.0,True,
|
||||
7,9,2.876695741106431,166.91183737821345,0.6157641864692897,0.12124276796057824,6,31.889839980753695,2.858085971145254,5.202834749913653,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
7,10,3.5227760858025783,151.5297683494061,0.8421725239616613,0.10512064341715703,6,18.208137447441548,0.00590396821278501,0.011716048666689089,1.0,True,
|
||||
8,9,3.469611483011297,133.86973595269956,0.5997550520514391,0.12251997518546373,6,26.58434328826198,0.1455308769088698,0.6888512968600221,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
8,10,8.226583602288494,175.42813022508852,0.5915068162625166,0.1229044134151169,6,28.111420003057543,0.008997663249148741,0.15439288797351725,1.0,True,
|
||||
8,11,1.865267488778983,173.7180803410843,0.6836137963957513,0.11276660610764273,6,37.61742547586071,0.1113060196402131,0.53316971567076,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
9,10,5.488028285806731,41.558394272381754,0.5969882593159775,0.11948988234049264,6,38.89573848099908,0.0030407162313759195,0.04107484998318919,1.0,True,
|
||||
9,11,4.718056434111957,39.84834438838189,0.593742114559677,0.12239484647700946,6,46.81089854115947,0.035258275490363276,0.11274694382482037,1.0,True,
|
||||
9,12,4.0664516687652705,11.300244475855449,0.6153363453815262,0.12504774785186165,6,23.277303065116303,0.0752860327247449,0.25939502405614967,1.0,True,
|
||||
10,11,8.48963239412477,1.7100498839998592,0.5888198757763975,0.12435475144629402,6,51.68982317778118,0.008996147539897087,0.2187503499467401,1.0,True,
|
||||
10,12,7.8510898708654135,30.258149796526293,0.5523974295600593,0.12600876663407296,6,24.040230371790127,0.0067913142930030295,0.19210976651930364,1.0,True,
|
||||
10,13,7.303425556850936,93.89697482620899,0.48622589531680444,0.12387372192061825,6,40.15734453396082,0.010457065237222635,0.051939983599844905,1.0,True,
|
||||
11,12,0.684356309096518,28.54809991252644,0.8081153752138841,0.09827968392237502,6,14.074429682972724,0.003170825002365958,0.052994059649638055,1.0,True,
|
||||
11,13,4.983180422136547,92.18692494220912,0.4904655770183259,0.13065086109778945,6,59.94917005604024,0.03026165336674197,0.022229096602302635,1.0,True,
|
||||
11,14,7.513058208926106,116.17598776756182,0.5502994743918836,0.13363854913882675,6,76.59552817138297,0.012728663920096705,0.13777836936248603,1.0,True,
|
||||
12,13,4.470221622128888,63.638825029682685,0.5394185760039418,0.1291861812905223,6,40.18929295270992,0.016569656651710556,0.06971243680146068,1.0,True,
|
||||
12,14,7.042432348069883,87.62788785503537,0.5838501763346711,0.13149007038687927,6,46.68047224389145,0.016475747103725697,0.027479170527625274,1.0,True,
|
||||
12,15,9.425648786798952,64.11974003309916,0.512344920771404,0.13771827771927672,6,83.08102250810028,0.1594220156320846,0.7727589642419826,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
13,14,2.6059162757855927,23.989062825352686,0.6685967722064802,0.11944947118678222,6,17.494241627788213,0.00889966253307221,0.1868503363813343,1.0,True,
|
||||
13,15,4.985522367267706,0.4809150034164723,0.6380833851897947,0.12132550246856395,6,19.285114979899365,0.011310526444604854,0.11618951108877962,1.0,True,
|
||||
13,16,5.330126890587504,0.570225046299944,0.5094008523439458,0.12969283093489314,6,23.287780580490335,0.01929138406979036,0.6381307294537529,1.0,False,forward_reverse_rotation
|
||||
14,15,2.3845573265941185,23.508147821936216,0.7915233415233415,0.11297936021484274,6,14.802453343324077,0.04714013527415902,0.5990777245962995,1.0,False,forward_reverse_rotation
|
||||
14,16,7.871327660405588,24.55928787165262,0.515650129902264,0.1258655742770286,6,27.98905523695805,0.019790578845351906,0.2492103704468241,1.0,True,
|
||||
14,17,6.334147330965719,0.13339551798522414,0.6016802569804793,0.12195151942370595,6,25.851285634799194,0.01939980613812204,0.1992615163030762,1.0,True,
|
||||
15,16,10.188241681839806,1.051140049716416,0.4590676165479315,0.13331101566670972,6,37.36189113980903,0.03193705319987942,0.25820325147713563,1.0,True,
|
||||
15,17,8.639041695194567,23.641543339921444,0.5389415876185721,0.1297159188720241,6,23.825996467364362,0.007152603768137945,0.01099978555369691,1.0,True,
|
||||
15,18,9.620687246763701,76.25281000098852,0.5202642612120442,0.13378678930694327,6,39.71428865706424,0.023698091864950893,0.7026335973530661,1.0,False,forward_reverse_rotation
|
||||
16,17,1.5531467972267479,24.69268338963786,0.7229415461973602,0.10903876418375444,6,27.565368871601265,0.004743603748938733,0.03110066538977425,1.0,True,
|
||||
16,18,0.5677742310084086,77.30395005070493,0.75774251343742,0.10672872147965808,6,23.961833593584473,0.00729627345064471,0.5370128665073076,1.0,False,forward_reverse_rotation
|
||||
16,19,2.9811390265794215,109.91770005034847,0.624375,0.11606003815798548,6,43.92870457834769,0.00942822069733503,0.10349184268425643,1.0,True,
|
||||
17,18,0.98603936348044,52.61126666106708,0.8035782747603833,0.11258525838778834,6,10.5263751279628,0.04456291633933312,0.40754887525204303,1.0,True,
|
||||
17,19,3.326868398717413,85.22501666071064,0.667207589564349,0.1151980232980854,6,37.82913063903142,0.002550382605456025,0.05599236133477508,1.0,True,
|
||||
17,20,1.702964560097507,146.95247656520317,0.6696552595024624,0.11181113630432839,6,22.83917826847431,0.009851652453759501,0.030403861001117326,1.0,True,
|
||||
18,19,3.0423123591285863,32.61374999964355,0.7803049555273189,0.10890373850857962,6,14.866928278077225,0.0027074586252039488,0.03345524421318835,1.0,True,
|
||||
18,20,1.0853574230860805,94.34120990413592,0.7890203137053227,0.110891959946164,6,9.73093989119612,0.07641393239305394,0.480643469622101,1.0,True,
|
||||
18,21,0.7190584934612743,134.43032499953063,0.7686106562539362,0.1151576675189467,6,11.132461972272983,0.012325022302827005,0.06184414805686676,1.0,True,
|
||||
19,20,1.9962827272376449,61.72745990449234,0.7206177800100452,0.11075109929370276,6,18.593207910714877,0.06755956873701982,0.6275145294718035,1.0,False,forward_reverse_rotation
|
||||
19,21,3.133843614009462,101.81657499988698,0.6599384615384616,0.1100348641166372,6,24.889550971173787,0.032753113036640115,0.1570816751009414,1.0,True,
|
||||
19,22,3.558853688535236,131.87013333415302,0.6354319180087847,0.11655958277402616,6,35.20679617831266,0.011678083327496752,0.14115606360994692,1.0,True,
|
||||
20,21,1.4312693216245356,40.08911509539463,0.8091622059006598,0.10860060068735904,6,9.844983425406634,0.004342811149796408,0.43079835067471983,1.0,True,
|
||||
20,22,2.8745434122269677,70.1426734296607,0.7075518262586377,0.11619246575360609,6,10.41692741032582,0.03233653011234762,0.3501326921447133,1.0,True,
|
||||
20,23,2.368927629183393,88.7954400941961,0.7127672722778395,0.11187552234677736,6,10.890181651202985,0.0036477332397017036,0.03588537530780425,1.0,True,
|
||||
21,22,1.9888588003695142,30.053558334266054,0.7451905626134301,0.11103160300292489,6,9.138865693353942,0.007025231148999399,0.031656114821116355,1.0,True,
|
||||
21,23,1.9477165743251885,48.70632499880147,0.7537566650508968,0.1091762180475198,6,10.033043192754391,0.0026262958914527778,0.05939834892732321,1.0,True,
|
||||
21,24,3.76075382790461,86.89547500633407,0.7108181370133071,0.1147630411233105,6,12.077810466162274,0.002611195084459223,0.042823121286843194,1.0,True,
|
||||
22,23,0.8778876913955667,18.652766664535406,0.8809323561215908,0.09065520084082093,6,9.426617095196006,0.00074030291113835,0.005760006892562891,1.0,True,
|
||||
22,24,2.2510479983846268,56.84191667206801,0.7459386832783681,0.1116686389188481,6,10.471980698049531,0.010212372381123916,0.01461127103474581,1.0,True,
|
||||
22,25,3.452614527841686,105.36038662410485,0.6250595521676989,0.10935727133461083,6,47.514851436189325,0.013489723519689117,0.0723917800582207,1.0,True,
|
||||
23,24,1.8212773036309853,38.18915000753262,0.7936138977244923,0.11107653584131949,6,10.660643490600723,0.008437497431588122,0.2765568737620497,1.0,True,
|
||||
23,25,2.999856552735995,86.70761995956946,0.6337826553739712,0.11196536842375276,6,43.84810300726168,0.02359951283541323,0.10308564406068685,1.0,True,
|
||||
23,26,1.7088514993141615,157.34900000166795,0.6073053892215569,0.11004156310392302,6,52.70992994760862,0.013062761031411258,0.0548185753269731,1.0,True,
|
||||
24,25,1.2059352225587747,48.51846995203683,0.7898629804777495,0.11080878069613129,6,17.323052863790767,0.024355899665769485,0.10833606018329535,1.0,True,
|
||||
24,26,1.1958827550030364,119.15984999413537,0.7515217920623326,0.10216449744519186,6,27.478293204817643,0.010967953426658109,0.044641472084512177,1.0,True,
|
||||
24,27,3.786115539231191,149.59794999698258,0.6259589210591437,0.11074683701330018,6,26.105827828207687,0.04374270593226806,0.34168251600562055,1.0,True,
|
||||
25,26,1.8270495459501794,70.64138004209858,0.8273774189718628,0.10613402567174812,6,17.478877580241615,0.018863132797671257,0.12251687299132978,1.0,True,
|
||||
25,27,4.741885618180033,101.07948004494568,0.6236883367506936,0.11275084173352168,6,26.65509822252546,0.005757137825848521,0.2558617834036415,1.0,True,
|
||||
25,28,3.8640843382258447,141.59343004213218,0.5871679316888045,0.11732003715351383,6,52.15762909847497,1.0486443236448235,4.536374847153588,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
26,27,2.939444415707082,30.43810000284707,0.6585129571324776,0.10848902314415616,6,30.227639491425478,0.002959097196576797,0.043581628031778105,1.0,True,
|
||||
26,28,2.285770454773634,70.95205000003368,0.6648410525062507,0.11120026543063653,6,45.565898082132684,0.012512803173543077,0.11122589391716618,1.0,True,
|
||||
26,29,3.6568094249627374,131.92299999999997,0.5895513507080804,0.11693751976202812,6,62.1400238672027,2.7672344834891307,9.168367379205225,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
27,28,2.3764785758716087,40.513949997186636,0.7954517962985364,0.10154002419986238,6,26.01517111533084,0.019135551099374308,0.08227568287606397,1.0,True,
|
||||
27,29,4.061689152213973,101.48489999715295,0.7446499818643453,0.1104935519181424,6,32.698094109014136,0.015390416284740782,0.08645102756020058,1.0,True,
|
||||
27,30,5.033742765789926,178.61309998505533,0.583363515634432,0.11446907648011068,6,105.65813856703667,0.017491035693048888,0.22327775856236398,0.0,False,multistart_instability
|
||||
28,29,1.7860052935263149,60.970949999966315,0.8317879220161674,0.10401467258245657,6,16.856539687156317,0.007126704551442278,0.059447117802805614,1.0,True,
|
||||
28,30,7.378510482049102,138.09914998778677,0.540360873694207,0.1217315596532435,6,243.03186464572013,0.02476143939627759,0.2783665026489561,1.0,True,
|
||||
28,31,8.396001985859645,161.15751666826222,0.5051186318198242,0.12667907013046822,6,145.12677311467613,0.01381623315543165,0.08866229443960012,1.0,True,
|
||||
29,30,8.925911734961222,77.12819998782052,0.5223065970574277,0.12780035193537048,6,94.88866430179526,0.06232017355575957,1.955625956099923,1.0,False,forward_reverse_rotation
|
||||
29,31,9.936139535050964,100.18656666829587,0.49440231130371975,0.12866710903574613,6,130.4988571288183,0.047123741889749056,0.2295904679982487,1.0,True,
|
||||
29,32,9.719184595429152,159.1482027457047,0.49802134548507015,0.12675987702934546,6,49.64602672737782,0.0323736334045635,0.06974950067660468,0.0,False,multistart_instability
|
||||
30,31,1.0176267260390144,23.058366680475352,0.8480586608967424,0.09667505139943416,6,22.5712438700004,0.01573204986385798,0.04061482397026774,1.0,True,
|
||||
30,32,0.8427696971904044,82.02000275788396,0.867680517303317,0.0986734040048058,6,23.367311395821964,0.002023273028745447,0.014811950864385802,1.0,True,
|
||||
30,33,1.1320092061720382,147.1799498371321,0.7995722941665676,0.10006346131537393,6,41.688755802373954,0.00887908049317058,0.048438711468841336,1.0,True,
|
||||
31,32,0.2771314063233405,58.9616360774086,0.8486026731470231,0.09714101002609693,6,27.412782801978597,0.012921368358244189,0.05809273577687571,1.0,True,
|
||||
31,33,0.5409306841643147,124.12158315665668,0.07871263259402121,0.14967084978243636,6,33.0912130689722,2.900706563387122,12.11541439568944,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
31,34,0.9901318542574941,155.5976733552555,0.7813404050144648,0.09211161518215291,6,17.31754498769328,3.351107529092291,8.098567652508597,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
32,33,0.38765596924480794,65.15994707924807,0.8592699327569645,0.08972486966187442,6,22.68434781952533,0.0035725430520846596,0.009168450597030125,1.0,True,
|
||||
32,34,1.0327853226589305,96.63603727784675,0.8328530259365994,0.09815070141607997,6,23.927327238925002,0.04150636593087017,1.4498937658029205,1.0,False,forward_reverse_rotation
|
||||
32,35,1.1947451269258382,131.6892972449946,0.8031309297912713,0.09391797152520207,6,30.896133075774756,0.007719527793580684,0.006042163579184511,0.0,False,multistart_instability
|
||||
33,34,0.7453275493729801,31.47609019859864,0.8676435549201811,0.0964383255266311,6,26.835745049910184,0.02602048161602559,1.951142742234491,1.0,False,forward_reverse_rotation
|
||||
33,35,1.0924186931915323,66.52935016574651,0.8329018592610026,0.09179860947142506,6,17.934600324801995,0.0072384331805243046,0.028209706778456432,1.0,True,
|
||||
33,36,3.335020341384374,123.21987515531416,0.760797342192691,0.09861359027653524,6,26.32312350297475,0.0022735557932631,0.12221117701824208,1.0,True,
|
||||
34,35,0.6186130964444055,35.05325996714787,0.8649093904448105,0.0788491357224537,6,23.02575456989985,0.0032110784907267375,0.015379666997505564,1.0,True,
|
||||
34,36,3.114273556969299,91.74378495671547,0.7729789590254706,0.09845381444357724,6,36.91664640012635,0.011794412591703963,0.006135949966801849,1.0,True,
|
||||
34,37,2.8155528331227235,126.08390993626574,0.722881252293017,0.09759501600127507,6,42.82845764024001,0.024742240312777743,0.08718512569927171,1.0,True,
|
||||
35,36,2.4989423629184655,56.69052498956759,0.7695298262665533,0.09495386680108024,6,27.978829086780696,0.006644101856716534,0.038610475450088305,1.0,True,
|
||||
35,37,2.2182956249874577,91.03064996911786,0.7217727327617437,0.10621413586570766,6,32.52725359303394,0.04503438705750094,1.5781812743250268,1.0,False,forward_reverse_rotation
|
||||
36,37,0.5129211474320938,34.34012497955026,0.8785082174462705,0.08834636280188386,6,23.324824248042262,0.003787237363253161,0.027331592166342466,1.0,True,
|
||||
|
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -1,176 +0,0 @@
|
||||
i,j,rtk_translation_m,rtk_rotation_deg,heldout_inlier_ratio,heldout_inlier_rmse_m,hessian_rank,hessian_condition,reverse_translation_m,reverse_rotation_deg,multistart_success_rate,accepted,rejection_reasons
|
||||
0,1,0.503306788400079,13.198324485965863,0.8131655372700871,0.09008834340898543,6,11.429289586039127,0.0005504552049968047,0.0032916777115618487,1.0,True,
|
||||
0,2,1.1601020961002007,26.306295080591493,0.8167658604533367,0.09980570916927256,6,11.00789089021616,0.00205395151868756,0.012799633971915293,1.0,True,
|
||||
0,3,1.1328421148283978,41.74134818989305,0.81441508497705,0.10115686777955255,6,13.837529680361882,0.01247476852224996,0.1726729736128094,1.0,True,
|
||||
0,4,1.0129799192122786,70.92934830291522,0.7761963190184049,0.10269905695661055,6,12.888313335228592,0.019300920937430674,0.12217052794626229,1.0,True,
|
||||
0,5,0.9014321094792815,89.27285808969826,0.7735100978813034,0.10986502690369805,6,24.919203290408266,0.0278108340699398,0.2717481445912482,1.0,True,
|
||||
1,2,0.6927979865343085,13.107970594625625,0.7850287907869482,0.08649388452503427,6,11.757459931907107,0.001960394451155616,0.002128143929965857,1.0,True,
|
||||
1,3,0.7002295454253883,28.543023703927183,0.7901992730918661,0.09109387779670658,6,12.066030386554463,0.004906057223105903,0.018773640398341965,1.0,True,
|
||||
1,4,0.8045641659338967,57.731023816949346,0.7439266236985622,0.09680131928922782,6,12.860596463339792,0.020081946886066578,1.2340284819813434,1.0,False,forward_reverse_rotation
|
||||
1,5,0.7829726518389362,76.07453360373235,0.7313806483915384,0.10148815288043825,6,12.782489568272775,0.016729134900242502,0.06734086907642674,1.0,True,
|
||||
1,6,0.7843480712672012,108.63393511864574,0.6214689265536724,0.10742176387842234,6,26.39069479810122,0.0027238824581310127,0.04004688680398286,1.0,True,
|
||||
2,3,0.13949776780243606,15.435053109301553,0.9176300578034682,0.07082931569628453,6,11.740335220665598,0.0016419682635548487,0.0015051284247814798,1.0,True,
|
||||
2,4,0.6701726006967371,44.62305322232371,0.840540189585768,0.08212714345330237,6,12.917292889611646,0.0015059170263523442,0.004861145455894842,1.0,True,
|
||||
2,5,0.8004226998626774,62.96656300910673,0.8300970873786407,0.0931389934573794,6,15.74858194202481,0.002555949510569182,0.008486377461290648,1.0,True,
|
||||
2,6,1.3845111356101025,95.5259645240201,0.6495130297446696,0.10354826871538153,6,15.262952717632835,0.008359384079660144,0.07441619593698878,1.0,True,
|
||||
2,7,2.161304865409727,119.81081101983789,0.8143257302921169,0.09968516416989152,6,30.540416925007207,0.013121403719342412,0.05039604031914888,1.0,True,
|
||||
3,4,0.5366157827594368,29.188000113022152,0.8855689764780674,0.07250679559229746,6,11.672391892657863,0.0006574625393083392,0.0008548453445728261,1.0,True,
|
||||
3,5,0.6760093701980526,47.53150989980517,0.8759311584895967,0.08543306637795112,6,15.073465050917047,0.0011073394938326227,0.004974962253885575,1.0,True,
|
||||
3,6,1.4361364806393455,80.09091141471853,0.678819891780469,0.10239967326707564,6,23.156860004682848,0.002691305620040523,0.027681404773136364,1.0,True,
|
||||
3,7,2.1008551524156123,104.37575791053635,0.7961101683853283,0.10180989325686585,6,29.756525470837225,0.023292005056525338,0.174639961337377,1.0,True,
|
||||
3,8,1.3108481779402037,133.20736428981294,0.11556480999479438,0.1433106939673773,6,147.35707874920627,1.368389528072986,15.0277123732284,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
4,5,0.17861948949299924,18.343509786783002,0.8685714285714285,0.07604325543622346,6,12.370184068840063,0.003904582263467037,0.011742002375313174,1.0,True,
|
||||
4,6,1.5807073315069207,50.90291130169636,0.6822060883963826,0.10567824422734254,6,18.163522769519293,0.011923922180697284,0.08008579261790838,1.0,True,
|
||||
4,7,1.7804201387535592,75.18775779751417,0.7963870967741935,0.10178391918503964,6,25.315987799570973,0.034377900701078094,0.11532260919614924,1.0,True,
|
||||
4,8,1.2818497736845669,104.01936417679076,0.7430850379518847,0.10189238984319506,6,25.19640114706588,0.034977663635135596,1.5753470469755222,1.0,False,forward_reverse_rotation
|
||||
4,9,1.5141865499370524,117.34615495378958,0.0841833440929632,0.15176056899798168,6,84.88516221869183,3.340127368239112,6.177861200193235,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
5,6,1.5316987521992935,32.55940151491337,0.6864384971693258,0.10315026388508003,6,11.505426214947052,0.002552272421554712,0.013899678900349944,1.0,True,
|
||||
5,7,1.6091514152462776,56.844248010731185,0.8104549602398644,0.09862238510016193,6,16.517299930207333,0.0037398303757535147,0.02129636862756259,1.0,True,
|
||||
5,8,1.1872485760158462,85.67585439000777,0.7806082661814401,0.10305777405801758,6,17.737819341361416,0.0056799616182010805,0.026182015145595202,1.0,True,
|
||||
5,9,1.380208837139288,99.00264516700663,0.7946777980693973,0.10313535699942489,6,14.147499646203187,0.06270036555847405,0.33857191756776234,1.0,True,
|
||||
5,10,1.7506315276318887,115.10127466723206,0.7847418443359877,0.10018431236036145,6,21.425526326745782,0.005291671170548772,0.044796841001461946,1.0,True,
|
||||
6,7,1.5692385974903325,24.284846495817817,0.7380672159016608,0.1046107504689679,6,13.947054238965462,0.010178220865611676,0.026255002379027764,1.0,True,
|
||||
6,8,0.5035712140385419,53.11645287509441,0.7026075619295958,0.10337241424878332,6,21.232848227388097,0.017447450845101884,0.1292664848366822,1.0,True,
|
||||
6,9,0.8508108569116883,66.44324365209327,0.6714210939544621,0.10949604778423808,6,34.21205543097956,0.09491078892598759,0.32255986251320673,1.0,False,forward_reverse_translation
|
||||
6,10,1.3331914459292633,82.5418731523187,0.6619427982478743,0.10510077922086615,6,31.101566437428897,0.003562383894956786,0.04536548719264196,1.0,True,
|
||||
6,11,3.16887584646763,119.46156331049859,0.638006230529595,0.1061575017783152,6,31.943306843588733,0.0059304520508679575,0.05963953472442965,1.0,True,
|
||||
7,8,1.1230406357307108,28.831606379276582,0.8040692297529396,0.09680052952350976,6,22.21980394474879,0.008328073875015064,0.06350723756646647,1.0,True,
|
||||
7,9,0.7186795671679719,42.158397156275456,0.8274764620076913,0.09296346258223728,6,22.096355228992984,0.0076981468910999155,0.020277033650542,1.0,True,
|
||||
7,10,0.39293571356254015,58.25702665650087,0.7992429186790236,0.10479299924479576,6,30.246845596558646,0.04240408437264065,1.1111803590641476,1.0,False,forward_reverse_rotation
|
||||
7,11,2.4544901278439815,95.17671681468076,0.7502523977788995,0.1028589368143411,6,19.931284665592216,0.005620519728387707,0.0303880139721275,1.0,True,
|
||||
7,12,3.0905023273107046,115.3786462164333,0.7054418372441338,0.10706084863640537,6,10.761985004268995,0.004629206980441498,0.07009059694702204,1.0,True,
|
||||
8,9,0.4407680716864947,13.326790776998866,0.8539132734003173,0.09158655960119419,6,31.292808872701826,0.0036696927581956673,0.014909075167693285,1.0,True,
|
||||
8,10,0.968166048306331,29.425420277224287,0.7989328474752733,0.09757506446655329,6,25.45934970841849,0.008689580645481346,0.052297435035790506,1.0,True,
|
||||
8,11,3.0540124804140354,66.34511043540417,0.6854158802063672,0.10755838259974337,6,42.46956117899474,0.00234148729464824,0.061662020462905004,1.0,True,
|
||||
8,12,3.652970846697932,86.5470398371567,0.6485376477909147,0.11031208403019,6,30.049664340525695,0.013830104055554564,0.11563630256710061,1.0,True,
|
||||
8,13,4.357531202056024,107.49623325505803,0.622879241516966,0.1149201207874094,6,27.04257772502661,0.005835943764379069,0.23887217550290574,1.0,True,
|
||||
9,10,0.5276649044820116,16.098629500225417,0.8486154649947754,0.09691837979566219,6,22.577834737399076,0.007052114296262031,0.2907164943632002,1.0,True,
|
||||
9,11,2.680593454060616,53.0183196584053,0.6963000378835712,0.10535088240045436,6,39.530571540955094,0.0034527207661208545,0.020238883727394828,1.0,True,
|
||||
9,12,3.2944321799753977,73.22024906015783,0.6584238791057825,0.10587631557960675,6,33.498316372288826,0.009482405807021465,0.07225186033919398,1.0,True,
|
||||
9,13,4.1147924441994315,94.16944247805917,0.621557336004006,0.11335203160746431,6,32.6975845279655,0.009011781776631502,0.0416830306778097,1.0,True,
|
||||
9,14,3.748520283907519,169.69584626488452,0.048890560361037984,0.15557197380465354,6,51.54107837707314,1.0864076801076206,7.184564913859941,0.5,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
10,11,2.249454986440387,36.91969015817989,0.7156362731683045,0.10422606491110974,6,45.45254669271086,0.004301999273657816,0.02361974788713562,1.0,True,
|
||||
10,12,2.8783779741216167,57.12161955993242,0.692875599852344,0.10691573133186853,6,39.11010182412513,0.004231594662123499,0.01872424915403812,1.0,True,
|
||||
10,13,3.84803303865234,78.07081297783375,0.6594721262950173,0.10871022234744057,6,43.1464607017966,0.006980776690426113,0.21408921607631748,1.0,True,
|
||||
10,14,3.3834992499807997,153.59721676465847,0.062245276028158575,0.15570919185507412,6,59.4104405367878,0.09444016841800375,2.8556589503073977,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
10,15,1.4957886147833104,171.72980583515965,0.6232518545542989,0.10969245891269987,6,43.00146365447724,3.0001452916434883,4.8522273147326915,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
11,12,0.636223282239137,20.20192940175253,0.8688915375446961,0.08660981428891965,6,18.033297623211222,0.005089530929434581,0.03170796903435614,1.0,True,
|
||||
11,13,2.0145868546459504,41.151122819653864,0.8021741727392188,0.10650327571539893,6,18.339724002801702,0.04485533499047456,0.27313934813876123,1.0,True,
|
||||
11,14,1.2405786051647103,116.67752660647822,0.7455741626794259,0.11063741423489376,6,20.694961222671232,0.007044503253911164,0.40985193451245056,1.0,True,
|
||||
11,15,1.4202907399288436,151.3505040066613,0.7466415272213057,0.10329430650455694,6,27.704214205277534,0.13203399207663835,1.4478079414864902,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
||||
11,16,2.7368971716016928,176.81389445065594,0.78639603721155,0.10318237166131629,6,34.658693494341755,0.008767229441137649,0.032548654712657686,1.0,True,
|
||||
12,13,1.5928240921286525,20.949193417901323,0.8217283366828231,0.10684146627686454,6,16.400323597466677,0.01763325318199382,0.09467538550034012,1.0,True,
|
||||
12,14,0.6774809171975742,96.47559720472566,0.7611268939393939,0.10826784336909262,6,10.635638995146312,0.0015931090432955832,0.024585439481643892,1.0,True,
|
||||
12,15,1.8892292086221258,131.14857460490867,0.7324469325868906,0.11113399174419211,6,19.970471882560926,0.0024416100752998565,0.009279978214820864,1.0,True,
|
||||
12,16,3.349436875941103,156.6119650489076,0.7500298864315601,0.10525785108693746,6,30.70828038440084,0.0024719134275639418,0.013160987022490347,1.0,True,
|
||||
12,17,6.51378754358264,111.73754176984298,0.6198434030618207,0.1221546776649581,6,23.54160456185754,0.013538123192296372,0.06236055877935909,1.0,True,
|
||||
13,14,0.9721300146850552,75.52640378682433,0.8278562107011508,0.10919246415865995,6,12.723402368483741,0.0049872211162087675,0.02566897887278044,1.0,True,
|
||||
13,15,2.426469315960194,110.19938118700728,0.7886616014026885,0.10800229196760061,6,13.66454080002457,0.0038816141140735755,0.020030260506224723,1.0,True,
|
||||
13,16,4.157137927296689,135.66277163100625,0.7477833692786964,0.10901310110319071,6,21.034367079780026,0.0024107551433002806,0.01470679715840447,1.0,True,
|
||||
13,17,6.347445048137873,90.78834835194164,0.6122042632935114,0.12126385638135445,6,27.550293797111443,0.010111269848200304,1.6132660739833655,1.0,False,forward_reverse_rotation
|
||||
13,18,4.26917989710959,135.79234683500215,0.6756988719960765,0.1140499722068077,6,13.668694297489541,0.010188526275029454,0.05757819838859875,1.0,True,
|
||||
14,15,2.176233486689304,34.672977400182965,0.7415730337078652,0.11426627617931512,6,13.437209845730706,0.009280638111078932,0.07287904157175247,1.0,True,
|
||||
14,16,3.79947855245853,60.13636784418198,0.7130173965206958,0.11194096137848975,6,36.60672565292038,0.0004016408999195087,0.020284570842973915,1.0,True,
|
||||
14,17,6.190474035537447,15.261944565117341,0.6118212736015011,0.11991020962112661,6,17.152314443231333,0.020151321868834806,0.2079666531947748,1.0,True,
|
||||
14,18,4.170672674565236,60.26594304817776,0.6756292203806016,0.1138218459710723,6,11.041435170572107,0.023698120255922432,0.1321697884270417,1.0,True,
|
||||
14,19,3.9999134794649294,94.30913149027137,0.6699172941612147,0.112413086617321,6,9.748384934395327,0.018186549996561333,1.2493862686891326,1.0,False,forward_reverse_rotation
|
||||
15,16,1.7328875589135755,25.463390443999018,0.7714218177520388,0.10015132538295435,6,21.49868629780701,0.007439481433750378,0.036349268136368934,1.0,True,
|
||||
15,17,8.352264787019024,19.411032835065622,0.5587657459840518,0.12832496127520593,6,38.53228682833441,0.005430301274456062,0.06530638270069017,1.0,True,
|
||||
15,18,6.346037230040679,25.592965647994788,0.615040502962157,0.11840729454457544,6,21.032065383701333,0.012871328625232324,0.037753566213482644,1.0,True,
|
||||
15,19,5.607512178689573,59.636154090088375,0.6234350309955026,0.11735435665832353,6,16.270394372537467,0.007833445352022574,0.016491686030251827,1.0,True,
|
||||
15,20,8.633248754322691,149.96154617253603,0.5562658304185261,0.13192141621005607,6,26.944324835385952,0.023510650105734523,0.673469393155776,1.0,False,forward_reverse_rotation
|
||||
16,17,9.849975560032544,44.87442327906463,0.5900047370914259,0.1265936504427481,6,90.52736834449948,0.011296955150750666,0.21501790849158783,1.0,True,
|
||||
16,18,7.9099649541476005,0.12957520399577063,0.6471098982882659,0.12019779875781794,6,38.44752656622707,0.016821400599454826,0.06522344447557246,1.0,True,
|
||||
16,19,6.609737623005951,34.172763646089344,0.6280868046894488,0.12242728727104996,6,33.04590657073916,0.06745241310870236,1.6183598806956783,1.0,False,forward_reverse_rotation
|
||||
16,20,9.541288639020653,124.49815572853673,0.5791883197228409,0.127436475456937,6,40.945151692592816,2.599521231663455,5.87323656621683,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
16,21,9.407850320517097,175.38299577015903,0.49424730531670097,0.13389808634231865,6,45.28912477989511,0.04478742358365585,0.10475809279093058,1.0,True,
|
||||
17,18,2.081596655954183,45.003998483060414,0.7629028349890962,0.10358638519274684,6,18.62989302843653,0.007136146106563684,0.13887814859043496,1.0,True,
|
||||
17,19,4.34928579963815,79.047186925154,0.6728380024360536,0.10727518915463004,6,33.81541258417489,0.0017207670327870018,0.035486867252565536,1.0,True,
|
||||
17,20,4.4206846692505035,169.37257900760153,0.6354846507130771,0.11168219002675635,6,38.40506040461897,0.020290214498459216,0.2831192364677464,1.0,True,
|
||||
17,21,6.400116212709096,139.7425809507742,0.5695348561959995,0.1231813524465837,6,57.78224861663297,0.009859208985419086,0.31053558178227947,1.0,True,
|
||||
17,22,8.798226453379533,96.08482130259475,0.5459048079246195,0.1287223998113377,6,62.758312260043155,0.03194615937469955,0.5773515871730924,1.0,False,forward_reverse_rotation
|
||||
18,19,3.459222336215112,34.04318844209358,0.7562595809913132,0.11139160446923307,6,9.605522638453952,0.013156152321531143,0.09121207325672914,1.0,True,
|
||||
18,20,4.94806857498194,124.36858052454107,0.6668778509883426,0.1149564835181103,6,18.76442873201715,0.0076759006397633354,0.11713903044228172,1.0,True,
|
||||
18,21,6.5847443121227816,175.25342056617163,0.5977438948803768,0.12625086545570646,6,26.666713369082085,0.021119392446408227,0.18202979634982344,1.0,True,
|
||||
18,22,9.20681259274594,141.08881978565523,0.5286220871327254,0.13561310095446666,6,65.22594013516775,0.03713510940704989,0.6483189610135184,1.0,False,forward_reverse_rotation
|
||||
18,23,13.164275822166362,109.14103513098563,0.47146496815286626,0.14168298172295804,6,112.91469628781756,0.01208598984738065,0.6082838532521024,1.0,False,forward_reverse_rotation
|
||||
19,20,3.031758915138065,90.32539208244746,0.7409531090723751,0.11651827109675585,6,10.060869835729363,0.0051743650857167135,0.07263138471648796,1.0,True,
|
||||
19,21,3.732423384308353,141.21023212407198,0.6563629565000623,0.12291187901196955,6,15.339627619255575,0.0075358808329252175,0.06620678153945667,1.0,True,
|
||||
19,22,6.37255124464243,175.13200822775684,0.5891492613346918,0.12519472268444526,6,28.30213785955742,0.010212573357042046,0.11056129275235052,1.0,True,
|
||||
19,23,10.37094757900695,143.1842235730793,0.4984627209838586,0.13443006540917718,6,96.22322799212152,0.02229275506707497,0.2903847316754494,1.0,True,
|
||||
19,24,7.752290899896264,99.91483670708607,0.5732565579014716,0.1300803661721533,6,45.37964984018865,0.07078749161909308,0.6582469262123747,1.0,False,forward_reverse_rotation
|
||||
20,21,2.057726146362386,50.884840041624386,0.7808828984790405,0.11119787612354914,6,10.584324886965474,0.0031476308503252674,0.022487168529706673,1.0,True,
|
||||
20,22,4.388457127616061,94.54259968980386,0.6557604850934815,0.11898674179731801,6,16.883756700029306,0.008500396323601164,0.06463449106814506,1.0,True,
|
||||
20,23,8.256548639868363,126.4903843444735,0.5833545108005083,0.12863144503040289,6,48.7242741462686,0.013890482176493085,0.14514813799569015,1.0,True,
|
||||
20,24,5.797339988732605,169.75977121046614,0.6348521385962685,0.11831053318180502,6,31.671029215629257,0.00725649124030166,0.27346756317921195,1.0,True,
|
||||
20,25,3.9584300095978864,99.81821396908215,0.7094296865164296,0.11461231093244224,6,16.207076145397426,0.005999722474434507,0.07369423294061743,1.0,True,
|
||||
21,22,2.665930009787955,43.65775964817948,0.7387220368310469,0.11520069835112393,6,16.025252253059314,0.0021484382611444506,0.09312757978022868,1.0,True,
|
||||
21,23,6.670349114267683,75.60554430284907,0.6238657551274084,0.11987801174807478,6,32.271874238810035,0.007927242611182222,0.08795608522524892,1.0,True,
|
||||
21,24,4.0761181240750135,118.87493116884215,0.6712192699279861,0.11172772070549726,6,21.556821129992922,0.04673963175312233,0.22273623714194987,1.0,True,
|
||||
21,25,2.626640779558244,150.70305401070652,0.699310174919931,0.11234398811200015,6,15.335715959290205,0.018678960705618183,0.1734016933410122,1.0,True,
|
||||
21,26,2.086425835169367,83.91641671720558,0.7166481550043194,0.11091971646336789,6,12.109452825928004,0.00977162867106615,0.0631934229127065,1.0,True,
|
||||
22,23,4.005228418650323,31.947784654669576,0.7252984505969012,0.11644876240570266,6,17.935878140214236,0.013002087545984397,0.11470841251550605,1.0,True,
|
||||
22,24,1.4231800329066895,75.21717152066267,0.7555499175440822,0.1089594430160029,6,14.427403734239578,0.0044919719825295985,0.008669004448985662,1.0,True,
|
||||
22,25,0.9824487562406952,165.63918634111403,0.7088209387190134,0.11165869814478939,6,23.409617344710625,0.005265657588642687,0.10195131255569051,1.0,True,
|
||||
22,26,3.0743977770984916,127.57417636538506,0.6831820474029249,0.11509847268437542,6,18.05320621935197,0.010313466929779696,0.02189018950989812,1.0,True,
|
||||
22,27,3.054692363582515,96.43768805753649,0.712325317889966,0.11297739003609018,6,14.570486778570737,0.006473104665270118,0.07627008321159866,1.0,True,
|
||||
23,24,2.6253440212817054,43.26938686599312,0.787546254944494,0.11112536909525574,6,13.667766137918514,0.003984385308883342,0.012875657690538275,1.0,True,
|
||||
23,25,4.30404511575867,133.69140168644446,0.6839640551828883,0.11561487899989795,6,40.22316701306096,0.009663940236368565,0.2587163051792124,1.0,True,
|
||||
23,26,6.580252552934289,159.52196102005493,0.6214811057570377,0.12356096223549605,6,59.18789907295204,0.03346639663911815,0.040359288659068966,1.0,True,
|
||||
23,27,6.261292051742304,128.3854727122061,0.6439154109155375,0.11883308044324468,6,48.252953818047224,0.01363202571969651,0.268813724284828,1.0,True,
|
||||
23,28,10.83543880539923,37.57580831192121,0.5551750380517504,0.13104613123848732,6,44.00847715074386,0.01592501575703795,0.1925228188054137,1.0,True,
|
||||
24,25,2.0155105379126432,90.4220148204513,0.7610619469026548,0.10446971824743242,6,22.29894236104597,0.01721789520038266,0.09266069416139829,1.0,True,
|
||||
24,26,4.344602404326578,157.20865211395216,0.6986700443318556,0.11795525471114342,6,30.354013262701024,0.0239438468814135,0.2700005297192175,1.0,True,
|
||||
24,27,4.194437499675352,171.65485957819877,0.7233746521629142,0.11505806966234582,6,24.218829066684407,0.0071603040929797065,0.1144565168286882,1.0,True,
|
||||
24,28,8.235813904401468,80.84519517791432,0.618522741669834,0.1218895746871147,6,34.55835443937473,0.008956300116613691,0.24319664279778394,1.0,True,
|
||||
24,29,2.7255497122534083,57.0551252080454,0.7635993899339095,0.11147922533242269,6,11.572525725181164,0.02246832420061709,0.4519372330281508,1.0,True,
|
||||
25,26,2.340922525153354,66.78663729350096,0.8176906646563639,0.1076748622762494,6,10.194320056407282,0.011727583881002021,0.04299213457608308,1.0,True,
|
||||
25,27,2.187150933555911,97.92312560134954,0.8508155583437892,0.10177690901936605,6,9.447407214356089,0.004921098494284399,0.04722324529000337,1.0,True,
|
||||
25,28,6.688406442287195,171.26720999836604,0.6552720874701521,0.1158307449607044,6,16.856702471098828,0.010027166778796438,0.069841684352646,1.0,True,
|
||||
25,29,1.411474020521721,147.47714002849685,0.7434442763489663,0.11227320270489505,6,19.207346965238134,0.005744598995027529,0.30413185830684,1.0,True,
|
||||
25,30,0.5324917816481396,118.09183853064393,0.7986111111111112,0.1061010900656728,6,14.995080416656407,0.004651476537899485,0.06337779258524685,1.0,True,
|
||||
26,27,0.6349061963265976,31.136488307848563,0.8835952231301069,0.10164781558403184,6,8.741658683232068,0.0010243697887392141,0.04630474119881922,1.0,True,
|
||||
26,28,5.008419104260455,121.94615270813352,0.7013346764039284,0.1156303654422143,6,10.39669408260953,0.016692399239017493,0.1408617368634082,1.0,True,
|
||||
26,29,2.224643201350574,145.73622267800252,0.7255273462170014,0.11343814488049828,6,14.171918272295303,0.0030631045126193056,0.10499072900350268,1.0,True,
|
||||
26,30,2.6736935848107626,175.1215241758588,0.7273877292852625,0.11178149175643984,6,20.18904245623208,0.002906139936114469,0.06343916895540856,1.0,True,
|
||||
26,31,6.867005915953627,145.82839665297058,0.6099962135554714,0.12348129729633385,6,43.35600938855713,0.011230201352754812,0.02547666380871279,1.0,True,
|
||||
27,28,5.622863551235285,90.80966440028486,0.7116811266188859,0.11387091842867321,6,10.816842555490693,0.011430300098438593,0.17858658887118406,1.0,True,
|
||||
27,29,2.458639091699311,114.59973437015381,0.7588294651866802,0.10898351788466304,6,11.452504275483305,0.002552411031283606,0.096053889700167,1.0,True,
|
||||
27,30,2.614522132166758,143.98503586800658,0.7699595755432036,0.10809432324457112,6,19.384788093934308,0.006419845852952349,0.09683669503892821,1.0,True,
|
||||
27,31,6.500901961576647,114.69190834512194,0.6407864885303756,0.12195597170279295,6,45.71099685307573,0.03203829198983217,0.21314212616538905,1.0,True,
|
||||
27,32,8.204840364723921,69.85846065460369,0.6120722798923491,0.12793570356778294,6,50.652407095458955,0.02476270066797521,0.18953242722457034,1.0,True,
|
||||
28,29,5.5254251571777075,23.790069969868927,0.6751737207833228,0.11816626627973714,6,15.58130572797597,0.0036062245918322845,0.04228106700822467,1.0,True,
|
||||
28,30,6.682895390289165,53.17537146772172,0.6476021763887132,0.12162303323930705,6,19.56745364015458,0.036760870155111036,0.061189777619461184,1.0,True,
|
||||
28,31,11.27338119162288,23.882243944837096,0.558515338972352,0.1324943729180927,6,46.74937334475135,0.011294835537302048,0.22869636591571527,1.0,True,
|
||||
28,32,12.874594603353637,20.951203745681177,0.5356867779204108,0.13793317141107478,6,47.36276875018484,0.009260784922164039,0.4736145985672847,1.0,True,
|
||||
28,33,13.796424177198949,59.567633422321585,0.49968659897204465,0.13776776698165452,6,59.65569689133837,0.03480709552965627,0.1328751318540718,1.0,True,
|
||||
29,30,1.1998189814142899,29.3853014978528,0.8126428027418127,0.11465616151168853,6,9.73882704495904,0.059123063905753705,0.35885997079417975,1.0,True,
|
||||
29,31,5.755712819539279,0.09217397496817598,0.7178318135764944,0.11831514282300001,6,19.572443598172608,0.03055388488044922,0.16788276176822323,1.0,True,
|
||||
29,32,7.3497160995478215,44.7412737155501,0.6866709594333548,0.12344589693451308,6,19.69654687402097,0.01847461637742171,0.10764991453418553,1.0,True,
|
||||
29,33,8.359102419083325,83.35770339219052,0.6151911468812877,0.12607024585552623,6,19.321252328369155,0.005640558537625966,0.060018922799470055,1.0,True,
|
||||
29,34,8.008340805017072,133.40261903862776,0.6489454636216149,0.1229782406905996,6,19.361211916752247,0.024951598645873194,0.20519725330906424,1.0,True,
|
||||
30,31,4.592128513798009,29.29312752288462,0.76103500761035,0.11418966560335815,6,16.756236396505788,0.010805499433410783,0.0764328342532002,1.0,True,
|
||||
30,32,6.2188831730504885,74.12657521340287,0.7051463949438926,0.11749249261595905,6,20.554466965332146,0.009422682577629085,0.037900377279079585,1.0,True,
|
||||
30,33,7.344850966128434,112.7430048900433,0.6064231738035264,0.12368925274094972,6,20.291143104650924,0.0147669225762263,0.09037435316445036,1.0,True,
|
||||
30,34,7.0347870411064095,162.78792053648075,0.6427946506686664,0.12352381867411776,6,21.47516866844496,3.1821548176867474,2.4099911828321603,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
30,35,9.281470813161304,166.02816439171798,0.690784364483562,0.12252550482763978,6,16.634587270964886,0.009824737708452339,0.14818916841264132,1.0,True,
|
||||
31,32,1.7043840283934533,44.83344769051828,0.7959078625659504,0.10965414855273443,6,12.882067962817517,0.009608345699366565,0.04073745185785107,1.0,True,
|
||||
31,33,3.3472002644811774,83.44987736715868,0.636089469716009,0.11810494837446742,6,13.701242758091343,0.01055590691460776,0.06271370554561609,1.0,True,
|
||||
31,34,3.311630046755356,133.49479301359594,0.6599326599326599,0.11513164519765848,6,15.199690471396346,0.002580333972310516,0.03476275200242535,1.0,True,
|
||||
31,35,4.805055361756335,164.67870808539848,0.7747933884297521,0.1099315236542533,6,13.983106845566315,0.002969158365974405,0.02482993261597952,1.0,True,
|
||||
31,36,5.921091773191984,169.88970913698103,0.79326799071447,0.11165900641583826,6,12.181844458808802,0.004151989866605975,0.032942374858786365,1.0,True,
|
||||
32,33,1.9394369188755196,38.61642967664041,0.7479238533282229,0.1140254726045761,6,12.862133545437658,0.005854383016193307,0.042356141718508426,1.0,True,
|
||||
32,34,2.1435446082722334,88.6613453230776,0.7426470588235294,0.11481351455228676,6,15.077486603426832,0.0025843844761028077,0.009064526838353553,1.0,True,
|
||||
32,35,3.1027737257249126,119.84526039487966,0.8209641402863523,0.10635864973089924,6,10.914008667831691,0.004447738370013908,0.11616784364821804,1.0,True,
|
||||
32,36,4.235311686227635,145.27684317250132,0.7613800341073068,0.10898930843612145,6,12.22686011069426,0.008302372052489307,0.047221847858387886,1.0,True,
|
||||
32,37,3.1931475050378357,118.78221388859481,0.6919923126201153,0.11497666348297753,6,16.91846428853567,0.00567166990415677,0.10190182649448634,1.0,True,
|
||||
33,34,0.5704890065278277,50.0449156464372,0.8247678018575851,0.10774292174748101,6,13.307399451848953,0.005218203824516851,0.054034410494683056,1.0,True,
|
||||
33,35,2.3843314966611664,81.22883071823928,0.7097435897435898,0.11216257584808116,6,12.920219167499932,0.0055472051152683725,0.06161504272529624,1.0,True,
|
||||
33,36,3.5605240220038255,106.66041349586102,0.62202304737516,0.11282494351568334,6,18.491230751066993,0.009207361872581905,0.07430583689659953,1.0,True,
|
||||
33,37,4.013080941901569,80.16578421195443,0.6328828828828829,0.12540757119118434,6,17.018187124475837,0.0036940550280262596,0.12927083795080543,1.0,True,
|
||||
34,35,2.9403442697824866,31.183915071802076,0.7146673451214858,0.11428688542425412,6,13.705016766266674,0.013436446539858737,0.05510481554154184,1.0,True,
|
||||
34,36,4.102710388907318,56.61549784942384,0.6465506288908652,0.11729490897001751,6,19.754334744118587,0.086056763787067,1.0836879727751445,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
34,37,3.6865711881345757,30.12086856551724,0.6899292189246243,0.11937420197560855,6,16.808577408839643,0.005083360744530967,0.10024940937419655,1.0,True,
|
||||
35,36,1.2045561534352058,25.431582777621774,0.8242496050552922,0.10907898456971946,6,9.660062538057792,0.0026096461304541156,0.07595057929078583,1.0,True,
|
||||
35,37,6.084062615492111,1.063046506284832,0.6924659295448702,0.12103303062396852,6,14.36240888115935,0.005914867863779407,0.06573359298031699,1.0,True,
|
||||
36,37,7.2781244258509386,26.494629283906598,0.6402157164869029,0.12413886977390769,6,20.08544982146282,0.008397869289519334,0.08479281990151419,1.0,True,
|
||||
|
File diff suppressed because it is too large
Load Diff
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -1,277 +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": 36,
|
||||
"translation_m": {
|
||||
"rms": 0.07402020402512643,
|
||||
"median": 0.05314343925871579,
|
||||
"p90": 0.1272969635290951,
|
||||
"p95": 0.14147696290683623,
|
||||
"max": 0.15743855349684896
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.1492603033421918,
|
||||
"median": 0.8694109171568597,
|
||||
"p90": 1.8257616608147966,
|
||||
"p95": 1.9151501783565152,
|
||||
"max": 2.3872345295181554
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.026175149113866782,
|
||||
"rotation_deg": 0.6391104344821068,
|
||||
"i": 0,
|
||||
"j": 1
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.05399080656776795,
|
||||
"rotation_deg": 0.7503462279599024,
|
||||
"i": 0,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.05685955205113222,
|
||||
"rotation_deg": 1.7528510918025244,
|
||||
"i": 1,
|
||||
"j": 2
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.04386400899992785,
|
||||
"rotation_deg": 1.8467495197498804,
|
||||
"i": 2,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.05626077366726556,
|
||||
"rotation_deg": 0.8079113372879332,
|
||||
"i": 2,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.09087008636486496,
|
||||
"rotation_deg": 1.026687291127638,
|
||||
"i": 3,
|
||||
"j": 4
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.011511013157318718,
|
||||
"rotation_deg": 0.19484251468537728,
|
||||
"i": 3,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.07294571613929493,
|
||||
"rotation_deg": 0.8223795285339977,
|
||||
"i": 4,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.03068183988034665,
|
||||
"rotation_deg": 0.28683040898343787,
|
||||
"i": 5,
|
||||
"j": 6
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.04275223212349703,
|
||||
"rotation_deg": 0.701351776008062,
|
||||
"i": 6,
|
||||
"j": 7
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.04932770426469867,
|
||||
"rotation_deg": 0.6710714316916605,
|
||||
"i": 6,
|
||||
"j": 9
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.04132200915104361,
|
||||
"rotation_deg": 1.0591594894230583,
|
||||
"i": 11,
|
||||
"j": 12
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.05457596514011665,
|
||||
"rotation_deg": 1.9025587382160285,
|
||||
"i": 16,
|
||||
"j": 17
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.1444364884565483,
|
||||
"rotation_deg": 0.878734531814174,
|
||||
"i": 17,
|
||||
"j": 18
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.08666942413173652,
|
||||
"rotation_deg": 0.8560485527931129,
|
||||
"i": 18,
|
||||
"j": 19
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.1401188917021612,
|
||||
"rotation_deg": 1.596129012793476,
|
||||
"i": 18,
|
||||
"j": 21
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.015218472185905801,
|
||||
"rotation_deg": 1.3639536065690272,
|
||||
"i": 20,
|
||||
"j": 21
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.044288628737202185,
|
||||
"rotation_deg": 0.24234091190374474,
|
||||
"i": 20,
|
||||
"j": 22
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.041873719472882466,
|
||||
"rotation_deg": 0.8630111615428633,
|
||||
"i": 20,
|
||||
"j": 23
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.04766076872791419,
|
||||
"rotation_deg": 1.592170300445217,
|
||||
"i": 21,
|
||||
"j": 22
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.06407386924590294,
|
||||
"rotation_deg": 0.6604716233222132,
|
||||
"i": 21,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.04694855582296258,
|
||||
"rotation_deg": 0.49059371565968823,
|
||||
"i": 22,
|
||||
"j": 23
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.01699246526900484,
|
||||
"rotation_deg": 1.4341910287168735,
|
||||
"i": 22,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.05229607194966363,
|
||||
"rotation_deg": 1.120879367830044,
|
||||
"i": 23,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.020810043376301643,
|
||||
"rotation_deg": 0.5009109191051434,
|
||||
"i": 24,
|
||||
"j": 25
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.043831190745566344,
|
||||
"rotation_deg": 0.47571695708233286,
|
||||
"i": 24,
|
||||
"j": 26
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.11447503535602893,
|
||||
"rotation_deg": 0.9257799651614966,
|
||||
"i": 25,
|
||||
"j": 26
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.061431194585866364,
|
||||
"rotation_deg": 1.8047738018797126,
|
||||
"i": 27,
|
||||
"j": 28
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.09966331456020154,
|
||||
"rotation_deg": 1.9529244987779755,
|
||||
"i": 27,
|
||||
"j": 29
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.07785577496744421,
|
||||
"rotation_deg": 0.5145378388092755,
|
||||
"i": 28,
|
||||
"j": 29
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.14049045439026553,
|
||||
"rotation_deg": 0.83303218231006,
|
||||
"i": 30,
|
||||
"j": 31
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.11441845590035929,
|
||||
"rotation_deg": 0.8758106727708561,
|
||||
"i": 30,
|
||||
"j": 33
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.15743855349684896,
|
||||
"rotation_deg": 0.5956528317274336,
|
||||
"i": 33,
|
||||
"j": 35
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.05714959629699741,
|
||||
"rotation_deg": 0.9388591914539489,
|
||||
"i": 34,
|
||||
"j": 35
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.019561513757857227,
|
||||
"rotation_deg": 2.3872345295181554,
|
||||
"i": 34,
|
||||
"j": 37
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.04089150700945596,
|
||||
"rotation_deg": 1.264595889658635,
|
||||
"i": 35,
|
||||
"j": 36
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,974 +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.2966739731400179,
|
||||
-0.0011818594987371798,
|
||||
0.7180500518063239
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.6862002617473892,
|
||||
1.312299105350799,
|
||||
-0.8320898359211147
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.005904487870209512,
|
||||
0.0114946916809237,
|
||||
-0.007192110236616905,
|
||||
0.9998906353379331
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9996322912270097,
|
||||
0.014246906812619035,
|
||||
0.02307180059101153,
|
||||
1.2966739731400179
|
||||
],
|
||||
[
|
||||
-0.014518387883026281,
|
||||
0.9998268211466698,
|
||||
0.011642342076367565,
|
||||
-0.0011818594987371798
|
||||
],
|
||||
[
|
||||
-0.02290193768039828,
|
||||
-0.011973026435188067,
|
||||
0.9996660181723022,
|
||||
0.7180500518063239
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 38,
|
||||
"pairs": 154,
|
||||
"residuals": {
|
||||
"pairs": 154,
|
||||
"translation_m": {
|
||||
"rms": 0.20169832880489094,
|
||||
"median": 0.07246524583191469,
|
||||
"p90": 0.2753897404824865,
|
||||
"p95": 0.4278425490718384,
|
||||
"max": 0.9413768077277793
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.6476271943239644,
|
||||
"median": 0.9814049617994354,
|
||||
"p90": 2.2430975893180514,
|
||||
"p95": 3.6234061615039397,
|
||||
"max": 5.555959014778186
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.09160335763349035,
|
||||
"rotation_deg": 0.41678911395678336
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.17342241098897876,
|
||||
"rotation_deg": 0.6069352741159756
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.15362597338796116,
|
||||
"rotation_deg": 0.5006592067754067
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.10307796882842468,
|
||||
"rotation_deg": 0.7631958047896495
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.034854178949377916,
|
||||
"rotation_deg": 0.7924566962483651
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.08229579737538087,
|
||||
"rotation_deg": 0.5218956920191586
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.07756178891403459,
|
||||
"rotation_deg": 0.8901066576751191
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.00515582594404776,
|
||||
"rotation_deg": 1.1480686598732328
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.11902969080451287,
|
||||
"rotation_deg": 0.7250267588731203
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.02041753358033973,
|
||||
"rotation_deg": 0.42666891550326913
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.05329811381948769,
|
||||
"rotation_deg": 1.3070274389810175
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.07255710332913211,
|
||||
"rotation_deg": 1.7485586508142046
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.12913362654704053,
|
||||
"rotation_deg": 1.0437199550859728
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.2784531278023029,
|
||||
"rotation_deg": 0.8165170023642168
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.0583276394617015,
|
||||
"rotation_deg": 0.9536825261286082
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.07766573568009734,
|
||||
"rotation_deg": 1.3734118987942618
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.08519120540755043,
|
||||
"rotation_deg": 1.5504754382194925
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.2392325556096795,
|
||||
"rotation_deg": 0.9306633614951502
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.02049661768665185,
|
||||
"rotation_deg": 0.5434732565644916
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.10141834924376554,
|
||||
"rotation_deg": 0.6801110152223822
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.1927601584704251,
|
||||
"rotation_deg": 0.7704160771388004
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.07237338833469728,
|
||||
"rotation_deg": 1.13355234535889
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.2380321686720521,
|
||||
"rotation_deg": 0.6543633179461373
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.022956329719636855,
|
||||
"rotation_deg": 0.34448651373739575
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.17365496513981085,
|
||||
"rotation_deg": 0.5919072614412411
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.26972044137209944,
|
||||
"rotation_deg": 0.394408649127183
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.16479297108025018,
|
||||
"rotation_deg": 0.8933837591188238
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.029173826915557283,
|
||||
"rotation_deg": 0.9823009311305465
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.16558352679496174,
|
||||
"rotation_deg": 1.6354580097985452
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.27142725327985545,
|
||||
"rotation_deg": 1.6832228760069097
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.03804497310320746,
|
||||
"rotation_deg": 0.6214699590261354
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.00988217602887705,
|
||||
"rotation_deg": 0.9654320545457507
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.20932389516970148,
|
||||
"rotation_deg": 1.235263404784913
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.23596844902340763,
|
||||
"rotation_deg": 0.9742253618504498
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.04810773998382127,
|
||||
"rotation_deg": 0.40079885061586873
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.1439097271676289,
|
||||
"rotation_deg": 0.5833810515945358
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.2770879492836139,
|
||||
"rotation_deg": 1.7217009121491587
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.2635733716233143,
|
||||
"rotation_deg": 0.959386216598219
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.30076236456889854,
|
||||
"rotation_deg": 0.5341538159367174
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.08917523492271737,
|
||||
"rotation_deg": 0.752253926704042
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.2283689383450832,
|
||||
"rotation_deg": 1.7644222410834363
|
||||
},
|
||||
{
|
||||
"pair_index": 41,
|
||||
"translation_m": 0.21162807555337518,
|
||||
"rotation_deg": 0.893481051160724
|
||||
},
|
||||
{
|
||||
"pair_index": 42,
|
||||
"translation_m": 0.14042093106101866,
|
||||
"rotation_deg": 1.2907294157599642
|
||||
},
|
||||
{
|
||||
"pair_index": 43,
|
||||
"translation_m": 0.1691423878395439,
|
||||
"rotation_deg": 2.377216518666257
|
||||
},
|
||||
{
|
||||
"pair_index": 44,
|
||||
"translation_m": 0.1169779798191509,
|
||||
"rotation_deg": 1.3815961809053516
|
||||
},
|
||||
{
|
||||
"pair_index": 45,
|
||||
"translation_m": 0.1387832440968286,
|
||||
"rotation_deg": 1.5947217176166877
|
||||
},
|
||||
{
|
||||
"pair_index": 46,
|
||||
"translation_m": 0.030062828340300884,
|
||||
"rotation_deg": 1.0704098135809084
|
||||
},
|
||||
{
|
||||
"pair_index": 47,
|
||||
"translation_m": 0.0680455433006411,
|
||||
"rotation_deg": 1.973549636369647
|
||||
},
|
||||
{
|
||||
"pair_index": 48,
|
||||
"translation_m": 0.031870236699333414,
|
||||
"rotation_deg": 2.4723290530697546
|
||||
},
|
||||
{
|
||||
"pair_index": 49,
|
||||
"translation_m": 0.38250571101706576,
|
||||
"rotation_deg": 1.1600905208267944
|
||||
},
|
||||
{
|
||||
"pair_index": 50,
|
||||
"translation_m": 0.06098547660886484,
|
||||
"rotation_deg": 1.5193006464833663
|
||||
},
|
||||
{
|
||||
"pair_index": 51,
|
||||
"translation_m": 0.020049847803449816,
|
||||
"rotation_deg": 2.0752028054797114
|
||||
},
|
||||
{
|
||||
"pair_index": 52,
|
||||
"translation_m": 0.14976699977401936,
|
||||
"rotation_deg": 0.9032263412060769
|
||||
},
|
||||
{
|
||||
"pair_index": 53,
|
||||
"translation_m": 0.38104623295208556,
|
||||
"rotation_deg": 0.6420289897992424
|
||||
},
|
||||
{
|
||||
"pair_index": 54,
|
||||
"translation_m": 0.6145427269990851,
|
||||
"rotation_deg": 4.320949352504073
|
||||
},
|
||||
{
|
||||
"pair_index": 55,
|
||||
"translation_m": 0.0644413592299437,
|
||||
"rotation_deg": 2.803369589869878
|
||||
},
|
||||
{
|
||||
"pair_index": 56,
|
||||
"translation_m": 0.18425354638764108,
|
||||
"rotation_deg": 0.6171163774519929
|
||||
},
|
||||
{
|
||||
"pair_index": 57,
|
||||
"translation_m": 0.3120883490259661,
|
||||
"rotation_deg": 0.4259486202878496
|
||||
},
|
||||
{
|
||||
"pair_index": 58,
|
||||
"translation_m": 0.418016177718232,
|
||||
"rotation_deg": 5.044083647712121
|
||||
},
|
||||
{
|
||||
"pair_index": 59,
|
||||
"translation_m": 0.07897058451306387,
|
||||
"rotation_deg": 2.708937676342093
|
||||
},
|
||||
{
|
||||
"pair_index": 60,
|
||||
"translation_m": 0.20725234914942078,
|
||||
"rotation_deg": 1.3373510925222083
|
||||
},
|
||||
{
|
||||
"pair_index": 61,
|
||||
"translation_m": 0.5028731993983374,
|
||||
"rotation_deg": 2.560781611577359
|
||||
},
|
||||
{
|
||||
"pair_index": 62,
|
||||
"translation_m": 0.324052710994222,
|
||||
"rotation_deg": 3.4637911605755742
|
||||
},
|
||||
{
|
||||
"pair_index": 63,
|
||||
"translation_m": 0.16594327616277665,
|
||||
"rotation_deg": 1.096300100186063
|
||||
},
|
||||
{
|
||||
"pair_index": 64,
|
||||
"translation_m": 0.9413768077277793,
|
||||
"rotation_deg": 4.623227467412829
|
||||
},
|
||||
{
|
||||
"pair_index": 65,
|
||||
"translation_m": 0.7475763787765258,
|
||||
"rotation_deg": 5.251990934487642
|
||||
},
|
||||
{
|
||||
"pair_index": 66,
|
||||
"translation_m": 0.44609152444282213,
|
||||
"rotation_deg": 5.555959014778186
|
||||
},
|
||||
{
|
||||
"pair_index": 67,
|
||||
"translation_m": 0.8772363359544111,
|
||||
"rotation_deg": 3.919834020370911
|
||||
},
|
||||
{
|
||||
"pair_index": 68,
|
||||
"translation_m": 0.7256840115352385,
|
||||
"rotation_deg": 4.7791807757364335
|
||||
},
|
||||
{
|
||||
"pair_index": 69,
|
||||
"translation_m": 0.5540111498360015,
|
||||
"rotation_deg": 5.404862742811467
|
||||
},
|
||||
{
|
||||
"pair_index": 70,
|
||||
"translation_m": 0.05427710319100646,
|
||||
"rotation_deg": 0.47262999225353286
|
||||
},
|
||||
{
|
||||
"pair_index": 71,
|
||||
"translation_m": 0.03494523113943391,
|
||||
"rotation_deg": 1.5163417215367592
|
||||
},
|
||||
{
|
||||
"pair_index": 72,
|
||||
"translation_m": 0.04639564426856145,
|
||||
"rotation_deg": 1.9306722358716086
|
||||
},
|
||||
{
|
||||
"pair_index": 73,
|
||||
"translation_m": 0.031597675421818384,
|
||||
"rotation_deg": 2.0321950625908634
|
||||
},
|
||||
{
|
||||
"pair_index": 74,
|
||||
"translation_m": 0.06688972458549529,
|
||||
"rotation_deg": 1.0363014892153515
|
||||
},
|
||||
{
|
||||
"pair_index": 75,
|
||||
"translation_m": 0.07495539042883581,
|
||||
"rotation_deg": 0.879604047232473
|
||||
},
|
||||
{
|
||||
"pair_index": 76,
|
||||
"translation_m": 0.07523266019202682,
|
||||
"rotation_deg": 0.6051319351741334
|
||||
},
|
||||
{
|
||||
"pair_index": 77,
|
||||
"translation_m": 0.07099230127095138,
|
||||
"rotation_deg": 1.202238808395327
|
||||
},
|
||||
{
|
||||
"pair_index": 78,
|
||||
"translation_m": 0.06557199988165952,
|
||||
"rotation_deg": 0.6765649678327575
|
||||
},
|
||||
{
|
||||
"pair_index": 79,
|
||||
"translation_m": 0.12154293847124566,
|
||||
"rotation_deg": 0.7325970122680316
|
||||
},
|
||||
{
|
||||
"pair_index": 80,
|
||||
"translation_m": 0.17982574134407248,
|
||||
"rotation_deg": 1.5473661961917022
|
||||
},
|
||||
{
|
||||
"pair_index": 81,
|
||||
"translation_m": 0.04462726734001729,
|
||||
"rotation_deg": 0.6937634461069877
|
||||
},
|
||||
{
|
||||
"pair_index": 82,
|
||||
"translation_m": 0.0613023995980599,
|
||||
"rotation_deg": 0.8119788473937047
|
||||
},
|
||||
{
|
||||
"pair_index": 83,
|
||||
"translation_m": 0.08503096310423741,
|
||||
"rotation_deg": 1.4444141387028644
|
||||
},
|
||||
{
|
||||
"pair_index": 84,
|
||||
"translation_m": 0.07464123857051043,
|
||||
"rotation_deg": 0.7030897752540571
|
||||
},
|
||||
{
|
||||
"pair_index": 85,
|
||||
"translation_m": 0.029868184350228942,
|
||||
"rotation_deg": 0.9135621919993453
|
||||
},
|
||||
{
|
||||
"pair_index": 86,
|
||||
"translation_m": 0.05823954193987833,
|
||||
"rotation_deg": 0.5917544307877486
|
||||
},
|
||||
{
|
||||
"pair_index": 87,
|
||||
"translation_m": 0.0551313637068964,
|
||||
"rotation_deg": 1.4516548104557863
|
||||
},
|
||||
{
|
||||
"pair_index": 88,
|
||||
"translation_m": 0.09827472129882013,
|
||||
"rotation_deg": 0.42188472344008116
|
||||
},
|
||||
{
|
||||
"pair_index": 89,
|
||||
"translation_m": 0.044245224544806644,
|
||||
"rotation_deg": 1.0004748409346111
|
||||
},
|
||||
{
|
||||
"pair_index": 90,
|
||||
"translation_m": 0.018832180352354084,
|
||||
"rotation_deg": 1.51099458595063
|
||||
},
|
||||
{
|
||||
"pair_index": 91,
|
||||
"translation_m": 0.020467010735194163,
|
||||
"rotation_deg": 1.7676261200773673
|
||||
},
|
||||
{
|
||||
"pair_index": 92,
|
||||
"translation_m": 0.0262975474700301,
|
||||
"rotation_deg": 0.8325617740533534
|
||||
},
|
||||
{
|
||||
"pair_index": 93,
|
||||
"translation_m": 0.04446844149309418,
|
||||
"rotation_deg": 1.5739586103283063
|
||||
},
|
||||
{
|
||||
"pair_index": 94,
|
||||
"translation_m": 0.01467330858160615,
|
||||
"rotation_deg": 1.2850817812632396
|
||||
},
|
||||
{
|
||||
"pair_index": 95,
|
||||
"translation_m": 0.06870746700120084,
|
||||
"rotation_deg": 1.7240767243237687
|
||||
},
|
||||
{
|
||||
"pair_index": 96,
|
||||
"translation_m": 0.05666264876403947,
|
||||
"rotation_deg": 1.0206213519210603
|
||||
},
|
||||
{
|
||||
"pair_index": 97,
|
||||
"translation_m": 0.07342527442704463,
|
||||
"rotation_deg": 0.7244552191419976
|
||||
},
|
||||
{
|
||||
"pair_index": 98,
|
||||
"translation_m": 0.054266286757252646,
|
||||
"rotation_deg": 0.4989636190115999
|
||||
},
|
||||
{
|
||||
"pair_index": 99,
|
||||
"translation_m": 0.047207658172525445,
|
||||
"rotation_deg": 0.23696494205729182
|
||||
},
|
||||
{
|
||||
"pair_index": 100,
|
||||
"translation_m": 0.11572080515326105,
|
||||
"rotation_deg": 1.7515941247741897
|
||||
},
|
||||
{
|
||||
"pair_index": 101,
|
||||
"translation_m": 0.06407086244008413,
|
||||
"rotation_deg": 0.46899277183017407
|
||||
},
|
||||
{
|
||||
"pair_index": 102,
|
||||
"translation_m": 0.15305365572855853,
|
||||
"rotation_deg": 0.9523959443915215
|
||||
},
|
||||
{
|
||||
"pair_index": 103,
|
||||
"translation_m": 0.05122753080254304,
|
||||
"rotation_deg": 1.3098743656013143
|
||||
},
|
||||
{
|
||||
"pair_index": 104,
|
||||
"translation_m": 0.0946533848328183,
|
||||
"rotation_deg": 0.7612204831612559
|
||||
},
|
||||
{
|
||||
"pair_index": 105,
|
||||
"translation_m": 0.07481645106107998,
|
||||
"rotation_deg": 0.7174903795296853
|
||||
},
|
||||
{
|
||||
"pair_index": 106,
|
||||
"translation_m": 0.07931651617250869,
|
||||
"rotation_deg": 0.3160868091969118
|
||||
},
|
||||
{
|
||||
"pair_index": 107,
|
||||
"translation_m": 0.0656383441135373,
|
||||
"rotation_deg": 0.7904846161787125
|
||||
},
|
||||
{
|
||||
"pair_index": 108,
|
||||
"translation_m": 0.03241931291657944,
|
||||
"rotation_deg": 1.535299757082846
|
||||
},
|
||||
{
|
||||
"pair_index": 109,
|
||||
"translation_m": 0.06234488990038783,
|
||||
"rotation_deg": 1.5874303211040584
|
||||
},
|
||||
{
|
||||
"pair_index": 110,
|
||||
"translation_m": 0.03506692454461985,
|
||||
"rotation_deg": 0.25002484619548354
|
||||
},
|
||||
{
|
||||
"pair_index": 111,
|
||||
"translation_m": 0.04125822298839504,
|
||||
"rotation_deg": 0.8720293199565927
|
||||
},
|
||||
{
|
||||
"pair_index": 112,
|
||||
"translation_m": 0.051725253480973364,
|
||||
"rotation_deg": 1.5059976851542656
|
||||
},
|
||||
{
|
||||
"pair_index": 113,
|
||||
"translation_m": 0.025726583767115428,
|
||||
"rotation_deg": 1.5673808264017624
|
||||
},
|
||||
{
|
||||
"pair_index": 114,
|
||||
"translation_m": 0.027511376068390752,
|
||||
"rotation_deg": 0.719385406633602
|
||||
},
|
||||
{
|
||||
"pair_index": 115,
|
||||
"translation_m": 0.04557229476188737,
|
||||
"rotation_deg": 0.8566964152790454
|
||||
},
|
||||
{
|
||||
"pair_index": 116,
|
||||
"translation_m": 0.1301226621807897,
|
||||
"rotation_deg": 1.9490463203564985
|
||||
},
|
||||
{
|
||||
"pair_index": 117,
|
||||
"translation_m": 0.08472149919727921,
|
||||
"rotation_deg": 1.4400759566709866
|
||||
},
|
||||
{
|
||||
"pair_index": 118,
|
||||
"translation_m": 0.03305331754412625,
|
||||
"rotation_deg": 1.5282125278120453
|
||||
},
|
||||
{
|
||||
"pair_index": 119,
|
||||
"translation_m": 0.06943760420453976,
|
||||
"rotation_deg": 0.14382473264645845
|
||||
},
|
||||
{
|
||||
"pair_index": 120,
|
||||
"translation_m": 0.1387812176775449,
|
||||
"rotation_deg": 0.9702105493011807
|
||||
},
|
||||
{
|
||||
"pair_index": 121,
|
||||
"translation_m": 0.050472774524194526,
|
||||
"rotation_deg": 0.7112996871542888
|
||||
},
|
||||
{
|
||||
"pair_index": 122,
|
||||
"translation_m": 0.040960987456295526,
|
||||
"rotation_deg": 0.8626492437182947
|
||||
},
|
||||
{
|
||||
"pair_index": 123,
|
||||
"translation_m": 0.1183520030349349,
|
||||
"rotation_deg": 1.9037016305375127
|
||||
},
|
||||
{
|
||||
"pair_index": 124,
|
||||
"translation_m": 0.13482286998475895,
|
||||
"rotation_deg": 1.1180532382465007
|
||||
},
|
||||
{
|
||||
"pair_index": 125,
|
||||
"translation_m": 0.08919768111421818,
|
||||
"rotation_deg": 0.7424305858785966
|
||||
},
|
||||
{
|
||||
"pair_index": 126,
|
||||
"translation_m": 0.03295290503021982,
|
||||
"rotation_deg": 0.8748115525182623
|
||||
},
|
||||
{
|
||||
"pair_index": 127,
|
||||
"translation_m": 0.07050270871439886,
|
||||
"rotation_deg": 2.315052496677334
|
||||
},
|
||||
{
|
||||
"pair_index": 128,
|
||||
"translation_m": 0.08399613707521804,
|
||||
"rotation_deg": 1.3430883907512923
|
||||
},
|
||||
{
|
||||
"pair_index": 129,
|
||||
"translation_m": 0.07635426719286566,
|
||||
"rotation_deg": 0.28938877097974597
|
||||
},
|
||||
{
|
||||
"pair_index": 130,
|
||||
"translation_m": 0.08599980484168654,
|
||||
"rotation_deg": 0.3232792746340014
|
||||
},
|
||||
{
|
||||
"pair_index": 131,
|
||||
"translation_m": 0.08546417597482137,
|
||||
"rotation_deg": 1.3709646380399854
|
||||
},
|
||||
{
|
||||
"pair_index": 132,
|
||||
"translation_m": 0.0586834782862356,
|
||||
"rotation_deg": 0.6222764869215247
|
||||
},
|
||||
{
|
||||
"pair_index": 133,
|
||||
"translation_m": 0.06297920233905209,
|
||||
"rotation_deg": 1.0415437811293622
|
||||
},
|
||||
{
|
||||
"pair_index": 134,
|
||||
"translation_m": 0.0421890137454461,
|
||||
"rotation_deg": 0.6732808756041617
|
||||
},
|
||||
{
|
||||
"pair_index": 135,
|
||||
"translation_m": 0.03384375753290982,
|
||||
"rotation_deg": 0.5098352623359285
|
||||
},
|
||||
{
|
||||
"pair_index": 136,
|
||||
"translation_m": 0.06075109086084829,
|
||||
"rotation_deg": 1.672042116085701
|
||||
},
|
||||
{
|
||||
"pair_index": 137,
|
||||
"translation_m": 0.1123184567166834,
|
||||
"rotation_deg": 1.9991157673182287
|
||||
},
|
||||
{
|
||||
"pair_index": 138,
|
||||
"translation_m": 0.06975029753182684,
|
||||
"rotation_deg": 1.0328391327124684
|
||||
},
|
||||
{
|
||||
"pair_index": 139,
|
||||
"translation_m": 0.04075826846987665,
|
||||
"rotation_deg": 0.4510416388715733
|
||||
},
|
||||
{
|
||||
"pair_index": 140,
|
||||
"translation_m": 0.028766437036440082,
|
||||
"rotation_deg": 1.1094878762620817
|
||||
},
|
||||
{
|
||||
"pair_index": 141,
|
||||
"translation_m": 0.038764927354221934,
|
||||
"rotation_deg": 1.0013714467514025
|
||||
},
|
||||
{
|
||||
"pair_index": 142,
|
||||
"translation_m": 0.04136840051779099,
|
||||
"rotation_deg": 0.3534868458036239
|
||||
},
|
||||
{
|
||||
"pair_index": 143,
|
||||
"translation_m": 0.07880312075185329,
|
||||
"rotation_deg": 1.224811122150423
|
||||
},
|
||||
{
|
||||
"pair_index": 144,
|
||||
"translation_m": 0.022505079629904813,
|
||||
"rotation_deg": 1.0872300390191383
|
||||
},
|
||||
{
|
||||
"pair_index": 145,
|
||||
"translation_m": 0.013450514823594537,
|
||||
"rotation_deg": 0.271332255193449
|
||||
},
|
||||
{
|
||||
"pair_index": 146,
|
||||
"translation_m": 0.05540964657988301,
|
||||
"rotation_deg": 0.9805089924683242
|
||||
},
|
||||
{
|
||||
"pair_index": 147,
|
||||
"translation_m": 0.048893074003320006,
|
||||
"rotation_deg": 2.060862589786143
|
||||
},
|
||||
{
|
||||
"pair_index": 148,
|
||||
"translation_m": 0.021147862897351464,
|
||||
"rotation_deg": 0.5068012746071331
|
||||
},
|
||||
{
|
||||
"pair_index": 149,
|
||||
"translation_m": 0.04215476698638024,
|
||||
"rotation_deg": 1.1176937562359575
|
||||
},
|
||||
{
|
||||
"pair_index": 150,
|
||||
"translation_m": 0.01586414732810156,
|
||||
"rotation_deg": 0.5375200318464232
|
||||
},
|
||||
{
|
||||
"pair_index": 151,
|
||||
"translation_m": 0.012304155965152195,
|
||||
"rotation_deg": 0.6912196843126612
|
||||
},
|
||||
{
|
||||
"pair_index": 152,
|
||||
"translation_m": 0.03401108751100478,
|
||||
"rotation_deg": 0.5348561807666571
|
||||
},
|
||||
{
|
||||
"pair_index": 153,
|
||||
"translation_m": 0.0382087841988423,
|
||||
"rotation_deg": 2.5751575774124302
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"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.003426069786439831,
|
||||
0.003661463644230861,
|
||||
0.005894947490840618
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.039743045073798305,
|
||||
0.03697641596170036,
|
||||
0.059566156250294475
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 9.240412309046407,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 986.8831673974076,
|
||||
"success": true,
|
||||
"translation_m": 1.781114124910946e-10,
|
||||
"rotation_deg": 1.5745193455937713e-09
|
||||
},
|
||||
{
|
||||
"cost": 986.883167397408,
|
||||
"success": true,
|
||||
"translation_m": 5.536606008158485e-10,
|
||||
"rotation_deg": 2.217262412976666e-08
|
||||
},
|
||||
{
|
||||
"cost": 986.883167397408,
|
||||
"success": true,
|
||||
"translation_m": 5.220783796487502e-10,
|
||||
"rotation_deg": 8.362101120716713e-09
|
||||
},
|
||||
{
|
||||
"cost": 986.883167397408,
|
||||
"success": true,
|
||||
"translation_m": 3.678710827203267e-10,
|
||||
"rotation_deg": 2.260153530135103e-08
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974075,
|
||||
"success": true,
|
||||
"translation_m": 1.7534776205532575e-10,
|
||||
"rotation_deg": 3.2670580143957554e-09
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974074,
|
||||
"success": true,
|
||||
"translation_m": 3.469446951953614e-18,
|
||||
"rotation_deg": 0.0
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974096,
|
||||
"success": true,
|
||||
"translation_m": 1.5270516044070511e-09,
|
||||
"rotation_deg": 7.02719130732361e-08
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974074,
|
||||
"success": true,
|
||||
"translation_m": 1.2027149370934288e-10,
|
||||
"rotation_deg": 3.250857426548915e-09
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974076,
|
||||
"success": true,
|
||||
"translation_m": 1.3743999906846494e-10,
|
||||
"rotation_deg": 3.478167640260992e-09
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974076,
|
||||
"success": true,
|
||||
"translation_m": 7.561012126834818e-11,
|
||||
"rotation_deg": 3.398361215816758e-09
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974076,
|
||||
"success": true,
|
||||
"translation_m": 1.5936145552789374e-10,
|
||||
"rotation_deg": 3.5265475806436176e-09
|
||||
},
|
||||
{
|
||||
"cost": 986.8831673974076,
|
||||
"success": true,
|
||||
"translation_m": 4.3707227819079805e-10,
|
||||
"rotation_deg": 7.230578566209689e-09
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.0016135672080709418,
|
||||
0.001712659789326026,
|
||||
0.0010626181065453576,
|
||||
0.04873173241478149,
|
||||
0.04696015710280561,
|
||||
0.05195409090162765
|
||||
],
|
||||
"p025": [
|
||||
1.293581085997522,
|
||||
-0.00451064016109812,
|
||||
0.7160005593474846,
|
||||
-0.782142502478161,
|
||||
1.2412102159858718,
|
||||
-0.9139767554692944
|
||||
],
|
||||
"p975": [
|
||||
1.2994255903325156,
|
||||
0.0023253494861594877,
|
||||
0.7196644226938009,
|
||||
-0.5899348985799792,
|
||||
1.40343833941121,
|
||||
-0.7335620940370134
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,604 +0,0 @@
|
||||
{
|
||||
"schema_version": 2,
|
||||
"success": true,
|
||||
"message": "Both `ftol` and `xtol` termination conditions are 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.2996301155160686,
|
||||
-0.0035647011612625953,
|
||||
0.7218861757826791
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7871563544979889,
|
||||
1.1414557494228486,
|
||||
-0.9357571721886107
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.00678727299452357,
|
||||
0.010016439179525593,
|
||||
-0.008096912246155998,
|
||||
0.9998940163254998
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9996682219164817,
|
||||
0.01605613959699911,
|
||||
0.02014066710864661,
|
||||
1.2996301155160686
|
||||
],
|
||||
[
|
||||
-0.016328076825577065,
|
||||
0.9997767458747517,
|
||||
0.013410902850672373,
|
||||
-0.0035647011612625953
|
||||
],
|
||||
[
|
||||
-0.019920843293337136,
|
||||
-0.01373531176689472,
|
||||
0.9997072077429213,
|
||||
0.7218861757826791
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 38,
|
||||
"pairs": 80,
|
||||
"residuals": {
|
||||
"pairs": 80,
|
||||
"translation_m": {
|
||||
"rms": 0.11870623426762615,
|
||||
"median": 0.06068763803103021,
|
||||
"p90": 0.19284315530447813,
|
||||
"p95": 0.23754716587335756,
|
||||
"max": 0.38871430386214717
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.135091439337232,
|
||||
"median": 0.9273339494951451,
|
||||
"p90": 1.649541165022219,
|
||||
"p95": 1.981621584559613,
|
||||
"max": 2.6935053979880266
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.09187263959310249,
|
||||
"rotation_deg": 0.38130732180446597
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.1769336788939164,
|
||||
"rotation_deg": 0.6450700969822144
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.15700274999733538,
|
||||
"rotation_deg": 0.5067710073297588
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.10662491066187467,
|
||||
"rotation_deg": 0.5429917463514751
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.029628667721179554,
|
||||
"rotation_deg": 0.5509223411600718
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.0843749108812327,
|
||||
"rotation_deg": 0.4940771462459488
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.07914868235296937,
|
||||
"rotation_deg": 0.8254472643593804
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.009453498887768107,
|
||||
"rotation_deg": 0.9043821535551515
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.019511765564918175,
|
||||
"rotation_deg": 0.3967737125225238
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.05341682547424797,
|
||||
"rotation_deg": 1.2338229281598583
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.07322963958641442,
|
||||
"rotation_deg": 1.6425496017479204
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.05907844745776598,
|
||||
"rotation_deg": 0.9099755792395827
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.07907007871308952,
|
||||
"rotation_deg": 1.294221003621915
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.02140092032185309,
|
||||
"rotation_deg": 0.5139343819205795
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.19209639243977228,
|
||||
"rotation_deg": 0.9890356204602867
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.23611357668308347,
|
||||
"rotation_deg": 0.7834325452898548
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.023285251634710107,
|
||||
"rotation_deg": 0.17520547335389194
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.26478536048856705,
|
||||
"rotation_deg": 0.710271315591521
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.16006553402191026,
|
||||
"rotation_deg": 0.957610910483182
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.027251092891380724,
|
||||
"rotation_deg": 0.9157938224757094
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.04027747409562381,
|
||||
"rotation_deg": 0.5934052344689098
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.009702766313815113,
|
||||
"rotation_deg": 0.8792062492068344
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.19956402108683025,
|
||||
"rotation_deg": 1.302707349930284
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.22208667994405493,
|
||||
"rotation_deg": 0.865060603092117
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.04688877706330616,
|
||||
"rotation_deg": 0.35495594456572455
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.14151886488940063,
|
||||
"rotation_deg": 0.6501219960603252
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.0884069047402769,
|
||||
"rotation_deg": 0.7969404276735993
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.16408056565265292,
|
||||
"rotation_deg": 2.4437963646244074
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.028664980727987763,
|
||||
"rotation_deg": 1.008490254813876
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.07072061490599325,
|
||||
"rotation_deg": 1.909748472867515
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.022948436582833444,
|
||||
"rotation_deg": 2.3225092779351595
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.38849552688092603,
|
||||
"rotation_deg": 1.2325430499166412
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.06230564324556168,
|
||||
"rotation_deg": 1.4474405187186974
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.017320969569104998,
|
||||
"rotation_deg": 1.9636801270135327
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.15793386431764098,
|
||||
"rotation_deg": 1.0830036872759303
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.38871430386214717,
|
||||
"rotation_deg": 0.6522443164627029
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.06172089136639702,
|
||||
"rotation_deg": 2.6935053979880266
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.1887406405132845,
|
||||
"rotation_deg": 0.5255100746087121
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.3189860513403679,
|
||||
"rotation_deg": 0.33497034772867773
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.08451339501338925,
|
||||
"rotation_deg": 2.6477889534666375
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.21900981683029178,
|
||||
"rotation_deg": 1.2331847283146662
|
||||
},
|
||||
{
|
||||
"pair_index": 41,
|
||||
"translation_m": 0.1713522910564131,
|
||||
"rotation_deg": 1.183067166836572
|
||||
},
|
||||
{
|
||||
"pair_index": 42,
|
||||
"translation_m": 0.0512348407217562,
|
||||
"rotation_deg": 0.6012483843007921
|
||||
},
|
||||
{
|
||||
"pair_index": 43,
|
||||
"translation_m": 0.07445293169404418,
|
||||
"rotation_deg": 1.1211247122462373
|
||||
},
|
||||
{
|
||||
"pair_index": 44,
|
||||
"translation_m": 0.04410762061472433,
|
||||
"rotation_deg": 0.6590595108738039
|
||||
},
|
||||
{
|
||||
"pair_index": 45,
|
||||
"translation_m": 0.033089648474152494,
|
||||
"rotation_deg": 1.0312870469391562
|
||||
},
|
||||
{
|
||||
"pair_index": 46,
|
||||
"translation_m": 0.06760165724961124,
|
||||
"rotation_deg": 0.4565827539034035
|
||||
},
|
||||
{
|
||||
"pair_index": 47,
|
||||
"translation_m": 0.025670738208830455,
|
||||
"rotation_deg": 1.4302485146055521
|
||||
},
|
||||
{
|
||||
"pair_index": 48,
|
||||
"translation_m": 0.028147203494591596,
|
||||
"rotation_deg": 1.830717268006061
|
||||
},
|
||||
{
|
||||
"pair_index": 49,
|
||||
"translation_m": 0.02266261366157945,
|
||||
"rotation_deg": 0.946299184254386
|
||||
},
|
||||
{
|
||||
"pair_index": 50,
|
||||
"translation_m": 0.04935636595094902,
|
||||
"rotation_deg": 1.3964539171127066
|
||||
},
|
||||
{
|
||||
"pair_index": 51,
|
||||
"translation_m": 0.05889812376302877,
|
||||
"rotation_deg": 1.4698930638008505
|
||||
},
|
||||
{
|
||||
"pair_index": 52,
|
||||
"translation_m": 0.05965438469566341,
|
||||
"rotation_deg": 0.9816847661944946
|
||||
},
|
||||
{
|
||||
"pair_index": 53,
|
||||
"translation_m": 0.06606854020326042,
|
||||
"rotation_deg": 0.27458915782550086
|
||||
},
|
||||
{
|
||||
"pair_index": 54,
|
||||
"translation_m": 0.05158448025659227,
|
||||
"rotation_deg": 0.9388740765145809
|
||||
},
|
||||
{
|
||||
"pair_index": 55,
|
||||
"translation_m": 0.07186821989401924,
|
||||
"rotation_deg": 0.7757369232394653
|
||||
},
|
||||
{
|
||||
"pair_index": 56,
|
||||
"translation_m": 0.08608318124462912,
|
||||
"rotation_deg": 0.30616875771444013
|
||||
},
|
||||
{
|
||||
"pair_index": 57,
|
||||
"translation_m": 0.06688749012833557,
|
||||
"rotation_deg": 0.5486461025019678
|
||||
},
|
||||
{
|
||||
"pair_index": 58,
|
||||
"translation_m": 0.05762085648043371,
|
||||
"rotation_deg": 1.372519412009814
|
||||
},
|
||||
{
|
||||
"pair_index": 59,
|
||||
"translation_m": 0.031564793498850025,
|
||||
"rotation_deg": 0.4468008042798917
|
||||
},
|
||||
{
|
||||
"pair_index": 60,
|
||||
"translation_m": 0.039910793641719705,
|
||||
"rotation_deg": 0.7939954067942764
|
||||
},
|
||||
{
|
||||
"pair_index": 61,
|
||||
"translation_m": 0.04407888801173494,
|
||||
"rotation_deg": 1.3107090439391704
|
||||
},
|
||||
{
|
||||
"pair_index": 62,
|
||||
"translation_m": 0.024691509543671615,
|
||||
"rotation_deg": 1.236002055213775
|
||||
},
|
||||
{
|
||||
"pair_index": 63,
|
||||
"translation_m": 0.02262539867961735,
|
||||
"rotation_deg": 0.5097286713719923
|
||||
},
|
||||
{
|
||||
"pair_index": 64,
|
||||
"translation_m": 0.12496365742733645,
|
||||
"rotation_deg": 1.7124652344909004
|
||||
},
|
||||
{
|
||||
"pair_index": 65,
|
||||
"translation_m": 0.07938704275394372,
|
||||
"rotation_deg": 1.136197571772474
|
||||
},
|
||||
{
|
||||
"pair_index": 66,
|
||||
"translation_m": 0.03041340784879668,
|
||||
"rotation_deg": 1.1835741189420788
|
||||
},
|
||||
{
|
||||
"pair_index": 67,
|
||||
"translation_m": 0.08565781637557897,
|
||||
"rotation_deg": 1.325342137093704
|
||||
},
|
||||
{
|
||||
"pair_index": 68,
|
||||
"translation_m": 0.06569851623164084,
|
||||
"rotation_deg": 0.5889885968744448
|
||||
},
|
||||
{
|
||||
"pair_index": 69,
|
||||
"translation_m": 0.028714655898349907,
|
||||
"rotation_deg": 0.6559696801898813
|
||||
},
|
||||
{
|
||||
"pair_index": 70,
|
||||
"translation_m": 0.06483297271989888,
|
||||
"rotation_deg": 1.3915654660441625
|
||||
},
|
||||
{
|
||||
"pair_index": 71,
|
||||
"translation_m": 0.03452933204038689,
|
||||
"rotation_deg": 0.8331166409438662
|
||||
},
|
||||
{
|
||||
"pair_index": 72,
|
||||
"translation_m": 0.026295103775966104,
|
||||
"rotation_deg": 1.2325751495683674
|
||||
},
|
||||
{
|
||||
"pair_index": 73,
|
||||
"translation_m": 0.04043011893578754,
|
||||
"rotation_deg": 1.2125523869736732
|
||||
},
|
||||
{
|
||||
"pair_index": 74,
|
||||
"translation_m": 0.04383537712573055,
|
||||
"rotation_deg": 0.5600272304118834
|
||||
},
|
||||
{
|
||||
"pair_index": 75,
|
||||
"translation_m": 0.0849604176628602,
|
||||
"rotation_deg": 1.2304564930488662
|
||||
},
|
||||
{
|
||||
"pair_index": 76,
|
||||
"translation_m": 0.010862365758746927,
|
||||
"rotation_deg": 0.38357205151967033
|
||||
},
|
||||
{
|
||||
"pair_index": 77,
|
||||
"translation_m": 0.053059282511333926,
|
||||
"rotation_deg": 0.9610287581424943
|
||||
},
|
||||
{
|
||||
"pair_index": 78,
|
||||
"translation_m": 0.04551226086388418,
|
||||
"rotation_deg": 1.1597952394623392
|
||||
},
|
||||
{
|
||||
"pair_index": 79,
|
||||
"translation_m": 0.016850631999927004,
|
||||
"rotation_deg": 0.7568718165305058
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"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.004217873072423743,
|
||||
0.004428061325180952,
|
||||
0.004921296107038976
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.04989765234702571,
|
||||
0.04568528000305764,
|
||||
0.12720936620845405
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 6.329566115808833,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 354.634716588624,
|
||||
"success": true,
|
||||
"translation_m": 3.20226506801652e-10,
|
||||
"rotation_deg": 2.202963019076451e-08
|
||||
},
|
||||
{
|
||||
"cost": 354.6347165886241,
|
||||
"success": true,
|
||||
"translation_m": 7.896781546182907e-10,
|
||||
"rotation_deg": 1.0169266187599847e-07
|
||||
},
|
||||
{
|
||||
"cost": 354.6347165886238,
|
||||
"success": true,
|
||||
"translation_m": 8.217931179403876e-10,
|
||||
"rotation_deg": 6.93412504585164e-08
|
||||
},
|
||||
{
|
||||
"cost": 354.6347165886237,
|
||||
"success": true,
|
||||
"translation_m": 3.469446951953614e-18,
|
||||
"rotation_deg": 0.0
|
||||
},
|
||||
{
|
||||
"cost": 354.6347165886238,
|
||||
"success": true,
|
||||
"translation_m": 1.3034248658026153e-10,
|
||||
"rotation_deg": 2.7806728416619943e-08
|
||||
},
|
||||
{
|
||||
"cost": 354.6347165886238,
|
||||
"success": true,
|
||||
"translation_m": 2.260359361733417e-10,
|
||||
"rotation_deg": 2.6418689746833837e-08
|
||||
},
|
||||
{
|
||||
"cost": 354.634716588624,
|
||||
"success": true,
|
||||
"translation_m": 7.589395457725227e-10,
|
||||
"rotation_deg": 4.91609016107675e-08
|
||||
},
|
||||
{
|
||||
"cost": 354.6347165886238,
|
||||
"success": true,
|
||||
"translation_m": 1.2750789560311927e-10,
|
||||
"rotation_deg": 7.759775984980414e-10
|
||||
},
|
||||
{
|
||||
"cost": 354.634716588624,
|
||||
"success": true,
|
||||
"translation_m": 7.805526908313695e-10,
|
||||
"rotation_deg": 6.684488070039544e-08
|
||||
},
|
||||
{
|
||||
"cost": 354.634716588624,
|
||||
"success": true,
|
||||
"translation_m": 7.763945275712574e-10,
|
||||
"rotation_deg": 8.931061588988946e-08
|
||||
},
|
||||
{
|
||||
"cost": 354.6347165886244,
|
||||
"success": true,
|
||||
"translation_m": 9.383630927674663e-10,
|
||||
"rotation_deg": 1.3000921411943172e-07
|
||||
},
|
||||
{
|
||||
"cost": 354.634716588624,
|
||||
"success": true,
|
||||
"translation_m": 6.944401937454179e-10,
|
||||
"rotation_deg": 7.954648404736881e-08
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.0017068433472663032,
|
||||
0.0019214288447692426,
|
||||
0.0013180801706094514,
|
||||
0.06849719260762097,
|
||||
0.05792635838163369,
|
||||
0.062093058432709465
|
||||
],
|
||||
"p025": [
|
||||
1.2968099294514637,
|
||||
-0.006751632579940086,
|
||||
0.7193267956198541,
|
||||
-0.9300082754299475,
|
||||
1.0287033540941326,
|
||||
-1.0774039892205403
|
||||
],
|
||||
"p975": [
|
||||
1.302479553931627,
|
||||
-0.0001961195897526885,
|
||||
0.7244258625906532,
|
||||
-0.6646157463265417,
|
||||
1.254120304082275,
|
||||
-0.8275267997610058
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,12 +0,0 @@
|
||||
# data4 独立标定结果
|
||||
|
||||
data4 含 34 个静止站点，LiDAR 来自逐站 dlog，RTK/IMU 来自独立 rscap。求解不使用手量外参初始化。
|
||||
|
||||
```text
|
||||
translation_m = [1.300376020, -0.001706691, 0.704877045]
|
||||
RPY_deg_xyz = [-0.791891617, 1.393823210, -0.970742631]
|
||||
AX RMS = 0.11762 m / 1.24257 deg (26 pairs)
|
||||
condition = 7.44293
|
||||
```
|
||||
|
||||
该结果用于独立对照，不是当前部署值。`final_extrinsic_data4.json` 与 `consensus/extrinsic.json` 数值相同；前者是便于下游读取的顶层副本。
|
||||
@@ -1,35 +0,0 @@
|
||||
time,nx,ny,nz,d,inliers,rms_m,frame_counter
|
||||
1784783825.357129,-0.011572516208853837,-0.01546146293660183,0.9998134926237273,0.9428804652027359,2148,0.011933750988078078,382
|
||||
1784783905.353819,0.0037183584307534687,-0.006818427483323246,0.9999698409738341,0.9412842800288904,1992,0.011942807895967073,1182
|
||||
1784783971.0503054,-0.021709843154394736,-0.005232422217807239,0.999750621139065,0.9451429710867566,1863,0.012080615710528817,1839
|
||||
1784784059.7468228,-0.02414670904491499,-0.0009406084776101358,0.9997079832120939,0.9517260475425733,1921,0.013468410314875789,2726
|
||||
1784784149.2434597,-0.034005828386544125,0.003026503321489513,0.9994170520425345,0.9257219945735469,1673,0.012714432789206961,3621
|
||||
1784784224.2408776,-0.02761369891036776,0.0016288328836576937,0.9996173420544108,0.9001377900125058,1799,0.012615520511386497,4371
|
||||
1784784301.6372502,-0.0071762210837946405,-0.011511959356927467,0.9999079840878965,0.9580809447760092,2113,0.012007730376482893,5145
|
||||
1784784387.733771,-0.010489658730106238,-0.009076163262051764,0.9999037905319524,0.9306898458653136,2187,0.01214615852280608,6006
|
||||
1784784474.9314597,-0.016114135387570262,-0.0509432368134707,0.9985715403834008,0.9072229543504865,1894,0.012012086783846727,6878
|
||||
1784784549.4274275,-0.025174259196536854,-0.013435577824823405,0.9995927880504234,0.9410339772836553,1603,0.01154746446935748,7623
|
||||
1784784614.7244046,-0.03173300872144653,-0.01870285516152855,0.9993213794202,0.932566466561638,1970,0.01233410897082844,8276
|
||||
1784784682.921899,-0.022665842512528424,-0.02998919824936144,0.9992932040054899,0.9145231002636922,1663,0.012948552270457988,8958
|
||||
1784784758.8187964,-0.023688193326187944,-0.030852054660206355,0.9992432237549499,0.9349709939783197,1756,0.013443733500218538,9717
|
||||
1784784836.0155501,-0.018461711654128285,-0.02498001140817507,0.9995174656967467,0.94720244509644,2057,0.013445893726732725,10489
|
||||
1784784921.1126208,-0.01993595874096254,-0.02538449671402936,0.9994789566947642,0.9120981909462803,1822,0.012774262320504949,11340
|
||||
1784784992.709947,-0.017778693539407146,-0.026803117327322167,0.9994826216386005,0.9369057170493221,2114,0.012253841210002203,12056
|
||||
1784785067.6067727,-0.03766905893322114,-0.00624993226014667,0.9992707242513558,0.9491859078478477,1753,0.01368084681318558,12805
|
||||
1784785215.9006598,-0.023255779098430147,-0.030526061005580876,0.9992633928739754,0.9173759052467025,2145,0.012710466955150847,14288
|
||||
1784785296.4990919,-0.014034225033964978,-0.025014032112439084,0.9995885847313253,0.9275558675790446,2445,0.012495377814995792,15094
|
||||
1784785363.1952267,-0.03352240146955635,-0.008711847039715113,0.9993999961581301,0.9543448006240765,1968,0.01391585588879859,15761
|
||||
1784785434.592462,-0.016701702947054253,0.016663838961403298,0.9997216460544089,0.9289121671743757,1857,0.011181698281215124,16475
|
||||
1784785506.389296,-0.03393096202185679,-0.007325597743377637,0.9993973311120926,0.9387396500642948,1844,0.01340053385502904,17193
|
||||
1784785587.5863533,-0.03382019410288933,-0.0007820019558384217,0.9994276276668481,0.9255866795455666,1691,0.012723688174337812,18005
|
||||
1784785681.9825997,-0.03422781555599595,-0.019255467037831406,0.999228544243718,0.9454924962027047,1959,0.01264678059788212,18949
|
||||
1784785815.4779446,-0.0011902245774294716,-0.023262098479360813,0.9997286922659526,0.9523139768255908,2026,0.012446728838862407,20284
|
||||
1784785891.9768085,-0.02758114176328463,0.0022252358991911286,0.9996170911625244,0.9417694614877323,1551,0.012616490996777729,21049
|
||||
1784785967.8726046,-0.031038730802887403,-0.010699305087773895,0.9994609157244643,0.9511405539250901,1512,0.013166899954854165,21808
|
||||
1784786031.5701303,-0.023257481598180085,-0.0214545061132305,0.9994992714940555,0.9380611442094842,1555,0.013339981386836522,22445
|
||||
1784786087.9670725,-0.02720411594210121,-0.012536181034248165,0.9995512894498637,0.942804167293392,1531,0.013246549575843425,23009
|
||||
1784786160.8647907,-0.030839502708575457,0.06473269778477829,0.9974259886879789,1.0783938869067702,1436,0.010501132985089602,23738
|
||||
1784786252.6621523,-0.040972893104105006,-0.02082299858725662,0.9989432540242288,0.9524735117369321,1736,0.012945834939507173,24656
|
||||
1784786319.6581354,-0.029761355495275914,-0.026565272466750618,0.9992039571668294,0.9594371422885729,1571,0.013108184070957877,25326
|
||||
1784786396.7558627,-0.030769194030483185,-0.022645428394942407,0.9992699541522921,0.9756606317668476,1326,0.013202989492489907,26097
|
||||
1784786557.4492514,-0.019983269314458783,-0.016965365794746882,0.9996563636124948,0.9409853928234136,1228,0.013269243263785414,27704
|
||||
|
@@ -1,340 +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": 42,
|
||||
"accepted_pairs": 26,
|
||||
"pairs": [
|
||||
{
|
||||
"i": 0,
|
||||
"j": 1,
|
||||
"open3d_small_translation_m": 0.014276441704401843,
|
||||
"open3d_small_rotation_deg": 0.61360593819684,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 0,
|
||||
"j": 2,
|
||||
"open3d_small_translation_m": 0.019952450418350955,
|
||||
"open3d_small_rotation_deg": 0.1460927002571647,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 1,
|
||||
"j": 2,
|
||||
"accepted": false,
|
||||
"reason": "not_in_small_gicp_refined"
|
||||
},
|
||||
{
|
||||
"i": 2,
|
||||
"j": 3,
|
||||
"open3d_small_translation_m": 0.02240729290024995,
|
||||
"open3d_small_rotation_deg": 0.16748011698659707,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 2,
|
||||
"j": 5,
|
||||
"open3d_small_translation_m": 0.0061616498009369,
|
||||
"open3d_small_rotation_deg": 0.13760629005733752,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 3,
|
||||
"j": 5,
|
||||
"open3d_small_translation_m": 0.03979893704050873,
|
||||
"open3d_small_rotation_deg": 0.21317157512260582,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 3,
|
||||
"j": 6,
|
||||
"open3d_small_translation_m": 0.012411893826144786,
|
||||
"open3d_small_rotation_deg": 0.6409039547122976,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 5,
|
||||
"j": 8,
|
||||
"open3d_small_translation_m": 0.02304199707639382,
|
||||
"open3d_small_rotation_deg": 0.37075901631515606,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 6,
|
||||
"j": 7,
|
||||
"open3d_small_translation_m": 0.008481323658868146,
|
||||
"open3d_small_rotation_deg": 0.2110246381252998,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 6,
|
||||
"j": 8,
|
||||
"open3d_small_translation_m": 0.03928926702287607,
|
||||
"open3d_small_rotation_deg": 0.3638880951335251,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 7,
|
||||
"j": 8,
|
||||
"open3d_small_translation_m": 0.008121615288998074,
|
||||
"open3d_small_rotation_deg": 0.6269514061788241,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 10,
|
||||
"j": 11,
|
||||
"open3d_small_translation_m": 0.021311366483594964,
|
||||
"open3d_small_rotation_deg": 0.5895827931090624,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 12,
|
||||
"j": 14,
|
||||
"open3d_small_translation_m": 0.021593615635265958,
|
||||
"open3d_small_rotation_deg": 0.28280557179019644,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 12,
|
||||
"j": 15,
|
||||
"open3d_small_translation_m": 0.03680193420722766,
|
||||
"open3d_small_rotation_deg": 0.560849139510841,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 13,
|
||||
"j": 15,
|
||||
"open3d_small_translation_m": 0.02340982602704091,
|
||||
"open3d_small_rotation_deg": 0.5560478634176494,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 13,
|
||||
"j": 16,
|
||||
"open3d_small_translation_m": 0.027602744462628358,
|
||||
"open3d_small_rotation_deg": 0.2529072067309942,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 16,
|
||||
"open3d_small_translation_m": 0.0209082447077901,
|
||||
"open3d_small_rotation_deg": 0.03400462126505844,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 17,
|
||||
"open3d_small_translation_m": 0.04186146133502697,
|
||||
"open3d_small_rotation_deg": 0.13358624916951176,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 18,
|
||||
"open3d_small_translation_m": 0.014610177110996908,
|
||||
"open3d_small_rotation_deg": 0.25378510251316655,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 16,
|
||||
"j": 19,
|
||||
"open3d_small_translation_m": 0.012703728170382652,
|
||||
"open3d_small_rotation_deg": 0.36607447371237334,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 17,
|
||||
"j": 18,
|
||||
"open3d_small_translation_m": 0.03054772105829047,
|
||||
"open3d_small_rotation_deg": 0.6293060553954827,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 22,
|
||||
"open3d_small_translation_m": 0.008438580476457845,
|
||||
"open3d_small_rotation_deg": 0.15351253406054555,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 23,
|
||||
"open3d_small_translation_m": 0.018479690388276085,
|
||||
"open3d_small_rotation_deg": 0.5781174905909121,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 24,
|
||||
"open3d_small_translation_m": 0.031284642427708814,
|
||||
"open3d_small_rotation_deg": 0.39218826727895556,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 22,
|
||||
"j": 23,
|
||||
"open3d_small_translation_m": 0.011568269703658672,
|
||||
"open3d_small_rotation_deg": 0.06135783639322868,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 22,
|
||||
"j": 25,
|
||||
"open3d_small_translation_m": 0.041018828349300214,
|
||||
"open3d_small_rotation_deg": 0.42033964239927313,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 23,
|
||||
"j": 24,
|
||||
"open3d_small_translation_m": 0.009191025383393128,
|
||||
"open3d_small_rotation_deg": 0.5691342385312416,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 26,
|
||||
"open3d_small_translation_m": 0.020915300595375722,
|
||||
"open3d_small_rotation_deg": 0.48750667250286367,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 27,
|
||||
"open3d_small_translation_m": 0.09632206662156453,
|
||||
"open3d_small_rotation_deg": 0.36999381209651033,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 28,
|
||||
"open3d_small_translation_m": 0.030138112812714665,
|
||||
"open3d_small_rotation_deg": 0.46587172800425714,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 27,
|
||||
"open3d_small_translation_m": 0.010107572072066551,
|
||||
"open3d_small_rotation_deg": 0.4201365669869145,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 28,
|
||||
"open3d_small_translation_m": 0.006243998078354416,
|
||||
"open3d_small_rotation_deg": 1.3246136424341453,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 29,
|
||||
"open3d_small_translation_m": 0.036178252863401886,
|
||||
"open3d_small_rotation_deg": 0.2782366609124745,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 27,
|
||||
"j": 28,
|
||||
"open3d_small_translation_m": 0.00854064689385523,
|
||||
"open3d_small_rotation_deg": 0.36768009631129556,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 27,
|
||||
"j": 29,
|
||||
"open3d_small_translation_m": 0.013991202401755857,
|
||||
"open3d_small_rotation_deg": 0.43729057730087656,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 29,
|
||||
"open3d_small_translation_m": 0.12711262123542336,
|
||||
"open3d_small_rotation_deg": 0.923125258275691,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 31,
|
||||
"accepted": false,
|
||||
"reason": "not_in_small_gicp_refined"
|
||||
},
|
||||
{
|
||||
"i": 29,
|
||||
"j": 30,
|
||||
"open3d_small_translation_m": 0.024384029932636327,
|
||||
"open3d_small_rotation_deg": 0.4169363425835767,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 29,
|
||||
"j": 31,
|
||||
"open3d_small_translation_m": 0.012379055939823663,
|
||||
"open3d_small_rotation_deg": 0.2514662901219578,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 30,
|
||||
"j": 31,
|
||||
"open3d_small_translation_m": 0.056474627862095485,
|
||||
"open3d_small_rotation_deg": 0.6666837905087916,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 30,
|
||||
"j": 32,
|
||||
"open3d_small_translation_m": 0.015291330212417978,
|
||||
"open3d_small_rotation_deg": 0.309282499387647,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 31,
|
||||
"j": 32,
|
||||
"accepted": false,
|
||||
"reason": "not_in_small_gicp_refined"
|
||||
}
|
||||
]
|
||||
}
|
||||
Binary file not shown.
@@ -1,334 +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.3003760202954853,
|
||||
-0.001706690964618711,
|
||||
0.7048770452870675
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7918916173570606,
|
||||
1.3938232102436765,
|
||||
-0.9707426305626278
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.006806709954738454,
|
||||
0.01222091143708808,
|
||||
-0.008386347182892065,
|
||||
0.9998669847555559
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9995606370091497,
|
||||
0.016604094942673456,
|
||||
0.024552738604839263,
|
||||
1.3003760202954853
|
||||
],
|
||||
[
|
||||
-0.016936831740812692,
|
||||
0.9997666757610401,
|
||||
0.01340663150469432,
|
||||
-0.001706690964618711
|
||||
],
|
||||
[
|
||||
-0.02432440487342451,
|
||||
-0.013816586729505535,
|
||||
0.9996086360464778,
|
||||
0.7048770452870675
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 34,
|
||||
"pairs": 26,
|
||||
"residuals": {
|
||||
"pairs": 26,
|
||||
"translation_m": {
|
||||
"rms": 0.11761746401526753,
|
||||
"median": 0.05973702338495346,
|
||||
"p90": 0.12346154791373619,
|
||||
"p95": 0.2642880752048252,
|
||||
"max": 0.38818289084930874
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.2425719632195558,
|
||||
"median": 0.7690245702483323,
|
||||
"p90": 1.8136122342812846,
|
||||
"p95": 1.9548737964733964,
|
||||
"max": 4.332147922413344
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.04628122614893134,
|
||||
"rotation_deg": 0.6982246240615032
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.060020104078054665,
|
||||
"rotation_deg": 0.3948778403656006
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.10360591701372288,
|
||||
"rotation_deg": 0.30363546700066535
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.06188990107567784,
|
||||
"rotation_deg": 0.5542261589736583
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.02429424053491548,
|
||||
"rotation_deg": 0.5086436052507228
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.06697411809016678,
|
||||
"rotation_deg": 0.7794510927257128
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.012085958852730979,
|
||||
"rotation_deg": 0.7997166092197476
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.04528858471141819,
|
||||
"rotation_deg": 0.868879957697597
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.05426127100895971,
|
||||
"rotation_deg": 1.8644817833901326
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.12845158967421863,
|
||||
"rotation_deg": 1.9850044675011511
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.06743761428527982,
|
||||
"rotation_deg": 0.23695300739340452
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.060359774544132216,
|
||||
"rotation_deg": 0.3380187067759931
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.03420362550416907,
|
||||
"rotation_deg": 0.49547670606543825
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.059453942691852245,
|
||||
"rotation_deg": 0.6142581264077737
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.048234437308639966,
|
||||
"rotation_deg": 1.7627426851724366
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.0572558948305829,
|
||||
"rotation_deg": 0.8826512445544471
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.11847150615325375,
|
||||
"rotation_deg": 4.332147922413344
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.05084797298915324,
|
||||
"rotation_deg": 0.9691629612561474
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.03403536387594384,
|
||||
"rotation_deg": 0.9311969881819248
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.05397038183164141,
|
||||
"rotation_deg": 0.5108403673418249
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.10079710084559501,
|
||||
"rotation_deg": 0.9132399116834199
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.0245653967740859,
|
||||
"rotation_deg": 0.29258746424545673
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.06599237622687693,
|
||||
"rotation_deg": 1.130391129168243
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.30956690371502743,
|
||||
"rotation_deg": 0.7585980477709516
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.38818289084930874,
|
||||
"rotation_deg": 0.6792102747820554
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.10874622325422871,
|
||||
"rotation_deg": 0.8215202229484776
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"ground": {
|
||||
"planes": 34,
|
||||
"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.00880269284499636,
|
||||
0.00901239388348182,
|
||||
0.0047231303576790815
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.08002846787949729,
|
||||
0.07607548557648909,
|
||||
0.16438009658312316
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 7.44293047593025,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 125.63045335315196,
|
||||
"success": true,
|
||||
"translation_m": 2.6261422580681395e-09,
|
||||
"rotation_deg": 5.414246532613018e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315196,
|
||||
"success": true,
|
||||
"translation_m": 1.4824807208067879e-09,
|
||||
"rotation_deg": 1.72860210982948e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315223,
|
||||
"success": true,
|
||||
"translation_m": 2.630839730785566e-09,
|
||||
"rotation_deg": 6.406686644059802e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315202,
|
||||
"success": true,
|
||||
"translation_m": 3.4171046507681323e-09,
|
||||
"rotation_deg": 6.031782751294924e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315198,
|
||||
"success": true,
|
||||
"translation_m": 1.024465800095368e-09,
|
||||
"rotation_deg": 5.593612335539163e-09
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315205,
|
||||
"success": true,
|
||||
"translation_m": 3.3380353081804456e-09,
|
||||
"rotation_deg": 5.989779005616406e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315195,
|
||||
"success": true,
|
||||
"translation_m": 1.3390324575379226e-09,
|
||||
"rotation_deg": 1.669449907197552e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315194,
|
||||
"success": true,
|
||||
"translation_m": 9.664926809065912e-10,
|
||||
"rotation_deg": 1.725605645088929e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315195,
|
||||
"success": true,
|
||||
"translation_m": 1.1236440604027455e-09,
|
||||
"rotation_deg": 8.844155186013785e-09
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315957,
|
||||
"success": true,
|
||||
"translation_m": 4.0330699471902714e-08,
|
||||
"rotation_deg": 2.7706147186959257e-07
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315196,
|
||||
"success": true,
|
||||
"translation_m": 1.1516332665838399e-10,
|
||||
"rotation_deg": 2.767718681568013e-09
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315192,
|
||||
"success": true,
|
||||
"translation_m": 2.220446049250313e-16,
|
||||
"rotation_deg": 0.0
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.004332764631777773,
|
||||
0.0052124106362270465,
|
||||
0.002294080818689292,
|
||||
0.09999053340933307,
|
||||
0.10013087210161215,
|
||||
0.1488688877903013
|
||||
],
|
||||
"p025": [
|
||||
1.2918362278137754,
|
||||
-0.009531330977586527,
|
||||
0.7011199682224338,
|
||||
-0.9599898042168808,
|
||||
1.2066688822355272,
|
||||
-1.2617485460367455
|
||||
],
|
||||
"p975": [
|
||||
1.3082553293462966,
|
||||
0.009443875945124016,
|
||||
0.7092373306937458,
|
||||
-0.5988363907444886,
|
||||
1.5590218787244785,
|
||||
-0.7051603064062103
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -1,334 +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.3003760202954853,
|
||||
-0.001706690964618711,
|
||||
0.7048770452870675
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7918916173570606,
|
||||
1.3938232102436765,
|
||||
-0.9707426305626278
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.006806709954738454,
|
||||
0.01222091143708808,
|
||||
-0.008386347182892065,
|
||||
0.9998669847555559
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9995606370091497,
|
||||
0.016604094942673456,
|
||||
0.024552738604839263,
|
||||
1.3003760202954853
|
||||
],
|
||||
[
|
||||
-0.016936831740812692,
|
||||
0.9997666757610401,
|
||||
0.01340663150469432,
|
||||
-0.001706690964618711
|
||||
],
|
||||
[
|
||||
-0.02432440487342451,
|
||||
-0.013816586729505535,
|
||||
0.9996086360464778,
|
||||
0.7048770452870675
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 34,
|
||||
"pairs": 26,
|
||||
"residuals": {
|
||||
"pairs": 26,
|
||||
"translation_m": {
|
||||
"rms": 0.11761746401526753,
|
||||
"median": 0.05973702338495346,
|
||||
"p90": 0.12346154791373619,
|
||||
"p95": 0.2642880752048252,
|
||||
"max": 0.38818289084930874
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.2425719632195558,
|
||||
"median": 0.7690245702483323,
|
||||
"p90": 1.8136122342812846,
|
||||
"p95": 1.9548737964733964,
|
||||
"max": 4.332147922413344
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.04628122614893134,
|
||||
"rotation_deg": 0.6982246240615032
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.060020104078054665,
|
||||
"rotation_deg": 0.3948778403656006
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.10360591701372288,
|
||||
"rotation_deg": 0.30363546700066535
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.06188990107567784,
|
||||
"rotation_deg": 0.5542261589736583
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.02429424053491548,
|
||||
"rotation_deg": 0.5086436052507228
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.06697411809016678,
|
||||
"rotation_deg": 0.7794510927257128
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.012085958852730979,
|
||||
"rotation_deg": 0.7997166092197476
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.04528858471141819,
|
||||
"rotation_deg": 0.868879957697597
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.05426127100895971,
|
||||
"rotation_deg": 1.8644817833901326
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.12845158967421863,
|
||||
"rotation_deg": 1.9850044675011511
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.06743761428527982,
|
||||
"rotation_deg": 0.23695300739340452
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.060359774544132216,
|
||||
"rotation_deg": 0.3380187067759931
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.03420362550416907,
|
||||
"rotation_deg": 0.49547670606543825
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.059453942691852245,
|
||||
"rotation_deg": 0.6142581264077737
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.048234437308639966,
|
||||
"rotation_deg": 1.7627426851724366
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.0572558948305829,
|
||||
"rotation_deg": 0.8826512445544471
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.11847150615325375,
|
||||
"rotation_deg": 4.332147922413344
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.05084797298915324,
|
||||
"rotation_deg": 0.9691629612561474
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.03403536387594384,
|
||||
"rotation_deg": 0.9311969881819248
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.05397038183164141,
|
||||
"rotation_deg": 0.5108403673418249
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.10079710084559501,
|
||||
"rotation_deg": 0.9132399116834199
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.0245653967740859,
|
||||
"rotation_deg": 0.29258746424545673
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.06599237622687693,
|
||||
"rotation_deg": 1.130391129168243
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.30956690371502743,
|
||||
"rotation_deg": 0.7585980477709516
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.38818289084930874,
|
||||
"rotation_deg": 0.6792102747820554
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.10874622325422871,
|
||||
"rotation_deg": 0.8215202229484776
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"ground": {
|
||||
"planes": 34,
|
||||
"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.00880269284499636,
|
||||
0.00901239388348182,
|
||||
0.0047231303576790815
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.08002846787949729,
|
||||
0.07607548557648909,
|
||||
0.16438009658312316
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 7.44293047593025,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 125.63045335315196,
|
||||
"success": true,
|
||||
"translation_m": 2.6261422580681395e-09,
|
||||
"rotation_deg": 5.414246532613018e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315196,
|
||||
"success": true,
|
||||
"translation_m": 1.4824807208067879e-09,
|
||||
"rotation_deg": 1.72860210982948e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315223,
|
||||
"success": true,
|
||||
"translation_m": 2.630839730785566e-09,
|
||||
"rotation_deg": 6.406686644059802e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315202,
|
||||
"success": true,
|
||||
"translation_m": 3.4171046507681323e-09,
|
||||
"rotation_deg": 6.031782751294924e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315198,
|
||||
"success": true,
|
||||
"translation_m": 1.024465800095368e-09,
|
||||
"rotation_deg": 5.593612335539163e-09
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315205,
|
||||
"success": true,
|
||||
"translation_m": 3.3380353081804456e-09,
|
||||
"rotation_deg": 5.989779005616406e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315195,
|
||||
"success": true,
|
||||
"translation_m": 1.3390324575379226e-09,
|
||||
"rotation_deg": 1.669449907197552e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315194,
|
||||
"success": true,
|
||||
"translation_m": 9.664926809065912e-10,
|
||||
"rotation_deg": 1.725605645088929e-08
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315195,
|
||||
"success": true,
|
||||
"translation_m": 1.1236440604027455e-09,
|
||||
"rotation_deg": 8.844155186013785e-09
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315957,
|
||||
"success": true,
|
||||
"translation_m": 4.0330699471902714e-08,
|
||||
"rotation_deg": 2.7706147186959257e-07
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315196,
|
||||
"success": true,
|
||||
"translation_m": 1.1516332665838399e-10,
|
||||
"rotation_deg": 2.767718681568013e-09
|
||||
},
|
||||
{
|
||||
"cost": 125.63045335315192,
|
||||
"success": true,
|
||||
"translation_m": 2.220446049250313e-16,
|
||||
"rotation_deg": 0.0
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.004332764631777773,
|
||||
0.0052124106362270465,
|
||||
0.002294080818689292,
|
||||
0.09999053340933307,
|
||||
0.10013087210161215,
|
||||
0.1488688877903013
|
||||
],
|
||||
"p025": [
|
||||
1.2918362278137754,
|
||||
-0.009531330977586527,
|
||||
0.7011199682224338,
|
||||
-0.9599898042168808,
|
||||
1.2066688822355272,
|
||||
-1.2617485460367455
|
||||
],
|
||||
"p975": [
|
||||
1.3082553293462966,
|
||||
0.009443875945124016,
|
||||
0.7092373306937458,
|
||||
-0.5988363907444886,
|
||||
1.5590218787244785,
|
||||
-0.7051603064062103
|
||||
]
|
||||
}
|
||||
}
|
||||
Binary file not shown.
@@ -1,97 +0,0 @@
|
||||
i,j,rtk_translation_m,rtk_rotation_deg,heldout_inlier_ratio,heldout_inlier_rmse_m,hessian_rank,hessian_condition,reverse_translation_m,reverse_rotation_deg,multistart_success_rate,accepted,rejection_reasons
|
||||
0,1,1.8477087194158957,24.171297449440786,0.8061657032755298,0.10961296014103376,6,2.7038608113687213,0.004225163540003135,0.15304490677206398,1.0,True,
|
||||
0,2,2.633731568575307,80.09074797031298,0.7489394523717702,0.116305716193008,6,3.0720058333957385,0.02073003109723772,0.12418344306820694,1.0,True,
|
||||
0,3,6.923255970100826,79.91583883299243,0.6310283235519265,0.12470979645173065,6,5.235632990817995,0.02154775870989581,0.2539904497958252,1.0,True,
|
||||
1,2,1.7155362084417105,55.91945052087219,0.7867383512544803,0.11079021393934936,6,3.282529873989152,0.0076823267394430066,0.05080433004685343,1.0,True,
|
||||
1,3,5.885389341942907,55.74454138355163,0.6794562317367552,0.1190716913809658,6,4.183386002322129,0.024832426027852336,0.29364033194785566,1.0,True,
|
||||
1,4,2.3031736807956613,106.08652205569953,0.6786112833230006,0.11255015823152649,6,3.2941312581877575,0.007769137931169012,0.07270245476793177,1.0,True,
|
||||
2,3,4.339151683168145,0.17490913732054883,0.7586776859504132,0.11541610277862563,6,3.730803369806123,0.007879904085793275,0.11659483159946175,1.0,True,
|
||||
2,4,0.6150408096900305,50.167071534827386,0.7854572527608884,0.1098649389241601,6,2.641287751481569,0.017042953274278242,0.09383247792992147,1.0,True,
|
||||
2,5,5.894923525501676,8.735318060700383,0.7074574574574575,0.11912871456224478,6,3.8527093190832473,0.016641241123247028,0.24777402858141082,1.0,True,
|
||||
3,4,3.9735035126146885,50.34198067214791,0.6974624291697462,0.1172763088894914,6,3.5010457716923225,0.012148721219467648,0.2079308684382429,1.0,True,
|
||||
3,5,2.3619393178889707,8.910227198020932,0.7962985964476462,0.10646082199215777,6,3.0377458538379902,0.011008298723511818,0.08217077610411756,1.0,True,
|
||||
3,6,2.2300116834828536,47.63555775102664,0.8376509054325956,0.10956583416752592,6,3.1930663579156233,0.001222821038316072,0.09207521511761255,1.0,True,
|
||||
4,5,5.370771070097231,41.431753474126985,0.6652516676773802,0.12322094568314959,6,4.985227704290952,0.005399786589867114,0.1389350777589359,1.0,True,
|
||||
4,6,6.142548765456278,97.97753842317456,0.6529585072428186,0.11878586101480827,6,5.324803753668097,0.011377811219747914,0.20418431358287809,1.0,True,
|
||||
4,7,5.314106070657687,123.28910472998356,0.6964418087472202,0.11691192595448072,6,4.414726450762486,0.017641796253111963,0.10886712169052283,1.0,True,
|
||||
5,6,1.9357867378988893,56.54578494904757,0.7526921648718901,0.10924852668609378,6,3.289802074469556,0.012374747586501101,0.14461856068326442,1.0,True,
|
||||
5,7,0.19501559913675365,81.85735125585654,0.7833561729164071,0.11683079277948674,6,2.8807765869032655,0.0136898748124479,0.2000656277647698,1.0,True,
|
||||
5,8,1.8038807059856978,172.47951556359365,0.7410703250525275,0.11436945722491815,6,3.9768956884456648,0.020336999081352437,0.07548452187379719,1.0,True,
|
||||
6,7,2.130456053070114,25.311566306808988,0.8497729566094854,0.10415909908074772,6,3.375936129784756,0.008986805974948919,0.1309947950641294,1.0,True,
|
||||
6,8,0.24494500622098103,115.93373061454483,0.7678928928928929,0.11166725079781287,6,3.2609205733873607,0.010085391730565987,0.11752836996157842,1.0,True,
|
||||
6,9,8.025985916230132,103.90638727582186,0.6071384156199477,0.12594282886521954,6,6.882498483502561,0.01655440444551025,0.4345945520890901,1.0,True,
|
||||
7,8,1.9962664218365056,90.62216430773583,0.8299748110831234,0.10748525688830209,6,3.0071179226782414,0.007106461978852689,0.10698728473689886,1.0,True,
|
||||
7,9,7.8811064994361235,78.59482096901284,0.6188509200150206,0.1271320507839345,6,6.716979637303383,0.026230622734929154,0.28112746782878123,1.0,True,
|
||||
7,10,7.620696678413973,134.60031955305035,0.5757088027733069,0.1313446077529101,6,5.755535843660075,0.012710332356906473,0.2297418063823675,1.0,True,
|
||||
8,9,7.803713858547152,12.027343338722998,0.6326834719980131,0.12084265385763356,6,5.539689880493177,0.02518064403521875,0.3859647522318924,1.0,True,
|
||||
8,10,8.062504137991457,43.97815524531457,0.6163861933423412,0.12936028463965984,6,4.789005902863088,0.011327540721525892,0.17059480981566058,1.0,True,
|
||||
8,11,11.084531710563947,22.606708130954026,0.5371195721380364,0.13463285672049757,6,5.833003178860405,0.017887959781482814,0.14919899299178907,1.0,True,
|
||||
9,10,2.04815062353057,56.00549858403758,0.6543345543345543,0.10729272360686735,6,3.3788563349731584,0.008725655639009402,0.02870462611907292,1.0,True,
|
||||
9,11,4.738677237611319,34.634051469677026,0.5818780055682106,0.11717687815100752,6,3.555986532172074,0.009813025835682346,0.0603546019895227,1.0,True,
|
||||
9,12,7.170741483679294,16.35622721712263,0.5379123584441162,0.12645695585785732,6,4.882558096197038,0.008862930161052695,0.11566409877698863,1.0,True,
|
||||
10,11,3.2047552083250137,21.371447114360556,0.7118898623279099,0.1192865608074921,6,3.2202034715576238,0.0029529340190147615,0.0041769080934441144,1.0,True,
|
||||
10,12,6.291813977735496,39.64927136691496,0.6120311738918656,0.1252565212812615,6,4.691686416856199,0.005686910809265337,0.10204044727301474,1.0,True,
|
||||
10,13,10.199392557022867,72.41150002956134,0.516551290119572,0.1354903305714049,6,7.346543684414892,0.01356504272158301,0.3255230434099281,1.0,True,
|
||||
11,12,3.467398797536633,18.277824252554396,0.650555275113579,0.12104882943540958,6,4.612247786063486,0.003583199374499245,0.03300207707174736,1.0,True,
|
||||
11,13,7.516502113110916,51.04005291520078,0.5698054068172914,0.12858662743221627,6,7.539783468898054,0.016001435752891854,0.11059625579949509,1.0,True,
|
||||
11,14,3.767517331528496,20.548768889074672,0.6420881321982974,0.12414062948335584,6,4.9522650472668115,0.012369101516230236,0.018705060406060074,1.0,True,
|
||||
12,13,4.049286119591895,32.762228662646386,0.6972966112450819,0.11680896213116294,6,4.282247993741361,0.011410238767832601,0.04779713993430858,1.0,True,
|
||||
12,14,0.97948616772873,2.2709446365202806,0.8749086479902558,0.09521299965540617,6,3.309695139564419,0.006159472215773642,0.014929948455572307,1.0,True,
|
||||
12,15,4.286747470271891,25.863710300929224,0.7022030893897189,0.11797995277580095,6,4.2772327867721325,0.008495008365045943,0.102782991447036,1.0,True,
|
||||
13,14,4.006260191078547,30.491284026126113,0.6955810147299509,0.1145595612270532,6,3.350289886810732,0.010228664633443074,0.03515966054944097,1.0,True,
|
||||
13,15,0.9562774815922267,6.898518361717157,0.868300353819945,0.10454213568084784,6,3.243735713395398,0.0023750253827712867,0.010725047644197173,1.0,True,
|
||||
13,16,3.565173336606111,18.944899794614482,0.7265456392027422,0.10962529664062398,6,3.522751424445623,0.008958927594995584,0.0304143851242741,1.0,True,
|
||||
14,15,4.019575892829469,23.592765664408944,0.7120070334086913,0.11868441290330693,6,4.620592469502459,0.002571958018982041,0.05506919751152759,1.0,True,
|
||||
14,16,7.5676649485439835,49.43618382074059,0.5918615984405458,0.12229328437386527,6,7.149509813179243,0.014273957859022303,0.25325650727956367,1.0,True,
|
||||
14,17,5.910977627463022,0.8461207481731591,0.6694009445687298,0.12443900216431929,6,5.157741429696001,0.017895201000461415,0.10920228290609475,1.0,True,
|
||||
15,16,3.7301261399251735,25.84341815633164,0.702887537993921,0.11495230769293868,6,3.540289976352534,0.013545291843393993,0.033466251783377816,1.0,True,
|
||||
15,17,2.2049738368271745,24.438886412582093,0.7429531936901991,0.11679524427533879,6,3.526664394280145,0.00989411002791081,0.07786907370564648,1.0,True,
|
||||
15,18,4.7000039832559155,3.452521908779401,0.7209645010046886,0.11716134583909153,6,4.125231895423432,0.011654729311847106,0.13683586564190353,1.0,True,
|
||||
16,17,3.368526196086246,50.282304568913744,0.618922305764411,0.11254196340939995,6,4.068632188828396,0.03104786021350874,0.10375098145235381,1.0,True,
|
||||
16,18,3.5240348999326185,22.39089624755224,0.6890156918687589,0.11084024896736888,6,4.42116710884217,0.01556225520371614,0.02495881796886513,1.0,True,
|
||||
16,19,2.146829294717985,30.035090485266103,0.8685060899826,0.10135543575024519,6,2.92007223300188,0.002952251383831446,0.02753369989560042,1.0,True,
|
||||
17,18,2.640403049812329,27.891408321361506,0.7697708305735859,0.10648049893472207,6,3.7928223564531667,0.010582276181446382,0.039599051949106026,1.0,True,
|
||||
17,19,3.933985934417215,80.31739505417984,0.6293759512937596,0.1095449771750205,6,3.6483069293931876,0.012240937390583118,0.060306184678878015,1.0,True,
|
||||
17,20,4.2254212089887,152.98392843416656,0.6014520938674964,0.12300562605352797,6,4.719447385686107,1.3272837904090529,0.4474442011368189,1.0,False,forward_reverse_translation
|
||||
18,19,2.4460967931915643,52.425986732818345,0.6827314510833881,0.10834806615100012,6,3.5073857685652805,0.012418496053917759,0.11905018881098527,1.0,True,
|
||||
18,20,6.593931011285688,125.09252011280485,0.5756313809779688,0.1265759478434112,6,5.389095141151797,0.012917057356045326,0.24042373011343365,1.0,True,
|
||||
18,21,11.793089868757727,175.70238585456048,0.4044519656339495,0.14396876383759685,6,10.041635341774429,0.06610149517267662,0.6118050873537593,1.0,False,forward_reverse_rotation
|
||||
19,20,6.650720121741557,72.6665333799865,0.6234734541714874,0.12775305804057488,6,5.958603794025073,0.009237066767190358,0.22540488888099894,1.0,True,
|
||||
19,21,12.053580264031138,123.27639912174077,0.3788200074840963,0.14687803396129923,6,10.170069582593054,0.1645149893124099,1.1204113986936788,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
19,22,14.836246959975925,128.85294276465592,0.3215252152521525,0.1505833656059155,6,15.25410245430046,0.08605091089946505,0.48945936193054473,1.0,False,heldout_inlier_ratio;forward_reverse_translation
|
||||
20,21,5.40286046809103,50.60986574175429,0.6596992097884272,0.12147955429086157,6,4.0820327359553845,0.014725537493637062,0.1988422472286775,1.0,True,
|
||||
20,22,8.200956565000565,56.18640938466938,0.5739414499308958,0.13255415786946786,6,5.328486341352201,0.006040617548520927,0.19706632354687328,1.0,True,
|
||||
20,23,5.175118275082073,70.79178325235415,0.6456945156330087,0.12075756038041646,6,3.9642702950866386,0.022949011013485506,0.1911210947924365,1.0,True,
|
||||
21,22,2.8644847627909416,5.57654364291509,0.7716237647919971,0.11632271358541554,6,2.79620153032919,0.00996357879827298,0.15332892195322273,1.0,True,
|
||||
21,23,1.2936223973175418,20.18191751059984,0.8576224819696593,0.09865676260631216,6,3.0367950693845156,0.0039225733250952055,0.00898487649368532,1.0,True,
|
||||
21,24,2.3128500583741403,54.59467208208593,0.7715940569126165,0.1136150455355286,6,2.76815824933335,0.007475673605589086,0.042277690012993266,1.0,True,
|
||||
22,23,3.7194009250537223,14.605373867684753,0.7396689147762109,0.11702962848624102,6,3.4148047250889095,0.01814015943430581,0.11847321816106485,1.0,True,
|
||||
22,24,4.786117710081478,49.01812843917083,0.6983240223463687,0.11898873157361621,6,3.6334655936865663,0.013438931050857202,0.08244161259412694,1.0,True,
|
||||
22,25,2.3747421598149763,12.391903814042255,0.736861094407697,0.11586394710110075,6,2.383007054117407,0.018559552524775386,0.0645260279790244,1.0,True,
|
||||
23,24,1.0880644725336985,34.41275457148609,0.7853164556962026,0.1128725442321241,6,2.409921828847164,0.002195759849115173,0.032112959181416705,1.0,True,
|
||||
23,25,5.033935954106079,2.213470053642494,0.6881127450980392,0.12303206700403986,6,2.9363727218037994,0.004939188019100004,0.12964064637100806,1.0,True,
|
||||
23,26,5.765534437288107,40.730927532824346,0.6852618757612667,0.12040544972155913,6,3.0590065093977192,0.006032445250250899,0.14039335223024835,1.0,True,
|
||||
24,25,6.097212810998921,36.62622462512857,0.677667493796526,0.12346315805814134,6,3.6286524357748307,0.02347922689116974,0.16273859629352566,1.0,True,
|
||||
24,26,6.852918693116795,6.318172961338249,0.6530209617755857,0.12710147612984257,6,3.5331859775372005,0.013311199903818141,0.18193579850152686,1.0,True,
|
||||
24,27,7.47982907414204,31.60254826458195,0.6649014778325123,0.1248115961442785,6,4.10006355979974,0.017872821205486625,0.1680556511699705,1.0,True,
|
||||
25,26,1.2882746074869595,42.944397586466835,0.9127837514934289,0.0958142916589384,6,2.9017658682436953,0.003534148840172682,0.018440169300173608,1.0,True,
|
||||
25,27,1.7666235781584831,68.22877288971053,0.8510739856801909,0.10418150333394147,6,2.368253417089997,0.006103232696002816,0.13366555345652814,1.0,True,
|
||||
25,28,2.1807182972588706,88.09106909546726,0.8853518429870751,0.10761251229652005,6,2.6372860976794645,0.003483231665912094,0.0369774420199415,1.0,True,
|
||||
26,27,0.6269948654714946,25.284375303243706,0.9183867141162515,0.09074909570650921,6,2.8142315359282506,0.00040199505815422204,0.011554314408129918,1.0,True,
|
||||
26,28,0.9431372257108486,45.14667150900044,0.8853200095170116,0.10604184939655073,6,2.656402027789519,0.007320240476189879,0.12930335868605478,1.0,True,
|
||||
26,29,1.129925637779203,87.41573662125148,0.7880466815984911,0.10871274240898265,6,3.111957466886604,0.0063952430610542755,0.039517035902874385,1.0,True,
|
||||
27,28,0.44809323479571145,19.862296205756735,0.9289448669201521,0.0850509551532673,6,2.96325286846982,0.005157655135591474,0.022666807877311387,1.0,True,
|
||||
27,29,1.160064136065596,62.13136131800778,0.7872365477452019,0.10455513464835667,6,2.9621627623005296,0.011650639607012138,0.1624719134077532,1.0,True,
|
||||
27,30,4.25412615059095,152.09392556214777,0.04792444029850746,0.15706827084964883,6,3.4115087753552786,2.5641513880069633,2.874944882076104,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
28,29,1.5995723610319212,42.26906511225104,0.8000944621560987,0.1086547547358129,6,3.1466224100058553,0.0011394164967304414,0.02119627748719656,1.0,True,
|
||||
28,30,4.295353381127667,132.23162935639104,0.042095416276894296,0.16025490727880456,6,4.631028587407634,2.5607552487652696,2.17230408479254,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
28,31,5.754426930327516,158.8421298011293,0.7465330381074466,0.1066966653439246,6,2.402598366822474,0.00635764905956918,0.040016615214377715,1.0,True,
|
||||
29,30,4.099530130205247,89.96256424413995,0.7248812145092132,0.10870831469083347,6,2.9471223804737057,0.00465638537716095,0.12465763189227934,1.0,True,
|
||||
29,31,4.890018768530818,116.57306468887772,0.7243012243012243,0.11138751941762699,6,2.7645021322697017,0.004263906321896464,0.046637396086304225,1.0,True,
|
||||
29,32,4.464737187763289,159.39493219688632,0.6956070563818748,0.1103354081949904,6,3.227012588611917,0.011088061861347823,0.34561739459504637,1.0,True,
|
||||
30,31,2.3862917603439455,26.61050044473775,0.8185562292643862,0.0961579573295129,6,2.899558208444643,0.004999163455591531,0.0213847958734735,1.0,True,
|
||||
30,32,1.1507634071714652,69.43236795274659,0.8041343079031521,0.099533431536842,6,2.6146168742247573,0.003171376495143,0.02376737774843464,1.0,True,
|
||||
30,33,2.4361832484951105,160.7145717128102,0.0824534345711235,0.13003518012314966,6,7.2913036106936,1.3743823383381788,1.9837113244583153,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
31,32,1.2514382602180638,42.82186750800885,0.8157085941946499,0.09925944770258267,6,3.0485411400768245,0.004719596234887584,0.06146741991681224,1.0,True,
|
||||
31,33,0.641507519046697,134.10407126807203,0.0911563017261764,0.12702324561674966,6,7.541483837366941,2.816294381538612,3.333105317420589,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
32,33,1.4370687242949811,91.2822037600631,0.7569928006609229,0.10008740880870744,6,3.6611044282667207,2.988193350509204,2.696443494598379,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
|
File diff suppressed because it is too large
Load Diff
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -1,414 +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.2975484155117711,
|
||||
-0.0014693441955134902,
|
||||
0.7052088406121522
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.8301548470744382,
|
||||
1.3787117674135543,
|
||||
-0.9873301733452331
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.007139952974435751,
|
||||
0.012092890421885221,
|
||||
-0.008527968948116832,
|
||||
0.9998650192992987
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9995620714937284,
|
||||
0.01688095033591077,
|
||||
0.02430429482463916,
|
||||
1.2975484155117711
|
||||
],
|
||||
[
|
||||
-0.017226321011659856,
|
||||
0.9997525896342856,
|
||||
0.014071722849138928,
|
||||
-0.0014693441955134902
|
||||
],
|
||||
[
|
||||
-0.024060737635611132,
|
||||
-0.014484234025182223,
|
||||
0.9996055661455342,
|
||||
0.7052088406121522
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 34,
|
||||
"pairs": 42,
|
||||
"residuals": {
|
||||
"pairs": 42,
|
||||
"translation_m": {
|
||||
"rms": 0.12183423858445137,
|
||||
"median": 0.060649486998667235,
|
||||
"p90": 0.11912435755512873,
|
||||
"p95": 0.3030566180355447,
|
||||
"max": 0.45978165540815585
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.0805282705084651,
|
||||
"median": 0.7524132662595089,
|
||||
"p90": 1.482445248533153,
|
||||
"p95": 1.8550286425106393,
|
||||
"max": 4.3358524167513774
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.038624527608401314,
|
||||
"rotation_deg": 0.8023305035396044
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.04180287320767011,
|
||||
"rotation_deg": 0.6893264946463747
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.019127950093528003,
|
||||
"rotation_deg": 1.0042991942789465
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.06137084310587195,
|
||||
"rotation_deg": 0.3949193126839051
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.1071111410675142,
|
||||
"rotation_deg": 0.29878675978634545
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.06364016847596804,
|
||||
"rotation_deg": 0.5544336747047969
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.06293697736756677,
|
||||
"rotation_deg": 0.5686016770604462
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.024970034635953178,
|
||||
"rotation_deg": 0.5027938164737027
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.06609908977135717,
|
||||
"rotation_deg": 0.769573579451918
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.015198604597359418,
|
||||
"rotation_deg": 0.7395239518119227
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.08603113369700931,
|
||||
"rotation_deg": 0.38160978353264396
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.023734833308603348,
|
||||
"rotation_deg": 0.5279761588421565
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.044728554479905425,
|
||||
"rotation_deg": 0.8691250339782107
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.07088160554682986,
|
||||
"rotation_deg": 0.450151294217732
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.08888554602585935,
|
||||
"rotation_deg": 0.6592285262192215
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.05233056995457292,
|
||||
"rotation_deg": 1.8589142482602727
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.12686760514047216,
|
||||
"rotation_deg": 1.9773175636228766
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.06853778836627006,
|
||||
"rotation_deg": 0.2507622154612463
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.0613281598931951,
|
||||
"rotation_deg": 0.3396141740929532
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.03461219125828727,
|
||||
"rotation_deg": 0.5120113019840405
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.003516478473153868,
|
||||
"rotation_deg": 0.3641614482709119
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.059970814104139375,
|
||||
"rotation_deg": 0.61595189426212
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.011156452422652521,
|
||||
"rotation_deg": 0.39742394993610364
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.04780738274324129,
|
||||
"rotation_deg": 1.7812021332676105
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.0580893770008635,
|
||||
"rotation_deg": 0.879332260016813
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.12024661823169551,
|
||||
"rotation_deg": 4.3358524167513774
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.06196910794842297,
|
||||
"rotation_deg": 1.5238281010443262
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.0503874055448458,
|
||||
"rotation_deg": 0.972956387575872
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.01757080571201053,
|
||||
"rotation_deg": 0.8111658366205472
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.032044188496992296,
|
||||
"rotation_deg": 0.943436351113904
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.05418626153418599,
|
||||
"rotation_deg": 0.5111200883804832
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.02622969795541729,
|
||||
"rotation_deg": 0.26503765103607685
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.10034938173040396,
|
||||
"rotation_deg": 0.8910089412227198
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.024232025179701657,
|
||||
"rotation_deg": 0.2983628682948933
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.06450981390919311,
|
||||
"rotation_deg": 1.1099995759326005
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.06841378002467255,
|
||||
"rotation_deg": 0.765302580707095
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.45978165540815585,
|
||||
"rotation_deg": 0.8318777085643659
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.31232972397739145,
|
||||
"rotation_deg": 0.7893009066081454
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.3910017597095041,
|
||||
"rotation_deg": 0.6583887677988087
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.1015358221759577,
|
||||
"rotation_deg": 0.8428062435042994
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.1090240114660279,
|
||||
"rotation_deg": 0.8418950507797021
|
||||
},
|
||||
{
|
||||
"pair_index": 41,
|
||||
"translation_m": 0.03539906786781987,
|
||||
"rotation_deg": 0.7658972874383352
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"ground": {
|
||||
"planes": 34,
|
||||
"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.007068161359903731,
|
||||
0.007140938677742994,
|
||||
0.00460528013456401
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.07009913972450056,
|
||||
0.0632417549466229,
|
||||
0.13781143232539678
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 6.8653146384480275,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 165.05915287927994,
|
||||
"success": true,
|
||||
"translation_m": 3.0576433724414325e-08,
|
||||
"rotation_deg": 1.5435010182701515e-07
|
||||
},
|
||||
{
|
||||
"cost": 165.05915287928036,
|
||||
"success": true,
|
||||
"translation_m": 3.132972747129673e-08,
|
||||
"rotation_deg": 1.5835217811052665e-07
|
||||
},
|
||||
{
|
||||
"cost": 165.0591528792959,
|
||||
"success": true,
|
||||
"translation_m": 5.032571008409966e-08,
|
||||
"rotation_deg": 6.019987077228086e-07
|
||||
},
|
||||
{
|
||||
"cost": 165.0591528792966,
|
||||
"success": true,
|
||||
"translation_m": 5.0134365128719625e-08,
|
||||
"rotation_deg": 6.036593761377996e-07
|
||||
},
|
||||
{
|
||||
"cost": 165.059152879271,
|
||||
"success": true,
|
||||
"translation_m": 4.0000992845476256e-09,
|
||||
"rotation_deg": 5.9504483976085874e-08
|
||||
},
|
||||
{
|
||||
"cost": 165.0591528792741,
|
||||
"success": true,
|
||||
"translation_m": 1.71650324878148e-08,
|
||||
"rotation_deg": 2.1433774624387286e-07
|
||||
},
|
||||
{
|
||||
"cost": 165.059152879271,
|
||||
"success": true,
|
||||
"translation_m": 3.8510946546090955e-09,
|
||||
"rotation_deg": 5.7484467409642816e-08
|
||||
},
|
||||
{
|
||||
"cost": 165.05915287927274,
|
||||
"success": true,
|
||||
"translation_m": 1.2609667860271796e-08,
|
||||
"rotation_deg": 1.3972470982253386e-07
|
||||
},
|
||||
{
|
||||
"cost": 165.05915287929693,
|
||||
"success": true,
|
||||
"translation_m": 5.0359957454032455e-08,
|
||||
"rotation_deg": 6.053505895170472e-07
|
||||
},
|
||||
{
|
||||
"cost": 165.05915287927098,
|
||||
"success": true,
|
||||
"translation_m": 0.0,
|
||||
"rotation_deg": 0.0
|
||||
},
|
||||
{
|
||||
"cost": 165.0591528792721,
|
||||
"success": true,
|
||||
"translation_m": 1.123025431013551e-08,
|
||||
"rotation_deg": 3.014422244971439e-08
|
||||
},
|
||||
{
|
||||
"cost": 165.0591528792711,
|
||||
"success": true,
|
||||
"translation_m": 2.4200230224271075e-09,
|
||||
"rotation_deg": 6.741856490734345e-08
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.0030749822448559927,
|
||||
0.0034380377435498433,
|
||||
0.0016315909218529902,
|
||||
0.07586073216888294,
|
||||
0.07187134364073751,
|
||||
0.11146702425116647
|
||||
],
|
||||
"p025": [
|
||||
1.292007464312339,
|
||||
-0.006797525226093335,
|
||||
0.701848741787294,
|
||||
-0.9789682101604196,
|
||||
1.2351532937224006,
|
||||
-1.211682139162798
|
||||
],
|
||||
"p975": [
|
||||
1.3029347232892419,
|
||||
0.005407572756192536,
|
||||
0.7084594772174496,
|
||||
-0.705679642362121,
|
||||
1.5258122853862286,
|
||||
-0.8045746213351143
|
||||
]
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user