完善 LiDAR–双天线 RTK 手眼标定仓库：补充旧式及多传感器数据导出、small_gicp/Open3D GICP 标定、结果复核与3D可视化流程，并整理三批数据和标定结果说明。
This commit is contained in:
lichun.qu
@@ -0,0 +1,194 @@
|
||||
木兰宽松许可证，第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
-1
@@ -33,7 +33,7 @@ norm = 7.9716 cm / 0.547109 deg
|
||||
查看原结果：
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
$Repo = "D:\你的代码目录\calibration"
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_open3d_result.ps1" -PairIndex 0
|
||||
```
|
||||
|
||||
|
||||
@@ -1,399 +1,226 @@
|
||||
# 双天线 RTK—3D 激光雷达严谨手眼标定
|
||||
# LiDAR–双天线 RTK 手眼标定
|
||||
|
||||
本仓库是当前建议提交和继续维护的正式版本。第二批 38 站高频 RTK 用于主要求解；第一批 38 站稀疏 RTK 只作辅助复核。Open3D GICP 和 small_gicp 分别生成雷达运动 (B)，两套独立结果及跨后端共识结果均保留。
|
||||
|
||||
实际部署只读取 [`results/final_extrinsic_deployment.json`](results/final_extrinsic_deployment.json)。算法原始求解、残差、Bootstrap 和筛选记录保留在其他结果文件中，不应与部署文件混用。
|
||||
|
||||
## 标定总体流程
|
||||
|
||||
1. **准备静止站点数据**：每站车辆静止，保存一帧原始雷达点云和该站 RTK；程序只使用雷达传感器坐标系下的 `points_raw`。
|
||||
2. **由 RTK 生成车体运动 (A)**：将双天线 RTK 的位置、水平航向、杆臂和 heading offset 转为车体位姿 (T_{WB_i})，再计算
|
||||
|
||||
\[
|
||||
A_{ij}=T_{WB_i}^{-1}T_{WB_j}.
|
||||
\]
|
||||
3. **由点云配准生成雷达运动 (B)**：Open3D GICP 和 small_gicp 分别把第 j 站点云配准到第 i 站，得到
|
||||
|
||||
\[
|
||||
B_{ij}=T_{L_iL_j}.
|
||||
\]
|
||||
4. **先验证和筛选 B，再求 X**：使用对称留出点、重叠率、留出点 RMSE、海森矩阵、正反向一致性、多初值稳定性、旋转共轭不变量和闭环误差检查 B。这个阶段不读取待求外参 X，也不使用 AX 残差筛选 B。
|
||||
5. **建立跨后端共识 B**：只保留 Open3D 与 small_gicp 对同一运动对的 B 相差不超过 `0.05 m / 0.50°` 的边；最终使用 Open3D B，small_gicp 作为独立一致性门控。
|
||||
6. **求解手眼外参 X**：固定 A 和通过筛选的 B 后，求解
|
||||
|
||||
\[
|
||||
A_{ij}X=XB_{ij},\qquad X=T_{body\leftarrow lidar}.
|
||||
\]
|
||||
|
||||
使用 Huber 鲁棒最小二乘、12 个初值、地面平面约束和 100 次 Bootstrap。
|
||||
7. **结果复核**：第二批共识对用于主要求解和整体残差统计；第一批重新独立生成 B，只作为稀疏 RTK 辅助复核；3D 可视化分别比较模式 3 的 GICP B 与模式 4 的 (X^{-1}AX)。
|
||||
8. **形成部署结果**：第二批跨后端共识结果给出完整的 `[x,y,z,roll,pitch,yaw]`；部署 JSON 直接采用该算法结果。。
|
||||
|
||||
### 验证逻辑
|
||||
|
||||
顺序固定为：
|
||||
本仓库提供一套可从原始 Medulla 记录复现的静态站点标定流程，求解三维激光雷达到后轮轴中心车体系的外参
|
||||
|
||||
```text
|
||||
先生成 B
|
||||
-> 用不依赖 X 的点云与几何指标筛 B
|
||||
-> 用另一个 GICP 后端复核同一 B
|
||||
-> 固定共识 B
|
||||
-> 最后才求 X
|
||||
-> AX 残差只用于结果评价，不反过来选择 B
|
||||
X = T_body_lidar
|
||||
```
|
||||
|
||||
代码证据：
|
||||
|
||||
- [`code/refine_pairs.py`](code/refine_pairs.py) 不读取外参文件，只使用留出点、正反向和旋转共轭不变量等指标；
|
||||
- [`code/cross_backend_filter.py`](code/cross_backend_filter.py) 只比较两个后端的 B，不读取 X；
|
||||
- [`code/rigorous_calibration.py`](code/rigorous_calibration.py) 在 B 集合固定后才执行 `calibrate` 求 X；
|
||||
- 第一批辅助复核的 B 也重新独立生成，且不使用 AX 残差筛选。
|
||||
|
||||
但这些检查仍不能把 B 变成绝对真值：重复结构、动态物体或错误局部最优仍可能让两个后端同时出错，所以必须结合模式 3 点云质量、留出集指标和多运动对整体统计判断。
|
||||
|
||||
## 最终部署外参
|
||||
|
||||
坐标变换定义：
|
||||
|
||||
\[
|
||||
X=T_{body\leftarrow lidar}
|
||||
\]
|
||||
|
||||
即将原始雷达点从雷达坐标系变换到以后轮轴中心为原点、X 前/Y 左/Z 上的车体坐标系。
|
||||
|
||||
部署值：
|
||||
|
||||
- 平移 `[x,y,z] = [1.297760, -0.000067, 0.720498] m`
|
||||
- RPY `[roll,pitch,yaw] = [-0.785151, 1.202661, -0.835510] deg`
|
||||
- 唯一部署文件：[`results/final_extrinsic_deployment.json`](results/final_extrinsic_deployment.json)
|
||||
|
||||
该值来自第二批 39 对跨后端共识、完整地面平面约束和 Bootstrap 稳定性检查。
|
||||
|
||||
算法在完整地面平面模型下的原始结果为：
|
||||
|
||||
- 平移 `[1.297760, -0.000067, 0.720498] m`
|
||||
- RPY `[-0.785151, 1.202661, -0.835510] deg`
|
||||
- 文件：[`results/final_extrinsic_recommended.json`](results/final_extrinsic_recommended.json)
|
||||
|
||||
`final_extrinsic_recommended.json` 保留完整求解、残差和 Bootstrap 记录，用于复现和审计；`final_extrinsic_deployment.json` 是唯一部署入口。
|
||||
|
||||
共识结果统计：
|
||||
|
||||
- 第二批 39 对 AX RMS：`0.07985 m / 0.96118°`
|
||||
- 第一批 22 对辅助复核：`0.06067 m / 1.00298°`
|
||||
- 结果摘要：[`results/final_summary.json`](results/final_summary.json)
|
||||
|
||||
两套后端分别求得：
|
||||
|
||||
| 后端 | 平移 `[x,y,z]` m | RPY deg | 第二批 AX RMS |
|
||||
|---|---|---|---|
|
||||
| Open3D GICP | `[1.297883,-0.003100,0.721789]` | `[-0.757207,1.146042,-0.879781]` | `0.1002 m / 1.0115°` |
|
||||
| small_gicp | `[1.299630,-0.003565,0.721886]` | `[-0.787156,1.141456,-0.935757]` | `0.1187 m / 1.1351°` |
|
||||
|
||||
两后端单独求出的 X 相差 1.81 mm、0.063°。共识求解的 100 次 Bootstrap 标准差 `[x,y,z,roll,pitch,yaw]` 为：
|
||||
约定 `T_A_B` 将 B 系坐标变换到 A 系。对任意站点 i、j：
|
||||
|
||||
```text
|
||||
[0.00322 m, 0.00333 m, 0.00169 m, 0.0980°, 0.0746°, 0.1189°]
|
||||
A_ij = T_W_Bi^-1 T_W_Bj # RTK 给出的车体相对运动
|
||||
B_ij = T_Li_Lj # GICP 给出的雷达相对运动
|
||||
A_ij X = X B_ij
|
||||
```
|
||||
|
||||
这里的 Z Bootstrap 只描述当前地面模型条件下的内部稳定性，不代表机械高度的绝对精度。
|
||||
当前部署建议仍采用 [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 残差更高，因此只作为候选和稳定性证据，不自动替换部署值。
|
||||
|
||||
重要限制：旋转 RMS 仍约 1°，所以这不是厘米级绝对精度认证。远距离点云仍可能看到角度错层；Bootstrap 也不包含 RTK 参考点、航向偏置或杆臂定义错误等系统误差。
|
||||
### `code/` 文件职责
|
||||
## 标定总流程
|
||||
|
||||
`code/` 中的 Python 脚本按“生成 B → 独立筛选 B → 求解 X → 汇总/诊断”的顺序组织。通常应通过 `run/` 下的 PowerShell 脚本调度，而不是手工跳过中间筛选步骤。
|
||||
|
||||
| 文件 | 职责 | 主要输入 → 主要输出 |
|
||||
|---|---|---|
|
||||
| [`rigorous_calibration.py`](code/rigorous_calibration.py) | **主求解器**，实现坐标变换、Open3D GICP/small_gicp 配准、质量计算和手眼优化。`ground` 从 `points_raw` 做 RANSAC 地面平面；`pairs` 将 RTK 车体轨迹构造成 A、将点云配准构造成 B，并输出海森矩阵、留出点、正反向和多初值质量报告；`calibrate` 用 Huber 鲁棒最小二乘、地面约束、多初值和 Bootstrap 求 `T_body_lidar`；`validate` 只计算给定 X 在独立 B 集上的 AX 残差。 | prepared 的 `frames_all`、车体位姿 CSV、B/地面文件 → `.npz` B 集、质量 JSON/CSV、外参 JSON、辅助复核 JSON。 |
|
||||
| [`refine_pairs.py`](code/refine_pairs.py) | **第二级 B 精筛器**。按留出点重叠率/RMSE、旋转共轭不变量、正反向一致性剔除差的 B；不读取 X，也不用 AX 残差选 B。 | 初始 B `.npz` + 配准质量 JSON → `*_refined.npz` + `*.refinement.json` 审计记录。 |
|
||||
| [`cross_backend_filter.py`](code/cross_backend_filter.py) | **跨后端一致性门控**。只保留 Open3D 与 small_gicp 对同一运动对的 B 差异不超过默认 `0.05 m / 0.50°` 的边；最终 B 本体采用 Open3D，small_gicp 仅作为独立门控。 | 两套 refined B `.npz` → `consensus/B_batch*_consensus.npz` + `*.consensus.json`。 |
|
||||
| [`finalize_consensus.py`](code/finalize_consensus.py) | **发布共识解**。读取共识 B 求得的外参、第一批辅助复核和两单后端外参，写出带选择依据的 `final_extrinsic_recommended.json` 与 `final_summary.json`。 | 共识外参/复核 JSON、两后端外参 JSON → 推荐外参和最终摘要 JSON。 |
|
||||
| [`summarize_results.py`](code/summarize_results.py) | **单后端阶段汇总器**。比较 Open3D 与 small_gicp 的外参差异、第二批拟合残差和第一批辅助复核，生成 `comparison_summary.json`；完整流程随后由 `finalize_consensus.py` 以共识解覆盖推荐结果。 | 两单后端外参、质量、辅助复核 JSON → 对比摘要 JSON 与阶段性推荐 JSON。 |
|
||||
| [`scan_extrinsic_sensitivity.py`](code/scan_extrinsic_sensitivity.py) | **局部灵敏度诊断器**。对 X 在车体系左乘小的 roll/pitch/yaw 修正，比较某一对与全部 B 对的残差、改善/变差数量、地面指标和 Z 可观性。只生成扫描 JSON/CSV，**绝不改写 X**。 | B `.npz`、外参 JSON、可选地面 CSV → `diagnostics/*_scan.json/.csv`。 |
|
||||
| [`visualize_pair_3d.py`](code/visualize_pair_3d.py) | **交互式 3D 点云核查器**。蓝色为目标站 i、橙色为源站 j；按键 1/2/3/4 分别查看原始、RTK 初值 A、GICP B、外参预测 `X^-1 A X`，并打印 `B^-1(X^-1AX)` 的 cm/deg 增量。可选按键 5 显示临时 RPY 修正，修正不写入文件。 | 站点点云、B `.npz`、外参 JSON → Open3D 交互窗口与终端增量。 |
|
||||
|
||||
## 坐标与公式
|
||||
|
||||
\[
|
||||
A_{ij}=T_{WB_i}^{-1}T_{WB_j},\qquad
|
||||
B_{ij}=T_{L_iL_j},\qquad
|
||||
A_{ij}X=XB_{ij}
|
||||
\]
|
||||
|
||||
`B_ij` 把第 j 站雷达点变换到第 i 站雷达坐标系。程序只读取 `points_raw`，禁止使用已经变到车体坐标系的点。
|
||||
|
||||
RTK 轨迹使用 `body_poses_rear_gga_raw_rear_to_front.csv`，对应后天线 GGA 位置和双天线后到前方向。
|
||||
|
||||
地面约束采用完整平面变换：
|
||||
|
||||
\[
|
||||
n_B=R_Xn_L,\qquad r_h=d_L-n_B^Tt_X-h_{body}
|
||||
\]
|
||||
|
||||
不再使用旧近似式 `d_L-(t_z+h_body)`。
|
||||
|
||||
## 详细求解流程
|
||||
|
||||
1. 第二批高频 RTK 生成 A；第一批稀疏 RTK 不参与主要求解。
|
||||
2. 不使用手量外参作初值：生成 B 时 (X_0=I)，所以 (B_0=A)；求 X 也从单位变换和随机初值开始。
|
||||
3. 两后端均采用三级粗到细：体素 0.30/0.15/0.08 m，对应距离 1.20/0.50/0.25 m。
|
||||
4. 逐对检查对称留出点、海森矩阵、正反向一致性、多初值稳定性和 B 三角闭环。
|
||||
5. 使用重叠率、留出点 RMSE 和旋转共轭不变量筛选 B；不使用 AX 残差筛选。
|
||||
6. 两后端分别求 X；随后使用跨后端 B 一致性得到最终 39 对共识集合。
|
||||
7. 使用 Huber 鲁棒最小二乘、12 个随机初值、完整地面约束和 100 次 Bootstrap 求算法 X。
|
||||
8. 第一批重新独立生成 B，仅作辅助复核，不参与第二批求解。
|
||||
9. 使用共识算法完整解形成精简部署 JSON，不覆盖算法原始结果。
|
||||
|
||||
## 目录
|
||||
|
||||
```text
|
||||
code/
|
||||
rigorous_calibration.py
|
||||
refine_pairs.py
|
||||
cross_backend_filter.py
|
||||
summarize_results.py
|
||||
finalize_consensus.py
|
||||
scan_extrinsic_sensitivity.py
|
||||
visualize_pair_3d.py
|
||||
run/
|
||||
run_all.ps1
|
||||
run_consensus_finish.ps1
|
||||
run_sensitivity_scan.ps1
|
||||
view_open3d_result.ps1
|
||||
view_small_gicp_result.ps1
|
||||
tools/
|
||||
frontlidar_dlog_export.py # 从单个原始 dlog 导出 LiDAR NPZ 与 RTK sidecar
|
||||
prepare_station_dataset.py # 从逐站导出结果构建标定所需 prepared 数据集
|
||||
results/
|
||||
common/
|
||||
open3d_gicp/
|
||||
small_gicp/
|
||||
consensus/
|
||||
diagnostics/
|
||||
final_extrinsic_deployment.json # 实际部署只读取这个文件
|
||||
final_extrinsic_recommended.json # 算法原始结果及详细诊断
|
||||
final_summary.json
|
||||
```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 可视化"]
|
||||
```
|
||||
|
||||
## 从原始数据到最终外参
|
||||
流程有两个原始数据入口：
|
||||
|
||||
下面是从云盘中的**原始 dlog**到最终 LiDAR—车体外参的完整流程。第 2、3 步由本仓库的 tools/ 脚本完成；第 4 步及之后由 run/ 调度。不要把云盘中的原始数据直接传给 run_all.ps1。
|
||||
- 旧式数据：LiDAR 和 `GPS-POST-Z` 位于每个站点 dlog 中，使用 `export_legacy_stations.ps1`。
|
||||
- 新式多传感器数据：LiDAR 位于逐站 dlog，RTK 与 IMU 是独立 `.rscap`，使用 `export_multisensor_stations.ps1`。处理顺序是统一时间轴、分别解析、按 LiDAR 帧关联、导出 NPZ。
|
||||
|
||||
```text
|
||||
原始静止站点 dlog（两批）
|
||||
→ 逐站导出 LiDAR 原始点云 + RTK 旁路表（NPZ）
|
||||
→ 逐站质量筛选、选取静止帧、重建 RTK 车体位姿（prepared）
|
||||
→ Open3D / small_gicp 分别求 B
|
||||
→ X 无关的 B 精筛与跨后端共识
|
||||
→ AX=XB + 地面平面约束求 X
|
||||
→ 第一批辅助复核、3D 点云核查、确认部署 JSON
|
||||
```
|
||||
IMU 会在新式数据中原样解析并随 LiDAR 帧关联保存，但当前 LiDAR–RTK 外参求解不使用 IMU，也不做运动畸变校正，因为每一站采集点云时车辆静止。IMU 外参应使用单独的激励数据和专用标定流程求解。
|
||||
|
||||
### 1. 整理原始数据与标定元数据
|
||||
## 三批数据的角色
|
||||
|
||||
每一个静止站点目录必须完整保留 LiDAR DObject 记录、对应 `.dorec` 文件和 RTK 日志。dlog 导出器要求站点目录至少具有：
|
||||
| 数据 | 原始格式 | 站点 | 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 | 独立重算与跨批比较 |
|
||||
|
||||
```text
|
||||
<station_dlog>/
|
||||
dobject/ # DObject 索引日志
|
||||
dobject_recording/ # 与索引对应的 .dorec 二进制记录
|
||||
GPS-POST-Z/ # RTK 文本记录；建议同时保留完整原始 N4 串口流
|
||||
```
|
||||
原始数据体积较大，不在 Git 仓库中。复现者应从云盘取得第一批、第二批或 data4 的原始目录，并在命令行传入路径。
|
||||
|
||||
两批数据要分开存放：第二批高频 RTK 用于主要求解，第一批仅用于辅助复核。与数据一同归档的标定元数据必须包括：LiDAR DObject 名称、时区、RTK 天线参考点、双天线基线方向、heading offset、天线到后轮轴中心的杆臂、车体系定义，以及 LiDAR/RTK/IMU 的时间基准。
|
||||
## 环境
|
||||
|
||||
对于后续新录数据，RTK 原始记录应保留 `gnss_week`、`gnss_tow_ms`、GGA UTC、`pitch_deg`、基线长度、解算状态/类型、标准差、HDOP 和完整原文。PC 接收时间只能作为延迟诊断，不能替代 GNSS 测量时间。
|
||||
- Windows PowerShell 5.1 或 PowerShell 7
|
||||
- Python 3.10+
|
||||
- `pip install -r requirements.txt`
|
||||
- `small_gicp` 后端需要可导入 `small_gicp`；Open3D 后端需要 `open3d`
|
||||
|
||||
### 2. 将每个原始站点 dlog 导出为 NPZ
|
||||
所有脚本从自身位置推导仓库根目录。数据和输出路径均由参数传入，不依赖开发者电脑上的固定路径。
|
||||
|
||||
本仓库的 [tools/frontlidar_dlog_export.py](tools/frontlidar_dlog_export.py) 从每个静止站点的原始 dlog 导出 LiDAR 原始点云与 RTK sidecar。对每个站点分别执行；必须开启 RTK sidecar 与审计报告：
|
||||
## 从原始数据开始复现
|
||||
|
||||
以下路径只表示格式，请替换为自己的目录。
|
||||
|
||||
### A. 第一批、第二批旧式 dlog
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
$Exporter = "$Repo\tools\frontlidar_dlog_export.py"
|
||||
$RawStation = "<某一个原始静止站点 dlog 目录>"
|
||||
$StationOut = "<工作目录>\export\001"
|
||||
$Repo = "D:\你的代码目录\calibration"
|
||||
|
||||
python $Exporter `
|
||||
--dlog "$RawStation" `
|
||||
--out "$StationOut" `
|
||||
--object frontlidar `
|
||||
--format npz `
|
||||
--timezone +08:00 `
|
||||
--rtk-sidecars `
|
||||
--write-reports
|
||||
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` 是本项目已有两批数据采用的配置，不是通用常数；换车或改变天线安装后必须重新确认。
|
||||
|
||||
```text
|
||||
<StationOut>/
|
||||
frames/ # 多帧 LiDAR NPZ；每帧保留 points_raw
|
||||
rtk/ # gps_post_z.csv/jsonl/npz 等 RTK 旁路表
|
||||
reports/manifest.csv # 每帧导出状态、时间及 RTK 匹配信息
|
||||
reports/validation_report.json
|
||||
reports/metadata.json
|
||||
```
|
||||
|
||||
先检查每个站点的 `validation_report.json`、`manifest.csv` 和 `rtk/gps_post_z.csv`：LiDAR payload 必须可读；候选帧必须存在；RTK 位置/航向有效；并记录 LiDAR—RTK 时间差。若新插件已开始输出 GNSS 周/周内时间与 pitch，导出器也必须同步扩展并写出这些字段；旧导出器只保留旧插件字段时，不能伪称已完成严格时间同步。
|
||||
|
||||
### 3. 从导出 NPZ 构建 prepared 静止站点数据
|
||||
|
||||
对每个批次，将所有站点导出目录汇总后执行以下预处理逻辑，生成供本仓库使用的 `prepared` 目录：
|
||||
|
||||
1. 每个站点只选择一帧**车辆已静止**且 RTK 质量合格的 LiDAR 帧；通常取有效候选帧中的中间帧，避免启动/停车边缘。
|
||||
2. 仅保留 `position_valid=true`、`heading_valid=true`、Fix 为 4 或 5，且航向标准差满足项目门限的 RTK 样本；记录每站的样本数、时间跨度、航向圆标准差与被剔除原因。
|
||||
3. 使用 GNSS 测量时间把位置、heading 和 pitch 配成同一时刻；将经纬高转换到同一 ENU 世界系；根据天线杆臂和双天线方向求后轮轴中心车体位姿 `T_WB`。双天线可提供 heading/pitch，但不能提供 roll；roll 在后续 LiDAR—IMU 流程中由 IMU 补充。
|
||||
4. 将选出的 LiDAR 帧原样复制到 `frames_all/`。标定程序从每帧的 `points_raw` 转为 LiDAR 传感器系 XYZ，不得使用已经变换到车体系的点。
|
||||
5. 输出时间戳车体位姿 CSV。当前主求解器要求列为：
|
||||
|
||||
```text
|
||||
time,x,y,z,qx,qy,qz,qw
|
||||
```
|
||||
|
||||
最终目录必须类似：
|
||||
|
||||
```text
|
||||
<BatchPrepared>/
|
||||
frames_all/
|
||||
station_01.npz
|
||||
station_02.npz
|
||||
...
|
||||
body_poses_rear_gga_raw_rear_to_front.csv
|
||||
station_summary.csv # 推荐保留：站点质量和选择原因
|
||||
manifest.json # 推荐保留：参考点、杆臂、heading/pitch 定义与筛选配置
|
||||
```
|
||||
|
||||
本仓库的 [tools/prepare_station_dataset.py](tools/prepare_station_dataset.py) 完成上述固定站点筛选、点云复制和车体位姿重建。它要求每个站点均已由上一步使用 --rtk-sidecars --write-reports 导出。下面的参数仅用于复现本仓库历史结果：lat/lon/h 被假定为后天线相位中心，raw_heading_deg 被假定为后天线指向前天线；车体系为后轮轴中心、X 前 Y 左 Z 上。
|
||||
|
||||
~~~powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
$Prepare = "$Repo\tools\prepare_station_dataset.py"
|
||||
|
||||
# 第二批：主要求解数据。ExportBatch2 下为 38 个逐站导出目录。
|
||||
python $Prepare --export-root "<ExportBatch2>" --output "<PreparedBatch2>" --expected-stations 38 --heading-std-limit-deg 0.5 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front
|
||||
|
||||
# 第一批：辅助复核数据。若航向质量较低，不应设置过严的 heading 标准差阈值。
|
||||
python $Prepare --export-root "<ExportBatch1>" --output "<PreparedBatch1>" --expected-stations 38 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front
|
||||
~~~
|
||||
|
||||
命令完成后检查 <PreparedBatch*>/station_summary.csv：每行均应有有效 RTK 样本，且没有超出设定的航向离散度。manifest.json 记录实际采用的杆臂、航向偏移、ENU 原点和输入站点；它应与原始数据一同归档。
|
||||
|
||||
不要把上述 21.226° 和 [-0.320,-0.365,0.620] m 当成通用常数：它们是本车、后天线、后天线到前天线航向定义、以及后轮轴中心车体原点的历史配置。换车、换参考天线、改变车体原点或改变 rawHeading 定义后，必须先复核并替换这些参数，再重建 A。
|
||||
|
||||
当前导出器可复现本仓库历史日志，但历史 RTK 插件只记录工控机接收时间，并未保存 GNSS 周/周内时间、双天线 pitch、质量标准差及完整原始串口流。因此它不能把旧数据宣称为严格 GNSS 时间同步。插件升级后，应先扩展导出器以保存新字段，再以 GNSS 测量时间重建位置/航向（及可用的 pitch）轨迹。
|
||||
### 4. 安装标定依赖并运行第一阶段
|
||||
### B. data4 式独立 RTK/IMU rscap
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
python -m pip install -r "$Repo\requirements.txt"
|
||||
python -c "import numpy, scipy, open3d, small_gicp; print('dependencies OK')"
|
||||
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 "<第一批 prepared 目录>" `
|
||||
-Batch2Prepared "<第二批 prepared 目录>"
|
||||
-Batch1Prepared "D:\你的输出目录\batch1_prepared" `
|
||||
-Batch2Prepared "D:\你的输出目录\batch2_prepared" `
|
||||
-OutputRoot "D:\你的输出目录\two_batch_calibration"
|
||||
```
|
||||
|
||||
`run_all.ps1` 完成：第二批地面拟合；Open3D 与 small_gicp 两套 B 生成和精筛；两套单后端 X 求解；第一批独立 B 与辅助复核；以及单后端对比摘要。第一阶段至少应生成：
|
||||
`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
|
||||
results/common/ground_planes_batch2.csv
|
||||
results/open3d_gicp/B_batch2_refined.npz
|
||||
results/small_gicp/B_batch2_refined.npz
|
||||
results/open3d_gicp/extrinsic_batch2_refined.json
|
||||
results/small_gicp/extrinsic_batch2_refined.json
|
||||
results/comparison_summary.json
|
||||
code/ 标定、配准筛选、共识、比较和可视化核心程序
|
||||
tools/ 原始 dlog/rscap 解析、时间关联、NPZ 导出和数据准备
|
||||
run/ 不含本机固定路径的 PowerShell 入口
|
||||
results/ 历史两批、data4 与跨批比较三个结果目录
|
||||
```
|
||||
|
||||
### 5. 构造跨后端共识并生成算法结果
|
||||
完整复现流程和所有主要文件职责均在本 README；`run/README.md`、`tools/README.md` 和 [results/README.md](results/README.md) 只是目录内快速索引。
|
||||
|
||||
第一阶段成功后执行：
|
||||
## 代码、工具和运行入口职责
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"
|
||||
```
|
||||
根 README 是本仓库唯一的完整复现说明。`run/README.md`、`tools/README.md` 和 `results/README.md` 只作为进入对应目录时的快速索引，不承载另一套流程。
|
||||
|
||||
该步骤以 `0.05 m / 0.50°` 门限对同一运动对的 Open3D 与 small_gicp B 做一致性门控，用共识 B 重求第二批外参，并将第一批只用于辅助复核。主要输出为：
|
||||
### 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
|
||||
results/consensus/B_batch2_consensus.npz
|
||||
results/consensus/extrinsic_batch2_consensus.json
|
||||
results/consensus/batch1_auxiliary_check.json
|
||||
results/final_extrinsic_recommended.json
|
||||
results/final_summary.json
|
||||
原始 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 做数值和三维检查
|
||||
```
|
||||
|
||||
### 6. 验证、可视化与部署确认
|
||||
|
||||
先检查 `results/final_summary.json`：当前历史数据的参考值是第二批共识 39 对、AX RMS 约 `0.07985 m / 0.96118°`。若明显偏离，应从第 2、3 步检查站点选择、RTK 时间/参考点/杆臂/heading 定义，而不是直接手调 X。
|
||||
## 重要限制
|
||||
|
||||
再使用 3D 可视化检查：模式 3 为点云配准 B，模式 4 为外参预测 `X^-1AX`；终端打印 `B^-1(X^-1AX)` 的平移与旋转增量。只有 B 质量、全体运动对统计、地面约束和可视化均合理时，才可确认外参。
|
||||
- RTK 车体姿态当前是双天线 heading 构造的 yaw-only 轨迹；没有用 RTK pitch/roll 构造 A。
|
||||
- 新式解析器保存 IMU 与 RTK pitch 等原始字段，但当前手眼方程未融合 IMU。
|
||||
- 静止站点法不估计 LiDAR–RTK 时间偏移；时间戳关联必须在导出阶段通过审计。
|
||||
- 地面约束负责 roll、pitch 和 z 的补充可观性，不会独立求出另一套六自由度外参。
|
||||
- 仓库归档的是结果和轻量 B 文件，不包含云盘中的原始点云数据。
|
||||
|
||||
`final_extrinsic_recommended.json` 是完整算法审计结果。`run_consensus_finish.ps1` 不会自动覆盖 `results/final_extrinsic_deployment.json`；部署 JSON 必须在人工确认坐标定义和高度约束后明确写入。
|
||||
## prepared 阶段的快速重跑
|
||||
## 专题说明
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
python -m pip install -r "$Repo\requirements.txt"
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1"
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"
|
||||
```
|
||||
|
||||
如果 prepared 数据移动了：
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" -Batch1Prepared "你的第一批prepared目录" -Batch2Prepared "你的第二批prepared目录"
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"
|
||||
```
|
||||
|
||||
重跑 `run_consensus_finish.ps1` 会重新生成算法原始结果，但不会自动生成或覆盖已经人工确认的 `final_extrinsic_deployment.json`。
|
||||
|
||||
## 3D 可视化
|
||||
|
||||
Open3D 单后端结果：
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_open3d_result.ps1" -PairIndex 0
|
||||
```
|
||||
|
||||
small_gicp 单后端结果：
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_small_gicp_result.ps1" -PairIndex 0
|
||||
```
|
||||
|
||||
蓝色是目标站 i，橙色是源站 j。按键：`1` 原始；`2` RTK 初始 A；`3` GICP B；`4` 外参预测的 (X^{-1}AX)；指定左乘 RPY 试验量后，`5` 为试验结果。
|
||||
|
||||
程序打印的差值为：
|
||||
|
||||
\[
|
||||
B^{-1}(X^{-1}AX)
|
||||
\]
|
||||
|
||||
其 xyz/RPY 分量位于站点 j 的雷达局部坐标系，不是屏幕坐标。模式 3 好而模式 4 差，说明 A/X 一致性不足；模式 3 本身差，说明该 B 不能用于判断外参。
|
||||
|
||||
共识运动对可直接指定部署外参：
|
||||
|
||||
```powershell
|
||||
python "$Repo\code\visualize_pair_3d.py" --frames "C:\Users\admin\Documents\Codex\2026-07-15\wo\outputs\calibration_data1_20260720\prepared\frames_all" --pairs "$Repo\results\consensus\B_batch2_consensus.npz" --extrinsic "$Repo\results\final_extrinsic_deployment.json" --pair-index 0
|
||||
```
|
||||
|
||||
局部灵敏度扫描和判断规则见 [`PAIR_DIAGNOSTICS.md`](PAIR_DIAGNOSTICS.md)。
|
||||
|
||||
## 关键质量文件
|
||||
|
||||
- [`results/open3d_gicp/B_batch2_quality.json`](results/open3d_gicp/B_batch2_quality.json)：海森矩阵、留出点、正反向和多初值信息；
|
||||
- [`results/open3d_gicp/B_batch2_refined.refinement.json`](results/open3d_gicp/B_batch2_refined.refinement.json)：二级筛选原因；
|
||||
- [`results/consensus/B_batch2_consensus.consensus.json`](results/consensus/B_batch2_consensus.consensus.json)：跨后端一致性筛选；
|
||||
- [`results/consensus/batch1_auxiliary_check.json`](results/consensus/batch1_auxiliary_check.json)：第一批辅助复核；
|
||||
- [`results/final_summary.json`](results/final_summary.json)：算法结果摘要；
|
||||
- [`results/diagnostics/`](results/diagnostics/)：局部修正对单对及全体运动对的影响。
|
||||
|
||||
## 必须外部确认的假设
|
||||
|
||||
- `lat/lon/h` 确为后天线相位中心；
|
||||
- `rawHeading` 确为后天线指向前天线；
|
||||
- heading offset 数值和正负号正确；
|
||||
|
||||
任何一项变化，都必须更新相应约束；RTK 定义变化时需要重新生成 A 并重跑。
|
||||
- [运动对诊断](PAIR_DIAGNOSTICS.md)
|
||||
- [双后端共识筛选](CONSENSUS_SELECTION.md)
|
||||
- [结果文件索引](results/README.md)
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
#!/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())
|
||||
@@ -0,0 +1,10 @@
|
||||
# 历史两批结果
|
||||
|
||||
第二批 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；其余文件用于审计和复现。
|
||||
+2
-2
@@ -6,8 +6,8 @@
|
||||
"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": "D:\\Outdoor Ackerman Cart Sensor Adaptation\\LiDAR_RTK_Calibration_Rigorous_20260721\\results\\consensus\\B_batch2_consensus.npz",
|
||||
"extrinsic_file": "D:\\Outdoor Ackerman Cart Sensor Adaptation\\LiDAR_RTK_Calibration_Rigorous_20260721\\results\\final_extrinsic_recommended.json",
|
||||
"pairs_file": "results\\01_previous_two_batches\\consensus\\B_batch2_consensus.npz",
|
||||
"extrinsic_file": "results\\01_previous_two_batches\\final_extrinsic_recommended.json",
|
||||
"stations": 38,
|
||||
"pairs": 39,
|
||||
"selected_pair_index": 0,
|
||||
+2
-2
@@ -6,8 +6,8 @@
|
||||
"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": "D:\\Outdoor Ackerman Cart Sensor Adaptation\\LiDAR_RTK_Calibration_Rigorous_20260721\\results\\open3d_gicp\\B_batch2_refined.npz",
|
||||
"extrinsic_file": "D:\\Outdoor Ackerman Cart Sensor Adaptation\\LiDAR_RTK_Calibration_Rigorous_20260721\\results\\final_extrinsic_recommended.json",
|
||||
"pairs_file": "results\\01_previous_two_batches\\open3d_gicp\\B_batch2_refined.npz",
|
||||
"extrinsic_file": "results\\01_previous_two_batches\\final_extrinsic_recommended.json",
|
||||
"stations": 38,
|
||||
"pairs": 66,
|
||||
"selected_pair_index": 0,
|
||||
+2
-2
@@ -6,8 +6,8 @@
|
||||
"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": "D:\\Outdoor Ackerman Cart Sensor Adaptation\\LiDAR_RTK_Calibration_Rigorous_20260721\\results\\small_gicp\\B_batch2_refined.npz",
|
||||
"extrinsic_file": "D:\\Outdoor Ackerman Cart Sensor Adaptation\\LiDAR_RTK_Calibration_Rigorous_20260721\\results\\final_extrinsic_recommended.json",
|
||||
"pairs_file": "results\\01_previous_two_batches\\small_gicp\\B_batch2_refined.npz",
|
||||
"extrinsic_file": "results\\01_previous_two_batches\\final_extrinsic_recommended.json",
|
||||
"stations": 38,
|
||||
"pairs": 80,
|
||||
"selected_pair_index": 0,
|
||||
+1
@@ -43,3 +43,4 @@
|
||||
]
|
||||
]
|
||||
}
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
# 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` 数值相同；前者是便于下游读取的顶层副本。
|
||||
@@ -0,0 +1,35 @@
|
||||
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
|
||||
|
@@ -0,0 +1,340 @@
|
||||
{
|
||||
"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.
@@ -0,0 +1,334 @@
|
||||
{
|
||||
"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
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,334 @@
|
||||
{
|
||||
"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.
@@ -0,0 +1,97 @@
|
||||
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
@@ -0,0 +1,414 @@
|
||||
{
|
||||
"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.
@@ -0,0 +1,156 @@
|
||||
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.8193962748876044,0.11049677170760565,6,14.01384280930539,0.026680306344951596,0.12400141971813113,1.0,True,
|
||||
0,2,2.633731568575307,80.09074797031298,0.7525388867463684,0.11492533799497681,6,14.962108606915407,0.0018465318763874656,0.03093450642761156,1.0,True,
|
||||
0,3,6.923255970100826,79.91583883299243,0.628093901505486,0.12365050970311489,6,23.49058941554809,0.010524333328229862,0.23898233567763014,1.0,True,
|
||||
0,4,2.952974950373882,130.25781950514033,0.693351593625498,0.11485738628517483,6,16.30512452170669,2.3214755241040135,1.4768350244766943,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
0,5,8.205074921973447,88.82606603101337,0.6015065913370998,0.12959862169013578,6,26.276485388438196,0.013711901937629547,0.26024107991311357,1.0,True,
|
||||
1,2,1.7155362084417105,55.91945052087219,0.7981310803891449,0.11167434332282761,6,13.484382276710292,0.050742368484802715,0.5170998105709517,1.0,False,forward_reverse_rotation
|
||||
1,3,5.885389341942907,55.74454138355163,0.678820988438572,0.12109867185657652,6,27.478714016209608,0.008324315958289934,0.25689852176843175,1.0,True,
|
||||
1,4,2.3031736807956613,106.08652205569953,0.6772473651580905,0.11157492180388107,6,13.25032679930311,0.00802452890723869,0.011334867255723058,1.0,True,
|
||||
1,5,7.590375029412286,64.65476858157257,0.618779694923731,0.12786963556791484,6,21.376356908960595,0.012044684321695526,0.5185219918090508,1.0,False,forward_reverse_rotation
|
||||
1,6,8.114848491620153,8.108983632525,0.6933789087226231,0.12162612003144421,6,23.560279629760274,0.008258211706006158,0.08942722541817279,1.0,True,
|
||||
2,3,4.339151683168145,0.17490913732054883,0.7609663064208518,0.11583878636897658,6,13.299472485774093,0.00784220284106163,0.01711532312869618,1.0,True,
|
||||
2,4,0.6150408096900305,50.167071534827386,0.796748976299789,0.11484342359047915,6,12.041666425070249,0.011210942709507262,0.11462542679280045,1.0,True,
|
||||
2,5,5.894923525501676,8.735318060700383,0.7112112112112112,0.12001317983504071,6,15.619771158055366,0.011298647069299764,0.02158504824207679,1.0,True,
|
||||
2,6,6.5470565092896145,47.810466888347186,0.7254529329785886,0.11794151013960824,6,22.73936753420594,0.004729020804343234,0.026463705119922912,1.0,True,
|
||||
2,7,5.848768648120739,73.12203319515616,0.7569187603621987,0.11528530285715083,6,13.439936375510058,0.011994525026310043,0.05882394078371607,1.0,True,
|
||||
3,4,3.9735035126146885,50.34198067214791,0.6957378664695738,0.11619002437046372,6,17.769677378118345,0.006018969660459514,0.057234124307389854,1.0,True,
|
||||
3,5,2.3619393178889707,8.910227198020932,0.7989069680784996,0.1077590577814381,6,12.151555874812605,0.01353229173052898,0.06486032517688407,1.0,True,
|
||||
3,6,2.2300116834828536,47.63555775102664,0.8435613682092555,0.11222309863990207,6,13.914901775514604,0.009512765198853305,0.09743636874085052,1.0,True,
|
||||
3,7,2.450274232383144,72.94712405783562,0.8651898734177215,0.11184568072686094,6,12.975777248765134,0.010827690226297962,0.12520280777371934,1.0,True,
|
||||
3,8,1.9936391786054533,163.56928836557165,0.825590155700653,0.10787987267070262,6,12.959410142765178,0.0056088079192386986,0.021536601402145895,1.0,True,
|
||||
4,5,5.370771070097231,41.431753474126985,0.6652516676773802,0.12444356767629798,6,17.0278455355532,0.003155412913416372,0.05824374716659687,1.0,True,
|
||||
4,6,6.142548765456278,97.97753842317456,0.6551681807021851,0.12030857820705748,6,24.380739297719824,0.011212969141455763,0.08373658947114908,0.5,True,
|
||||
4,7,5.314106070657687,123.28910472998356,0.696936001976773,0.11799538994568655,6,16.89426071448657,0.00409529177569414,0.0519770010059835,0.5,True,
|
||||
4,8,5.8981084980454765,146.08873096228052,0.7129198332924737,0.11349036082807595,6,18.616925126379197,0.01249802665331653,0.04549823831834878,1.0,True,
|
||||
4,9,3.1650365894055956,158.11607430100375,0.6097234068478128,0.11677937926620108,6,25.94675052408701,0.00616984405743089,0.11548555225177864,0.5,True,
|
||||
5,6,1.9357867378988893,56.54578494904757,0.7604901596732269,0.11101741624951145,6,16.791344333152654,0.01028641250952863,0.031127073736479716,1.0,True,
|
||||
5,7,0.19501559913675365,81.85735125585654,0.8092687180764918,0.10777871969807898,6,15.20338641054919,0.010579714886860828,0.03333492483252218,1.0,True,
|
||||
5,8,1.8038807059856978,172.47951556359365,0.7295760721789643,0.11311887218463837,6,17.053462425193878,0.009759205787181369,0.05935114015788255,1.0,True,
|
||||
5,9,7.881613621164215,160.45217222486949,0.514987714987715,0.13057312144553648,6,36.76355827214787,0.013423709236357283,0.8456480282562385,1.0,False,forward_reverse_rotation
|
||||
5,10,7.671857805815354,143.54232919109316,0.5442391832766165,0.13173165083201857,6,26.337222871823027,0.00523999417676037,0.17134499262937464,1.0,True,
|
||||
6,7,2.130456053070114,25.311566306808988,0.8539354187689203,0.10462253085153163,6,12.502910766598342,0.002843314342407394,0.028424505305675103,1.0,True,
|
||||
6,8,0.24494500622098103,115.93373061454483,0.7757757757757757,0.1131640002846509,6,15.521747346102597,0.007224674181694773,0.10395594559619498,1.0,True,
|
||||
6,9,8.025985916230132,103.90638727582186,0.6009202835468226,0.1259448851291812,6,28.795189977892573,0.007975110549369148,0.07338758946554978,1.0,True,
|
||||
6,10,8.303463402700086,159.91188585985975,0.5363513347275187,0.13083118553166714,6,31.167661263990606,1.7146158775247784,14.688249303878628,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
6,11,11.328882063792355,138.54043874549896,0.4633337584491774,0.13769942152040132,6,58.377014583638996,0.023339879627157865,0.18888808553225306,1.0,True,
|
||||
7,8,1.9962664218365056,90.62216430773583,0.8340050377833753,0.11132245152738919,6,16.170350777028464,0.004806949653405585,0.02034039865601464,1.0,True,
|
||||
7,9,7.8811064994361235,78.59482096901284,0.6160971335586432,0.12637042711002067,6,24.17866477904224,0.012112046262530643,0.5927664140170301,1.0,False,forward_reverse_rotation
|
||||
7,10,7.620696678413973,134.60031955305035,0.5721183607775164,0.13040415535689726,6,20.571872802604858,0.010507785715791607,0.10324111221368969,1.0,True,
|
||||
7,11,10.356268683459426,113.22887243868989,0.5179098728976762,0.13593896500168,6,36.18276452817704,0.01570096854689765,0.062213006599939585,1.0,True,
|
||||
7,12,13.76202076313695,94.9510481861355,0.419173636250156,0.14753718360891266,6,89.28960611274712,0.018245717875293024,0.41212759156893974,1.0,True,
|
||||
8,9,7.803713858547152,12.027343338722998,0.6407549981373402,0.12189583104066963,6,28.06713348388497,0.025170709673794655,0.14112050756711447,1.0,True,
|
||||
8,10,8.062504137991457,43.97815524531457,0.6075420709986488,0.12933255373443525,6,20.429632894689156,0.01256038832173067,0.12386976869589704,1.0,True,
|
||||
8,11,11.084531710563947,22.606708130954026,0.5447599643448364,0.13547919137380177,6,27.72840331379869,0.006462771421800638,0.08629803406158434,1.0,True,
|
||||
8,12,14.350449135419003,4.328883878399632,0.46489164086687307,0.14530908872417966,6,37.958964738084276,0.00955628888567663,0.1112688142308669,1.0,True,
|
||||
8,13,18.245302761704593,28.43334478424675,0.37832991803278687,0.15442802982944018,6,101.4897081447731,0.027875630486669568,0.37387837894914167,1.0,True,
|
||||
9,10,2.04815062353057,56.00549858403758,0.6576312576312576,0.10693844153477818,6,15.957672354569297,0.0038184479242649575,0.016135929192430513,0.5,True,
|
||||
9,11,4.738677237611319,34.634051469677026,0.5883320678309288,0.11815838246331245,6,21.526672921842792,0.008866435423074387,0.05039949658393549,1.0,True,
|
||||
9,12,7.170741483679294,16.35622721712263,0.5413589364844904,0.12469816852157387,6,38.0354745717217,0.03403608365914784,0.249719392637206,1.0,True,
|
||||
9,13,10.733928030180744,16.40600144552375,0.5075728649611811,0.13356609480985904,6,50.3483756147633,0.0053243668825205025,0.1401976908512776,1.0,True,
|
||||
9,14,7.8912187855633125,14.085282580602351,0.5416463116756228,0.12859551377809317,6,26.721469573230642,0.011377143482081005,0.045883349808202654,1.0,True,
|
||||
10,11,3.2047552083250137,21.371447114360556,0.7131414267834794,0.12095106901516857,6,13.404202373771971,0.006897671932212844,0.18540056788520093,1.0,True,
|
||||
10,12,6.291813977735496,39.64927136691496,0.6117876278616659,0.125002241212049,6,19.806559061723195,0.006509808900811586,0.06742381610992963,1.0,True,
|
||||
10,13,10.199392557022867,72.41150002956134,0.5244808055380743,0.13689263771908436,6,36.18235272080672,0.0046518429660042555,0.17931102925270673,1.0,True,
|
||||
10,14,6.819808471778838,41.920216003435236,0.5996858385693572,0.12955691635611957,6,17.916357473627098,0.009593578345611205,0.05753783232412823,1.0,True,
|
||||
10,15,10.561163834622734,65.51298166784417,0.5048970366649924,0.13966622273060364,6,30.173625507677322,0.006074999657630774,0.04867559411570644,1.0,True,
|
||||
11,12,3.467398797536633,18.277824252554396,0.6508076728924785,0.12017753597340274,6,15.211760062254479,0.016302173123952383,0.05141023051579345,1.0,True,
|
||||
11,13,7.516502113110916,51.04005291520078,0.5666710199817161,0.12966061077144844,6,26.14207974844453,0.010250004424021303,0.0630753341579125,1.0,True,
|
||||
11,14,3.767517331528496,20.548768889074672,0.64271407110666,0.12109534664165014,6,14.04589685067402,0.007610676428698485,0.05350102444543152,1.0,True,
|
||||
11,15,7.712397127778454,44.141534553483616,0.570479416362689,0.12836105648377344,6,18.857231656557765,0.006898635416764969,0.028532809525243653,1.0,True,
|
||||
11,16,11.0395297023812,69.98495270981527,0.4761423882857864,0.13515636052103228,6,45.4042112112498,0.037021121414326036,0.5129154434742894,1.0,False,forward_reverse_rotation
|
||||
12,13,4.049286119591895,32.762228662646386,0.7056733087955325,0.11689859246507509,6,14.509757355016381,0.0020816591744367207,0.08889045467662131,1.0,True,
|
||||
12,14,0.97948616772873,2.2709446365202806,0.8745432399512789,0.09766069029332589,6,11.857488007889668,0.002473515580842477,0.018069288085025077,1.0,True,
|
||||
12,15,4.286747470271891,25.863710300929224,0.7033426183844012,0.12103901189560114,6,11.859397045728326,0.022780233796594867,0.04927694758545139,1.0,True,
|
||||
12,16,7.5836501880550475,51.70712845726087,0.5820235756385069,0.12458133956199553,6,32.351864267271,0.00989839849833309,0.03808519306435704,1.0,True,
|
||||
12,17,6.351478009829798,1.4248238883471207,0.6542219994988725,0.12658997093251892,6,11.229951110937455,0.019742680150836883,0.07899236042083516,1.0,True,
|
||||
13,14,4.006260191078547,30.491284026126113,0.6984766461034874,0.11406275231178287,6,13.924718120717541,0.012533581961738958,0.10861604448395465,1.0,True,
|
||||
13,15,0.9562774815922267,6.898518361717157,0.8794391298650243,0.09903209123829654,6,10.514819201987352,0.006638809913641406,0.04266354358349458,1.0,True,
|
||||
13,16,3.565173336606111,18.944899794614482,0.7273073505141552,0.10958532316684438,6,19.40505650408657,0.005441055528592516,0.12519365495730794,1.0,True,
|
||||
13,17,2.967150651379281,31.337404774299262,0.7130265716137395,0.11779895987026284,6,12.816390530620689,0.013263327701592687,0.16221305155705806,1.0,True,
|
||||
13,18,5.249882044431148,3.4459964529377567,0.7019876443728176,0.11940768524727291,6,25.985486690964983,0.008995185113784413,0.05198334813202341,1.0,True,
|
||||
14,15,4.019575892829469,23.592765664408944,0.7035920622959055,0.12189498753760801,6,10.295943232842452,0.010817230757509965,0.008099442158890762,0.5,True,
|
||||
14,16,7.5676649485439835,49.43618382074059,0.5923489278752436,0.12241126089476961,6,33.65365690477826,0.003104373432332903,0.13386003286196627,1.0,True,
|
||||
14,17,5.910977627463022,0.8461207481731591,0.6687795177728063,0.12452036369759786,6,8.95305147071513,0.01159286549234958,0.10132041390926738,1.0,True,
|
||||
14,18,8.548241724186095,27.045287573188347,0.6196476790536196,0.12845393182350578,6,17.805505225184664,0.010070495463159433,0.13041926369959572,1.0,True,
|
||||
14,19,9.249254057266956,79.47127430600668,0.5845660749506904,0.12584132662356784,6,30.335272352492872,0.016830803029542436,0.2574761671757917,1.0,True,
|
||||
15,16,3.7301261399251735,25.84341815633164,0.7032674772036475,0.11602216892855616,6,22.696890954829907,0.0009988867448423883,0.09036168134112703,1.0,True,
|
||||
15,17,2.2049738368271745,24.438886412582093,0.7489009568140678,0.11923375870790391,6,6.944925131742929,0.02582458487951314,0.095039680889365,1.0,True,
|
||||
15,18,4.7000039832559155,3.452521908779401,0.7165438713998661,0.11783922962071142,6,19.334165264487982,0.009354386824208725,0.17478401338848865,1.0,True,
|
||||
15,19,5.238340351181781,55.87850864159773,0.6924358974358974,0.11506895717743916,6,20.41712608461488,0.012497900854287812,0.3255615001296644,1.0,True,
|
||||
15,20,2.0380809160783224,128.5450420215843,0.6751867872591427,0.11901811643083532,6,27.381712286843808,0.007126007329084919,0.09473387178666172,1.0,True,
|
||||
16,17,3.368526196086246,50.282304568913744,0.6284461152882206,0.1113618900510703,6,25.184005110635376,0.015370940622322818,0.07520125332617648,1.0,True,
|
||||
16,18,3.5240348999326185,22.39089624755224,0.6921281286473868,0.10919235768364483,6,39.42922894015919,0.005355462743716769,0.03654303027439279,1.0,True,
|
||||
16,19,2.146829294717985,30.035090485266103,0.8880188913745961,0.0968346861370537,6,11.70909430755386,0.006745004702225108,0.04851926363283949,1.0,True,
|
||||
16,20,4.728364693236263,102.70162386525263,0.6307301587301587,0.12362874516637085,6,35.70048091760535,0.007128919591300971,0.10475436005961275,1.0,True,
|
||||
16,21,10.094336026449797,153.311489607007,0.40465918895599656,0.14174574821871383,6,57.2667389243621,0.01442382622115287,0.2621394134233604,1.0,True,
|
||||
17,18,2.640403049812329,27.891408321361506,0.7665916015366274,0.11462276561713726,6,9.759321661407776,0.003676908482849856,0.03455537183665902,1.0,True,
|
||||
17,19,3.933985934417215,80.31739505417984,0.645738203957382,0.11467441399973681,6,29.93502749301381,0.002658154417044598,0.03997293849541112,1.0,True,
|
||||
17,20,4.2254212089887,152.98392843416656,0.6031375599636977,0.12542519992547976,6,27.708613247812377,4.543486491493239,1.5708713272202308,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
||||
17,21,9.21529677253323,156.40620582407942,0.4672368255565338,0.13751533766984328,6,32.19730123852794,0.13403735422224766,1.1805230479043929,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
||||
17,22,11.781237287606983,150.82966218116434,0.4255952380952381,0.1417851723285674,6,38.71155142811069,0.01638337185583796,0.14112774930143776,1.0,True,
|
||||
18,19,2.4460967931915643,52.425986732818345,0.6999343401181878,0.10876407223187468,6,36.919102980185954,0.0029722527816906422,0.028553700929610463,1.0,True,
|
||||
18,20,6.593931011285688,125.09252011280485,0.5796614723267061,0.12778176089815,6,32.46060078153023,0.0849644734357879,0.24110896992060843,1.0,False,forward_reverse_translation
|
||||
18,21,11.793089868757727,175.70238585456048,0.42345743296016664,0.14463219993853524,6,41.677643204951686,0.008594432490503082,0.044791316017877565,1.0,True,
|
||||
18,22,14.40479158879449,178.72107050256645,0.35586914688903143,0.14801012931814386,6,118.81863294454718,0.05364920712348454,0.15560820359743163,1.0,True,
|
||||
18,23,11.744734461596845,164.11569663484164,0.3967277486910995,0.14073241205403209,6,67.8157115569044,0.0379980141073483,0.09667096210106078,1.0,True,
|
||||
19,20,6.650720121741557,72.6665333799865,0.6260444787247719,0.12668491637849374,6,25.97982149253148,0.022781715838261912,0.19834674514582162,1.0,True,
|
||||
19,21,12.053580264031138,123.27639912174077,0.3952850193339154,0.1421446220617431,6,61.553005632745744,0.01796074748252817,0.16263425635632095,1.0,True,
|
||||
19,22,14.836246959975925,128.85294276465592,0.3237392373923739,0.15069330869045588,6,75.70505048512227,0.2579934626807855,1.1847082611411373,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
19,23,11.740810561661805,143.45831663234065,0.37768025078369905,0.1432975121556299,6,38.93505210462392,0.07411982241929344,0.4458370864472602,1.0,True,
|
||||
19,24,11.093516369446519,177.87107120383473,0.43504761904761907,0.13924446561169374,6,41.93660606132572,0.04258350776081339,0.7879276702809055,0.5,False,forward_reverse_rotation
|
||||
20,21,5.40286046809103,50.60986574175429,0.6607188376242672,0.12214671257866079,6,14.082798146569438,0.01471691635641488,0.0747935794181239,1.0,True,
|
||||
20,22,8.200956565000565,56.18640938466938,0.576328684508104,0.13245799460807672,6,16.034011252149995,0.02165175771302933,0.4612076789897289,1.0,True,
|
||||
20,23,5.175118275082073,70.79178325235415,0.6451819579702717,0.12102263178677423,6,12.511468996120387,0.018249896901851893,0.12115759970382113,1.0,True,
|
||||
20,24,4.710308396362637,105.20453782384023,0.7383177570093458,0.11441422046802802,6,12.152737785424229,0.009011279667019194,0.06430292411421322,1.0,True,
|
||||
20,25,8.226491803107123,68.57831319871164,0.6182822702159718,0.12791257682460647,6,29.104161441720954,0.014703530260578728,0.32055535743282454,1.0,True,
|
||||
21,22,2.8644847627909416,5.57654364291509,0.7740636818348177,0.11564788773425824,6,12.209574213097333,0.0065184199045192235,0.06991965908247275,1.0,True,
|
||||
21,23,1.2936223973175418,20.18191751059984,0.862223327530465,0.10307116830776755,6,11.124499228103288,0.0026785916987678697,0.01879221122979615,1.0,True,
|
||||
21,24,2.3128500583741403,54.59467208208593,0.7795265676152102,0.11182158240581822,6,15.934013426145448,0.003491995501356654,0.0374256510953578,1.0,True,
|
||||
21,25,3.793272903723766,17.968447456957346,0.7340892465252378,0.12002239056891174,6,16.57216298005221,0.037735058311297705,0.2878144229712423,1.0,True,
|
||||
21,26,4.631707245574304,60.912845043424184,0.7147358216190014,0.11997311573294966,6,17.372315653280065,0.012351932089198563,0.10379217190868555,1.0,True,
|
||||
22,23,3.7194009250537223,14.605373867684753,0.7408951563458002,0.11726725109752717,6,12.610213323576254,0.009206244303295173,0.09601986001481369,1.0,True,
|
||||
22,24,4.786117710081478,49.01812843917083,0.6936064556176288,0.11914624513148218,6,13.20338761495324,0.006090737153726605,0.027557138417491776,1.0,True,
|
||||
22,25,2.3747421598149763,12.391903814042255,0.7356584485868911,0.11474070552638106,6,16.673633938013044,0.0019617091597581428,0.011643770804738128,1.0,True,
|
||||
22,26,2.381693697571969,55.336301400509086,0.7422594142259414,0.11765221626900083,6,18.880420396501957,0.007814937371704293,0.039342553564881436,1.0,True,
|
||||
22,27,2.9603952927690247,80.62067670375279,0.7254925373134329,0.11870181965154768,6,21.309389349405485,0.018241823604788005,0.08279236858582419,1.0,True,
|
||||
23,24,1.0880644725336985,34.41275457148609,0.7884810126582279,0.10662565692629543,6,10.611199681165669,0.001882338148224756,0.020239211377623818,1.0,True,
|
||||
23,25,5.033935954106079,2.213470053642494,0.6843137254901961,0.12171091934898426,6,15.150809064137142,0.014639828785635214,0.2046006298281892,1.0,True,
|
||||
23,26,5.765534437288107,40.730927532824346,0.6772228989037758,0.12237954766026346,6,16.382334072569904,0.04518647006837157,0.20367965681897066,1.0,True,
|
||||
23,27,6.392376191887118,66.01530283606805,0.6637469586374696,0.12340951265232122,6,20.64383986204732,0.010626193556947957,1.1817079481882757,1.0,False,forward_reverse_rotation
|
||||
23,28,6.631292861991925,85.8775990418248,0.6720351390922401,0.12122778564083748,6,19.20657067904157,0.043540468687755365,0.26051478047218213,1.0,True,
|
||||
24,25,6.097212810998921,36.62622462512857,0.6764267990074442,0.12367636388755719,6,21.50176872604179,0.035816405766439664,0.2046504337319023,1.0,True,
|
||||
24,26,6.852918693116795,6.318172961338249,0.6524044389642417,0.12740232296321938,6,21.753053297058692,0.062038641373543625,0.12012273822228971,1.0,True,
|
||||
24,27,7.47982907414204,31.60254826458195,0.6546798029556651,0.12425657449629145,6,26.295019203914197,0.05403266260962086,0.2126374478532415,1.0,True,
|
||||
24,28,7.71913193400284,51.4648444703387,0.6614377470355731,0.12010155595807545,6,20.98500095492876,0.049490676797893776,0.25138526875505346,1.0,True,
|
||||
24,29,7.213852273638132,93.73390958258973,0.6620579958399608,0.12331341986261203,6,28.045882177977884,0.08010108996241684,0.7934458070763889,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
25,26,1.2882746074869595,42.944397586466835,0.9137395459976105,0.08148336624555251,6,16.56293455758718,0.0032405549728505064,0.006669174585592186,1.0,True,
|
||||
25,27,1.7666235781584831,68.22877288971053,0.8596658711217183,0.1096145882030974,6,22.188153380460985,0.010624789877375215,0.04475651255674721,1.0,True,
|
||||
25,28,2.1807182972588706,88.09106909546726,0.8839157491622786,0.10490699814192792,6,16.17217948348072,0.00283419137499405,0.01585308444597649,1.0,True,
|
||||
25,29,1.118287177357716,130.36013420771832,0.7857227558401518,0.10954358768244267,6,20.132087187715307,0.003247718742817276,0.01654235380830592,1.0,True,
|
||||
25,30,5.200639419572971,139.6773015481418,0.05061061531235322,0.1582745227634446,6,186.52424080569466,2.4401001990983833,2.63047633237589,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
26,27,0.6269948654714946,25.284375303243706,0.9188612099644128,0.07520973241900301,6,20.799051531446562,0.0009920873307779914,0.005485007138529723,1.0,True,
|
||||
26,28,0.9431372257108486,45.14667150900044,0.9182726623840114,0.07611768518866212,6,21.99541958409829,0.004984978187527545,0.01173507098896779,1.0,True,
|
||||
26,29,1.129925637779203,87.41573662125148,0.7856890251090416,0.10851393061782179,6,24.280602674778656,0.002586732426991948,0.12205883610743135,1.0,True,
|
||||
26,30,4.786676837961511,177.3783008654109,0.7634835395750642,0.10495724850674527,6,34.80647378548578,0.008489213184554775,0.021733210949123293,1.0,True,
|
||||
26,31,5.900105669197046,156.01119868987135,0.7374054682955207,0.1073752166304431,6,38.310555401782544,3.545322953478412,6.459518815762061,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
27,28,0.44809323479571145,19.862296205756735,0.9281131178707225,0.07220943509634768,6,20.093208902964978,0.0023003913248606234,0.001591751329336308,1.0,True,
|
||||
27,29,1.160064136065596,62.13136131800778,0.7871188037207112,0.10966659884725236,6,24.009650087098436,0.005134403698947268,0.02434844099203523,1.0,True,
|
||||
27,30,4.25412615059095,152.09392556214777,0.7922108208955224,0.10147035321534553,6,32.52026042532504,2.795897032209197,3.237329131939042,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
27,31,5.542871649427442,178.70442600700352,0.7592097617664149,0.10539218018667602,6,48.70669958381899,0.0020939062619997405,0.027400662783563425,1.0,True,
|
||||
27,32,4.8829350360277,138.47370648510565,0.7480278422273782,0.10408734715846867,6,42.21510199826003,0.004407463145306061,0.03490543724836902,1.0,True,
|
||||
28,29,1.5995723610319212,42.26906511225104,0.787814381863266,0.10901449493832843,6,25.913625419675043,0.01570175790237609,0.03569660498137675,1.0,True,
|
||||
28,30,4.295353381127667,132.23162935639104,0.7791159962581852,0.10438560755009844,6,36.501677925790155,0.01170149615735483,0.047047156693309715,1.0,True,
|
||||
28,31,5.754426930327516,158.8421298011293,0.7449015266285981,0.10668967013687296,6,48.04636971250818,0.024421374377245158,0.6810283060803514,0.5,False,forward_reverse_rotation
|
||||
28,32,5.013331477001236,158.33600269086247,0.7309213587715216,0.1081318944415262,6,52.23779663520043,0.10324080390042788,0.9479198721286788,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
28,33,5.317102997899902,67.05379893079937,0.6692465836255895,0.10871806386783629,6,54.67376255043738,0.13259351588266985,0.659391929056215,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
29,30,4.099530130205247,89.96256424413995,0.7320662880982732,0.10929719051930432,6,30.745355096911158,0.005463604154015333,0.018583483487870766,1.0,True,
|
||||
29,31,4.890018768530818,116.57306468887772,0.7254562254562255,0.11323336570178015,6,49.09678922599757,0.0050777995851252426,0.07265894091769333,1.0,True,
|
||||
29,32,4.464737187763289,159.39493219688632,0.695837657096737,0.10999820041072961,6,72.25486343140591,0.011220553177198233,0.14599088546526143,1.0,True,
|
||||
29,33,4.34670323578768,109.32286404305042,0.6382698298586149,0.11215583949685817,6,72.66655529463955,0.018977478007060265,0.05615608666616428,1.0,True,
|
||||
30,31,2.3862917603439455,26.61050044473775,0.8286237272623269,0.0964659694513182,6,41.03513305025223,0.018905314487385798,0.08632503464137033,1.0,True,
|
||||
30,32,1.1507634071714652,69.43236795274659,0.8065326633165829,0.09815063400019142,6,35.25635856167173,0.007341509941520892,0.023973741904832375,1.0,True,
|
||||
30,33,2.4361832484951105,160.7145717128102,0.6965239055641239,0.10032655671840386,6,47.08947384000088,2.6257249086936962,3.1197630566421646,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
31,32,1.2514382602180638,42.82186750800885,0.8146841206602162,0.09914628416022417,6,34.33173181492779,0.06936877058296162,0.45703051311645637,1.0,True,
|
||||
31,33,0.641507519046697,134.10407126807203,0.7551430598250177,0.1003246191461096,6,28.537349904719164,0.005559091155811048,0.0330053182501105,1.0,True,
|
||||
32,33,1.4370687242949811,91.2822037600631,0.043432078366576185,0.1661924920133195,6,88.4956235178996,0.946171384319174,3.5092968853566924,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
|
File diff suppressed because it is too large
Load Diff
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,489 @@
|
||||
{
|
||||
"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.2944883843818025,
|
||||
-0.0008838118010001346,
|
||||
0.7058442435591689
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7159364624845366,
|
||||
1.3526878670674298,
|
||||
-0.975972089807914
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.006146489369094902,
|
||||
0.011856702812467925,
|
||||
-0.008442354678834595,
|
||||
0.999875175166545
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9995762904917872,
|
||||
0.016736847531056993,
|
||||
0.02381422728959523,
|
||||
1.2944883843818025
|
||||
],
|
||||
[
|
||||
-0.01702835592221438,
|
||||
0.9997818946318248,
|
||||
0.01209124728823738,
|
||||
-0.0008838118010001346
|
||||
],
|
||||
[
|
||||
-0.023606663916460907,
|
||||
-0.01249164125009491,
|
||||
0.9996432785337048,
|
||||
0.7058442435591689
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"estimation": {
|
||||
"stations": 34,
|
||||
"pairs": 57,
|
||||
"residuals": {
|
||||
"pairs": 57,
|
||||
"translation_m": {
|
||||
"rms": 0.1544348214275169,
|
||||
"median": 0.0705120546692258,
|
||||
"p90": 0.2796247931030114,
|
||||
"p95": 0.38830732511177307,
|
||||
"max": 0.42173620405868173
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.6127366820434261,
|
||||
"median": 0.7802394472449041,
|
||||
"p90": 2.0427026963913084,
|
||||
"p95": 4.139878905679253,
|
||||
"max": 4.927876933555878
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.04921586129986855,
|
||||
"rotation_deg": 0.7802394472449041
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.06022467069161827,
|
||||
"rotation_deg": 0.8076387678918286
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.05733334031955927,
|
||||
"rotation_deg": 0.3801287351167008
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.10178704595686662,
|
||||
"rotation_deg": 0.23294290596786388
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.0950710667744258,
|
||||
"rotation_deg": 0.8441747973849835
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.1482159791451249,
|
||||
"rotation_deg": 0.681326797444399
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.02325894326521952,
|
||||
"rotation_deg": 0.6033104606942519
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.07474087499522525,
|
||||
"rotation_deg": 0.6507288891948437
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.18825515016508942,
|
||||
"rotation_deg": 0.26486292076967255
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.04309283328904936,
|
||||
"rotation_deg": 0.13788972833567684
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.04634466299044426,
|
||||
"rotation_deg": 0.727856852897771
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.043890799917503956,
|
||||
"rotation_deg": 0.7033085139548623
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.06569153273548976,
|
||||
"rotation_deg": 0.8215251942968772
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.05085505830053342,
|
||||
"rotation_deg": 0.5703942163369722
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.08080041374696341,
|
||||
"rotation_deg": 0.7042489825699358
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.028283768727744116,
|
||||
"rotation_deg": 0.5815552521395532
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.034152028863445455,
|
||||
"rotation_deg": 1.0076363281960339
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.10661935094245772,
|
||||
"rotation_deg": 0.585736616100307
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.0705120546692258,
|
||||
"rotation_deg": 0.8052707169746115
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.06442322486081886,
|
||||
"rotation_deg": 2.1113003980243557
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.058531034305082885,
|
||||
"rotation_deg": 0.5873890947329841
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.06915436202628812,
|
||||
"rotation_deg": 0.4455496836075116
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.1484005911609239,
|
||||
"rotation_deg": 1.9969708953026104
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.10429385187976488,
|
||||
"rotation_deg": 0.12781289285231975
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.06859842051942554,
|
||||
"rotation_deg": 0.3198403871694416
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.037990236699121674,
|
||||
"rotation_deg": 0.631070403868711
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.03189885614531801,
|
||||
"rotation_deg": 0.7541329389301039
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.07672961622319935,
|
||||
"rotation_deg": 1.5806114049925588
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.069983101896218,
|
||||
"rotation_deg": 0.4729689795147634
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.016135065100391668,
|
||||
"rotation_deg": 0.3789407761818106
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.07432095926140471,
|
||||
"rotation_deg": 1.8120862981058055
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.17890921343127023,
|
||||
"rotation_deg": 3.97184190714068
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.2459042707502872,
|
||||
"rotation_deg": 4.617881335971262
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.06334407979243162,
|
||||
"rotation_deg": 0.8238155509065317
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.07743513463604834,
|
||||
"rotation_deg": 4.0203782981062535
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.15971289980951447,
|
||||
"rotation_deg": 4.84466491195667
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.13395086756218255,
|
||||
"rotation_deg": 4.927876933555878
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.06599998782040693,
|
||||
"rotation_deg": 1.9960824610834838
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.04503412561992633,
|
||||
"rotation_deg": 1.279332716204477
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.11215573119400024,
|
||||
"rotation_deg": 0.5578231171630792
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.05509905448014895,
|
||||
"rotation_deg": 0.6608982729619334
|
||||
},
|
||||
{
|
||||
"pair_index": 41,
|
||||
"translation_m": 0.007272665876828039,
|
||||
"rotation_deg": 1.2124341117031159
|
||||
},
|
||||
{
|
||||
"pair_index": 42,
|
||||
"translation_m": 0.055811383114362484,
|
||||
"rotation_deg": 0.8153248357421402
|
||||
},
|
||||
{
|
||||
"pair_index": 43,
|
||||
"translation_m": 0.03138324591270751,
|
||||
"rotation_deg": 1.2136591021205085
|
||||
},
|
||||
{
|
||||
"pair_index": 44,
|
||||
"translation_m": 0.13982233632280833,
|
||||
"rotation_deg": 1.2089791291543672
|
||||
},
|
||||
{
|
||||
"pair_index": 45,
|
||||
"translation_m": 0.42173620405868173,
|
||||
"rotation_deg": 1.126390973143559
|
||||
},
|
||||
{
|
||||
"pair_index": 46,
|
||||
"translation_m": 0.027936086071527602,
|
||||
"rotation_deg": 0.5561027320483051
|
||||
},
|
||||
{
|
||||
"pair_index": 47,
|
||||
"translation_m": 0.08146548874814821,
|
||||
"rotation_deg": 1.5984131745791201
|
||||
},
|
||||
{
|
||||
"pair_index": 48,
|
||||
"translation_m": 0.40597723810744263,
|
||||
"rotation_deg": 1.3479854285502366
|
||||
},
|
||||
{
|
||||
"pair_index": 49,
|
||||
"translation_m": 0.3864154920341234,
|
||||
"rotation_deg": 0.6215042157245596
|
||||
},
|
||||
{
|
||||
"pair_index": 50,
|
||||
"translation_m": 0.18945577362130134,
|
||||
"rotation_deg": 0.596177211598774
|
||||
},
|
||||
{
|
||||
"pair_index": 51,
|
||||
"translation_m": 0.37813335218979344,
|
||||
"rotation_deg": 1.0668602650100487
|
||||
},
|
||||
{
|
||||
"pair_index": 52,
|
||||
"translation_m": 0.330205576632098,
|
||||
"rotation_deg": 0.40979383494055255
|
||||
},
|
||||
{
|
||||
"pair_index": 53,
|
||||
"translation_m": 0.39587465742237204,
|
||||
"rotation_deg": 0.7934150366951904
|
||||
},
|
||||
{
|
||||
"pair_index": 54,
|
||||
"translation_m": 0.060998991546499626,
|
||||
"rotation_deg": 0.9224390837063629
|
||||
},
|
||||
{
|
||||
"pair_index": 55,
|
||||
"translation_m": 0.0952510183208545,
|
||||
"rotation_deg": 0.8616718556937568
|
||||
},
|
||||
{
|
||||
"pair_index": 56,
|
||||
"translation_m": 0.10976329201195999,
|
||||
"rotation_deg": 0.5415573303416338
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"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.006072210871275005,
|
||||
0.0060852661675311025,
|
||||
0.005465720025368553
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.06255688904352227,
|
||||
0.06213349734715941,
|
||||
0.13055508827073753
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"weighted_jacobian_condition_number": 6.040138286165465,
|
||||
"solver_multistart": {
|
||||
"runs": 12,
|
||||
"candidates_relative_to_best": [
|
||||
{
|
||||
"cost": 324.2098227991519,
|
||||
"success": true,
|
||||
"translation_m": 5.570266299009551e-09,
|
||||
"rotation_deg": 1.4455049406405054e-07
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279914983,
|
||||
"success": true,
|
||||
"translation_m": 0.0,
|
||||
"rotation_deg": 0.0
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279915165,
|
||||
"success": true,
|
||||
"translation_m": 5.216823166795417e-09,
|
||||
"rotation_deg": 1.973338877475283e-07
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279915034,
|
||||
"success": true,
|
||||
"translation_m": 1.3901066622882166e-09,
|
||||
"rotation_deg": 6.335210062786435e-08
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279914995,
|
||||
"success": true,
|
||||
"translation_m": 3.578733410578741e-10,
|
||||
"rotation_deg": 6.852733510041159e-09
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279914983,
|
||||
"success": true,
|
||||
"translation_m": 6.594966941115627e-10,
|
||||
"rotation_deg": 2.2216701960527812e-08
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279914983,
|
||||
"success": true,
|
||||
"translation_m": 2.1650538139486925e-09,
|
||||
"rotation_deg": 3.6255288379996055e-08
|
||||
},
|
||||
{
|
||||
"cost": 324.2098227991501,
|
||||
"success": true,
|
||||
"translation_m": 1.5027981325208515e-09,
|
||||
"rotation_deg": 6.326578543393082e-08
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279914983,
|
||||
"success": true,
|
||||
"translation_m": 6.094320168262204e-10,
|
||||
"rotation_deg": 1.025130160040861e-08
|
||||
},
|
||||
{
|
||||
"cost": 324.2098227991552,
|
||||
"success": true,
|
||||
"translation_m": 1.057266183614633e-08,
|
||||
"rotation_deg": 3.643863231629731e-07
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279915063,
|
||||
"success": true,
|
||||
"translation_m": 3.9489601150102006e-09,
|
||||
"rotation_deg": 1.359274031832874e-07
|
||||
},
|
||||
{
|
||||
"cost": 324.20982279914983,
|
||||
"success": true,
|
||||
"translation_m": 6.276644321676319e-10,
|
||||
"rotation_deg": 2.14493230752589e-08
|
||||
}
|
||||
]
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.003048001100427371,
|
||||
0.0025869752895426976,
|
||||
0.0014870952541001317,
|
||||
0.09174691854497609,
|
||||
0.06502786700039134,
|
||||
0.11777557058586356
|
||||
],
|
||||
"p025": [
|
||||
1.2892012505330883,
|
||||
-0.0051467048024525786,
|
||||
0.7035478241042927,
|
||||
-0.8634755073850175,
|
||||
1.2139289883714506,
|
||||
-1.173046710195043
|
||||
],
|
||||
"p975": [
|
||||
1.2997598841406142,
|
||||
0.005003259375615847,
|
||||
0.7090711194920217,
|
||||
-0.48035430043405547,
|
||||
1.4534667429791082,
|
||||
-0.7593758945747364
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,7 @@
|
||||
# 跨批比较
|
||||
|
||||
- `extrinsic_difference.json`：data4 X 相对历史部署 X 的严格 SE(3) 差，约 `1.592 cm / 0.234°`。
|
||||
- `old_extrinsic_on_data4.json`：历史部署 X 在 data4 B 上的残差，约 `0.11953 m / 1.24836°`。
|
||||
- `data4_extrinsic_on_previous_batch2.json`：data4 X 在历史第二批 B 上的残差，约 `0.07931 m / 0.98926°`。
|
||||
|
||||
两套 X 的跨批表现接近，而 data4 自身估计残差更高；因此维持历史部署值，data4 只作为候选。
|
||||
@@ -0,0 +1,298 @@
|
||||
{
|
||||
"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": 39,
|
||||
"translation_m": {
|
||||
"rms": 0.07931219315845621,
|
||||
"median": 0.0521831021438143,
|
||||
"p90": 0.1205091988381832,
|
||||
"p95": 0.14405284012014574,
|
||||
"max": 0.23166749945892554
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 0.9892579681085032,
|
||||
"median": 0.8987168885592437,
|
||||
"p90": 1.5241504377222574,
|
||||
"p95": 1.5453098245907155,
|
||||
"max": 1.8288069823538258
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.13184857122951013,
|
||||
"rotation_deg": 0.4578882478759752,
|
||||
"i": 0,
|
||||
"j": 2
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.11088249231082419,
|
||||
"rotation_deg": 0.49123829458862667,
|
||||
"i": 0,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.06122066543511726,
|
||||
"rotation_deg": 0.46678985990983063,
|
||||
"i": 1,
|
||||
"j": 2
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.05361370234686629,
|
||||
"rotation_deg": 0.6513155675962176,
|
||||
"i": 1,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.023371189257265605,
|
||||
"rotation_deg": 0.3726639533978929,
|
||||
"i": 2,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.07839704351948763,
|
||||
"rotation_deg": 0.9876540671880906,
|
||||
"i": 2,
|
||||
"j": 4
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.06371505126502003,
|
||||
"rotation_deg": 0.8674541404693262,
|
||||
"i": 3,
|
||||
"j": 4
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.09440077842634854,
|
||||
"rotation_deg": 1.176934949298133,
|
||||
"i": 3,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.034738581722701674,
|
||||
"rotation_deg": 0.46221894567574257,
|
||||
"i": 4,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.1368264467472551,
|
||||
"rotation_deg": 1.0614354353201445,
|
||||
"i": 6,
|
||||
"j": 7
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.003100407410401193,
|
||||
"rotation_deg": 0.6140746474880827,
|
||||
"i": 7,
|
||||
"j": 9
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.06611151014323934,
|
||||
"rotation_deg": 0.09617682103039936,
|
||||
"i": 8,
|
||||
"j": 9
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.11767435574035143,
|
||||
"rotation_deg": 0.8987168885592437,
|
||||
"i": 8,
|
||||
"j": 10
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.05534374617362978,
|
||||
"rotation_deg": 0.9177398533680782,
|
||||
"i": 9,
|
||||
"j": 10
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.016375449524654653,
|
||||
"rotation_deg": 1.8288069823538258,
|
||||
"i": 12,
|
||||
"j": 14
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.23166749945892554,
|
||||
"rotation_deg": 0.4806436956517463,
|
||||
"i": 13,
|
||||
"j": 15
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.20909038047616038,
|
||||
"rotation_deg": 1.1888833539076575,
|
||||
"i": 14,
|
||||
"j": 16
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.04954727606060992,
|
||||
"rotation_deg": 0.7479656125581431,
|
||||
"i": 17,
|
||||
"j": 18
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.0401572454810912,
|
||||
"rotation_deg": 1.042619605722841,
|
||||
"i": 18,
|
||||
"j": 19
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.0432861701880587,
|
||||
"rotation_deg": 0.6965786478374109,
|
||||
"i": 20,
|
||||
"j": 21
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.010709015812516234,
|
||||
"rotation_deg": 1.6253345198252291,
|
||||
"i": 22,
|
||||
"j": 23
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.058199199584633175,
|
||||
"rotation_deg": 0.36595813742919014,
|
||||
"i": 22,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.030262860466569532,
|
||||
"rotation_deg": 1.1081362850905554,
|
||||
"i": 22,
|
||||
"j": 25
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.04000865510964372,
|
||||
"rotation_deg": 1.189922759500818,
|
||||
"i": 23,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.06464346915846221,
|
||||
"rotation_deg": 0.7699901460922239,
|
||||
"i": 24,
|
||||
"j": 25
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.0521831021438143,
|
||||
"rotation_deg": 0.18489023256895315,
|
||||
"i": 25,
|
||||
"j": 26
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.08267778191372169,
|
||||
"rotation_deg": 0.9059437837656906,
|
||||
"i": 25,
|
||||
"j": 27
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.038589920757972004,
|
||||
"rotation_deg": 0.8404190717226915,
|
||||
"i": 26,
|
||||
"j": 27
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.05488335734683258,
|
||||
"rotation_deg": 1.5217701038732556,
|
||||
"i": 26,
|
||||
"j": 28
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.022562161090077113,
|
||||
"rotation_deg": 1.5336717731182645,
|
||||
"i": 26,
|
||||
"j": 29
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.016794927865731942,
|
||||
"rotation_deg": 1.4945757001638067,
|
||||
"i": 27,
|
||||
"j": 30
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.08618762377782405,
|
||||
"rotation_deg": 1.5364181917868804,
|
||||
"i": 30,
|
||||
"j": 31
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.06980420384374487,
|
||||
"rotation_deg": 0.7086752430201493,
|
||||
"i": 30,
|
||||
"j": 32
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.032647030192474995,
|
||||
"rotation_deg": 1.2921319816631744,
|
||||
"i": 32,
|
||||
"j": 33
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.0345860258877476,
|
||||
"rotation_deg": 1.374123371412574,
|
||||
"i": 32,
|
||||
"j": 34
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.05213790087077668,
|
||||
"rotation_deg": 0.46701073818826994,
|
||||
"i": 32,
|
||||
"j": 35
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.023288081661695596,
|
||||
"rotation_deg": 0.15454273191995999,
|
||||
"i": 33,
|
||||
"j": 34
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.035256268159629366,
|
||||
"rotation_deg": 0.9445397555422734,
|
||||
"i": 33,
|
||||
"j": 35
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.020736197670988343,
|
||||
"rotation_deg": 1.137619185440334,
|
||||
"i": 35,
|
||||
"j": 36
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
{
|
||||
"convention": "delta = inverse(reference) @ candidate",
|
||||
"reference": "results\\01_previous_two_batches\\final_extrinsic_deployment.json",
|
||||
"candidate": "results\\02_data4_calibration\\consensus\\extrinsic.json",
|
||||
"candidate_minus_reference_translation_xyz_m": [
|
||||
0.002616328047628791,
|
||||
-0.0016393602019739468,
|
||||
-0.01562078923962118
|
||||
],
|
||||
"candidate_minus_reference_rpy_xyz_deg": [
|
||||
-0.006740470942029009,
|
||||
0.1911623881626312,
|
||||
-0.13523257777677677
|
||||
],
|
||||
"relative_translation_norm_m": 0.01592299377608647,
|
||||
"relative_rotation_deg": 0.23416878770707064,
|
||||
"relative_matrix_4x4": [
|
||||
[
|
||||
0.9999916502190469,
|
||||
0.0023133957658166954,
|
||||
0.003368633583168801,
|
||||
0.0029672356800607425
|
||||
],
|
||||
[
|
||||
-0.0023136119190219507,
|
||||
0.9999973217800623,
|
||||
6.027097638170339e-05,
|
||||
-0.0013876386622047616
|
||||
],
|
||||
[
|
||||
-0.0033684851306055737,
|
||||
-6.80641839411564e-05,
|
||||
0.999994324321489,
|
||||
-0.015582416441470737
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,207 @@
|
||||
{
|
||||
"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": 34,
|
||||
"metrics": {
|
||||
"pairs": 26,
|
||||
"translation_m": {
|
||||
"rms": 0.11953342461680516,
|
||||
"median": 0.059870553274397435,
|
||||
"p90": 0.11852081980116268,
|
||||
"p95": 0.27001896145006843,
|
||||
"max": 0.3924432090639719
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.248363434126574,
|
||||
"median": 0.7472573318442859,
|
||||
"p90": 1.7836452528035456,
|
||||
"p95": 2.0119235382379705,
|
||||
"max": 4.302962170390954
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.05017939301544147,
|
||||
"rotation_deg": 0.7621142667147064,
|
||||
"i": 0,
|
||||
"j": 2
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.05476119605552225,
|
||||
"rotation_deg": 0.39488243852657817,
|
||||
"i": 2,
|
||||
"j": 3
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.11117501211320562,
|
||||
"rotation_deg": 0.29330702638456563,
|
||||
"i": 2,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.06643052326439366,
|
||||
"rotation_deg": 0.5530967273631149,
|
||||
"i": 3,
|
||||
"j": 5
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.028182994383263396,
|
||||
"rotation_deg": 0.5499639343413396,
|
||||
"i": 5,
|
||||
"j": 8
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.06086463130525318,
|
||||
"rotation_deg": 0.6985420749542534,
|
||||
"i": 6,
|
||||
"j": 7
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.020630277131513722,
|
||||
"rotation_deg": 0.6820744752666614,
|
||||
"i": 6,
|
||||
"j": 8
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.046848360772821,
|
||||
"rotation_deg": 0.8723393366986318,
|
||||
"i": 12,
|
||||
"j": 14
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.05887647524354169,
|
||||
"rotation_deg": 1.9071782288631753,
|
||||
"i": 13,
|
||||
"j": 16
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.11661768017132504,
|
||||
"rotation_deg": 2.046838641362902,
|
||||
"i": 15,
|
||||
"j": 16
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.06473893802255462,
|
||||
"rotation_deg": 0.30644679433866145,
|
||||
"i": 15,
|
||||
"j": 17
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.04963073408543951,
|
||||
"rotation_deg": 0.33998803648541337,
|
||||
"i": 15,
|
||||
"j": 18
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.03313618555659487,
|
||||
"rotation_deg": 0.5283154781000993,
|
||||
"i": 16,
|
||||
"j": 19
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.0691797699772094,
|
||||
"rotation_deg": 0.6320358674350408,
|
||||
"i": 21,
|
||||
"j": 22
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.042364105706927925,
|
||||
"rotation_deg": 1.660112276743916,
|
||||
"i": 21,
|
||||
"j": 24
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.07073443988383908,
|
||||
"rotation_deg": 0.8343384286314613,
|
||||
"i": 22,
|
||||
"j": 23
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.12042395943100032,
|
||||
"rotation_deg": 4.302962170390954,
|
||||
"i": 22,
|
||||
"j": 25
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.05181438442870987,
|
||||
"rotation_deg": 0.8455629957527,
|
||||
"i": 25,
|
||||
"j": 26
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.032857670083788,
|
||||
"rotation_deg": 0.7324003969738653,
|
||||
"i": 25,
|
||||
"j": 28
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.05285636518357229,
|
||||
"rotation_deg": 0.4369186254063126,
|
||||
"i": 26,
|
||||
"j": 27
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.10251045240958172,
|
||||
"rotation_deg": 1.0529040723274141,
|
||||
"i": 26,
|
||||
"j": 29
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.023054151271876995,
|
||||
"rotation_deg": 0.24976946582216739,
|
||||
"i": 27,
|
||||
"j": 28
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.06593100197822659,
|
||||
"rotation_deg": 1.2515362824917693,
|
||||
"i": 27,
|
||||
"j": 29
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.31988396212309117,
|
||||
"rotation_deg": 1.0256849788735003,
|
||||
"i": 29,
|
||||
"j": 30
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.3924432090639719,
|
||||
"rotation_deg": 0.8042671494244682,
|
||||
"i": 29,
|
||||
"j": 31
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.11357259403662164,
|
||||
"rotation_deg": 0.901297615800165,
|
||||
"i": 30,
|
||||
"j": 32
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
+15
-54
@@ -1,60 +1,21 @@
|
||||
# 标定结果索引
|
||||
# 结果目录
|
||||
|
||||
本目录保存本次双天线 RTK—3D LiDAR 手眼标定的可复现结果。坐标约定统一为：
|
||||
本目录只归档轻量标定产物，不包含原始 dlog、rscap、逐帧 NPZ 或完整点云。
|
||||
|
||||
```text
|
||||
X = T_body_lidar
|
||||
```
|
||||
|
||||
即外参将**原始 LiDAR 点**变换到以**后轮轴中心**为原点、X 前/Y 左/Z 上的车体系。
|
||||
|
||||
## 部署时只使用这一个文件
|
||||
|
||||
[`final_extrinsic_deployment.json`](final_extrinsic_deployment.json) 是唯一用于部署的外参文件：
|
||||
|
||||
```text
|
||||
translation [m] = [ 1.297759692, -0.000067331, 0.720497835 ]
|
||||
RPY xyz [deg] = [ -0.785151146, 1.202660822, -0.835510053 ]
|
||||
```
|
||||
|
||||
## 顶层汇总文件
|
||||
|
||||
| 文件 | 用途 | 是否用于部署 |
|
||||
| 目录 | 内容 | 使用建议 |
|
||||
|---|---|---|
|
||||
| [`final_extrinsic_deployment.json`](final_extrinsic_deployment.json) | 精简、固定的最终外参；含平移、RPY、四元数和 4×4 矩阵。 | **是，唯一入口** |
|
||||
| [`final_extrinsic_recommended.json`](final_extrinsic_recommended.json) | 同一算法外参的完整求解记录、残差、地面约束、条件数和 Bootstrap。 | 否；用于审计/复现。 |
|
||||
| [`final_summary.json`](final_summary.json) | 共识结果的摘要。 | 否；用于查看指标。 |
|
||||
| [`comparison_summary.json`](comparison_summary.json) | Open3D GICP、small_gicp 与共识解的对比。 | 否；用于方法对比。 |
|
||||
| `01_previous_two_batches/` | 第二批求解、第一批辅助复核的历史部署结果；含 Open3D、small_gicp、consensus B、质量和灵敏度文件 | 当前部署基线 |
|
||||
| `02_data4_calibration/` | data4 的 Open3D、small_gicp、consensus B 和独立 X | 候选/稳定性证据 |
|
||||
| `03_comparison/` | 两套 X 的 SE(3) 差以及互换批次后的 AX 残差 | 跨批判断依据 |
|
||||
|
||||
`final_extrinsic_deployment.json` 与 `final_extrinsic_recommended.json` 的外参数值相同；前者只是删除了求解过程字段，供上游程序稳定读取。
|
||||
最终部署文件是 `01_previous_two_batches/final_extrinsic_deployment.json`。`02_data4_calibration/final_extrinsic_data4.json` 不应在没有新增独立证据时覆盖它。
|
||||
|
||||
## 子目录
|
||||
常见文件：
|
||||
|
||||
| 目录 | 内容 | 用途 |
|
||||
|---|---|---|
|
||||
| [`common/`](common/) | 第二批每个静止站点拟合得到的地面平面。 | 地面约束检查。 |
|
||||
| [`open3d_gicp/`](open3d_gicp/) | Open3D GICP 生成的 LiDAR 相对运动 B、质量指标、精筛记录和对应外参。 | 后端一的独立结果。 |
|
||||
| [`small_gicp/`](small_gicp/) | small_gicp 生成的 LiDAR 相对运动 B、质量指标、精筛记录和对应外参。 | 后端二的独立结果。 |
|
||||
| [`consensus/`](consensus/) | 两后端一致性筛选后的共识 B，共识外参和第一批辅助复核。 | 主要求解依据。 |
|
||||
| [`diagnostics/`](diagnostics/) | 单运动对 roll/pitch/yaw 局部灵敏度扫描。 | 仅诊断，不可直接当外参。 |
|
||||
|
||||
## 如何识别 B 文件
|
||||
|
||||
每个 `.npz` 都是 LiDAR 相对运动集合 `B_ij = T_Li_Lj`，把第 j 个静止站点的点云变换到第 i 个站点 LiDAR 坐标系。它们不是车体—雷达外参，不能直接写入部署配置。
|
||||
|
||||
- `B_batch2_*`：第二批 38 站高频 RTK 数据，主要求解使用。
|
||||
- `B_batch1_*`：第一批 38 站稀疏 RTK 数据，仅作辅助复核。
|
||||
- `*_estimation.npz`：点云配准的初始候选集合。
|
||||
- `*_refined.npz`：经过点云质量门限后的集合。
|
||||
- `consensus/B_batch2_consensus.npz`：Open3D 与 small_gicp 对同一运动对的 B 差异不超过 `0.05 m / 0.50°` 后保留的 39 对；它是最终主求解输入。
|
||||
|
||||
同名的 `.quality.json/.csv` 记录配准质量，`.refinement.json` 记录精筛过程，`.consensus.json` 记录跨后端一致性筛选。它们都用于追溯，不用于部署。
|
||||
|
||||
## 结果使用边界
|
||||
|
||||
- 主结果由第二批数据求解；第一批只作为独立辅助检查，不参与主结果拟合。
|
||||
- B 的筛选先于 X 的求解，并且不读取待求外参 X，不用 AX 残差反过来选择 B。
|
||||
- 第二批共识集的 AX RMS 为 `0.07985 m / 0.96118°`；旋转残差约 1°，因此不是厘米级绝对精度认证。
|
||||
- 当前 RTK 轨迹只有位置和 yaw，没有 roll/pitch。AX 残差对 Z 不可观；Z 必须结合地面约束和独立机械量测确认。
|
||||
|
||||
更多流程、运行命令和可视化说明见仓库根目录的 [`README.md`](../README.md)。
|
||||
- `B_*_estimation.npz`：后端原始运动对 B。
|
||||
- `B_*_quality.json/csv`：配准和 Hessian/信息矩阵质量。
|
||||
- `B_*_refined.npz`：X 无关筛选后的 B。
|
||||
- `B_*_consensus.npz` 或 `consensus/B_consensus.npz`：双后端认可的 B。
|
||||
- `extrinsic*.json`：由对应 B 和地面约束求出的 X。
|
||||
- `*_check.json`：把给定 X 应用于另一批 B 的残差，不是重新求 X。
|
||||
- `ground_planes*.csv`：每站地面平面参数。
|
||||
|
||||
@@ -0,0 +1,27 @@
|
||||
# run
|
||||
|
||||
本目录是 PowerShell 运行入口；完整流程、参数解释、验收方法和代码职责统一见仓库根目录 `README.md`。
|
||||
|
||||
| 脚本 | 使用时机 |
|
||||
|---|---|
|
||||
| `export_legacy_stations.ps1` | 第一、第二批式逐站 dlog 导出。 |
|
||||
| `export_multisensor_stations.ps1` | LiDAR dlog + 独立 RTK/IMU rscap 导出和时间关联。 |
|
||||
| `prepare_legacy_dataset.ps1` | 旧式导出结果生成 prepared。 |
|
||||
| `prepare_multisensor_dataset.ps1` | 多传感器 combined 结果生成 prepared。 |
|
||||
| `run_single_dataset.ps1` | 单批数据运行 small_gicp、Open3D、consensus 和 X 求解。 |
|
||||
| `run_all.ps1` | 第二批求解、第一批辅助复核的历史流程。 |
|
||||
| `run_consensus_finish.ps1` | 从已有两个后端 B 重做 consensus。 |
|
||||
| `run_sensitivity_scan.ps1` | 指定 Pair 的外参角度灵敏度扫描。 |
|
||||
| `view_result.ps1` | 传入 frames、B、X 查看 3D 配准及增量。 |
|
||||
|
||||
最常用的 3D 入口：
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File ".\run\view_result.ps1" `
|
||||
-Frames "D:\你的数据目录\prepared\frames_all" `
|
||||
-Pairs "D:\你的结果目录\B_consensus.npz" `
|
||||
-Extrinsic "D:\你的结果目录\extrinsic.json" `
|
||||
-PairIndex 0
|
||||
```
|
||||
|
||||
`frames_all` 必须与生成 B 时的站点顺序一致。
|
||||
@@ -0,0 +1,34 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$DataRoot,
|
||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
||||
[string]$LidarObject = "frontlidar",
|
||||
[string]$Timezone = "+08:00",
|
||||
[string[]]$StationNames = @(),
|
||||
[int]$Stride = 1,
|
||||
[double]$RtkMaxDtMs = 150.0
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
$Exporter = Join-Path $Repo "tools\frontlidar_dlog_export.py"
|
||||
if (-not (Test-Path -LiteralPath $DataRoot)) { throw "DataRoot does not exist: $DataRoot" }
|
||||
if ($Stride -lt 1) { throw "Stride must be at least 1" }
|
||||
if ($StationNames.Count -gt 0) {
|
||||
$Stations = @($StationNames | ForEach-Object { Get-Item -LiteralPath (Join-Path $DataRoot $_) })
|
||||
} else {
|
||||
$Stations = @(Get-ChildItem -LiteralPath $DataRoot -Directory | Where-Object {
|
||||
(Test-Path -LiteralPath (Join-Path $_.FullName "dobject")) -and
|
||||
(Test-Path -LiteralPath (Join-Path $_.FullName "dobject_recording"))
|
||||
} | Sort-Object Name)
|
||||
}
|
||||
if ($Stations.Count -eq 0) { throw "No station directory containing dobject and dobject_recording was found" }
|
||||
New-Item -ItemType Directory -Force -Path $OutputRoot | Out-Null
|
||||
foreach ($Station in $Stations) {
|
||||
$Out = Join-Path $OutputRoot $Station.Name
|
||||
Write-Host "[legacy station $($Station.Name)]"
|
||||
& python $Exporter --dlog $Station.FullName --out $Out --object $LidarObject --format npz `
|
||||
--timezone $Timezone --stride $Stride --compress --rtk-sidecars --write-reports --resume `
|
||||
--rtk-max-dt-ms $RtkMaxDtMs
|
||||
if ($LASTEXITCODE -ne 0) { throw "Export failed for station $($Station.Name)" }
|
||||
}
|
||||
Write-Host "Exported stations: $($Stations.Count)"
|
||||
@@ -0,0 +1,89 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$DataRoot,
|
||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
||||
[Parameter(Mandatory = $true)][string]$RtkCapture,
|
||||
[Parameter(Mandatory = $true)][string]$ImuCapture,
|
||||
[string]$LidarObject = "frontlidar",
|
||||
[string]$Timezone = "+08:00",
|
||||
[string[]]$StationNames = @(),
|
||||
[int]$Stride = 1,
|
||||
[double]$RtkMaxDtMs = 150.0,
|
||||
[double]$ImuBeforeMs = 100.0,
|
||||
[double]$ImuAfterMs = 100.0,
|
||||
[switch]$SkipLidarExport,
|
||||
[switch]$SkipSerialParsing
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$RepoRoot = Split-Path -Parent $PSScriptRoot
|
||||
$Exporter = Join-Path $RepoRoot "tools\frontlidar_dlog_export.py"
|
||||
$Builder = Join-Path $RepoRoot "tools\build_multisensor_npz.py"
|
||||
$Parser = Join-Path $RepoRoot "tools\rscap_v2\parse_rtk_imu_v2.py"
|
||||
$Auditor = Join-Path $RepoRoot "tools\rscap_v2\audit_capture_v2.py"
|
||||
$ExportRoot = Join-Path $OutputRoot "export"
|
||||
$ParsedRoot = Join-Path $OutputRoot "parsed"
|
||||
$CombinedRoot = Join-Path $OutputRoot "combined"
|
||||
|
||||
function Run-Python {
|
||||
param([string]$Stage, [string[]]$Arguments)
|
||||
Write-Host "[$Stage]"
|
||||
& python @Arguments
|
||||
if ($LASTEXITCODE -ne 0) {
|
||||
throw "$Stage failed with Python exit code $LASTEXITCODE"
|
||||
}
|
||||
}
|
||||
|
||||
foreach ($Path in @($DataRoot, $RtkCapture, $ImuCapture)) {
|
||||
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
|
||||
}
|
||||
if ($Stride -lt 1) { throw "Stride must be at least 1" }
|
||||
|
||||
if ($StationNames.Count -gt 0) {
|
||||
$Stations = @($StationNames | ForEach-Object { Get-Item -LiteralPath (Join-Path $DataRoot $_) })
|
||||
} else {
|
||||
$Stations = @(Get-ChildItem -LiteralPath $DataRoot -Directory | Where-Object {
|
||||
(Test-Path -LiteralPath (Join-Path $_.FullName "dobject")) -and
|
||||
(Test-Path -LiteralPath (Join-Path $_.FullName "dobject_recording"))
|
||||
} | Sort-Object Name)
|
||||
}
|
||||
if ($Stations.Count -eq 0) { throw "No station directory containing dobject and dobject_recording was found" }
|
||||
|
||||
New-Item -ItemType Directory -Force -Path $OutputRoot | Out-Null
|
||||
if (-not $SkipSerialParsing) {
|
||||
New-Item -ItemType Directory -Force -Path $ParsedRoot | Out-Null
|
||||
Run-Python "capture audit" @($Auditor, $RtkCapture, $ImuCapture, "--out", (Join-Path $OutputRoot "capture_audit.json"))
|
||||
Run-Python "RTK/IMU parse" @($Parser, "--rtk", $RtkCapture, "--imu", $ImuCapture, "--out", $ParsedRoot)
|
||||
}
|
||||
|
||||
foreach ($Station in $Stations) {
|
||||
$StationOut = Join-Path $ExportRoot $Station.Name
|
||||
if (-not $SkipLidarExport) {
|
||||
Run-Python "LiDAR station $($Station.Name)" @(
|
||||
$Exporter, "--dlog", $Station.FullName, "--out", $StationOut,
|
||||
"--object", $LidarObject, "--format", "npz", "--timezone", $Timezone,
|
||||
"--stride", "$Stride", "--compress", "--skip-rtk", "--write-reports", "--resume"
|
||||
)
|
||||
}
|
||||
if (-not (Test-Path -LiteralPath (Join-Path $StationOut "frames"))) {
|
||||
throw "Exported frame directory is absent for station $($Station.Name): $StationOut"
|
||||
}
|
||||
}
|
||||
|
||||
$BuildArgs = @($Builder)
|
||||
foreach ($Station in $Stations) {
|
||||
$Frames = Join-Path (Join-Path $ExportRoot $Station.Name) "frames"
|
||||
$BuildArgs += @("--lidar", "$($Station.Name)=$Frames")
|
||||
}
|
||||
$BuildArgs += @(
|
||||
"--rtk", (Join-Path $ParsedRoot "rtk.jsonl"),
|
||||
"--imu", (Join-Path $ParsedRoot "imu.jsonl"),
|
||||
"--out", $CombinedRoot,
|
||||
"--rtk-max-dt-ms", "$RtkMaxDtMs",
|
||||
"--imu-before-ms", "$ImuBeforeMs",
|
||||
"--imu-after-ms", "$ImuAfterMs",
|
||||
"--overwrite"
|
||||
)
|
||||
Run-Python "LiDAR/RTK/IMU association" $BuildArgs
|
||||
|
||||
Write-Host "Completed stations: $($Stations.Count)"
|
||||
Write-Host "Combined NPZ: $CombinedRoot"
|
||||
@@ -0,0 +1,21 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$ExportRoot,
|
||||
[Parameter(Mandatory = $true)][string]$Output,
|
||||
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
|
||||
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
|
||||
[int]$MinStations = 30,
|
||||
[int]$ExpectedStations = 0,
|
||||
[double]$HeadingStdLimitDeg = [double]::PositiveInfinity,
|
||||
[switch]$Overwrite
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
if ($AntennaLever.Count -ne 3) { throw "AntennaLever must contain X,Y,Z in body coordinates" }
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
$Args = @((Join-Path $Repo "tools\prepare_station_dataset.py"), "--export-root", $ExportRoot,
|
||||
"--output", $Output, "--heading-offset-deg", "$HeadingOffsetDeg", "--antenna-lever") +
|
||||
@($AntennaLever | ForEach-Object { "$_" }) + @("--min-stations", "$MinStations",
|
||||
"--expected-stations", "$ExpectedStations", "--heading-std-limit-deg", "$HeadingStdLimitDeg")
|
||||
if ($Overwrite) { $Args += "--overwrite" }
|
||||
& python @Args
|
||||
if ($LASTEXITCODE -ne 0) { throw "Legacy dataset preparation failed" }
|
||||
@@ -0,0 +1,21 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$CombinedRoot,
|
||||
[Parameter(Mandatory = $true)][string]$Output,
|
||||
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
|
||||
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
|
||||
[int]$MinStations = 30,
|
||||
[int]$ExpectedStations = 0,
|
||||
[double]$HeadingStdLimitDeg = 0.5,
|
||||
[switch]$Overwrite
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
if ($AntennaLever.Count -ne 3) { throw "AntennaLever must contain X,Y,Z in body coordinates" }
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
$Args = @((Join-Path $Repo "tools\prepare_multisensor_station_dataset.py"), "--combined-root", $CombinedRoot,
|
||||
"--output", $Output, "--heading-offset-deg", "$HeadingOffsetDeg", "--antenna-lever") +
|
||||
@($AntennaLever | ForEach-Object { "$_" }) + @("--min-stations", "$MinStations",
|
||||
"--expected-stations", "$ExpectedStations", "--heading-std-limit-deg", "$HeadingStdLimitDeg")
|
||||
if ($Overwrite) { $Args += "--overwrite" }
|
||||
& python @Args
|
||||
if ($LASTEXITCODE -ne 0) { throw "Multisensor dataset preparation failed" }
|
||||
+59
-17
@@ -1,32 +1,74 @@
|
||||
param(
|
||||
[string]$Batch1Prepared = "C:\Users\admin\Documents\Codex\2026-07-15\wo\outputs\calibration_20260717\prepared",
|
||||
[string]$Batch2Prepared = "C:\Users\admin\Documents\Codex\2026-07-15\wo\outputs\calibration_data1_20260720\prepared"
|
||||
[Parameter(Mandatory = $true)][string]$Batch1Prepared,
|
||||
[Parameter(Mandatory = $true)][string]$Batch2Prepared,
|
||||
[string]$OutputRoot = ""
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
if ([string]::IsNullOrWhiteSpace($OutputRoot)) {
|
||||
$OutputRoot = Join-Path $Repo "results\01_previous_two_batches"
|
||||
}
|
||||
$Code = Join-Path $Repo "code\rigorous_calibration.py"
|
||||
$Refine = Join-Path $Repo "code\refine_pairs.py"
|
||||
$Summary = Join-Path $Repo "code\summarize_results.py"
|
||||
$R = Join-Path $Repo "results"
|
||||
$B1Frames = Join-Path $Batch1Prepared "frames_all"
|
||||
$B2Frames = Join-Path $Batch2Prepared "frames_all"
|
||||
$B1Body = Join-Path $Batch1Prepared "body_poses_rear_gga_raw_rear_to_front.csv"
|
||||
$B2Body = Join-Path $Batch2Prepared "body_poses_rear_gga_raw_rear_to_front.csv"
|
||||
|
||||
python $Code ground --frames $B2Frames --output "$R\common\ground_planes_batch2.csv"
|
||||
function Run-Python {
|
||||
param([string]$Stage, [string[]]$Arguments)
|
||||
Write-Host "[$Stage]"
|
||||
& python @Arguments
|
||||
if ($LASTEXITCODE -ne 0) { throw "$Stage failed with Python exit code $LASTEXITCODE" }
|
||||
}
|
||||
|
||||
python $Code pairs --backend small_gicp --frames $B2Frames --body $B2Body --output "$R\small_gicp\B_batch2_estimation.npz" --quality-json "$R\small_gicp\B_batch2_quality.json" --quality-csv "$R\small_gicp\B_batch2_quality.csv"
|
||||
python $Code pairs --backend open3d --frames $B2Frames --body $B2Body --output "$R\open3d_gicp\B_batch2_estimation.npz" --quality-json "$R\open3d_gicp\B_batch2_quality.json" --quality-csv "$R\open3d_gicp\B_batch2_quality.csv" --max-gap 3 --multistart 1 --iterations 40
|
||||
python $Refine --pairs "$R\small_gicp\B_batch2_estimation.npz" --quality-json "$R\small_gicp\B_batch2_quality.json" --output "$R\small_gicp\B_batch2_refined.npz"
|
||||
python $Refine --pairs "$R\open3d_gicp\B_batch2_estimation.npz" --quality-json "$R\open3d_gicp\B_batch2_quality.json" --output "$R\open3d_gicp\B_batch2_refined.npz"
|
||||
python $Code calibrate --pairs "$R\small_gicp\B_batch2_refined.npz" --ground-planes "$R\common\ground_planes_batch2.csv" --output "$R\small_gicp\extrinsic_batch2_refined.json"
|
||||
python $Code calibrate --pairs "$R\open3d_gicp\B_batch2_refined.npz" --ground-planes "$R\common\ground_planes_batch2.csv" --output "$R\open3d_gicp\extrinsic_batch2_refined.json"
|
||||
foreach ($Path in @($B1Frames, $B2Frames, $B1Body, $B2Body)) {
|
||||
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
|
||||
}
|
||||
$Common = Join-Path $OutputRoot "common"
|
||||
$Small = Join-Path $OutputRoot "small_gicp"
|
||||
$Open = Join-Path $OutputRoot "open3d_gicp"
|
||||
New-Item -ItemType Directory -Force -Path $Common,$Small,$Open | Out-Null
|
||||
|
||||
python $Code pairs --backend small_gicp --frames $B1Frames --body $B1Body --output "$R\small_gicp\B_batch1_auxiliary.npz" --quality-json "$R\small_gicp\B_batch1_quality.json" --quality-csv "$R\small_gicp\B_batch1_quality.csv" --max-gap 3 --multistart 1 --iterations 40
|
||||
python $Code pairs --backend open3d --frames $B1Frames --body $B1Body --output "$R\open3d_gicp\B_batch1_auxiliary.npz" --quality-json "$R\open3d_gicp\B_batch1_quality.json" --quality-csv "$R\open3d_gicp\B_batch1_quality.csv" --max-gap 3 --multistart 1 --iterations 40
|
||||
python $Refine --pairs "$R\small_gicp\B_batch1_auxiliary.npz" --quality-json "$R\small_gicp\B_batch1_quality.json" --output "$R\small_gicp\B_batch1_auxiliary_refined.npz"
|
||||
python $Refine --pairs "$R\open3d_gicp\B_batch1_auxiliary.npz" --quality-json "$R\open3d_gicp\B_batch1_quality.json" --output "$R\open3d_gicp\B_batch1_auxiliary_refined.npz"
|
||||
python $Code validate --pairs "$R\small_gicp\B_batch1_auxiliary_refined.npz" --extrinsic "$R\small_gicp\extrinsic_batch2_refined.json" --output "$R\small_gicp\batch1_auxiliary_check.json"
|
||||
python $Code validate --pairs "$R\open3d_gicp\B_batch1_auxiliary_refined.npz" --extrinsic "$R\open3d_gicp\extrinsic_batch2_refined.json" --output "$R\open3d_gicp\batch1_auxiliary_check.json"
|
||||
Run-Python "batch2 ground planes" @($Code, "ground", "--frames", $B2Frames, "--output", (Join-Path $Common "ground_planes_batch2.csv"))
|
||||
|
||||
python $Summary --open3d "$R\open3d_gicp\extrinsic_batch2_refined.json" --small "$R\small_gicp\extrinsic_batch2_refined.json" --open3d-quality "$R\open3d_gicp\B_batch2_quality.json" --small-quality "$R\small_gicp\B_batch2_quality.json" --open3d-check "$R\open3d_gicp\batch1_auxiliary_check.json" --small-check "$R\small_gicp\batch1_auxiliary_check.json" --output "$R\comparison_summary.json" --recommended-output "$R\final_extrinsic_recommended.json"
|
||||
foreach ($Backend in @("small_gicp", "open3d")) {
|
||||
$Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
|
||||
$Extra = @()
|
||||
if ($Backend -eq "open3d") { $Extra = @("--max-gap", "3", "--multistart", "1", "--iterations", "40") }
|
||||
$B2Raw = Join-Path $Directory "B_batch2_estimation.npz"
|
||||
$B2QualityJson = Join-Path $Directory "B_batch2_quality.json"
|
||||
$B2QualityCsv = Join-Path $Directory "B_batch2_quality.csv"
|
||||
Run-Python "$Backend batch2 pairs" (@($Code, "pairs", "--backend", $Backend, "--frames", $B2Frames, "--body", $B2Body,
|
||||
"--output", $B2Raw, "--quality-json", $B2QualityJson, "--quality-csv", $B2QualityCsv) + $Extra)
|
||||
$B2Refined = Join-Path $Directory "B_batch2_refined.npz"
|
||||
Run-Python "$Backend batch2 refinement" @($Refine, "--pairs", $B2Raw, "--quality-json", $B2QualityJson, "--output", $B2Refined)
|
||||
$Extrinsic = Join-Path $Directory "extrinsic_batch2_refined.json"
|
||||
Run-Python "$Backend batch2 calibration" @($Code, "calibrate", "--pairs", $B2Refined,
|
||||
"--ground-planes", (Join-Path $Common "ground_planes_batch2.csv"), "--output", $Extrinsic)
|
||||
|
||||
$B1Raw = Join-Path $Directory "B_batch1_auxiliary.npz"
|
||||
$B1QualityJson = Join-Path $Directory "B_batch1_quality.json"
|
||||
$B1QualityCsv = Join-Path $Directory "B_batch1_quality.csv"
|
||||
Run-Python "$Backend batch1 auxiliary pairs" @($Code, "pairs", "--backend", $Backend, "--frames", $B1Frames, "--body", $B1Body,
|
||||
"--output", $B1Raw, "--quality-json", $B1QualityJson, "--quality-csv", $B1QualityCsv,
|
||||
"--max-gap", "3", "--multistart", "1", "--iterations", "40")
|
||||
$B1Refined = Join-Path $Directory "B_batch1_auxiliary_refined.npz"
|
||||
Run-Python "$Backend batch1 refinement" @($Refine, "--pairs", $B1Raw, "--quality-json", $B1QualityJson, "--output", $B1Refined)
|
||||
Run-Python "$Backend batch1 auxiliary check" @($Code, "validate", "--pairs", $B1Refined,
|
||||
"--extrinsic", $Extrinsic, "--output", (Join-Path $Directory "batch1_auxiliary_check.json"))
|
||||
}
|
||||
|
||||
Run-Python "backend summary" @($Summary,
|
||||
"--open3d", (Join-Path $Open "extrinsic_batch2_refined.json"),
|
||||
"--small", (Join-Path $Small "extrinsic_batch2_refined.json"),
|
||||
"--open3d-quality", (Join-Path $Open "B_batch2_quality.json"),
|
||||
"--small-quality", (Join-Path $Small "B_batch2_quality.json"),
|
||||
"--open3d-check", (Join-Path $Open "batch1_auxiliary_check.json"),
|
||||
"--small-check", (Join-Path $Small "batch1_auxiliary_check.json"),
|
||||
"--output", (Join-Path $OutputRoot "comparison_summary.json"),
|
||||
"--recommended-output", (Join-Path $OutputRoot "final_extrinsic_recommended.json"))
|
||||
|
||||
Write-Host "Two-batch results: $OutputRoot"
|
||||
|
||||
@@ -1,12 +1,34 @@
|
||||
param([string]$ResultRoot = "")
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
if ([string]::IsNullOrWhiteSpace($ResultRoot)) { $ResultRoot = Join-Path $Repo "results\01_previous_two_batches" }
|
||||
$Code = Join-Path $Repo "code\rigorous_calibration.py"
|
||||
$Filter = Join-Path $Repo "code\cross_backend_filter.py"
|
||||
$Finalize = Join-Path $Repo "code\finalize_consensus.py"
|
||||
$R = Join-Path $Repo "results"
|
||||
|
||||
python $Filter --open3d-pairs "$R\open3d_gicp\B_batch2_refined.npz" --small-pairs "$R\small_gicp\B_batch2_refined.npz" --output "$R\consensus\B_batch2_consensus.npz"
|
||||
python $Filter --open3d-pairs "$R\open3d_gicp\B_batch1_auxiliary_refined.npz" --small-pairs "$R\small_gicp\B_batch1_auxiliary_refined.npz" --output "$R\consensus\B_batch1_consensus.npz" --min-pairs 15
|
||||
python $Code calibrate --pairs "$R\consensus\B_batch2_consensus.npz" --ground-planes "$R\common\ground_planes_batch2.csv" --output "$R\consensus\extrinsic_batch2_consensus.json"
|
||||
python $Code validate --pairs "$R\consensus\B_batch1_consensus.npz" --extrinsic "$R\consensus\extrinsic_batch2_consensus.json" --output "$R\consensus\batch1_auxiliary_check.json"
|
||||
python $Finalize --consensus-extrinsic "$R\consensus\extrinsic_batch2_consensus.json" --consensus-check "$R\consensus\batch1_auxiliary_check.json" --open3d-extrinsic "$R\open3d_gicp\extrinsic_batch2_refined.json" --small-extrinsic "$R\small_gicp\extrinsic_batch2_refined.json" --output "$R\final_extrinsic_recommended.json" --summary "$R\final_summary.json"
|
||||
function Run-Python {
|
||||
param([string[]]$Arguments)
|
||||
& python @Arguments
|
||||
if ($LASTEXITCODE -ne 0) { throw "Python failed with exit code $LASTEXITCODE" }
|
||||
}
|
||||
|
||||
Run-Python @($Filter, "--open3d-pairs", (Join-Path $ResultRoot "open3d_gicp\B_batch2_refined.npz"),
|
||||
"--small-pairs", (Join-Path $ResultRoot "small_gicp\B_batch2_refined.npz"),
|
||||
"--output", (Join-Path $ResultRoot "consensus\B_batch2_consensus.npz"))
|
||||
Run-Python @($Filter, "--open3d-pairs", (Join-Path $ResultRoot "open3d_gicp\B_batch1_auxiliary_refined.npz"),
|
||||
"--small-pairs", (Join-Path $ResultRoot "small_gicp\B_batch1_auxiliary_refined.npz"),
|
||||
"--output", (Join-Path $ResultRoot "consensus\B_batch1_consensus.npz"), "--min-pairs", "15")
|
||||
Run-Python @($Code, "calibrate", "--pairs", (Join-Path $ResultRoot "consensus\B_batch2_consensus.npz"),
|
||||
"--ground-planes", (Join-Path $ResultRoot "common\ground_planes_batch2.csv"),
|
||||
"--output", (Join-Path $ResultRoot "consensus\extrinsic_batch2_consensus.json"))
|
||||
Run-Python @($Code, "validate", "--pairs", (Join-Path $ResultRoot "consensus\B_batch1_consensus.npz"),
|
||||
"--extrinsic", (Join-Path $ResultRoot "consensus\extrinsic_batch2_consensus.json"),
|
||||
"--output", (Join-Path $ResultRoot "consensus\batch1_auxiliary_check.json"))
|
||||
Run-Python @($Finalize,
|
||||
"--consensus-extrinsic", (Join-Path $ResultRoot "consensus\extrinsic_batch2_consensus.json"),
|
||||
"--consensus-check", (Join-Path $ResultRoot "consensus\batch1_auxiliary_check.json"),
|
||||
"--open3d-extrinsic", (Join-Path $ResultRoot "open3d_gicp\extrinsic_batch2_refined.json"),
|
||||
"--small-extrinsic", (Join-Path $ResultRoot "small_gicp\extrinsic_batch2_refined.json"),
|
||||
"--output", (Join-Path $ResultRoot "final_extrinsic_recommended.json"),
|
||||
"--summary", (Join-Path $ResultRoot "final_summary.json"))
|
||||
|
||||
@@ -1,15 +1,22 @@
|
||||
param([int]$PairIndex = 0)
|
||||
param(
|
||||
[int]$PairIndex = 0,
|
||||
[string]$ResultRoot = ""
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
$Code = "$Repo\code\scan_extrinsic_sensitivity.py"
|
||||
$X = "$Repo\results\final_extrinsic_recommended.json"
|
||||
$Ground = "$Repo\results\common\ground_planes_batch2.csv"
|
||||
$Out = "$Repo\results\diagnostics"
|
||||
if ([string]::IsNullOrWhiteSpace($ResultRoot)) { $ResultRoot = Join-Path $Repo "results\01_previous_two_batches" }
|
||||
$Code = Join-Path $Repo "code\scan_extrinsic_sensitivity.py"
|
||||
$X = Join-Path $ResultRoot "final_extrinsic_recommended.json"
|
||||
$Ground = Join-Path $ResultRoot "common\ground_planes_batch2.csv"
|
||||
$Out = Join-Path $ResultRoot "diagnostics"
|
||||
|
||||
python $Code --pairs "$Repo\results\open3d_gicp\B_batch2_refined.npz" --extrinsic $X --ground-planes $Ground --pair-index $PairIndex --output "$Out\open3d_refined_pair${PairIndex}_left_rpy_scan.json"
|
||||
if ($LASTEXITCODE -ne 0) { exit $LASTEXITCODE }
|
||||
|
||||
python $Code --pairs "$Repo\results\small_gicp\B_batch2_refined.npz" --extrinsic $X --ground-planes $Ground --pair-index $PairIndex --output "$Out\small_gicp_refined_pair${PairIndex}_left_rpy_scan.json"
|
||||
if ($LASTEXITCODE -ne 0) { exit $LASTEXITCODE }
|
||||
|
||||
python $Code --pairs "$Repo\results\consensus\B_batch2_consensus.npz" --extrinsic $X --ground-planes $Ground --pair-index $PairIndex --output "$Out\consensus_pair${PairIndex}_left_rpy_scan.json"
|
||||
exit $LASTEXITCODE
|
||||
foreach ($Backend in @("open3d_gicp", "small_gicp")) {
|
||||
$Label = if ($Backend -eq "open3d_gicp") { "open3d_refined" } else { "small_gicp_refined" }
|
||||
& python $Code --pairs (Join-Path $ResultRoot "$Backend\B_batch2_refined.npz") --extrinsic $X `
|
||||
--ground-planes $Ground --pair-index $PairIndex --output (Join-Path $Out "${Label}_pair${PairIndex}_left_rpy_scan.json")
|
||||
if ($LASTEXITCODE -ne 0) { throw "$Backend sensitivity scan failed" }
|
||||
}
|
||||
& python $Code --pairs (Join-Path $ResultRoot "consensus\B_batch2_consensus.npz") --extrinsic $X `
|
||||
--ground-planes $Ground --pair-index $PairIndex --output (Join-Path $Out "consensus_pair${PairIndex}_left_rpy_scan.json")
|
||||
if ($LASTEXITCODE -ne 0) { throw "Consensus sensitivity scan failed" }
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$Prepared,
|
||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
||||
[string]$PoseName = "rear_gga_raw_rear_to_front",
|
||||
[double]$BodyHeight = 0.2335,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 100
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
$Code = Join-Path $Repo "code\rigorous_calibration.py"
|
||||
$Refine = Join-Path $Repo "code\refine_pairs.py"
|
||||
$Consensus = Join-Path $Repo "code\cross_backend_filter.py"
|
||||
$Frames = Join-Path $Prepared "frames_all"
|
||||
$Body = Join-Path $Prepared "body_poses_$PoseName.csv"
|
||||
$Common = Join-Path $OutputRoot "common"
|
||||
$Open = Join-Path $OutputRoot "open3d_gicp"
|
||||
$Small = Join-Path $OutputRoot "small_gicp"
|
||||
$ConsensusOut = Join-Path $OutputRoot "consensus"
|
||||
|
||||
function Run-Python {
|
||||
param([string]$Stage, [string[]]$Arguments)
|
||||
Write-Host "[$Stage]"
|
||||
& python @Arguments
|
||||
if ($LASTEXITCODE -ne 0) { throw "$Stage failed with Python exit code $LASTEXITCODE" }
|
||||
}
|
||||
|
||||
foreach ($Path in @($Frames, $Body)) {
|
||||
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
|
||||
}
|
||||
New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null
|
||||
|
||||
$Ground = Join-Path $Common "ground_planes.csv"
|
||||
Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground)
|
||||
|
||||
foreach ($Backend in @("small_gicp", "open3d")) {
|
||||
$Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
|
||||
$Raw = Join-Path $Directory "B_estimation.npz"
|
||||
$QualityJson = Join-Path $Directory "B_quality.json"
|
||||
$QualityCsv = Join-Path $Directory "B_quality.csv"
|
||||
$PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--body", $Body,
|
||||
"--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv,
|
||||
"--min-pairs", "$MinPairs")
|
||||
if ($Backend -eq "open3d") { $PairArgs += @("--max-gap", "3", "--multistart", "1", "--iterations", "40") }
|
||||
Run-Python "$Backend pairs" $PairArgs
|
||||
Run-Python "$Backend X-independent refinement" @(
|
||||
$Refine, "--pairs", $Raw, "--quality-json", $QualityJson,
|
||||
"--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs"
|
||||
)
|
||||
Run-Python "$Backend calibration" @(
|
||||
$Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
|
||||
"--ground-planes", $Ground, "--body-height", "$BodyHeight",
|
||||
"--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
|
||||
)
|
||||
}
|
||||
|
||||
$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
|
||||
Run-Python "cross-backend consensus" @(
|
||||
$Consensus, "--open3d-pairs", (Join-Path $Open "B_refined.npz"),
|
||||
"--small-pairs", (Join-Path $Small "B_refined.npz"),
|
||||
"--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
|
||||
)
|
||||
Run-Python "consensus calibration" @(
|
||||
$Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
|
||||
"--body-height", "$BodyHeight", "--bootstrap", "$Bootstrap",
|
||||
"--output", (Join-Path $ConsensusOut "extrinsic.json")
|
||||
)
|
||||
|
||||
Write-Host "Calibration results: $OutputRoot"
|
||||
@@ -1,14 +0,0 @@
|
||||
param(
|
||||
[int]$PairIndex = 0,
|
||||
[string]$Frames = "C:\Users\admin\Documents\Codex\2026-07-15\wo\outputs\calibration_data1_20260720\prepared\frames_all",
|
||||
[double]$LeftRollDeg = 0.0,
|
||||
[double]$LeftPitchDeg = 0.0,
|
||||
[double]$LeftYawDeg = 0.0
|
||||
)
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
python "$Repo\code\visualize_pair_3d.py" `
|
||||
--frames $Frames `
|
||||
--pairs "$Repo\results\open3d_gicp\B_batch2_refined.npz" `
|
||||
--extrinsic "$Repo\results\final_extrinsic_recommended.json" `
|
||||
--pair-index $PairIndex `
|
||||
--left-rpy-deg $LeftRollDeg $LeftPitchDeg $LeftYawDeg
|
||||
@@ -0,0 +1,19 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$Frames,
|
||||
[Parameter(Mandatory = $true)][string]$Pairs,
|
||||
[Parameter(Mandatory = $true)][string]$Extrinsic,
|
||||
[int]$PairIndex = 0,
|
||||
[double]$LeftRollDeg = 0.0,
|
||||
[double]$LeftPitchDeg = 0.0,
|
||||
[double]$LeftYawDeg = 0.0
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
foreach ($Path in @($Frames, $Pairs, $Extrinsic)) {
|
||||
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
|
||||
}
|
||||
& python (Join-Path $Repo "code\visualize_pair_3d.py") `
|
||||
--frames $Frames --pairs $Pairs --extrinsic $Extrinsic --pair-index $PairIndex `
|
||||
--left-rpy-deg $LeftRollDeg $LeftPitchDeg $LeftYawDeg
|
||||
if ($LASTEXITCODE -ne 0) { throw "Visualization failed with Python exit code $LASTEXITCODE" }
|
||||
@@ -1,18 +0,0 @@
|
||||
param(
|
||||
[int]$PairIndex = 0,
|
||||
[string]$Frames = "C:\Users\admin\Documents\Codex\2026-07-15\wo\outputs\calibration_data1_20260720\prepared\frames_all",
|
||||
[string]$Extrinsic = "",
|
||||
[double]$LeftRollDeg = 0.0,
|
||||
[double]$LeftPitchDeg = 0.0,
|
||||
[double]$LeftYawDeg = 0.0
|
||||
)
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
if ([string]::IsNullOrWhiteSpace($Extrinsic)) {
|
||||
$Extrinsic = "$Repo\results\small_gicp\extrinsic_batch2_refined.json"
|
||||
}
|
||||
python "$Repo\code\visualize_pair_3d.py" `
|
||||
--frames $Frames `
|
||||
--pairs "$Repo\results\small_gicp\B_batch2_refined.npz" `
|
||||
--extrinsic $Extrinsic `
|
||||
--pair-index $PairIndex `
|
||||
--left-rpy-deg $LeftRollDeg $LeftPitchDeg $LeftYawDeg
|
||||
@@ -0,0 +1,26 @@
|
||||
# tools
|
||||
|
||||
本目录负责从原始 dlog/rscap 导出 NPZ、关联 RTK/IMU 并生成 prepared；完整流程和参数统一见仓库根目录 `README.md`。
|
||||
|
||||
| 文件 | 作用 |
|
||||
|---|---|
|
||||
| `frontlidar_dlog_export.py` | 从单个 Medulla dlog 导出原始 LiDAR NPZ，可匹配站内 RTK。 |
|
||||
| `prepare_station_dataset.py` | 旧式逐站导出生成 prepared。 |
|
||||
| `build_multisensor_npz.py` | 以 LiDAR 帧为索引关联独立 RTK/IMU。 |
|
||||
| `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 为 JSONL。 |
|
||||
| `rscap_v2/pipeline_common*.py` | 时间和协议解析共用逻辑。 |
|
||||
|
||||
统一输出接口：
|
||||
|
||||
```text
|
||||
prepared/
|
||||
frames_all/station_*.npz
|
||||
body_poses_rear_gga_raw_rear_to_front.csv
|
||||
station_summary.csv
|
||||
manifest.json
|
||||
```
|
||||
|
||||
后续配准必须使用 `points_raw`，不能使用已经由某套外参变换后的点云。
|
||||
@@ -0,0 +1,312 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window.
|
||||
|
||||
Inputs are LiDAR frame NPZ files from frontlidar_dlog_export.py and parsed
|
||||
RTK/IMU JSONL files from parse_rtk_imu_v2.py. Raw .rscap files remain the
|
||||
traceability source; this script never modifies them.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
|
||||
|
||||
|
||||
def parse_named_path(text: str) -> tuple[str, Path]:
|
||||
if "=" not in text:
|
||||
raise argparse.ArgumentTypeError("expected NAME=PATH")
|
||||
name, raw_path = text.split("=", 1)
|
||||
if not name.strip():
|
||||
raise argparse.ArgumentTypeError("segment name is empty")
|
||||
return name.strip(), Path(raw_path)
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument(
|
||||
"--lidar",
|
||||
type=parse_named_path,
|
||||
action="append",
|
||||
required=True,
|
||||
metavar="NAME=FRAMES_DIR",
|
||||
help="Repeat for each LiDAR segment; directory contains exported *.npz frames.",
|
||||
)
|
||||
parser.add_argument("--rtk", type=Path, action="append", required=True, help="Parsed rtk.jsonl; repeat per session.")
|
||||
parser.add_argument("--imu", type=Path, action="append", required=True, help="Parsed imu.jsonl; repeat per session.")
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
||||
parser.add_argument("--imu-before-ms", type=float, default=100.0)
|
||||
parser.add_argument("--imu-after-ms", type=float, default=100.0)
|
||||
parser.add_argument("--gps-utc-leap-seconds", type=int, default=18)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def load_jsonl(paths: list[Path]) -> list[dict[str, Any]]:
|
||||
rows: list[dict[str, Any]] = []
|
||||
for source_index, path in enumerate(paths):
|
||||
source_file = str(path.resolve())
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
for line_number, line in enumerate(stream, start=1):
|
||||
if not line.strip():
|
||||
continue
|
||||
row = json.loads(line)
|
||||
row["_source_file"] = source_file
|
||||
row["_source_index"] = source_index
|
||||
row["_source_line"] = line_number
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def utf8_array(value: Any) -> np.ndarray:
|
||||
return np.frombuffer(str(value if value is not None else "").encode("utf-8"), dtype=np.uint8)
|
||||
|
||||
|
||||
def scalar(array: np.ndarray) -> Any:
|
||||
return array.reshape(-1)[0].item()
|
||||
|
||||
|
||||
def nearest_index(times: np.ndarray, target: int) -> int:
|
||||
if not len(times):
|
||||
return -1
|
||||
right = int(np.searchsorted(times, target, side="left"))
|
||||
candidates = [index for index in (right - 1, right) if 0 <= index < len(times)]
|
||||
return min(candidates, key=lambda index: abs(int(times[index]) - target))
|
||||
|
||||
|
||||
def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
|
||||
"""Recover 100 Hz timing inside each serial chunk from device timestamps.
|
||||
|
||||
A capture chunk has one host receive timestamp but may contain several IMU
|
||||
frames. The last frame is anchored to the chunk receive time and earlier
|
||||
frames are moved backwards by their device timestamp difference.
|
||||
"""
|
||||
groups: dict[tuple[int, int], list[dict[str, Any]]] = {}
|
||||
for row in rows:
|
||||
if not row.get("crc_valid") or row.get("device_timestamp_ms") is None:
|
||||
continue
|
||||
key = (int(row["_source_index"]), int(row.get("source_chunk_sequence_last", -1)))
|
||||
groups.setdefault(key, []).append(row)
|
||||
result: list[dict[str, Any]] = []
|
||||
for group in groups.values():
|
||||
group.sort(key=lambda row: (int(row["device_timestamp_ms"]), int(row["_source_line"])))
|
||||
last_device = int(group[-1]["device_timestamp_ms"])
|
||||
host_ns = int(group[-1]["host_receive_utc_ns"])
|
||||
for row in group:
|
||||
delta_ms = (last_device - int(row["device_timestamp_ms"])) & 0xFFFFFFFF
|
||||
if delta_ms > 60_000:
|
||||
delta_ms = 0
|
||||
copied = dict(row)
|
||||
copied["estimated_time_ns"] = host_ns - delta_ms * 1_000_000
|
||||
result.append(copied)
|
||||
result.sort(key=lambda row: int(row["estimated_time_ns"]))
|
||||
return result
|
||||
|
||||
|
||||
def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
|
||||
week, tow_ms = row.get("gnss_week"), row.get("gnss_tow_ms")
|
||||
if week is None or tow_ms is None:
|
||||
return None
|
||||
seconds = int(week) * 604800 + float(tow_ms) / 1000.0 - leap_seconds
|
||||
return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
|
||||
|
||||
|
||||
def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray:
|
||||
return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype)
|
||||
|
||||
|
||||
def raw_frame_matrix(rows: list[dict[str, Any]]) -> tuple[np.ndarray, np.ndarray]:
|
||||
frames = [bytes.fromhex(str(row.get("raw_frame_hex", ""))) for row in rows]
|
||||
lengths = np.asarray([len(frame) for frame in frames], dtype=np.int32)
|
||||
width = max(lengths, default=0)
|
||||
matrix = np.zeros((len(frames), width), dtype=np.uint8)
|
||||
for index, frame in enumerate(frames):
|
||||
matrix[index, : len(frame)] = np.frombuffer(frame, dtype=np.uint8)
|
||||
return matrix, lengths
|
||||
|
||||
|
||||
def add_rtk(values: dict[str, np.ndarray], prefix: str, row: dict[str, Any] | None, dt_ns: int | None) -> None:
|
||||
values[f"{prefix}_valid"] = np.asarray([row is not None], dtype=np.uint8)
|
||||
values[f"{prefix}_dt_ns"] = np.asarray([dt_ns or 0], dtype=np.int64)
|
||||
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([0], dtype=np.int64)
|
||||
values[f"{prefix}_raw_utf8"] = utf8_array("")
|
||||
values[f"{prefix}_source_file_utf8"] = utf8_array("")
|
||||
values[f"{prefix}_source_raw_file_offset"] = np.asarray([-1], dtype=np.int64)
|
||||
values[f"{prefix}_source_raw_byte_length"] = np.asarray([0], dtype=np.int32)
|
||||
if row is None:
|
||||
return
|
||||
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([row.get("host_receive_utc_ns", 0)], dtype=np.int64)
|
||||
values[f"{prefix}_raw_utf8"] = utf8_array(row.get("raw_line", ""))
|
||||
values[f"{prefix}_source_file_utf8"] = utf8_array(row.get("_source_file", ""))
|
||||
values[f"{prefix}_source_raw_file_offset"] = np.asarray([row.get("source_raw_file_offset", -1)], dtype=np.int64)
|
||||
values[f"{prefix}_source_raw_byte_length"] = np.asarray([row.get("source_raw_byte_length", 0)], dtype=np.int32)
|
||||
|
||||
|
||||
def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
|
||||
for key, dtype, default in (
|
||||
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
|
||||
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
|
||||
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
|
||||
("differential_age_s", np.float64, np.nan),
|
||||
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
||||
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
||||
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
|
||||
("solution_satellites", np.int32, -1),
|
||||
):
|
||||
values[f"rtk_{key}"] = np.asarray([default], dtype=dtype)
|
||||
values["rtk_fixed"] = np.asarray([0], dtype=np.uint8)
|
||||
values["rtk_heading_solution_utf8"] = utf8_array("")
|
||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64)
|
||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64)
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.out.exists() and any(args.out.iterdir()) and not args.overwrite:
|
||||
raise FileExistsError(f"{args.out} is non-empty; pass --overwrite")
|
||||
frames_out = args.out / "frames"
|
||||
frames_out.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
rtk_rows = load_jsonl(args.rtk)
|
||||
gga = sorted(
|
||||
[row for row in rtk_rows if row.get("type") == "GGA" and row.get("checksum_valid") and row.get("lat_deg") is not None],
|
||||
key=lambda row: int(row["host_receive_utc_ns"]),
|
||||
)
|
||||
heading = sorted(
|
||||
[row for row in rtk_rows if row.get("type") == "UNIHEADINGA" and row.get("checksum_valid") and row.get("heading_valid")],
|
||||
key=lambda row: int(row["host_receive_utc_ns"]),
|
||||
)
|
||||
imu = estimate_imu_times(load_jsonl(args.imu))
|
||||
gga_times = np.asarray([int(row["host_receive_utc_ns"]) for row in gga], dtype=np.int64)
|
||||
heading_times = np.asarray([int(row["host_receive_utc_ns"]) for row in heading], dtype=np.int64)
|
||||
imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64)
|
||||
|
||||
manifest: list[dict[str, Any]] = []
|
||||
global_index = 0
|
||||
max_rtk_ns = int(args.rtk_max_dt_ms * 1_000_000)
|
||||
before_ns = int(args.imu_before_ms * 1_000_000)
|
||||
after_ns = int(args.imu_after_ms * 1_000_000)
|
||||
|
||||
for segment_name, frame_dir in args.lidar:
|
||||
frame_paths = sorted(frame_dir.glob("*.npz"))
|
||||
if not frame_paths:
|
||||
raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
|
||||
for segment_index, source in enumerate(frame_paths):
|
||||
with np.load(source, allow_pickle=False) as frame:
|
||||
values = {key: np.asarray(frame[key]) for key in frame.files}
|
||||
lidar_time_ns = int(scalar(values["unix_time_ns"]))
|
||||
|
||||
gga_index = nearest_index(gga_times, lidar_time_ns)
|
||||
heading_index = nearest_index(heading_times, lidar_time_ns)
|
||||
gga_row = gga[gga_index] if gga_index >= 0 else None
|
||||
heading_row = heading[heading_index] if heading_index >= 0 else None
|
||||
gga_dt = int(gga_times[gga_index]) - lidar_time_ns if gga_index >= 0 else None
|
||||
heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None
|
||||
gga_ok = gga_row is not None and abs(gga_dt or 0) <= max_rtk_ns
|
||||
heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns
|
||||
add_rtk(values, "rtk_gga", gga_row if gga_ok else None, gga_dt)
|
||||
add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt)
|
||||
initialize_rtk_measurements(values)
|
||||
|
||||
if gga_ok and gga_row:
|
||||
for key, dtype, default in (
|
||||
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
|
||||
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
|
||||
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
|
||||
("differential_age_s", np.float64, np.nan),
|
||||
):
|
||||
values[f"rtk_{key}"] = np.asarray([gga_row.get(key, default)], dtype=dtype)
|
||||
values["rtk_gga_satellites"] = np.asarray([gga_row.get("satellites", -1)], dtype=np.int32)
|
||||
values["rtk_fixed"] = np.asarray([int(gga_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
|
||||
if heading_ok and heading_row:
|
||||
for key, dtype, default in (
|
||||
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
||||
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
||||
("pitch_stddev_deg", np.float64, np.nan),
|
||||
("solution_satellites", np.int32, -1),
|
||||
):
|
||||
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
|
||||
values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
|
||||
values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
|
||||
device_ns = gnss_utc_ns(heading_row, args.gps_utc_leap_seconds)
|
||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64)
|
||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray(
|
||||
[int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64
|
||||
)
|
||||
|
||||
left = int(np.searchsorted(imu_times, lidar_time_ns - before_ns, side="left"))
|
||||
right = int(np.searchsorted(imu_times, lidar_time_ns + after_ns, side="right"))
|
||||
window = imu[left:right]
|
||||
values["imu_window_count"] = np.asarray([len(window)], dtype=np.int32)
|
||||
values["imu_valid"] = np.asarray([bool(window)], dtype=np.uint8)
|
||||
values["imu_time_ns"] = numeric_array(window, "estimated_time_ns", np.int64, 0)
|
||||
values["imu_host_receive_utc_ns"] = numeric_array(window, "host_receive_utc_ns", np.int64, 0)
|
||||
for key in ("device_timestamp_ms", "pps_sync_stamp_ms", "tag"):
|
||||
values[f"imu_{key}"] = numeric_array(window, key, np.int64, -1)
|
||||
for key in (
|
||||
"temperature_c", "air_pressure_pa", "accel_x_mps2", "accel_y_mps2", "accel_z_mps2",
|
||||
"gyro_x_radps", "gyro_y_radps", "gyro_z_radps", "mag_x_ut", "mag_y_ut", "mag_z_ut",
|
||||
"roll_deg", "pitch_deg", "yaw_deg", "quaternion_w", "quaternion_x", "quaternion_y", "quaternion_z",
|
||||
):
|
||||
values[f"imu_{key}"] = numeric_array(window, key, np.float64, np.nan)
|
||||
values["imu_source_index"] = numeric_array(window, "_source_index", np.int32, -1)
|
||||
values["imu_source_raw_file_offset"] = numeric_array(window, "source_raw_file_offset", np.int64, -1)
|
||||
raw_matrix, raw_lengths = raw_frame_matrix(window)
|
||||
values["imu_raw_frame_bytes"] = raw_matrix
|
||||
values["imu_raw_frame_length"] = raw_lengths
|
||||
values["imu_source_files_json_utf8"] = utf8_array(json.dumps([str(path.resolve()) for path in args.imu], ensure_ascii=False))
|
||||
values["source_lidar_file_utf8"] = utf8_array(source.resolve())
|
||||
values["segment_name_utf8"] = utf8_array(segment_name)
|
||||
|
||||
output = frames_out / f"{segment_name}_{segment_index:06d}.npz"
|
||||
np.savez_compressed(output, **values)
|
||||
manifest.append({
|
||||
"global_index": global_index,
|
||||
"segment": segment_name,
|
||||
"segment_index": segment_index,
|
||||
"output": str(output.relative_to(args.out)),
|
||||
"source_lidar": str(source.resolve()),
|
||||
"lidar_time_ns": lidar_time_ns,
|
||||
"rtk_gga_dt_ns": gga_dt,
|
||||
"rtk_heading_dt_ns": heading_dt,
|
||||
"rtk_valid": gga_ok,
|
||||
"heading_valid": heading_ok,
|
||||
"rtk_fix_quality": gga_row.get("fix_quality") if gga_ok and gga_row else None,
|
||||
"rtk_fixed": bool(gga_ok and gga_row and int(gga_row.get("fix_quality", -1)) in {4, 5}),
|
||||
"imu_window_count": len(window),
|
||||
})
|
||||
global_index += 1
|
||||
|
||||
fields = sorted({key for row in manifest for key in row})
|
||||
with (args.out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
||||
writer.writeheader()
|
||||
writer.writerows(manifest)
|
||||
summary = {
|
||||
"frames": len(manifest),
|
||||
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in args.lidar},
|
||||
"rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest),
|
||||
"heading_valid": sum(bool(row["heading_valid"]) for row in manifest),
|
||||
"rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest),
|
||||
"imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest),
|
||||
"rtk_max_dt_ms": args.rtk_max_dt_ms,
|
||||
"imu_window_ms": [-args.imu_before_ms, args.imu_after_ms],
|
||||
"time_basis": "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement",
|
||||
"imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied",
|
||||
}
|
||||
(args.out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,170 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare one static LiDAR frame and one yaw-only RTK body pose per NPZ segment."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import shutil
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
|
||||
|
||||
|
||||
def natural_key(value: str) -> list[Any]:
|
||||
return [int(part) if part.isdigit() else part.lower() for part in re.split(r"(\d+)", value)]
|
||||
|
||||
|
||||
def truth(value: Any) -> bool:
|
||||
return str(value).strip().lower() in {"1", "true", "yes", "y"}
|
||||
|
||||
|
||||
def circular_mean_deg(values: np.ndarray) -> float:
|
||||
radians = np.deg2rad(values)
|
||||
return float(np.rad2deg(math.atan2(np.mean(np.sin(radians)), np.mean(np.cos(radians)))) % 360.0)
|
||||
|
||||
|
||||
def circular_std_deg(values: np.ndarray) -> float:
|
||||
radians = np.deg2rad(values)
|
||||
resultant = max(math.hypot(np.mean(np.cos(radians)), np.mean(np.sin(radians))), 1e-12)
|
||||
return float(np.rad2deg(math.sqrt(-2.0 * math.log(resultant))))
|
||||
|
||||
|
||||
def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray:
|
||||
a, e2 = 6378137.0, 6.69437999014e-3
|
||||
lat, lon = math.radians(lat_deg), math.radians(lon_deg)
|
||||
sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
n = a / math.sqrt(1.0 - e2 * sin_lat * sin_lat)
|
||||
return np.array([(n + height_m) * cos_lat * cos_lon, (n + height_m) * cos_lat * sin_lon,
|
||||
(n * (1.0 - e2) + height_m) * sin_lat], dtype=float)
|
||||
|
||||
|
||||
def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: float) -> np.ndarray:
|
||||
lat, lon = math.radians(lat_deg), math.radians(lon_deg)
|
||||
slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
rotation = np.array([[-slon, clon, 0.0], [-slat * clon, -slat * slon, clat],
|
||||
[clat * clon, clat * slon, slat]], dtype=float)
|
||||
return rotation @ (ecef - origin)
|
||||
|
||||
|
||||
def yaw_rotation(yaw: float) -> np.ndarray:
|
||||
c, s = math.cos(yaw), math.sin(yaw)
|
||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
|
||||
|
||||
|
||||
def scalar(data: np.lib.npyio.NpzFile, name: str) -> float:
|
||||
return float(np.asarray(data[name]).reshape(-1)[0])
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--combined-root", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--pose-name", default="rear_gga_raw_rear_to_front")
|
||||
parser.add_argument("--heading-offset-deg", type=float, required=True)
|
||||
parser.add_argument("--antenna-lever", type=float, nargs=3, required=True, metavar=("X", "Y", "Z"))
|
||||
parser.add_argument("--accepted-fixes", type=int, nargs="+", default=[4, 5])
|
||||
parser.add_argument("--heading-std-limit-deg", type=float, default=0.5)
|
||||
parser.add_argument("--min-stations", type=int, default=30)
|
||||
parser.add_argument("--expected-stations", type=int, default=0)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
manifest_path = args.combined_root / "manifest.csv"
|
||||
with manifest_path.open("r", encoding="utf-8-sig", newline="") as stream:
|
||||
rows = list(csv.DictReader(stream))
|
||||
required = {"segment", "output", "lidar_time_ns", "rtk_valid", "heading_valid", "rtk_fix_quality"}
|
||||
if not rows or not required.issubset(rows[0]):
|
||||
raise ValueError(f"{manifest_path} is empty or lacks {sorted(required)}")
|
||||
groups: dict[str, list[dict[str, str]]] = {}
|
||||
for row in rows:
|
||||
groups.setdefault(row["segment"], []).append(row)
|
||||
|
||||
selected, summaries, rejected = [], [], []
|
||||
accepted_fixes = set(args.accepted_fixes)
|
||||
for segment in sorted(groups, key=natural_key):
|
||||
group = sorted(groups[segment], key=lambda row: int(row["lidar_time_ns"]))
|
||||
good = [row for row in group if truth(row["rtk_valid"]) and truth(row["heading_valid"])
|
||||
and int(row["rtk_fix_quality"]) in accepted_fixes]
|
||||
if not good:
|
||||
rejected.append({"station": segment, "reason": "no associated fixed RTK position and valid heading"})
|
||||
continue
|
||||
samples = []
|
||||
for row in good:
|
||||
path = args.combined_root / Path(row["output"])
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
samples.append((scalar(data, "rtk_lat_deg"), scalar(data, "rtk_lon_deg"),
|
||||
scalar(data, "rtk_altitude_m"), scalar(data, "rtk_raw_heading_deg"),
|
||||
scalar(data, "rtk_pitch_deg"), scalar(data, "rtk_heading_stddev_deg")))
|
||||
values = np.asarray(samples, dtype=float)
|
||||
heading_std = circular_std_deg(values[:, 3])
|
||||
if heading_std > args.heading_std_limit_deg:
|
||||
rejected.append({"station": segment, "reason": f"heading std {heading_std:.4f} deg exceeds limit"})
|
||||
continue
|
||||
frame = good[len(good) // 2]
|
||||
source = args.combined_root / Path(frame["output"])
|
||||
selected.append({"station": segment, "source": source, "time": int(frame["lidar_time_ns"]) / 1e9,
|
||||
"lat": float(np.mean(values[:, 0])), "lon": float(np.mean(values[:, 1])),
|
||||
"alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3])})
|
||||
summaries.append({"station": segment, "frames": len(group), "valid_fixed_frames": len(good),
|
||||
"heading_mean_deg": circular_mean_deg(values[:, 3]),
|
||||
"heading_circular_std_deg": heading_std, "rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
|
||||
"reported_heading_std_mean_deg": float(np.nanmean(values[:, 5])),
|
||||
"altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source)})
|
||||
|
||||
if args.expected_stations and len(selected) != args.expected_stations:
|
||||
raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
||||
if len(selected) < args.min_stations:
|
||||
raise RuntimeError(f"need at least {args.min_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
||||
if args.output.exists() and any(args.output.iterdir()) and not args.overwrite:
|
||||
raise FileExistsError(f"{args.output} is non-empty; pass --overwrite")
|
||||
frames = args.output / "frames_all"
|
||||
frames.mkdir(parents=True, exist_ok=True)
|
||||
origin = selected[0]
|
||||
origin_ecef = geodetic_to_ecef(origin["lat"], origin["lon"], origin["alt"])
|
||||
lever = np.asarray(args.antenna_lever, dtype=float)
|
||||
pose_rows = []
|
||||
for index, item in enumerate(selected, 1):
|
||||
destination = frames / f"station_{index:02d}.npz"
|
||||
shutil.copy2(item["source"], destination)
|
||||
antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
|
||||
origin["lat"], origin["lon"])
|
||||
corrected_heading = (item["heading"] + args.heading_offset_deg) % 360.0
|
||||
yaw = math.radians(90.0 - corrected_heading)
|
||||
body = antenna - yaw_rotation(yaw) @ lever
|
||||
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *body, 0.0, 0.0,
|
||||
math.sin(yaw / 2.0), math.cos(yaw / 2.0)])))
|
||||
summaries[index - 1].update({"sequence": index, "prepared_frame": destination.name,
|
||||
"corrected_heading_deg": corrected_heading})
|
||||
pose_path = args.output / f"body_poses_{args.pose_name}.csv"
|
||||
with pose_path.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=POSE_FIELDS); writer.writeheader(); writer.writerows(pose_rows)
|
||||
with (args.output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
fields = sorted({key for row in summaries for key in row})
|
||||
writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
|
||||
document = {"source_combined_root": str(args.combined_root.resolve()), "station_count": len(selected),
|
||||
"rejected": rejected, "pose_csv": pose_path.name,
|
||||
"selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
|
||||
"body_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg,
|
||||
"antenna_lever_body_m": args.antenna_lever,
|
||||
"orientation_model": "yaw-only, identical to the previous calibration workflow"},
|
||||
"stations": [{"sequence": i + 1, "source_station": item["station"],
|
||||
"source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
|
||||
for i, item in enumerate(selected)]}
|
||||
(args.output / "manifest.json").write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps({"prepared": str(args.output.resolve()), "stations": len(selected),
|
||||
"rejected": rejected, "pose_csv": pose_path.name}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,22 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture
|
||||
from pipeline_common import write_json
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("captures", nargs="+", type=Path)
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
summaries = [file_summary(read_capture(path)) for path in args.captures]
|
||||
write_json(args.out, {"captures": summaries})
|
||||
for summary in summaries:
|
||||
print(summary)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,254 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import binascii
|
||||
import io
|
||||
import struct
|
||||
from dataclasses import dataclass, asdict
|
||||
from pathlib import Path
|
||||
from typing import BinaryIO, Iterator
|
||||
|
||||
|
||||
FILE_MAGIC = "RAW_SERIAL_CAPTURE_FILE_V2"
|
||||
RECORD_MAGIC = "RAW_SERIAL_RECORD_V2"
|
||||
FOOTER_MAGIC = "RAW_SERIAL_CAPTURE_FOOTER_V2"
|
||||
|
||||
|
||||
def read_7bit_int(stream: BinaryIO) -> int:
|
||||
value = 0
|
||||
shift = 0
|
||||
while True:
|
||||
raw = stream.read(1)
|
||||
if not raw:
|
||||
raise EOFError("truncated .NET string length")
|
||||
value |= (raw[0] & 0x7F) << shift
|
||||
if not raw[0] & 0x80:
|
||||
return value
|
||||
shift += 7
|
||||
if shift > 35:
|
||||
raise ValueError("invalid .NET string length")
|
||||
|
||||
|
||||
def read_dotnet_string(stream: BinaryIO) -> str:
|
||||
length = read_7bit_int(stream)
|
||||
raw = stream.read(length)
|
||||
if len(raw) != length:
|
||||
raise EOFError("truncated .NET string")
|
||||
return raw.decode("utf-8")
|
||||
|
||||
|
||||
def read_i32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated int32")
|
||||
return struct.unpack("<i", raw)[0]
|
||||
|
||||
|
||||
def read_i64(stream: BinaryIO) -> int:
|
||||
raw = stream.read(8)
|
||||
if len(raw) != 8:
|
||||
raise EOFError("truncated int64")
|
||||
return struct.unpack("<q", raw)[0]
|
||||
|
||||
|
||||
def read_u32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated uint32")
|
||||
return struct.unpack("<I", raw)[0]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CaptureHeader:
|
||||
sensor_kind: str
|
||||
session_id: str
|
||||
session_start_utc_ticks: int
|
||||
session_start_monotonic_ticks: int
|
||||
monotonic_frequency: int
|
||||
port: str
|
||||
baud: int
|
||||
file_start_utc_ticks: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RawChunk:
|
||||
sequence: int
|
||||
receive_utc_ticks: int
|
||||
receive_monotonic_ticks: int
|
||||
raw: bytes
|
||||
record_file_offset: int
|
||||
raw_file_offset: int
|
||||
record_crc32: int
|
||||
crc_valid: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CaptureFooter:
|
||||
clean_close: bool
|
||||
records: int
|
||||
bytes: int
|
||||
first_sequence: int
|
||||
last_sequence: int
|
||||
dropped_chunks: int
|
||||
dropped_bytes: int
|
||||
crc_valid: bool
|
||||
|
||||
|
||||
@dataclass
|
||||
class CaptureFile:
|
||||
path: str
|
||||
header: CaptureHeader
|
||||
chunks: list[RawChunk]
|
||||
footer: CaptureFooter | None
|
||||
truncated_tail: bool = False
|
||||
|
||||
|
||||
def read_header(stream: BinaryIO) -> CaptureHeader:
|
||||
if read_dotnet_string(stream) != FILE_MAGIC:
|
||||
raise ValueError("not a V2 raw capture file")
|
||||
version = read_i32(stream)
|
||||
if version != 2:
|
||||
raise ValueError(f"unsupported capture version: {version}")
|
||||
return CaptureHeader(
|
||||
sensor_kind=read_dotnet_string(stream),
|
||||
session_id=read_dotnet_string(stream),
|
||||
session_start_utc_ticks=read_i64(stream),
|
||||
session_start_monotonic_ticks=read_i64(stream),
|
||||
monotonic_frequency=read_i64(stream),
|
||||
port=read_dotnet_string(stream),
|
||||
baud=read_i32(stream),
|
||||
file_start_utc_ticks=read_i64(stream),
|
||||
)
|
||||
|
||||
|
||||
def parse_record_body(body: bytes, record_file_offset: int, record_crc: int) -> RawChunk:
|
||||
stream = io.BytesIO(body)
|
||||
if read_dotnet_string(stream) != RECORD_MAGIC:
|
||||
raise ValueError("invalid record magic")
|
||||
sequence = read_i64(stream)
|
||||
receive_utc_ticks = read_i64(stream)
|
||||
receive_monotonic_ticks = read_i64(stream)
|
||||
raw_length = read_i32(stream)
|
||||
if raw_length < 0 or raw_length > 64 * 1024 * 1024:
|
||||
raise ValueError(f"invalid raw length: {raw_length}")
|
||||
raw_offset = record_file_offset + 4 + stream.tell()
|
||||
raw = stream.read(raw_length)
|
||||
if len(raw) != raw_length:
|
||||
raise EOFError("truncated raw bytes")
|
||||
crc_valid = (binascii.crc32(body) & 0xFFFFFFFF) == record_crc
|
||||
return RawChunk(
|
||||
sequence=sequence,
|
||||
receive_utc_ticks=receive_utc_ticks,
|
||||
receive_monotonic_ticks=receive_monotonic_ticks,
|
||||
raw=raw,
|
||||
record_file_offset=record_file_offset,
|
||||
raw_file_offset=raw_offset,
|
||||
record_crc32=record_crc,
|
||||
crc_valid=crc_valid,
|
||||
)
|
||||
|
||||
|
||||
def parse_footer(body: bytes, expected_crc: int) -> CaptureFooter:
|
||||
stream = io.BytesIO(body)
|
||||
if read_dotnet_string(stream) != FOOTER_MAGIC:
|
||||
raise ValueError("invalid footer magic")
|
||||
clean_close = stream.read(1) == b"\x01"
|
||||
records = read_i64(stream)
|
||||
raw_bytes = read_i64(stream)
|
||||
first_sequence = read_i64(stream)
|
||||
last_sequence = read_i64(stream)
|
||||
dropped_chunks = read_i64(stream)
|
||||
dropped_bytes = read_i64(stream)
|
||||
return CaptureFooter(
|
||||
clean_close=clean_close,
|
||||
records=records,
|
||||
bytes=raw_bytes,
|
||||
first_sequence=first_sequence,
|
||||
last_sequence=last_sequence,
|
||||
dropped_chunks=dropped_chunks,
|
||||
dropped_bytes=dropped_bytes,
|
||||
crc_valid=(binascii.crc32(body) & 0xFFFFFFFF) == expected_crc,
|
||||
)
|
||||
|
||||
|
||||
def read_capture(path: Path) -> CaptureFile:
|
||||
chunks: list[RawChunk] = []
|
||||
footer = None
|
||||
truncated = False
|
||||
with path.open("rb") as stream:
|
||||
header = read_header(stream)
|
||||
while True:
|
||||
record_offset = stream.tell()
|
||||
length_raw = stream.read(4)
|
||||
if not length_raw:
|
||||
break
|
||||
if len(length_raw) != 4:
|
||||
truncated = True
|
||||
break
|
||||
length = struct.unpack("<i", length_raw)[0]
|
||||
try:
|
||||
if length == -1:
|
||||
footer_length = read_i32(stream)
|
||||
if footer_length < 0 or footer_length > 1024 * 1024:
|
||||
raise ValueError("invalid footer length")
|
||||
footer_body = stream.read(footer_length)
|
||||
if len(footer_body) != footer_length:
|
||||
raise EOFError("truncated footer")
|
||||
footer = parse_footer(footer_body, read_u32(stream))
|
||||
break
|
||||
if length <= 0 or length > 64 * 1024 * 1024:
|
||||
raise ValueError("invalid record length")
|
||||
body = stream.read(length)
|
||||
if len(body) != length:
|
||||
raise EOFError("truncated record body")
|
||||
record_crc = read_u32(stream)
|
||||
chunks.append(parse_record_body(body, record_offset, record_crc))
|
||||
except (EOFError, ValueError):
|
||||
truncated = True
|
||||
break
|
||||
return CaptureFile(str(path), header, chunks, footer, truncated)
|
||||
|
||||
|
||||
def sequence_gaps(chunks: list[RawChunk]) -> list[tuple[int, int, int]]:
|
||||
result = []
|
||||
for previous, current in zip(chunks, chunks[1:]):
|
||||
if current.sequence > previous.sequence + 1:
|
||||
result.append((previous.sequence, current.sequence, current.sequence - previous.sequence - 1))
|
||||
return result
|
||||
|
||||
|
||||
def file_summary(capture: CaptureFile) -> dict:
|
||||
gaps = sequence_gaps(capture.chunks)
|
||||
sequences = [chunk.sequence for chunk in capture.chunks]
|
||||
return {
|
||||
"path": capture.path,
|
||||
"sensor": capture.header.sensor_kind,
|
||||
"session_id": capture.header.session_id,
|
||||
"port": capture.header.port,
|
||||
"baud": capture.header.baud,
|
||||
"chunks_read": len(capture.chunks),
|
||||
"bytes_read": sum(len(chunk.raw) for chunk in capture.chunks),
|
||||
"first_sequence": sequences[0] if sequences else None,
|
||||
"last_sequence": sequences[-1] if sequences else None,
|
||||
"missing_chunks": sum(gap[2] for gap in gaps),
|
||||
"gap_count": len(gaps),
|
||||
"bad_record_crc": sum(not chunk.crc_valid for chunk in capture.chunks),
|
||||
"truncated_tail": capture.truncated_tail,
|
||||
"footer": None if capture.footer is None else asdict(capture.footer),
|
||||
"gaps": gaps[:100],
|
||||
}
|
||||
|
||||
|
||||
def iter_contiguous_segments(chunks: list[RawChunk]) -> Iterator[tuple[int, list[RawChunk]]]:
|
||||
if not chunks:
|
||||
return
|
||||
segment_id = 0
|
||||
current = [chunks[0]]
|
||||
for previous, chunk in zip(chunks, chunks[1:]):
|
||||
if chunk.sequence != previous.sequence + 1:
|
||||
yield segment_id, current
|
||||
segment_id += 1
|
||||
current = [chunk]
|
||||
else:
|
||||
current.append(chunk)
|
||||
yield segment_id, current
|
||||
|
||||
@@ -0,0 +1,35 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture
|
||||
from pipeline_common_corrected import parse_imu_capture, parse_rtk_capture, write_json, write_jsonl
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--rtk", type=Path, required=True)
|
||||
parser.add_argument("--imu", type=Path, required=True)
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
args.out.mkdir(parents=True, exist_ok=True)
|
||||
rtk_capture = read_capture(args.rtk)
|
||||
imu_capture = read_capture(args.imu)
|
||||
rtk_rows = parse_rtk_capture(rtk_capture)
|
||||
imu_rows = parse_imu_capture(imu_capture)
|
||||
write_jsonl(args.out / "rtk.jsonl", rtk_rows)
|
||||
write_jsonl(args.out / "imu.jsonl", imu_rows)
|
||||
write_json(args.out / "parse_summary.json", {
|
||||
"rtk_capture": file_summary(rtk_capture),
|
||||
"imu_capture": file_summary(imu_capture),
|
||||
"rtk_records": len(rtk_rows),
|
||||
"rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows),
|
||||
"imu_frames": len(imu_rows),
|
||||
"imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows),
|
||||
})
|
||||
print(f"RTK records={len(rtk_rows)}, IMU frames={len(imu_rows)}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,300 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import binascii
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments, read_capture
|
||||
|
||||
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
|
||||
|
||||
def ticks_to_unix_ns(ticks: int) -> int:
|
||||
return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100
|
||||
|
||||
|
||||
def safe_float(value: str, default=None):
|
||||
try:
|
||||
return float(value)
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def safe_int(value: str, default=None):
|
||||
try:
|
||||
return int(value)
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def nmea_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1:star + 3], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
value = 0
|
||||
for char in line[1:star]:
|
||||
value ^= ord(char)
|
||||
return value == expected
|
||||
|
||||
|
||||
def unicore_crc32(text: str) -> int:
|
||||
crc = 0
|
||||
for value in text.encode("ascii", "replace"):
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = (crc >> 1) ^ (0xEDB88320 if crc & 1 else 0)
|
||||
return crc & 0xFFFFFFFF
|
||||
|
||||
|
||||
def unicore_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0 or len(line) < star + 9:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1:star + 9], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
return unicore_crc32(line[1:star]) == expected
|
||||
|
||||
|
||||
def parse_checksum(line: str) -> bool:
|
||||
if line.startswith("$"):
|
||||
return nmea_checksum_valid(line)
|
||||
if line.startswith("#"):
|
||||
return unicore_checksum_valid(line)
|
||||
return False
|
||||
|
||||
|
||||
def parse_nmea_latlon(value: str, hemisphere: str):
|
||||
raw = safe_float(value)
|
||||
if raw is None:
|
||||
return None
|
||||
degrees = math.floor(raw / 100.0)
|
||||
result = degrees + (raw - degrees * 100.0) / 60.0
|
||||
if hemisphere.upper() in ("S", "W"):
|
||||
result = -result
|
||||
return result
|
||||
|
||||
|
||||
def parse_gga(line: str) -> dict:
|
||||
fields = line[:line.rfind("*")].split(",")
|
||||
if len(fields) < 10:
|
||||
raise ValueError("GGA has too few fields")
|
||||
return {
|
||||
"type": "GGA",
|
||||
"position_time_utc": fields[1],
|
||||
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
|
||||
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
|
||||
"fix_quality": safe_int(fields[6], -1),
|
||||
"satellites": safe_int(fields[7], -1),
|
||||
"hdop": safe_float(fields[8]),
|
||||
"altitude_m": safe_float(fields[9]),
|
||||
"geoid_separation_m": safe_float(fields[11]) if len(fields) > 11 else None,
|
||||
"differential_age_s": safe_float(fields[13]) if len(fields) > 13 else None,
|
||||
"station_id": fields[14].strip('"') if len(fields) > 14 else "",
|
||||
}
|
||||
|
||||
|
||||
def parse_heading(line: str) -> dict:
|
||||
before_crc = line[:line.rfind("*")]
|
||||
header, payload = before_crc.split(";", 1)
|
||||
header_fields = header.split(",")
|
||||
fields = payload.split(",")
|
||||
if len(fields) < 7:
|
||||
raise ValueError("UNIHEADINGA has too few fields")
|
||||
raw_heading = safe_float(fields[3])
|
||||
return {
|
||||
"type": "UNIHEADINGA",
|
||||
"gnss_week": safe_int(header_fields[4]) if len(header_fields) > 4 else None,
|
||||
"gnss_tow_ms": safe_int(header_fields[5]) if len(header_fields) > 5 else None,
|
||||
"heading_status": fields[0],
|
||||
"heading_solution": fields[1],
|
||||
"baseline_length_m": safe_float(fields[2]),
|
||||
"raw_heading_deg": raw_heading,
|
||||
"pitch_deg": safe_float(fields[4]),
|
||||
"heading_stddev_deg": safe_float(fields[6]),
|
||||
"pitch_stddev_deg": safe_float(fields[7]) if len(fields) > 7 else None,
|
||||
"station_id": fields[8].strip('"') if len(fields) > 8 else "",
|
||||
"satellites": safe_int(fields[9], -1) if len(fields) > 9 else -1,
|
||||
"solution_satellites": safe_int(fields[10], -1) if len(fields) > 10 else -1,
|
||||
"observations": safe_int(fields[11], -1) if len(fields) > 11 else -1,
|
||||
"multi_count": safe_int(fields[12], -1) if len(fields) > 12 else -1,
|
||||
"heading_valid": fields[0] == "SOL_COMPUTED" and fields[1] in {"NARROW_INT", "NARROW_FLOAT"},
|
||||
}
|
||||
|
||||
|
||||
def chunk_source(chunks: list[RawChunk], offset: int, end: int) -> dict:
|
||||
first = chunks[0]
|
||||
last = chunks[-1]
|
||||
cursor = 0
|
||||
start_chunk = first
|
||||
end_chunk = last
|
||||
for chunk in chunks:
|
||||
chunk_start = cursor
|
||||
chunk_end = cursor + len(chunk.raw)
|
||||
if chunk_start <= offset < chunk_end:
|
||||
start_chunk = chunk
|
||||
if chunk_start < end <= chunk_end:
|
||||
end_chunk = chunk
|
||||
break
|
||||
cursor = chunk_end
|
||||
return {
|
||||
"source_segment_id": None,
|
||||
"source_chunk_sequence_first": start_chunk.sequence,
|
||||
"source_chunk_sequence_last": end_chunk.sequence,
|
||||
"source_raw_file_offset": start_chunk.raw_file_offset + max(0, offset - sum(len(c.raw) for c in chunks if c.sequence < start_chunk.sequence)),
|
||||
"source_raw_byte_length": max(0, end - offset),
|
||||
}
|
||||
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
valid = parse_checksum(line)
|
||||
row = {
|
||||
"type": "UNKNOWN",
|
||||
"raw_line": line,
|
||||
"checksum_valid": valid,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(chunks[-1].receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": chunks[-1].receive_monotonic_ticks,
|
||||
"source_segment_id": segment_id,
|
||||
"source_byte_offset_in_segment": cursor - len(raw_line) - 1,
|
||||
"source_byte_length": len(raw_line) + 1,
|
||||
}
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def crc16_hi13(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def decode_hi91(frame: bytes) -> dict:
|
||||
f32 = lambda i: struct.unpack_from("<f", frame, i)[0]
|
||||
return {
|
||||
"tag": 0x91,
|
||||
"pps_sync_stamp_ms": int.from_bytes(frame[7:9], "little"),
|
||||
"temperature_c": struct.unpack_from("<b", frame, 9)[0],
|
||||
"air_pressure_pa": f32(10),
|
||||
"device_timestamp_ms": int.from_bytes(frame[14:18], "little"),
|
||||
"accel_x_mps2": f32(18) * 9.80665,
|
||||
"accel_y_mps2": f32(22) * 9.80665,
|
||||
"accel_z_mps2": f32(26) * 9.80665,
|
||||
"gyro_x_radps": f32(30) * math.pi / 180.0,
|
||||
"gyro_y_radps": f32(34) * math.pi / 180.0,
|
||||
"gyro_z_radps": f32(38) * math.pi / 180.0,
|
||||
"mag_x_ut": f32(42), "mag_y_ut": f32(46), "mag_z_ut": f32(50),
|
||||
"roll_deg": f32(54), "pitch_deg": f32(58), "yaw_deg": f32(62),
|
||||
"quaternion_w": f32(66), "quaternion_x": f32(70),
|
||||
"quaternion_y": f32(74), "quaternion_z": f32(78),
|
||||
}
|
||||
|
||||
|
||||
def decode_hi92(frame: bytes) -> dict:
|
||||
i16 = lambda i: struct.unpack_from("<h", frame, i)[0]
|
||||
i32 = lambda i: struct.unpack_from("<i", frame, i)[0]
|
||||
return {
|
||||
"tag": 0x92,
|
||||
"status": int.from_bytes(frame[7:9], "little"),
|
||||
"temperature_c": struct.unpack_from("<b", frame, 9)[0],
|
||||
"pps_sync_stamp_ms": int.from_bytes(frame[10:12], "little"),
|
||||
"air_pressure_pa": i16(12) + 100000.0,
|
||||
"heave_m": i16(14) * 0.001,
|
||||
"gyro_x_radps": i16(16) * 0.001, "gyro_y_radps": i16(18) * 0.001, "gyro_z_radps": i16(20) * 0.001,
|
||||
"accel_x_mps2": i16(22) * 0.0048828, "accel_y_mps2": i16(24) * 0.0048828, "accel_z_mps2": i16(26) * 0.0048828,
|
||||
"mag_x_ut": i16(28) * 0.030517, "mag_y_ut": i16(30) * 0.030517, "mag_z_ut": i16(32) * 0.030517,
|
||||
"roll_deg": i32(34) * 0.001, "pitch_deg": i32(38) * 0.001, "yaw_deg": i32(42) * 0.001,
|
||||
"quaternion_w": i16(46) * 0.0001, "quaternion_x": i16(48) * 0.0001,
|
||||
"quaternion_y": i16(50) * 0.0001, "quaternion_z": i16(52) * 0.0001,
|
||||
}
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while True:
|
||||
start = stream.find(b"\x5a\xa5", cursor)
|
||||
if start < 0 or start + 6 > len(stream):
|
||||
break
|
||||
payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
|
||||
frame_length = 6 + payload_length
|
||||
if payload_length <= 0 or payload_length > 512:
|
||||
cursor = start + 1
|
||||
continue
|
||||
if start + frame_length > len(stream):
|
||||
break
|
||||
frame = stream[start:start + frame_length]
|
||||
expected = int.from_bytes(frame[4:6], "little")
|
||||
actual = crc16_hi13(frame[:4] + frame[6:])
|
||||
end = start + frame_length
|
||||
source = chunk_source(chunks, start, end)
|
||||
source["source_segment_id"] = segment_id
|
||||
row = {
|
||||
"type": "HI13",
|
||||
"tag": frame[6],
|
||||
"frame_length": frame_length,
|
||||
"crc_valid": expected == actual,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(chunks[-1].receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": chunks[-1].receive_monotonic_ticks,
|
||||
"source_segment_id": segment_id,
|
||||
"source_byte_offset_in_segment": start,
|
||||
"source_byte_length": frame_length,
|
||||
"raw_frame_hex": frame.hex(),
|
||||
}
|
||||
if expected == actual:
|
||||
try:
|
||||
row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end
|
||||
return rows
|
||||
|
||||
|
||||
def write_jsonl(path: Path, rows: Iterable[dict]) -> None:
|
||||
with path.open("w", encoding="utf-8", newline="\n") as stream:
|
||||
for row in rows:
|
||||
stream.write(json.dumps(row, ensure_ascii=False, separators=(",", ":")) + "\n")
|
||||
|
||||
|
||||
def write_json(path: Path, value: dict) -> None:
|
||||
path.write_text(json.dumps(value, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
|
||||
def load_jsonl(path: Path) -> list[dict]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
return [json.loads(line) for line in stream if line.strip()]
|
||||
@@ -0,0 +1,106 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import bisect
|
||||
|
||||
from pipeline_common import *
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
|
||||
|
||||
_SPAN_CACHE: dict[int, tuple[list[RawChunk], list[int]]] = {}
|
||||
|
||||
|
||||
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
|
||||
key = id(chunks)
|
||||
cached = _SPAN_CACHE.get(key)
|
||||
if cached is not None and cached[0] is chunks:
|
||||
return cached[1]
|
||||
starts = []
|
||||
cursor = 0
|
||||
for chunk in chunks:
|
||||
starts.append(cursor)
|
||||
cursor += len(chunk.raw)
|
||||
_SPAN_CACHE[key] = (chunks, starts)
|
||||
return starts
|
||||
|
||||
|
||||
def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: int) -> dict:
|
||||
starts = _chunk_starts(chunks)
|
||||
start_index = max(0, min(len(chunks) - 1, bisect.bisect_right(starts, start) - 1))
|
||||
end_index = max(start_index, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
|
||||
start_chunk = chunks[start_index]
|
||||
end_chunk = chunks[end_index]
|
||||
return {
|
||||
"source_segment_id": segment_id,
|
||||
"source_chunk_sequence_first": start_chunk.sequence,
|
||||
"source_chunk_sequence_last": end_chunk.sequence,
|
||||
"source_raw_file_offset": start_chunk.raw_file_offset + (start - starts[start_index]),
|
||||
"source_raw_byte_length": end - start,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(end_chunk.receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": end_chunk.receive_monotonic_ticks,
|
||||
}
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
start = cursor
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while True:
|
||||
start = stream.find(b"\x5a\xa5", cursor)
|
||||
if start < 0 or start + 6 > len(stream):
|
||||
break
|
||||
payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
|
||||
frame_length = 6 + payload_length
|
||||
if payload_length <= 0 or payload_length > 512:
|
||||
cursor = start + 1
|
||||
continue
|
||||
if start + frame_length > len(stream):
|
||||
break
|
||||
frame = stream[start:start + frame_length]
|
||||
expected = int.from_bytes(frame[4:6], "little")
|
||||
actual = crc16_hi13(frame[:4] + frame[6:])
|
||||
end = start + frame_length
|
||||
row = {
|
||||
"type": "HI13",
|
||||
"tag": frame[6],
|
||||
"frame_length": frame_length,
|
||||
"crc_valid": expected == actual,
|
||||
"raw_frame_hex": frame.hex(),
|
||||
}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
if row["crc_valid"]:
|
||||
try:
|
||||
row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end
|
||||
return rows
|
||||
Reference in New Issue
Block a user