新增雷达到RTK直接手眼标定流程
This commit is contained in:
lichun.qu
@@ -0,0 +1,24 @@
|
||||
# Python
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.pytest_cache/
|
||||
.venv/
|
||||
venv/
|
||||
|
||||
# IDE / OS
|
||||
.idea/
|
||||
.vscode/
|
||||
.DS_Store
|
||||
Thumbs.db
|
||||
|
||||
# Raw data and generated outputs
|
||||
data/raw/
|
||||
work/
|
||||
outputs/
|
||||
*.rscap
|
||||
*.dorec
|
||||
*.log
|
||||
|
||||
# Large generated point clouds outside the archived reference result
|
||||
**/frames/
|
||||
**/frames_all/
|
||||
@@ -0,0 +1,185 @@
|
||||
# 双天线RTK—3D LiDAR直接手眼标定
|
||||
|
||||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达坐标转换到RTK导航坐标系。它**不是** `base_link` 车体外参，也不会在求解阶段使用车体航向偏置或RTK到后轮轴的XY杆臂。
|
||||
|
||||
## 1. 输出坐标约定
|
||||
|
||||
统一约定 `T_A_B` 把B系点变换到A系：
|
||||
|
||||
```text
|
||||
p_RTK = T_RTK_lidar · p_lidar
|
||||
```
|
||||
|
||||
RTK导航系在本仓库中定义为：
|
||||
|
||||
- 原点：GGA位置参考点（通常为ANT1相位中心，必须结合接收机配置确认）；
|
||||
- X轴：`rawHeading`所表示的双天线基线在水平面的投影；
|
||||
- Y轴：左；
|
||||
- Z轴：上；
|
||||
- ENU航向：`yaw = 90° - rawHeading`；
|
||||
- roll、pitch：当前轨迹中固定为0。
|
||||
|
||||
如果下游需要 `T_body_lidar`，必须另有经过确认的 `T_body_rtk`：
|
||||
|
||||
```text
|
||||
T_body_lidar = T_body_rtk · T_RTK_lidar
|
||||
```
|
||||
|
||||
## 2. 算法流程
|
||||
|
||||
```text
|
||||
逐站LiDAR dlog + RTK.rscap + IMU.rscap
|
||||
→ 分别解析并保留原始字段
|
||||
→ 以LiDAR帧时间为索引关联RTK/IMU，生成combined NPZ
|
||||
→ 每站选择一帧静态点云，GGA转局部ENU，rawHeading构造yaw-only RTK pose
|
||||
→ Open3D GICP和small_gicp分别求 B_ij = T_Li_Lj
|
||||
→ 留出点、Hessian、正反向、多初值和旋转共轭不变量筛选
|
||||
→ 两后端共同认可的边形成consensus B
|
||||
→ A_ij X = X B_ij + 地面法向/高度约束求 X = T_RTK_lidar
|
||||
→ bootstrap、双后端差异、逐对残差和3D可视化检查
|
||||
```
|
||||
|
||||
代码实际使用：
|
||||
|
||||
```text
|
||||
A_ij = inv(T_W_Ri) · T_W_Rj = T_Ri_Rj
|
||||
B_ij = T_Li_Lj # 将站点j点云变换到站点i
|
||||
A_ij · X = X · B_ij
|
||||
X = T_RTK_lidar
|
||||
```
|
||||
|
||||
## 3. 原始数据目录
|
||||
|
||||
大体积数据不提交Git。`DataRoot`下每个站点必须是一个独立dlog目录，至少包含：
|
||||
|
||||
```text
|
||||
raw_dataset/
|
||||
├── stations/
|
||||
│ ├── 001/
|
||||
│ │ ├── dobject/
|
||||
│ │ └── dobject_recording/
|
||||
│ ├── 002/
|
||||
│ └── ...
|
||||
└── captures/
|
||||
├── rtk.rscap
|
||||
└── imu.rscap
|
||||
```
|
||||
|
||||
每个站点应在车辆完全静止后记录点云；建议不少于30站，并包含充足的直行、左转、右转和大角度转向姿态变化。
|
||||
|
||||
## 4. 环境安装
|
||||
|
||||
已验证环境为Windows、PowerShell、Python 3.11。安装依赖：
|
||||
|
||||
```powershell
|
||||
python -m pip install -r requirements.txt
|
||||
```
|
||||
|
||||
依赖包括NumPy、SciPy、Open3D和small_gicp。若small_gicp没有对应Windows wheel，可在WSL2中安装后运行Python核心命令，或先只运行Open3D后端；完整共识流程需要两个后端都可用。
|
||||
|
||||
## 5. 从原始数据一键复现
|
||||
|
||||
在仓库根目录执行，路径由使用者通过参数传入，脚本内没有本机绝对路径：
|
||||
|
||||
```powershell
|
||||
$Repo = (Resolve-Path ".").Path
|
||||
$Raw = "E:\calibration_data\data4"
|
||||
$Out = "E:\calibration_output\rtk_lidar"
|
||||
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" `
|
||||
-DataRoot "$Raw\stations" `
|
||||
-RtkCapture "$Raw\captures\rtk.rscap" `
|
||||
-ImuCapture "$Raw\captures\imu.rscap" `
|
||||
-OutputRoot $Out `
|
||||
-RtkReferenceHeightAboveGroundM 0.8535 `
|
||||
-ExpectedStations 34
|
||||
```
|
||||
|
||||
主要输出：
|
||||
|
||||
```text
|
||||
$Out/
|
||||
├── exported/
|
||||
│ ├── export/ # 各站LiDAR逐帧NPZ
|
||||
│ ├── parsed/ # RTK/IMU JSONL
|
||||
│ └── combined/ # 按LiDAR帧关联后的多传感器NPZ
|
||||
├── prepared_rtk_direct/
|
||||
│ ├── frames_all/ # 每站选中的静态帧
|
||||
│ └── reference_poses_rtk_gga_raw_heading.csv
|
||||
└── calibration/
|
||||
├── open3d_gicp/
|
||||
├── small_gicp/
|
||||
├── consensus/
|
||||
├── summary.json
|
||||
└── final_T_RTK_lidar.json
|
||||
```
|
||||
|
||||
若已经有`combined/`，可跳过原始导出：
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
|
||||
-CombinedRoot "E:\calibration_output\exported\combined" `
|
||||
-WorkRoot "E:\calibration_output\prepared_rtk_direct" `
|
||||
-OutputRoot "E:\calibration_output\calibration" `
|
||||
-RtkReferenceHeightAboveGroundM 0.8535 `
|
||||
-ExpectedStations 34
|
||||
```
|
||||
|
||||
## 6. 3D可视化
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
|
||||
-Frames "$Out\prepared_rtk_direct\frames_all" `
|
||||
-Pairs "$Out\calibration\consensus\B_consensus.npz" `
|
||||
-Extrinsic "$Out\calibration\final_T_RTK_lidar.json" `
|
||||
-PairIndex 0
|
||||
```
|
||||
|
||||
窗口中：
|
||||
|
||||
- 蓝色：目标站点i；橙色：站点j；
|
||||
- `1`：原始点云；
|
||||
- `2`：RTK运动A直接作为初值；
|
||||
- `3`：GICP测得的B；
|
||||
- `4`：最终外参预测的 `X^-1 A X`；
|
||||
- `Q/Esc`：退出。
|
||||
|
||||
模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应查看多对，不能只挑视觉效果最好的一对。
|
||||
|
||||
## 7. data4参考结果
|
||||
|
||||
仓库保留了精简参考产物，见[`results/reference_data4`](results/reference_data4/README.md)：
|
||||
|
||||
```text
|
||||
translation_m = [1.638179350, -0.240844799, 0.084481236]
|
||||
RPY_deg_xyz = [-0.817167459, 1.323288119, -22.104163318]
|
||||
|
||||
站点：34
|
||||
共识运动对：25
|
||||
AX Translation RMS：0.100207 m
|
||||
AX Rotation RMS：1.252794°
|
||||
Weighted Jacobian condition：7.739413
|
||||
Open3D vs small_gicp：0.003889 m / 0.188431°
|
||||
```
|
||||
|
||||
唯一建议下游读取的参考结果是[`final_T_RTK_lidar.json`](results/reference_data4/final_T_RTK_lidar.json)。
|
||||
|
||||
## 8. z与精度限制
|
||||
|
||||
平面阿克曼运动不能独立观测z。参考结果使用34站地面平面和RTK参考点离地`0.8535 m`约束z；该高度必须量到实际GGA参考点/天线相位中心。更改参考高度后必须重新求解。
|
||||
|
||||
AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。
|
||||
|
||||
此外，代码无法单独证明GGA对应哪根物理天线、`rawHeading`是ANT1→ANT2还是ANT2→ANT1；必须用接收机配置、接线和现场运动实验确认。方向错误会导致RTK坐标系yaw相差约180°。
|
||||
|
||||
## 9. 仓库目录
|
||||
|
||||
| 目录 | 职责 |
|
||||
|---|---|
|
||||
| [`code/`](code/) | GICP、运动对质量评价、AX=XB求解、结果封装和3D可视化 |
|
||||
| [`tools/`](tools/) | 原始dlog/rscap解析、按LiDAR帧关联及静态站点prepared生成 |
|
||||
| [`run/`](run/) | PowerShell入口；所有数据和输出路径都通过参数传入 |
|
||||
| [`results/reference_data4/`](results/reference_data4/) | 可提交Git的精简参考结果，不包含点云和本机过程目录 |
|
||||
| `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 |
|
||||
|
||||
各代码文件职责见[`code/README.md`](code/README.md)，命令索引见[`run/README.md`](run/README.md)，工具说明见[`tools/README.md`](tools/README.md)。
|
||||
@@ -0,0 +1,12 @@
|
||||
# code目录
|
||||
|
||||
| 文件 | 职责 |
|
||||
|---|---|
|
||||
| `rigorous_calibration.py` | 核心CLI：读取静态点云/RTK位姿，Open3D或small_gicp求B，拟合地面，求解/验证AX=XB |
|
||||
| `refine_pairs.py` | 不使用最终X，按留出点重叠率、RMSE、旋转共轭不变量和正反向一致性精筛运动对 |
|
||||
| `cross_backend_filter.py` | 保留Open3D与small_gicp共同认可且变换接近的边；共识B数值取Open3D结果 |
|
||||
| `finalize_direct_rtk_lidar.py` | 将三路求解结果封装为明确方向的`T_RTK_lidar`，选择consensus为最终结果 |
|
||||
| `visualize_pair_3d.py` | 交互显示原始、RTK初值、GICP B和`X^-1AX`，并打印增量 |
|
||||
| `compare_extrinsics.py` | 计算两套外参的SE(3)平移/旋转差异 |
|
||||
|
||||
核心约定：`A=T_Ri_Rj`、`B=T_Li_Lj`、`X=T_RTK_lidar`，满足`A X = X B`。点云配准以i为target、j为source，B将j帧点云变换到i帧。
|
||||
@@ -0,0 +1,43 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Compare two homogeneous-extrinsic JSON files in parameter space and on SE(3)."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--reference", type=Path, required=True)
|
||||
parser.add_argument("--candidate", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
reference = json.loads(args.reference.read_text(encoding="utf-8-sig"))
|
||||
candidate = json.loads(args.candidate.read_text(encoding="utf-8-sig"))
|
||||
a = np.asarray(reference["matrix_4x4"], dtype=float)
|
||||
b = np.asarray(candidate["matrix_4x4"], dtype=float)
|
||||
delta = np.linalg.inv(a) @ b
|
||||
result = {
|
||||
"convention": "delta = inverse(reference) @ candidate",
|
||||
"reference": str(args.reference.resolve()),
|
||||
"candidate": str(args.candidate.resolve()),
|
||||
"candidate_minus_reference_translation_xyz_m": (b[:3, 3] - a[:3, 3]).tolist(),
|
||||
"candidate_minus_reference_rpy_xyz_deg": (
|
||||
np.asarray(candidate["rotation_rpy_deg_xyz"], float)
|
||||
- np.asarray(reference["rotation_rpy_deg_xyz"], float)
|
||||
).tolist(),
|
||||
"relative_translation_norm_m": float(np.linalg.norm(delta[:3, 3])),
|
||||
"relative_rotation_deg": float(np.degrees(Rotation.from_matrix(delta[:3, :3]).magnitude())),
|
||||
"relative_matrix_4x4": delta.tolist(),
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(result, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,81 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Keep common A/B edges on which Open3D and small_gicp agree, without using X."""
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def key(meta):
|
||||
return int(meta[0]), int(meta[1])
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--open3d-pairs", required=True)
|
||||
parser.add_argument("--small-pairs", required=True)
|
||||
parser.add_argument("--output", required=True)
|
||||
parser.add_argument("--audit")
|
||||
parser.add_argument("--max-translation", type=float, default=0.05)
|
||||
parser.add_argument("--max-rotation", type=float, default=0.50)
|
||||
parser.add_argument("--min-pairs", type=int, default=25)
|
||||
args = parser.parse_args()
|
||||
|
||||
with np.load(args.open3d_pairs, allow_pickle=False) as source:
|
||||
open_a = np.asarray(source["A"], float)
|
||||
open_b = np.asarray(source["B"], float)
|
||||
open_meta = np.asarray(source["meta"], float)
|
||||
station_times = np.asarray(source["station_times"])
|
||||
rtk_dt = np.asarray(source["rtk_nearest_dt_s"])
|
||||
with np.load(args.small_pairs, allow_pickle=False) as source:
|
||||
small = {key(meta): np.asarray(b, float)
|
||||
for meta, b in zip(source["meta"], source["B"])}
|
||||
|
||||
keep, audit = [], []
|
||||
for meta, b_open in zip(open_meta, open_b):
|
||||
edge = key(meta)
|
||||
if edge not in small:
|
||||
audit.append({"i": edge[0], "j": edge[1], "accepted": False,
|
||||
"reason": "not_in_small_gicp_refined"})
|
||||
keep.append(False)
|
||||
continue
|
||||
delta = np.linalg.inv(b_open) @ small[edge]
|
||||
translation = float(np.linalg.norm(delta[:3, 3]))
|
||||
rotation = float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude()))
|
||||
accepted = translation <= args.max_translation and rotation <= args.max_rotation
|
||||
keep.append(accepted)
|
||||
audit.append({
|
||||
"i": edge[0], "j": edge[1],
|
||||
"open3d_small_translation_m": translation,
|
||||
"open3d_small_rotation_deg": rotation,
|
||||
"accepted": accepted,
|
||||
"reason": "" if accepted else "backend_disagreement",
|
||||
})
|
||||
keep = np.asarray(keep, bool)
|
||||
output = Path(args.output)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(
|
||||
output, A=open_a[keep], B=open_b[keep], meta=open_meta[keep],
|
||||
station_times=station_times, rtk_nearest_dt_s=rtk_dt,
|
||||
backend=np.asarray("open3d_gicp_cross_backend_consensus"),
|
||||
)
|
||||
audit_path = Path(args.audit or output.with_suffix(".consensus.json"))
|
||||
audit_path.write_text(json.dumps({
|
||||
"selection_is_X_independent": True,
|
||||
"B_source": "Open3D; small_gicp is used only as an agreement gate",
|
||||
"max_translation_m": args.max_translation,
|
||||
"max_rotation_deg": args.max_rotation,
|
||||
"input_open3d_pairs": len(open_b),
|
||||
"accepted_pairs": int(np.count_nonzero(keep)),
|
||||
"pairs": audit,
|
||||
}, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
if np.count_nonzero(keep) < args.min_pairs:
|
||||
raise RuntimeError(f"only {np.count_nonzero(keep)} consensus pairs")
|
||||
print(json.dumps({"accepted_pairs": int(np.count_nonzero(keep)),
|
||||
"output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,128 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def load(path: Path) -> dict:
|
||||
return json.loads(path.read_text(encoding="utf-8-sig"))
|
||||
|
||||
|
||||
def write(path: Path, document: dict) -> None:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
|
||||
def inverse(t: np.ndarray) -> np.ndarray:
|
||||
result = np.eye(4)
|
||||
result[:3, :3] = t[:3, :3].T
|
||||
result[:3, 3] = -result[:3, :3] @ t[:3, 3]
|
||||
return result
|
||||
|
||||
|
||||
def delta(a: np.ndarray, b: np.ndarray) -> dict:
|
||||
d = inverse(a) @ b
|
||||
return {
|
||||
"translation_m": float(np.linalg.norm(d[:3, 3])),
|
||||
"rotation_deg": float(np.linalg.norm(Rotation.from_matrix(d[:3, :3]).as_rotvec()) * 180.0 / math.pi),
|
||||
"delta_matrix_4x4": d.tolist(),
|
||||
}
|
||||
|
||||
|
||||
def corrected(raw: dict, backend: str, reference_height: float) -> dict:
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"success": bool(raw["success"]),
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
||||
"frames": {
|
||||
"RTK": {
|
||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||
"x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver",
|
||||
"y_axis": "left",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": "90 - rawHeadingDeg",
|
||||
},
|
||||
"LiDAR": "raw LiDAR sensor frame",
|
||||
},
|
||||
"backend": backend,
|
||||
"measured_lidar_extrinsic_used_as_initial": False,
|
||||
"body_heading_offset_used": False,
|
||||
"body_antenna_lever_xy_used": False,
|
||||
"translation_m": raw["translation_m"],
|
||||
"rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
|
||||
"quaternion_xyzw": raw["quaternion_xyzw"],
|
||||
"matrix_4x4": raw["matrix_4x4"],
|
||||
"quality": {
|
||||
"stations": raw["estimation"]["stations"],
|
||||
"pairs": raw["estimation"]["pairs"],
|
||||
"residuals": raw["estimation"]["residuals"],
|
||||
"weighted_jacobian_condition_number": raw["weighted_jacobian_condition_number"],
|
||||
"linearized_one_sigma": raw["linearized_one_sigma"],
|
||||
"bootstrap": raw["bootstrap"],
|
||||
},
|
||||
"z_constraint": {
|
||||
"observable_from_planar_AX_XB": False,
|
||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
||||
"rtk_reference_height_above_ground_m": reference_height,
|
||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion",
|
||||
},
|
||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
|
||||
}
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--result-root", type=Path, required=True)
|
||||
parser.add_argument("--reference-height", type=float, required=True)
|
||||
args = parser.parse_args()
|
||||
|
||||
def solver_output(directory: str) -> Path:
|
||||
raw = args.result_root / directory / "extrinsic_raw.json"
|
||||
standard = args.result_root / directory / "extrinsic.json"
|
||||
return raw if raw.exists() else standard
|
||||
|
||||
paths = {
|
||||
"open3d_gicp": solver_output("open3d_gicp"),
|
||||
"small_gicp": solver_output("small_gicp"),
|
||||
"consensus": solver_output("consensus"),
|
||||
}
|
||||
docs = {}
|
||||
for backend, path in paths.items():
|
||||
document = corrected(load(path), backend, args.reference_height)
|
||||
write(path.with_name("extrinsic_rtk_lidar.json"), document)
|
||||
docs[backend] = document
|
||||
|
||||
open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
|
||||
small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
|
||||
final = dict(docs["consensus"])
|
||||
final["selection"] = {
|
||||
"recommended": True,
|
||||
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
||||
"open3d_vs_small_gicp": delta(open_t, small_t),
|
||||
}
|
||||
|
||||
|
||||
write(args.result_root / "final_T_RTK_lidar.json", final)
|
||||
summary = {
|
||||
"final": {
|
||||
"translation_m": final["translation_m"],
|
||||
"rotation_rpy_deg_xyz": final["rotation_rpy_deg_xyz"],
|
||||
"pairs": final["quality"]["pairs"],
|
||||
"translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"],
|
||||
"rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"],
|
||||
"condition_number": final["quality"]["weighted_jacobian_condition_number"],
|
||||
},
|
||||
"backend_difference": delta(open_t, small_t),
|
||||
}
|
||||
write(args.result_root / "summary.json", summary)
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,87 @@
|
||||
#!/usr/bin/env python3
|
||||
"""X-independent second-stage filter for stationary A/B pairs."""
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from rigorous_calibration import read_pairs, rotation_angle_deg
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--pairs", required=True)
|
||||
parser.add_argument("--quality-json", required=True)
|
||||
parser.add_argument("--output", required=True)
|
||||
parser.add_argument("--audit")
|
||||
parser.add_argument("--min-pairs", type=int, default=25)
|
||||
parser.add_argument("--min-inlier-ratio", type=float, default=0.70)
|
||||
parser.add_argument("--max-inlier-rmse", type=float, default=0.13)
|
||||
parser.add_argument("--max-rotation-invariant-error", type=float, default=0.75)
|
||||
parser.add_argument("--reverse-translation-tolerance", type=float, default=0.05)
|
||||
parser.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
|
||||
args = parser.parse_args()
|
||||
|
||||
a_array, b_array, meta, _ = read_pairs(args.pairs)
|
||||
quality = json.loads(Path(args.quality_json).read_text(encoding="utf-8-sig"))
|
||||
reports = {(int(item["i"]), int(item["j"])): item for item in quality["pairs"]}
|
||||
keep, audit = [], []
|
||||
for a_ij, b_ij, item_meta in zip(a_array, b_array, meta):
|
||||
key = (int(item_meta[0]), int(item_meta[1]))
|
||||
report = reports[key]
|
||||
heldout = report["heldout_symmetric"]
|
||||
reverse = report["forward_reverse"]
|
||||
invariant = abs(rotation_angle_deg(a_ij[:3, :3]) - rotation_angle_deg(b_ij[:3, :3]))
|
||||
reasons = []
|
||||
if heldout["inlier_ratio"] < args.min_inlier_ratio:
|
||||
reasons.append("overlap_ratio")
|
||||
if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
|
||||
reasons.append("heldout_rmse")
|
||||
if invariant > args.max_rotation_invariant_error:
|
||||
reasons.append("rotation_conjugacy_invariant")
|
||||
if reverse["translation_m"] > args.reverse_translation_tolerance:
|
||||
reasons.append("forward_reverse_translation")
|
||||
if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
|
||||
reasons.append("forward_reverse_rotation")
|
||||
accepted = not reasons
|
||||
keep.append(accepted)
|
||||
audit.append({
|
||||
"i": key[0], "j": key[1], "heldout_inlier_ratio": heldout["inlier_ratio"],
|
||||
"heldout_inlier_rmse_m": heldout["inlier_rmse_m"],
|
||||
"rotation_invariant_error_deg": invariant,
|
||||
"reverse_translation_m": reverse["translation_m"],
|
||||
"reverse_rotation_deg": reverse["rotation_deg"],
|
||||
"accepted": accepted, "rejection_reasons": reasons,
|
||||
})
|
||||
keep = np.asarray(keep, bool)
|
||||
output = Path(args.output)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
with np.load(args.pairs, allow_pickle=False) as source:
|
||||
np.savez_compressed(
|
||||
output, A=a_array[keep], B=b_array[keep], meta=meta[keep],
|
||||
station_times=np.asarray(source["station_times"]),
|
||||
rtk_nearest_dt_s=np.asarray(source["rtk_nearest_dt_s"]),
|
||||
backend=np.asarray(source["backend"]),
|
||||
)
|
||||
audit_path = Path(args.audit or output.with_suffix(".refinement.json"))
|
||||
audit_path.write_text(json.dumps({
|
||||
"selection_is_X_independent": True,
|
||||
"criteria": {
|
||||
"min_inlier_ratio": args.min_inlier_ratio,
|
||||
"max_inlier_rmse_m": args.max_inlier_rmse,
|
||||
"max_rotation_invariant_error_deg": args.max_rotation_invariant_error,
|
||||
"reverse_translation_tolerance_m": args.reverse_translation_tolerance,
|
||||
"reverse_rotation_tolerance_deg": args.reverse_rotation_tolerance,
|
||||
},
|
||||
"input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
|
||||
"pairs": audit,
|
||||
}, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
if np.count_nonzero(keep) < args.min_pairs:
|
||||
raise RuntimeError(f"only {np.count_nonzero(keep)} refined pairs; need {args.min_pairs}")
|
||||
print(json.dumps({"input_pairs": len(keep), "accepted_pairs": int(np.count_nonzero(keep)),
|
||||
"output": str(output.resolve()), "audit": str(audit_path.resolve())}, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,771 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Rigorous stationary LiDAR / reference-trajectory hand-eye calibration.
|
||||
|
||||
Convention: T_A_B maps points from frame B into frame A.
|
||||
For this repository the reference frame is the RTK navigation frame.
|
||||
X = T_RTK_lidar, A_ij = T_W_Ri^-1 T_W_Rj, B_ij = T_Li_Lj,
|
||||
therefore A_ij X = X B_ij. Raw sensor-frame points_raw are used.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
|
||||
def skew(v):
|
||||
x, y, z = v
|
||||
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
|
||||
|
||||
|
||||
def so3_exp(v):
|
||||
angle = float(np.linalg.norm(v))
|
||||
if angle < 1e-12:
|
||||
return np.eye(3) + skew(v)
|
||||
k = skew(np.asarray(v, float) / angle)
|
||||
return np.eye(3) + math.sin(angle) * k + (1.0 - math.cos(angle)) * k @ k
|
||||
|
||||
|
||||
def so3_log(rotation):
|
||||
cosine = float(np.clip((np.trace(rotation) - 1.0) / 2.0, -1.0, 1.0))
|
||||
angle = math.acos(cosine)
|
||||
vee = np.array([
|
||||
rotation[2, 1] - rotation[1, 2],
|
||||
rotation[0, 2] - rotation[2, 0],
|
||||
rotation[1, 0] - rotation[0, 1],
|
||||
])
|
||||
if angle < 1e-9:
|
||||
return vee / 2.0
|
||||
if abs(math.pi - angle) < 1e-5:
|
||||
values, vectors = np.linalg.eigh((rotation + np.eye(3)) / 2.0)
|
||||
return vectors[:, int(np.argmax(values))] * angle
|
||||
return vee * angle / (2.0 * math.sin(angle))
|
||||
|
||||
|
||||
def quat_to_rotation(q):
|
||||
x, y, z, w = np.asarray(q, float) / np.linalg.norm(q)
|
||||
return np.array([
|
||||
[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
|
||||
[2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
|
||||
[2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)],
|
||||
])
|
||||
|
||||
|
||||
def rotation_to_quat(rotation):
|
||||
from scipy.spatial.transform import Rotation
|
||||
return Rotation.from_matrix(rotation).as_quat()
|
||||
|
||||
|
||||
def rpy_deg(rotation):
|
||||
from scipy.spatial.transform import Rotation
|
||||
return Rotation.from_matrix(rotation).as_euler("xyz", degrees=True).tolist()
|
||||
|
||||
|
||||
def make_transform(translation, rotation):
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = rotation
|
||||
transform[:3, 3] = translation
|
||||
return transform
|
||||
|
||||
|
||||
def params_transform(params):
|
||||
return make_transform(params[:3], so3_exp(params[3:]))
|
||||
|
||||
|
||||
def inverse_transform(transform):
|
||||
answer = np.eye(4)
|
||||
answer[:3, :3] = transform[:3, :3].T
|
||||
answer[:3, 3] = -answer[:3, :3] @ transform[:3, 3]
|
||||
return answer
|
||||
|
||||
|
||||
def transform_points(points, transform):
|
||||
return points @ transform[:3, :3].T + transform[:3, 3]
|
||||
|
||||
|
||||
def rotation_angle_deg(rotation):
|
||||
return math.degrees(np.linalg.norm(so3_log(rotation)))
|
||||
|
||||
|
||||
@dataclass
|
||||
class PoseSeries:
|
||||
time: np.ndarray
|
||||
transforms: np.ndarray
|
||||
|
||||
|
||||
def read_poses(path):
|
||||
timestamps, transforms = [], []
|
||||
with Path(path).open(encoding="utf-8-sig", newline="") as stream:
|
||||
reader = csv.DictReader(stream)
|
||||
required = ("time", "x", "y", "z", "qx", "qy", "qz", "qw")
|
||||
missing = [key for key in required if key not in (reader.fieldnames or [])]
|
||||
if missing:
|
||||
raise ValueError(f"{path}: missing pose fields {missing}")
|
||||
for row in reader:
|
||||
timestamps.append(float(row["time"]))
|
||||
translation = np.array([float(row[k]) for k in ("x", "y", "z")])
|
||||
quaternion = np.array([float(row[k]) for k in ("qx", "qy", "qz", "qw")])
|
||||
transforms.append(make_transform(translation, quat_to_rotation(quaternion)))
|
||||
order = np.argsort(timestamps)
|
||||
return PoseSeries(np.asarray(timestamps)[order], np.asarray(transforms)[order])
|
||||
|
||||
|
||||
def nearest_pose(series, timestamp):
|
||||
index = int(np.argmin(np.abs(series.time - timestamp)))
|
||||
return series.transforms[index], float(abs(series.time[index] - timestamp))
|
||||
|
||||
|
||||
def npz_files(root):
|
||||
files = sorted(Path(root).rglob("*.npz"))
|
||||
if not files:
|
||||
raise FileNotFoundError(f"no NPZ files under {root}")
|
||||
return files
|
||||
|
||||
|
||||
def load_npz_xyz(path, min_range=1.0, max_range=50.0):
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
if "points_raw" not in data:
|
||||
raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden")
|
||||
raw = np.asarray(data["points_raw"], dtype=np.float64)
|
||||
timestamp = float(np.ravel(data["unix_time_ns"])[0]) / 1e9
|
||||
counter = int(np.ravel(data["frame_counter"])[0])
|
||||
distance = raw[:, 0] * 0.001
|
||||
azimuth = np.deg2rad(raw[:, 1])
|
||||
altitude = np.deg2rad(raw[:, 2])
|
||||
valid = (
|
||||
np.isfinite(distance + azimuth + altitude)
|
||||
& (distance >= min_range)
|
||||
& (distance <= max_range)
|
||||
)
|
||||
distance, azimuth, altitude = distance[valid], azimuth[valid], altitude[valid]
|
||||
xyz = np.column_stack((
|
||||
distance * np.cos(altitude) * np.cos(azimuth),
|
||||
distance * np.cos(altitude) * np.sin(azimuth),
|
||||
distance * np.sin(altitude),
|
||||
))
|
||||
return timestamp, counter, xyz
|
||||
|
||||
|
||||
def load_stations(root, min_range, max_range, z_min=None, z_max=None):
|
||||
stations = []
|
||||
for path in npz_files(root):
|
||||
timestamp, counter, xyz = load_npz_xyz(path, min_range, max_range)
|
||||
if z_min is not None:
|
||||
xyz = xyz[(xyz[:, 2] >= z_min) & (xyz[:, 2] <= z_max)]
|
||||
stations.append((timestamp, counter, path, xyz))
|
||||
stations.sort(key=lambda item: item[0])
|
||||
return stations
|
||||
|
||||
|
||||
def split_holdout(points, fraction, phase):
|
||||
stride = max(int(round(1.0 / fraction)), 2)
|
||||
index = np.arange(len(points))
|
||||
holdout = ((index + phase) % stride) == 0
|
||||
return points[~holdout], points[holdout]
|
||||
|
||||
|
||||
def make_o3d_cloud(points, voxel):
|
||||
import open3d as o3d
|
||||
cloud = o3d.geometry.PointCloud()
|
||||
cloud.points = o3d.utility.Vector3dVector(np.asarray(points, float))
|
||||
return cloud.voxel_down_sample(voxel)
|
||||
|
||||
|
||||
def align_open3d(target, source, initial, voxels, correspondences, iterations):
|
||||
import open3d as o3d
|
||||
registration = o3d.pipelines.registration
|
||||
estimate = registration.TransformationEstimationForGeneralizedICP()
|
||||
criteria = registration.ICPConvergenceCriteria(max_iteration=iterations)
|
||||
transform, stages = np.asarray(initial, float), []
|
||||
final_target = final_source = final_answer = None
|
||||
started = time.perf_counter()
|
||||
for voxel, correspondence in zip(voxels, correspondences):
|
||||
target_cloud = make_o3d_cloud(target, voxel)
|
||||
source_cloud = make_o3d_cloud(source, voxel)
|
||||
answer = registration.registration_generalized_icp(
|
||||
source_cloud, target_cloud, correspondence, transform, estimate, criteria
|
||||
)
|
||||
transform = np.asarray(answer.transformation, float)
|
||||
stages.append({
|
||||
"voxel_m": voxel,
|
||||
"max_correspondence_m": correspondence,
|
||||
"fitness": float(answer.fitness),
|
||||
"inlier_rmse_m": float(answer.inlier_rmse),
|
||||
"target_points": len(target_cloud.points),
|
||||
"source_points": len(source_cloud.points),
|
||||
})
|
||||
final_target, final_source, final_answer = target_cloud, source_cloud, answer
|
||||
information = registration.get_information_matrix_from_point_clouds(
|
||||
final_source, final_target, correspondences[-1], transform
|
||||
)
|
||||
inliers = int(round(float(final_answer.fitness) * len(final_source.points)))
|
||||
return {
|
||||
"transform": transform,
|
||||
"hessian": np.asarray(information, float),
|
||||
"converged": None,
|
||||
"iterations": None,
|
||||
"num_inliers": inliers,
|
||||
"objective": float(final_answer.inlier_rmse ** 2 * max(inliers, 1)),
|
||||
"elapsed_sec": time.perf_counter() - started,
|
||||
"stages": stages,
|
||||
}
|
||||
|
||||
|
||||
def align_small_gicp(target, source, initial, voxels, correspondences, iterations, threads):
|
||||
import small_gicp
|
||||
transform, stages, result = np.asarray(initial, float), [], None
|
||||
started = time.perf_counter()
|
||||
for voxel, correspondence in zip(voxels, correspondences):
|
||||
result = small_gicp.align(
|
||||
np.ascontiguousarray(target),
|
||||
np.ascontiguousarray(source),
|
||||
transform,
|
||||
registration_type="GICP",
|
||||
downsampling_resolution=voxel,
|
||||
max_correspondence_distance=correspondence,
|
||||
num_threads=threads,
|
||||
max_iterations=iterations,
|
||||
rotation_epsilon=math.radians(0.005),
|
||||
translation_epsilon=0.0005,
|
||||
verbose=False,
|
||||
)
|
||||
transform = np.asarray(result.T_target_source, float)
|
||||
stages.append({
|
||||
"voxel_m": voxel,
|
||||
"max_correspondence_m": correspondence,
|
||||
"converged": bool(result.converged),
|
||||
"iterations": int(result.iterations),
|
||||
"num_inliers": int(result.num_inliers),
|
||||
"objective": float(result.error),
|
||||
})
|
||||
return {
|
||||
"transform": transform,
|
||||
"hessian": np.asarray(result.H, float),
|
||||
"converged": bool(result.converged),
|
||||
"iterations": int(result.iterations),
|
||||
"num_inliers": int(result.num_inliers),
|
||||
"objective": float(result.error),
|
||||
"elapsed_sec": time.perf_counter() - started,
|
||||
"stages": stages,
|
||||
}
|
||||
|
||||
|
||||
def align_backend(backend, target, source, initial, args):
|
||||
if backend == "open3d":
|
||||
return align_open3d(
|
||||
target, source, initial, args.voxels, args.correspondences, args.iterations
|
||||
)
|
||||
return align_small_gicp(
|
||||
target, source, initial, args.voxels, args.correspondences,
|
||||
args.iterations, args.threads
|
||||
)
|
||||
|
||||
|
||||
def symmetric_heldout_metrics(target_fit, target_holdout, source_fit, source_holdout,
|
||||
transform, threshold):
|
||||
transformed_source_fit = transform_points(source_fit, transform)
|
||||
transformed_source_holdout = transform_points(source_holdout, transform)
|
||||
forward = cKDTree(target_fit).query(transformed_source_holdout, workers=-1)[0]
|
||||
reverse = cKDTree(transformed_source_fit).query(target_holdout, workers=-1)[0]
|
||||
distances = np.concatenate((forward, reverse))
|
||||
inliers = distances[distances <= threshold]
|
||||
return {
|
||||
"evaluated": int(len(distances)),
|
||||
"inliers": int(len(inliers)),
|
||||
"inlier_ratio": float(len(inliers) / max(len(distances), 1)),
|
||||
"inlier_rmse_m": float(np.sqrt(np.mean(inliers**2))) if len(inliers) else None,
|
||||
"median_m": float(np.median(distances)),
|
||||
"p90_m": float(np.quantile(distances, 0.90)),
|
||||
"p95_m": float(np.quantile(distances, 0.95)),
|
||||
}
|
||||
|
||||
|
||||
def hessian_metrics(hessian, characteristic_length=10.0):
|
||||
hessian = 0.5 * (np.asarray(hessian, float) + np.asarray(hessian, float).T)
|
||||
scale = np.diag([1.0 / characteristic_length] * 3 + [1.0] * 3)
|
||||
scaled = scale.T @ hessian @ scale
|
||||
values, vectors = np.linalg.eigh(scaled)
|
||||
largest = max(float(np.max(np.abs(values))), np.finfo(float).eps)
|
||||
positive = values[values > largest * 1e-9]
|
||||
condition = float(positive[-1] / positive[0]) if len(positive) else float("inf")
|
||||
return {
|
||||
"native_order": ["rx_rad", "ry_rad", "rz_rad", "tx_m", "ty_m", "tz_m"],
|
||||
"scaled_eigenvalues": values.tolist(),
|
||||
"effective_rank": int(len(positive)),
|
||||
"scaled_condition_number": condition,
|
||||
"weakest_scaled_direction": vectors[:, int(np.argmin(values))].tolist(),
|
||||
}
|
||||
|
||||
|
||||
def transform_difference(reference, candidate):
|
||||
delta = inverse_transform(reference) @ candidate
|
||||
return {
|
||||
"translation_m": float(np.linalg.norm(delta[:3, 3])),
|
||||
"rotation_deg": rotation_angle_deg(delta[:3, :3]),
|
||||
}
|
||||
|
||||
|
||||
def loop_metrics(transforms):
|
||||
loops = []
|
||||
for (i, j), b_ij in transforms.items():
|
||||
for (j2, k), b_jk in transforms.items():
|
||||
if j2 != j or (i, k) not in transforms:
|
||||
continue
|
||||
loops.append(transform_difference(transforms[(i, k)], b_ij @ b_jk))
|
||||
if not loops:
|
||||
return {"count": 0}
|
||||
translation = np.array([item["translation_m"] for item in loops])
|
||||
rotation = np.array([item["rotation_deg"] for item in loops])
|
||||
return {
|
||||
"count": len(loops),
|
||||
"translation_rms_m": float(np.sqrt(np.mean(translation**2))),
|
||||
"translation_p95_m": float(np.quantile(translation, 0.95)),
|
||||
"rotation_rms_deg": float(np.sqrt(np.mean(rotation**2))),
|
||||
"rotation_p95_deg": float(np.quantile(rotation, 0.95)),
|
||||
}
|
||||
|
||||
|
||||
def cmd_ground(args):
|
||||
stations = load_stations(args.frames, args.min_range, args.max_range)
|
||||
rows = []
|
||||
for timestamp, counter, _, xyz in stations:
|
||||
roi = xyz[(xyz[:, 2] >= args.z_min) & (xyz[:, 2] <= args.z_max)]
|
||||
if len(roi) < args.min_inliers:
|
||||
continue
|
||||
cloud = make_o3d_cloud(roi, args.voxel)
|
||||
plane, indexes = cloud.segment_plane(
|
||||
args.distance_threshold, 3, args.ransac_iterations
|
||||
)
|
||||
normal = np.asarray(plane[:3], float)
|
||||
norm = np.linalg.norm(normal)
|
||||
normal, distance = normal / norm, float(plane[3] / norm)
|
||||
if distance < 0:
|
||||
normal, distance = -normal, -distance
|
||||
points = np.asarray(cloud.points)[indexes]
|
||||
rms = float(np.sqrt(np.mean((points @ normal + distance) ** 2)))
|
||||
if len(indexes) >= args.min_inliers and rms <= args.max_rms:
|
||||
rows.append([timestamp, *normal, distance, len(indexes), rms, counter])
|
||||
output = Path(args.output)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
with output.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.writer(stream)
|
||||
writer.writerow(["time", "nx", "ny", "nz", "d", "inliers", "rms_m", "frame_counter"])
|
||||
writer.writerows(rows)
|
||||
print(json.dumps({"planes": len(rows), "output": str(output.resolve())}, indent=2))
|
||||
|
||||
|
||||
def cmd_pairs(args):
|
||||
if len(args.voxels) != len(args.correspondences):
|
||||
raise ValueError("--voxels and --correspondences must have equal lengths")
|
||||
stations = load_stations(
|
||||
args.frames, args.min_range, args.max_range, args.z_min, args.z_max
|
||||
)
|
||||
reference = read_poses(args.reference_poses)
|
||||
if len(stations) < args.min_stations:
|
||||
raise ValueError(f"need at least {args.min_stations} stations, got {len(stations)}")
|
||||
reference_poses, reference_dt = [], []
|
||||
for timestamp, _, _, xyz in stations:
|
||||
if len(xyz) < args.min_roi_points:
|
||||
raise ValueError(f"station at {timestamp} has only {len(xyz)} ROI points")
|
||||
pose, dt = nearest_pose(reference, timestamp + args.time_offset)
|
||||
reference_poses.append(pose)
|
||||
reference_dt.append(dt)
|
||||
reference_poses = np.asarray(reference_poses)
|
||||
split = [split_holdout(station[3], args.holdout_fraction, i)
|
||||
for i, station in enumerate(stations)]
|
||||
rng = np.random.default_rng(args.seed)
|
||||
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
|
||||
accepted_transforms = {}
|
||||
for i in range(len(stations)):
|
||||
for j in range(i + args.min_gap, min(len(stations), i + args.max_gap + 1)):
|
||||
a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j]
|
||||
translation = float(np.linalg.norm(a_ij[:2, 3]))
|
||||
rotation = rotation_angle_deg(a_ij[:3, :3])
|
||||
if translation < args.min_translation and rotation < args.min_rotation:
|
||||
continue
|
||||
initial_b = a_ij.copy() # X0=I; no measured extrinsic.
|
||||
target_fit, target_holdout = split[i]
|
||||
source_fit, source_holdout = split[j]
|
||||
forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
|
||||
heldout = symmetric_heldout_metrics(
|
||||
target_fit, target_holdout, source_fit, source_holdout,
|
||||
forward["transform"], args.evaluation_distance
|
||||
)
|
||||
hessian = hessian_metrics(forward["hessian"])
|
||||
reverse_answer = align_backend(
|
||||
args.backend, source_fit, target_fit, inverse_transform(initial_b), args
|
||||
)
|
||||
reverse = transform_difference(
|
||||
forward["transform"], inverse_transform(reverse_answer["transform"])
|
||||
)
|
||||
multistart = []
|
||||
for _ in range(args.multistart):
|
||||
perturb = np.r_[
|
||||
rng.normal(0.0, args.multistart_translation_sigma, 3),
|
||||
np.deg2rad(rng.normal(0.0, args.multistart_rotation_sigma, 3)),
|
||||
]
|
||||
candidate = align_backend(
|
||||
args.backend, target_fit, source_fit,
|
||||
params_transform(perturb) @ initial_b, args
|
||||
)
|
||||
multistart.append(transform_difference(forward["transform"], candidate["transform"]))
|
||||
stable = [
|
||||
item["translation_m"] <= args.multistart_translation_tolerance
|
||||
and item["rotation_deg"] <= args.multistart_rotation_tolerance
|
||||
for item in multistart
|
||||
]
|
||||
success_rate = float(np.mean(stable)) if stable else 1.0
|
||||
reasons = []
|
||||
if forward["converged"] is False:
|
||||
reasons.append("backend_not_converged")
|
||||
if heldout["inlier_ratio"] < args.min_inlier_ratio:
|
||||
reasons.append("heldout_inlier_ratio")
|
||||
if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse:
|
||||
reasons.append("heldout_inlier_rmse")
|
||||
if hessian["effective_rank"] < 6:
|
||||
reasons.append("hessian_rank")
|
||||
if hessian["scaled_condition_number"] > args.max_hessian_condition:
|
||||
reasons.append("hessian_condition")
|
||||
if reverse["translation_m"] > args.reverse_translation_tolerance:
|
||||
reasons.append("forward_reverse_translation")
|
||||
if reverse["rotation_deg"] > args.reverse_rotation_tolerance:
|
||||
reasons.append("forward_reverse_rotation")
|
||||
if success_rate < args.min_multistart_success:
|
||||
reasons.append("multistart_instability")
|
||||
accepted = not reasons
|
||||
report = {
|
||||
"i": i, "j": j,
|
||||
"lidar_time_i": stations[i][0], "lidar_time_j": stations[j][0],
|
||||
"frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1],
|
||||
"rtk_translation_m": translation, "rtk_rotation_deg": rotation,
|
||||
"nearest_rtk_dt_i_s": reference_dt[i], "nearest_rtk_dt_j_s": reference_dt[j],
|
||||
"initial_B_source": "X0=identity; B0=A (no measured extrinsic)",
|
||||
"B_ij_4x4": forward["transform"].tolist(),
|
||||
"backend": args.backend, "backend_converged": forward["converged"],
|
||||
"backend_iterations": forward["iterations"],
|
||||
"backend_num_inliers": forward["num_inliers"],
|
||||
"backend_objective": forward["objective"],
|
||||
"backend_elapsed_sec": forward["elapsed_sec"],
|
||||
"multiscale_stages": forward["stages"],
|
||||
"heldout_symmetric": heldout, "hessian": hessian,
|
||||
"forward_reverse": reverse,
|
||||
"multistart": {"runs": len(multistart), "success_rate": success_rate,
|
||||
"deltas": multistart},
|
||||
"accepted": accepted, "rejection_reasons": reasons,
|
||||
}
|
||||
reports.append(report)
|
||||
print(f"{args.backend} {i:02d}->{j:02d} rmse={heldout['inlier_rmse_m']} "
|
||||
f"ratio={heldout['inlier_ratio']:.3f} accepted={accepted}")
|
||||
if accepted:
|
||||
accepted_a.append(a_ij)
|
||||
accepted_b.append(forward["transform"])
|
||||
accepted_meta.append([i, j, stations[i][0], stations[j][0]])
|
||||
accepted_transforms[(i, j)] = forward["transform"]
|
||||
output = Path(args.output)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(
|
||||
output, A=np.asarray(accepted_a), B=np.asarray(accepted_b),
|
||||
meta=np.asarray(accepted_meta),
|
||||
station_times=np.asarray([item[0] for item in stations]),
|
||||
rtk_nearest_dt_s=np.asarray(reference_dt), backend=np.asarray(args.backend),
|
||||
)
|
||||
quality = {
|
||||
"schema_version": 2,
|
||||
"backend": args.backend,
|
||||
"transform_convention": "B_ij=T_Li_Lj maps station j points into station i",
|
||||
"raw_point_field": "points_raw",
|
||||
"measured_extrinsic_used_as_initial": False,
|
||||
"stations": len(stations), "candidate_pairs": len(reports),
|
||||
"accepted_pairs": len(accepted_a),
|
||||
"parameters": vars(args),
|
||||
"accepted_loop_closure": loop_metrics(accepted_transforms),
|
||||
"pairs": reports,
|
||||
}
|
||||
quality["parameters"].pop("func", None)
|
||||
quality_path = Path(args.quality_json or output.with_suffix(".quality.json"))
|
||||
quality_path.write_text(json.dumps(quality, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
csv_path = Path(args.quality_csv or output.with_suffix(".quality.csv"))
|
||||
with csv_path.open("w", encoding="utf-8", newline="") as stream:
|
||||
fields = ["i", "j", "rtk_translation_m", "rtk_rotation_deg",
|
||||
"heldout_inlier_ratio", "heldout_inlier_rmse_m",
|
||||
"hessian_rank", "hessian_condition", "reverse_translation_m",
|
||||
"reverse_rotation_deg", "multistart_success_rate", "accepted",
|
||||
"rejection_reasons"]
|
||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
||||
writer.writeheader()
|
||||
for item in reports:
|
||||
writer.writerow({
|
||||
"i": item["i"], "j": item["j"],
|
||||
"rtk_translation_m": item["rtk_translation_m"],
|
||||
"rtk_rotation_deg": item["rtk_rotation_deg"],
|
||||
"heldout_inlier_ratio": item["heldout_symmetric"]["inlier_ratio"],
|
||||
"heldout_inlier_rmse_m": item["heldout_symmetric"]["inlier_rmse_m"],
|
||||
"hessian_rank": item["hessian"]["effective_rank"],
|
||||
"hessian_condition": item["hessian"]["scaled_condition_number"],
|
||||
"reverse_translation_m": item["forward_reverse"]["translation_m"],
|
||||
"reverse_rotation_deg": item["forward_reverse"]["rotation_deg"],
|
||||
"multistart_success_rate": item["multistart"]["success_rate"],
|
||||
"accepted": item["accepted"],
|
||||
"rejection_reasons": ";".join(item["rejection_reasons"]),
|
||||
})
|
||||
if len(accepted_a) < args.min_pairs:
|
||||
raise RuntimeError(f"only {len(accepted_a)} accepted pairs; need {args.min_pairs}")
|
||||
print(json.dumps({
|
||||
"backend": args.backend, "stations": len(stations),
|
||||
"candidate_pairs": len(reports), "accepted_pairs": len(accepted_a),
|
||||
"output": str(output.resolve()), "quality_json": str(quality_path.resolve()),
|
||||
"loop": quality["accepted_loop_closure"],
|
||||
}, indent=2))
|
||||
|
||||
|
||||
def read_planes(path):
|
||||
planes = []
|
||||
with Path(path).open(encoding="utf-8-sig", newline="") as stream:
|
||||
for row in csv.DictReader(stream):
|
||||
normal = np.array([float(row[k]) for k in ("nx", "ny", "nz")])
|
||||
norm = np.linalg.norm(normal)
|
||||
normal, distance = normal / norm, float(row["d"]) / norm
|
||||
if distance < 0:
|
||||
normal, distance = -normal, -distance
|
||||
planes.append([*normal, distance])
|
||||
return np.asarray(planes)
|
||||
|
||||
|
||||
def read_pairs(path):
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
return (np.asarray(data["A"], float), np.asarray(data["B"], float),
|
||||
np.asarray(data["meta"], float), len(data["station_times"]))
|
||||
|
||||
|
||||
def calibration_residual(params, a_array, b_array, planes, args):
|
||||
x = params_transform(params)
|
||||
values = []
|
||||
for a_ij, b_ij in zip(a_array, b_array):
|
||||
error = inverse_transform(a_ij @ x) @ x @ b_ij
|
||||
values.extend((error[:3, 3] / args.translation_sigma).tolist())
|
||||
values.extend((so3_log(error[:3, :3]) / math.radians(args.rotation_sigma)).tolist())
|
||||
body_up = np.array([0.0, 0.0, 1.0])
|
||||
for plane in planes:
|
||||
normal_body = x[:3, :3] @ plane[:3]
|
||||
values.extend((np.cross(normal_body, body_up) / args.plane_normal_sigma).tolist())
|
||||
body_distance = plane[3] - float(normal_body @ x[:3, 3])
|
||||
values.append((body_distance - args.reference_height) / args.plane_height_sigma)
|
||||
return np.asarray(values)
|
||||
|
||||
|
||||
def pair_metrics(a_array, b_array, x):
|
||||
translation, rotation, rows = [], [], []
|
||||
for index, (a_ij, b_ij) in enumerate(zip(a_array, b_array)):
|
||||
predicted = inverse_transform(x) @ a_ij @ x
|
||||
delta = inverse_transform(b_ij) @ predicted
|
||||
t = float(np.linalg.norm(delta[:3, 3]))
|
||||
r = rotation_angle_deg(delta[:3, :3])
|
||||
translation.append(t); rotation.append(r)
|
||||
rows.append({"pair_index": index, "translation_m": t, "rotation_deg": r})
|
||||
translation, rotation = np.asarray(translation), np.asarray(rotation)
|
||||
def stats(values):
|
||||
return {
|
||||
"rms": float(np.sqrt(np.mean(values**2))),
|
||||
"median": float(np.median(values)),
|
||||
"p90": float(np.quantile(values, 0.90)),
|
||||
"p95": float(np.quantile(values, 0.95)),
|
||||
"max": float(np.max(values)),
|
||||
}
|
||||
return {"pairs": len(rows), "translation_m": stats(translation),
|
||||
"rotation_deg": stats(rotation), "per_pair": rows}
|
||||
|
||||
|
||||
def solve_extrinsic(a_array, b_array, planes, args):
|
||||
rng = np.random.default_rng(args.seed)
|
||||
starts = [np.zeros(6)]
|
||||
for _ in range(args.solver_multistart - 1):
|
||||
starts.append(np.r_[
|
||||
rng.normal(0.0, args.start_translation_sigma, 3),
|
||||
np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
|
||||
])
|
||||
candidates = []
|
||||
lower = np.r_[[-5.0] * 3, [-math.pi] * 3]
|
||||
upper = np.r_[[5.0] * 3, [math.pi] * 3]
|
||||
for start in starts:
|
||||
answer = least_squares(
|
||||
calibration_residual, np.clip(start, lower, upper),
|
||||
args=(a_array, b_array, planes, args),
|
||||
bounds=(lower, upper), loss="huber", f_scale=1.5,
|
||||
x_scale="jac", max_nfev=args.max_nfev,
|
||||
)
|
||||
candidates.append(answer)
|
||||
best = min(candidates, key=lambda item: item.cost)
|
||||
return best, candidates
|
||||
|
||||
|
||||
def cmd_calibrate(args):
|
||||
a_array, b_array, meta, stations = read_pairs(args.pairs)
|
||||
planes = read_planes(args.ground_planes)
|
||||
best, candidates = solve_extrinsic(a_array, b_array, planes, args)
|
||||
x = params_transform(best.x)
|
||||
residual = calibration_residual(best.x, a_array, b_array, planes, args)
|
||||
absolute = np.abs(residual)
|
||||
weights = np.ones_like(residual)
|
||||
weights[absolute > 1.5] = 1.5 / absolute[absolute > 1.5]
|
||||
weighted_jacobian = best.jac * np.sqrt(weights)[:, None]
|
||||
singular = np.linalg.svd(weighted_jacobian, compute_uv=False)
|
||||
condition = float(singular[0] / max(singular[-1], 1e-15))
|
||||
dof = max(len(residual) - 6, 1)
|
||||
covariance = np.linalg.pinv(weighted_jacobian.T @ weighted_jacobian) * float(
|
||||
np.sum(weights * residual**2) / dof
|
||||
)
|
||||
sigma = np.sqrt(np.maximum(np.diag(covariance), 0.0))
|
||||
candidate_summary = []
|
||||
for item in candidates:
|
||||
candidate_x = params_transform(item.x)
|
||||
candidate_summary.append({
|
||||
"cost": float(item.cost), "success": bool(item.success),
|
||||
**transform_difference(x, candidate_x),
|
||||
})
|
||||
bootstrap = []
|
||||
rng = np.random.default_rng(args.seed + 1)
|
||||
for _ in range(args.bootstrap):
|
||||
indexes = rng.integers(0, len(a_array), len(a_array))
|
||||
answer = least_squares(
|
||||
calibration_residual, best.x,
|
||||
args=(a_array[indexes], b_array[indexes], planes, args),
|
||||
loss="huber", f_scale=1.5, x_scale="jac", max_nfev=args.max_nfev,
|
||||
)
|
||||
bootstrap.append(np.r_[answer.x[:3], rpy_deg(so3_exp(answer.x[3:]))])
|
||||
bootstrap = np.asarray(bootstrap)
|
||||
result = {
|
||||
"schema_version": 2,
|
||||
"success": bool(best.success),
|
||||
"message": best.message,
|
||||
"convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame",
|
||||
"equation": "A_ij X = X B_ij",
|
||||
"measured_extrinsic_used_as_initial": False,
|
||||
"translation_m": x[:3, 3].tolist(),
|
||||
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
|
||||
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
|
||||
"matrix_4x4": x.tolist(),
|
||||
"estimation": {"stations": stations, "pairs": len(a_array),
|
||||
"residuals": pair_metrics(a_array, b_array, x)},
|
||||
"ground": {
|
||||
"planes": len(planes),
|
||||
"reference_origin_height_above_ground_m": args.reference_height,
|
||||
"formula": "d_lidar - (R_X n_lidar)^T t_X - reference_height",
|
||||
},
|
||||
"linearized_one_sigma": {
|
||||
"translation_m": sigma[:3].tolist(),
|
||||
"rotation_deg": np.rad2deg(sigma[3:]).tolist(),
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check",
|
||||
},
|
||||
"weighted_jacobian_condition_number": condition,
|
||||
"solver_multistart": {
|
||||
"runs": len(candidates), "candidates_relative_to_best": candidate_summary,
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": len(bootstrap),
|
||||
"order": ["x_m", "y_m", "z_m", "roll_deg", "pitch_deg", "yaw_deg"],
|
||||
"std": np.std(bootstrap, axis=0, ddof=1).tolist() if len(bootstrap) > 1 else None,
|
||||
"p025": np.quantile(bootstrap, 0.025, axis=0).tolist() if len(bootstrap) else None,
|
||||
"p975": np.quantile(bootstrap, 0.975, axis=0).tolist() if len(bootstrap) else None,
|
||||
},
|
||||
}
|
||||
output = Path(args.output)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(result, ensure_ascii=False, indent=2))
|
||||
|
||||
|
||||
def cmd_validate(args):
|
||||
result = json.loads(Path(args.extrinsic).read_text(encoding="utf-8-sig"))
|
||||
x = np.asarray(result["matrix_4x4"], float)
|
||||
a_array, b_array, meta, stations = read_pairs(args.pairs)
|
||||
metrics = pair_metrics(a_array, b_array, x)
|
||||
for row, pair_meta in zip(metrics["per_pair"], meta):
|
||||
row.update({"i": int(pair_meta[0]), "j": int(pair_meta[1])})
|
||||
report = {
|
||||
"role": "auxiliary check only; first-batch RTK is sparse",
|
||||
"blind_with_respect_to_X": True,
|
||||
"note": "No AX residual was used to select these pairs",
|
||||
"stations": stations, "metrics": metrics,
|
||||
}
|
||||
output = Path(args.output)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
output.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(report, ensure_ascii=False, indent=2))
|
||||
|
||||
|
||||
def build_parser():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
commands = parser.add_subparsers(dest="command", required=True)
|
||||
ground = commands.add_parser("ground")
|
||||
ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True)
|
||||
ground.add_argument("--min-range", type=float, default=1.0); ground.add_argument("--max-range", type=float, default=30.0)
|
||||
ground.add_argument("--z-min", type=float, default=-1.4); ground.add_argument("--z-max", type=float, default=-0.4)
|
||||
ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025)
|
||||
ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500)
|
||||
ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground)
|
||||
|
||||
pairs = commands.add_parser("pairs")
|
||||
pairs.add_argument("--backend", choices=["open3d", "small_gicp"], required=True)
|
||||
pairs.add_argument("--frames", required=True)
|
||||
pairs.add_argument("--reference-poses", "--body", dest="reference_poses", required=True)
|
||||
pairs.add_argument("--output", required=True); pairs.add_argument("--quality-json"); pairs.add_argument("--quality-csv")
|
||||
pairs.add_argument("--time-offset", type=float, default=0.0)
|
||||
pairs.add_argument("--min-stations", type=int, default=30); pairs.add_argument("--min-pairs", type=int, default=25)
|
||||
pairs.add_argument("--min-gap", type=int, default=1); pairs.add_argument("--max-gap", type=int, default=5)
|
||||
pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0)
|
||||
pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0)
|
||||
pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0)
|
||||
pairs.add_argument("--min-roi-points", type=int, default=1000)
|
||||
pairs.add_argument("--holdout-fraction", type=float, default=0.20)
|
||||
pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08])
|
||||
pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25])
|
||||
pairs.add_argument("--iterations", type=int, default=60); pairs.add_argument("--threads", type=int, default=8)
|
||||
pairs.add_argument("--evaluation-distance", type=float, default=0.25)
|
||||
pairs.add_argument("--min-inlier-ratio", type=float, default=0.35); pairs.add_argument("--max-inlier-rmse", type=float, default=0.16)
|
||||
pairs.add_argument("--max-hessian-condition", type=float, default=1e8)
|
||||
pairs.add_argument("--reverse-translation-tolerance", type=float, default=0.08)
|
||||
pairs.add_argument("--reverse-rotation-tolerance", type=float, default=0.50)
|
||||
pairs.add_argument("--multistart", type=int, default=2)
|
||||
pairs.add_argument("--multistart-translation-sigma", type=float, default=0.30)
|
||||
pairs.add_argument("--multistart-rotation-sigma", type=float, default=3.0)
|
||||
pairs.add_argument("--multistart-translation-tolerance", type=float, default=0.08)
|
||||
pairs.add_argument("--multistart-rotation-tolerance", type=float, default=0.50)
|
||||
pairs.add_argument("--min-multistart-success", type=float, default=0.50)
|
||||
pairs.add_argument("--seed", type=int, default=20260721); pairs.set_defaults(func=cmd_pairs)
|
||||
|
||||
calibrate = commands.add_parser("calibrate")
|
||||
calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True)
|
||||
calibrate.add_argument("--output", required=True)
|
||||
calibrate.add_argument("--translation-sigma", type=float, default=0.05)
|
||||
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
|
||||
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
|
||||
calibrate.add_argument("--plane-height-sigma", type=float, default=0.03)
|
||||
calibrate.add_argument("--reference-height", "--body-height", dest="reference_height", type=float, default=0.8535)
|
||||
calibrate.add_argument("--solver-multistart", type=int, default=12)
|
||||
calibrate.add_argument("--start-translation-sigma", type=float, default=1.0)
|
||||
calibrate.add_argument("--start-rotation-sigma", type=float, default=20.0)
|
||||
calibrate.add_argument("--bootstrap", type=int, default=100)
|
||||
calibrate.add_argument("--max-nfev", type=int, default=1000)
|
||||
calibrate.add_argument("--seed", type=int, default=20260721); calibrate.set_defaults(func=cmd_calibrate)
|
||||
|
||||
validate = commands.add_parser("validate")
|
||||
validate.add_argument("--pairs", required=True); validate.add_argument("--extrinsic", required=True)
|
||||
validate.add_argument("--output", required=True); validate.set_defaults(func=cmd_validate)
|
||||
return parser
|
||||
|
||||
|
||||
def main():
|
||||
args = build_parser().parse_args()
|
||||
args.func(args)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,169 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion."""
|
||||
import argparse
|
||||
import json
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from rigorous_calibration import (
|
||||
inverse_transform, load_stations, rotation_angle_deg, rpy_deg, transform_points,
|
||||
)
|
||||
|
||||
|
||||
COLORS = {
|
||||
"target": [0.10, 0.65, 1.00],
|
||||
"source": [1.00, 0.35, 0.05],
|
||||
}
|
||||
|
||||
|
||||
def cloud(o3d, points, color, voxel):
|
||||
item = o3d.geometry.PointCloud()
|
||||
item.points = o3d.utility.Vector3dVector(points)
|
||||
item = item.voxel_down_sample(voxel)
|
||||
item.paint_uniform_color(color)
|
||||
return item
|
||||
|
||||
|
||||
def delta_components(reference, candidate):
|
||||
"""Components of reference^-1*candidate, plus coordinate-invariant norms."""
|
||||
delta = inverse_transform(reference) @ candidate
|
||||
translation = np.asarray(delta[:3, 3], float)
|
||||
return {
|
||||
"translation_xyz_cm": (translation * 100.0).tolist(),
|
||||
"translation_norm_cm": float(np.linalg.norm(translation) * 100.0),
|
||||
"rotation_rpy_deg_xyz": rpy_deg(delta[:3, :3]),
|
||||
"rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
|
||||
}
|
||||
|
||||
|
||||
def body_left_rpy(x, rpy_correction_deg):
|
||||
correction = np.eye(4)
|
||||
correction[:3, :3] = Rotation.from_euler(
|
||||
"xyz", np.asarray(rpy_correction_deg, float), degrees=True
|
||||
).as_matrix()
|
||||
return correction @ x
|
||||
|
||||
|
||||
def print_delta(name, reference, candidate):
|
||||
item = delta_components(reference, candidate)
|
||||
tx, ty, tz = item["translation_xyz_cm"]
|
||||
roll, pitch, yaw = item["rotation_rpy_deg_xyz"]
|
||||
print(
|
||||
f"{name}: B^-1*motion translation xyz = "
|
||||
f"[{tx:+.4f}, {ty:+.4f}, {tz:+.4f}] cm; "
|
||||
f"rpy xyz = [{roll:+.4f}, {pitch:+.4f}, {yaw:+.4f}] deg; "
|
||||
f"norm = {item['translation_norm_cm']:.4f} cm / "
|
||||
f"{item['rotation_angle_deg']:.6f} deg"
|
||||
)
|
||||
return item
|
||||
|
||||
|
||||
def main():
|
||||
import open3d as o3d
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--frames", required=True)
|
||||
parser.add_argument("--pairs", required=True)
|
||||
parser.add_argument("--extrinsic", required=True)
|
||||
parser.add_argument("--pair-index", type=int, default=0)
|
||||
parser.add_argument("--voxel", type=float, default=0.10)
|
||||
parser.add_argument(
|
||||
"--left-rpy-deg", nargs=3, type=float, default=[0.0, 0.0, 0.0],
|
||||
metavar=("ROLL", "PITCH", "YAW"),
|
||||
help="optional body-frame left correction applied as DeltaR_body * X",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
stations = load_stations(args.frames, 1.0, 60.0)
|
||||
with np.load(args.pairs, allow_pickle=False) as data:
|
||||
if len(stations) != len(data["station_times"]):
|
||||
raise ValueError(
|
||||
f"frames contain {len(stations)} stations but pair file records "
|
||||
f"{len(data['station_times'])}"
|
||||
)
|
||||
if not 0 <= args.pair_index < len(data["A"]):
|
||||
raise IndexError(
|
||||
f"pair-index {args.pair_index} outside [0,{len(data['A']) - 1}]"
|
||||
)
|
||||
a_ij = np.asarray(data["A"][args.pair_index], float)
|
||||
b_gicp = np.asarray(data["B"][args.pair_index], float)
|
||||
i, j = np.asarray(data["meta"][args.pair_index, :2], int)
|
||||
|
||||
with open(args.extrinsic, encoding="utf-8-sig") as stream:
|
||||
result = json.load(stream)
|
||||
x = np.asarray(result["matrix_4x4"], float)
|
||||
b_calibrated = inverse_transform(x) @ a_ij @ x
|
||||
|
||||
transforms = {
|
||||
"1 raw": np.eye(4),
|
||||
"2 RTK initial (X0=I)": a_ij,
|
||||
"3 GICP B": b_gicp,
|
||||
"4 calibrated X^-1 A X": b_calibrated,
|
||||
}
|
||||
correction = np.asarray(args.left_rpy_deg, float)
|
||||
if np.any(np.abs(correction) > 0.0):
|
||||
x_test = body_left_rpy(x, correction)
|
||||
transforms[
|
||||
f"5 test body-left RPY {correction.tolist()} deg"
|
||||
] = inverse_transform(x_test) @ a_ij @ x_test
|
||||
|
||||
target = stations[i][3]
|
||||
source = stations[j][3]
|
||||
print(f"pair_index={args.pair_index}, station {i} <- {j}")
|
||||
print("blue = target station i; orange = source station j after selected transform")
|
||||
print("keys: 1 raw | 2 RTK initial | 3 GICP | 4 calibrated | 5 test correction | Q/Esc exit")
|
||||
print(
|
||||
"IMPORTANT: delta xyz/rpy are components of B^-1*(X^-1*A*X), expressed "
|
||||
"in station-j LiDAR coordinates; screen-left/right depends on the 3D camera view."
|
||||
)
|
||||
baseline = print_delta("mode 4 minus mode 3", b_gicp, b_calibrated)
|
||||
roll, pitch, yaw = np.abs(baseline["rotation_rpy_deg_xyz"])
|
||||
if max(roll, pitch) > max(0.10, 2.0 * yaw):
|
||||
print("diagnosis: roll/pitch components dominate yaw; do not prioritize yaw tuning for this pair.")
|
||||
tx, ty, tz = np.abs(baseline["translation_xyz_cm"])
|
||||
if tz > max(tx, ty):
|
||||
print("diagnosis: the largest translation component is relative Z, not lateral XY.")
|
||||
body_up = np.array([0.0, 0.0, 1.0])
|
||||
if np.linalg.norm(a_ij[:3, :3] @ body_up - body_up) < 1e-8:
|
||||
print(
|
||||
"observability: this A preserves the body Z axis, so body-left X.z "
|
||||
"translation is unobservable from this pair; use ground/external height constraints."
|
||||
)
|
||||
if "5 test body-left RPY " + str(correction.tolist()) + " deg" in transforms:
|
||||
print_delta("mode 5 minus mode 3", b_gicp, list(transforms.values())[-1])
|
||||
|
||||
viewer = o3d.visualization.VisualizerWithKeyCallback()
|
||||
viewer.create_window("Rigorous LiDAR registration inspection - 3D", 1400, 900)
|
||||
target_cloud = cloud(o3d, target, COLORS["target"], args.voxel)
|
||||
source_cloud = cloud(o3d, source, COLORS["source"], args.voxel)
|
||||
viewer.add_geometry(target_cloud)
|
||||
viewer.add_geometry(source_cloud)
|
||||
axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0)
|
||||
viewer.add_geometry(axes)
|
||||
current = np.eye(4)
|
||||
|
||||
def select(name):
|
||||
def callback(vis):
|
||||
nonlocal current
|
||||
desired = transforms[name]
|
||||
source_cloud.transform(desired @ inverse_transform(current))
|
||||
current = desired
|
||||
vis.update_geometry(source_cloud)
|
||||
if name == "3 GICP B":
|
||||
print(f"{name}: reference registration B; delta = 0")
|
||||
else:
|
||||
print_delta(name + " minus mode 3", b_gicp, desired)
|
||||
return False
|
||||
return callback
|
||||
|
||||
for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4"), ord("5")), transforms):
|
||||
viewer.register_key_callback(key, select(name))
|
||||
viewer.get_render_option().background_color = np.array([0.02, 0.02, 0.02])
|
||||
viewer.get_render_option().point_size = 2.0
|
||||
viewer.run()
|
||||
viewer.destroy_window()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,5 @@
|
||||
# 数据说明
|
||||
|
||||
原始LiDAR dlog、RTK/IMU rscap、逐帧NPZ和prepared点云体积较大，不进入Git。请从项目云盘取得数据，并按根README中的目录示例放置；实际路径通过命令参数传入。
|
||||
|
||||
公开数据包应同时提供：采集日期、车辆/传感器安装版本、站点数量、ANT1/ANT2接线、rawHeading方向、RTK参考点离地高度及其测量方法。
|
||||
@@ -0,0 +1,4 @@
|
||||
numpy>=1.26
|
||||
scipy>=1.11
|
||||
open3d>=0.18
|
||||
small-gicp
|
||||
@@ -0,0 +1,5 @@
|
||||
# results目录
|
||||
|
||||
`reference_data4/`是本仓库附带的精简参考结果。新的运行结果应写到仓库外目录或`outputs/`，不要覆盖参考结果。
|
||||
|
||||
参考结果保留最终矩阵、共识B、两后端精筛B、逐对CSV/筛选审计和地面平面；未保留原始点云、逐帧combined数据、冗长的初筛JSON和带本机绝对路径的过程文件。
|
||||
@@ -0,0 +1,23 @@
|
||||
# data4参考结果
|
||||
|
||||
推荐下游只读取`final_T_RTK_lidar.json`，其方向为：
|
||||
|
||||
```text
|
||||
p_RTK = T_RTK_lidar · p_lidar
|
||||
```
|
||||
|
||||
```text
|
||||
translation_m = [1.638179350, -0.240844799, 0.084481236]
|
||||
RPY_deg_xyz = [-0.817167459, 1.323288119, -22.104163318]
|
||||
```
|
||||
|
||||
| 路径 | 内容 |
|
||||
|---|---|
|
||||
| `final_T_RTK_lidar.json` | 唯一推荐使用的最终外参 |
|
||||
| `summary.json` | 最终残差、条件数和两后端差异摘要 |
|
||||
| `common/ground_planes.csv` | 34站地面RANSAC平面 |
|
||||
| `open3d_gicp/` | Open3D精筛B、精筛审计、逐对质量CSV和独立X |
|
||||
| `small_gicp/` | small_gicp对应产物 |
|
||||
| `consensus/` | 两后端共同认可的25对B、共识审计和最终X原始求解记录 |
|
||||
|
||||
`z=0.084481 m`依赖RTK参考点离地`0.8535 m`，不是平面AX=XB独立观测值。当前AX RMS约`0.100207 m / 1.252794°`，结果适合算法联调和继续验证，不应据此单独宣称逐帧GT达到±3 cm。
|
||||
@@ -0,0 +1,35 @@
|
||||
time,nx,ny,nz,d,inliers,rms_m,frame_counter
|
||||
1784783825.357129,-0.0071009788712051635,-0.01614775434066578,0.9998444009588814,0.9449714797885561,2216,0.01353681235386204,382
|
||||
1784783905.353819,0.0037577953662433442,-0.00645689216229629,0.999972093369405,0.9404093678203617,1992,0.01182484680356077,1182
|
||||
1784783971.0503054,-0.021571557237587136,-0.004187980739348835,0.9997585352152151,0.9464426916222599,1922,0.01241302113439391,1839
|
||||
1784784059.7468228,-0.02328381206588687,0.004386003115594592,0.999719274132669,0.9506398743758987,1825,0.013207042662497559,2726
|
||||
1784784149.2434597,-0.03034651356298797,8.407602826756324e-05,0.9995394349628197,0.9287260086761917,1631,0.012431422856007433,3621
|
||||
1784784224.2408776,-0.02735108660242708,0.010336463557273514,0.9995724463903534,0.8697042041945898,1745,0.012010699567274814,4371
|
||||
1784784301.6372502,-0.008085199791897242,-0.01261499998640764,0.9998877393586082,0.9555128377061561,2057,0.011385954851802133,5145
|
||||
1784784387.733771,-0.007167201772462397,-0.008280272080485561,0.9999400323584541,0.9272403036781977,2190,0.012588169113362788,6006
|
||||
1784784474.9314597,-0.015033673776128801,-0.05115510005163043,0.9985775605287256,0.9072885818985176,1852,0.011643717831315507,6878
|
||||
1784784549.4274275,-0.03189811408163763,0.004490776571011993,0.999481036960594,0.9463330979345341,1638,0.013973864979699106,7623
|
||||
1784784614.7244046,-0.027669126419476022,-0.013618714393779237,0.999524361914928,0.9348654978703125,1794,0.011102769699524444,8276
|
||||
1784784682.921899,-0.022151829941254066,-0.026156017173182243,0.9994124069651578,0.9227652769218206,1638,0.01266204532799032,8958
|
||||
1784784758.8187964,-0.022601789108727538,-0.030665670278286643,0.999274124450077,0.9305425654631599,1795,0.013340493954266352,9717
|
||||
1784784836.0155501,-0.017598498968453644,-0.02647415434199472,0.999494578267403,0.961769336191532,2015,0.01382548272951725,10489
|
||||
1784784921.1126208,-0.021874728045639568,-0.019005922983034846,0.9995800474021539,0.9186945373736978,1808,0.01319739563436461,11340
|
||||
1784784992.709947,-0.019622211270580107,-0.02528822634559132,0.9994876059427386,0.9414876680920201,2158,0.013280256398986076,12056
|
||||
1784785067.6067727,-0.031060743296391496,-0.010214597374617485,0.9994653031628212,0.9402571806911639,1758,0.013392641298608525,12805
|
||||
1784785215.9006598,-0.023673459396853343,-0.027330465291762113,0.9993460926961797,0.9143984233881569,2134,0.012198902426752438,14288
|
||||
1784785296.4990919,-0.013579953767407775,-0.025158411654770292,0.9995912360453568,0.929929365058787,2445,0.012790980094197171,15094
|
||||
1784785363.1952267,-0.0316919344947604,-0.008177115456525313,0.9994642345130668,0.9579125765731408,1861,0.012743464679231025,15761
|
||||
1784785434.592462,-0.022678088603046032,0.004435617789851151,0.9997329791459992,0.9537383917106543,1853,0.013464094518301148,16475
|
||||
1784785506.389296,-0.0273694753756875,-0.01771941378886425,0.9994683257575693,0.9327361226688458,1749,0.011502338837958854,17193
|
||||
1784785587.5863533,-0.034617559115875766,0.0015843237885758451,0.999399376885433,0.9191959537091571,1744,0.013401267879037225,18005
|
||||
1784785681.9825997,-0.033298152875437845,-0.018946770164947627,0.9992658569747096,0.9446967789786688,2037,0.012804059875312601,18949
|
||||
1784785815.4779446,-0.006681441650905292,-0.023720354219030532,0.9996963054514052,0.964084392350492,2080,0.013159652224503205,20284
|
||||
1784785891.9768085,-0.026044255070688936,0.004021590005713901,0.9996527014876911,0.934003424141447,1610,0.012619787010493816,21049
|
||||
1784785967.8726046,-0.026983516555153,-0.01791249491380091,0.9994753785663161,0.9339463871372334,1482,0.013831424226303278,21808
|
||||
1784786031.5701303,-0.028810092762610772,-0.015551490666087386,0.9994639211562729,0.938325903591574,1592,0.013415706854842701,22445
|
||||
1784786087.9670725,-0.026424364888569394,-0.014575043111831797,0.9995445568150146,0.9345909622822591,1466,0.013090809334738121,23009
|
||||
1784786160.8647907,-0.032665212336793446,0.0597663130328534,0.9976777895340014,1.0611447790248285,1511,0.012251618222434443,23738
|
||||
1784786252.6621523,-0.03732336904542465,-0.020702346452411244,0.9990887743211128,0.9478808317179221,1719,0.012520822005912273,24656
|
||||
1784786319.6581354,-0.025461816426307887,-0.02602901616210242,0.9993368732424047,0.9466783627035876,1589,0.013468160971926036,25326
|
||||
1784786396.7558627,-0.025295174442520576,-0.02343833470627969,0.9994052224278793,0.9710592140122102,1321,0.012895976784738191,26097
|
||||
1784786557.4492514,-0.014426534652625146,-0.00926546906583815,0.9998530022862894,0.935013905231254,1251,0.012477706451163199,27704
|
||||
|
+332
@@ -0,0 +1,332 @@
|
||||
{
|
||||
"selection_is_X_independent": true,
|
||||
"B_source": "Open3D; small_gicp is used only as an agreement gate",
|
||||
"max_translation_m": 0.05,
|
||||
"max_rotation_deg": 0.5,
|
||||
"input_open3d_pairs": 41,
|
||||
"accepted_pairs": 25,
|
||||
"pairs": [
|
||||
{
|
||||
"i": 0,
|
||||
"j": 1,
|
||||
"open3d_small_translation_m": 0.014276780914058016,
|
||||
"open3d_small_rotation_deg": 0.6136066194510507,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 0,
|
||||
"j": 2,
|
||||
"open3d_small_translation_m": 0.019952450418738,
|
||||
"open3d_small_rotation_deg": 0.14609270025586996,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 1,
|
||||
"j": 2,
|
||||
"accepted": false,
|
||||
"reason": "not_in_small_gicp_refined"
|
||||
},
|
||||
{
|
||||
"i": 2,
|
||||
"j": 3,
|
||||
"open3d_small_translation_m": 0.022407292900448784,
|
||||
"open3d_small_rotation_deg": 0.1674801169908669,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 2,
|
||||
"j": 5,
|
||||
"open3d_small_translation_m": 0.006161707315193906,
|
||||
"open3d_small_rotation_deg": 0.13760639079130288,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 3,
|
||||
"j": 5,
|
||||
"open3d_small_translation_m": 0.039798937040509075,
|
||||
"open3d_small_rotation_deg": 0.21317157512260662,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 3,
|
||||
"j": 6,
|
||||
"open3d_small_translation_m": 0.012411893826145848,
|
||||
"open3d_small_rotation_deg": 0.6409039547122766,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 6,
|
||||
"j": 7,
|
||||
"open3d_small_translation_m": 0.008481323658875535,
|
||||
"open3d_small_rotation_deg": 0.21102463812659789,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 6,
|
||||
"j": 8,
|
||||
"open3d_small_translation_m": 0.0392892670228761,
|
||||
"open3d_small_rotation_deg": 0.3638880951335145,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 7,
|
||||
"j": 8,
|
||||
"open3d_small_translation_m": 0.008121615288997118,
|
||||
"open3d_small_rotation_deg": 0.6269514061788293,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 10,
|
||||
"j": 11,
|
||||
"open3d_small_translation_m": 0.021311366483595485,
|
||||
"open3d_small_rotation_deg": 0.5895827931090589,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 12,
|
||||
"j": 14,
|
||||
"open3d_small_translation_m": 0.02159358315642783,
|
||||
"open3d_small_rotation_deg": 0.2828054398005336,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 12,
|
||||
"j": 15,
|
||||
"open3d_small_translation_m": 0.036801934207227605,
|
||||
"open3d_small_rotation_deg": 0.5608491395108458,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 13,
|
||||
"j": 15,
|
||||
"open3d_small_translation_m": 0.02340982602704092,
|
||||
"open3d_small_rotation_deg": 0.5560478634176542,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 13,
|
||||
"j": 16,
|
||||
"open3d_small_translation_m": 0.027602744462629097,
|
||||
"open3d_small_rotation_deg": 0.2529072067309812,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 16,
|
||||
"open3d_small_translation_m": 0.02090824470779122,
|
||||
"open3d_small_rotation_deg": 0.034004621265071464,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 17,
|
||||
"open3d_small_translation_m": 0.04186146133502706,
|
||||
"open3d_small_rotation_deg": 0.13358624916951445,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 18,
|
||||
"open3d_small_translation_m": 0.01461017711105615,
|
||||
"open3d_small_rotation_deg": 0.2537851025085552,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 16,
|
||||
"j": 19,
|
||||
"open3d_small_translation_m": 0.012703728170379172,
|
||||
"open3d_small_rotation_deg": 0.36607447371235324,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 17,
|
||||
"j": 18,
|
||||
"open3d_small_translation_m": 0.030547382773647488,
|
||||
"open3d_small_rotation_deg": 0.6293030402470645,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 22,
|
||||
"open3d_small_translation_m": 0.008438605953756149,
|
||||
"open3d_small_rotation_deg": 0.15351222882817242,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 23,
|
||||
"open3d_small_translation_m": 0.0184790436482599,
|
||||
"open3d_small_rotation_deg": 0.5781132894399331,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 24,
|
||||
"open3d_small_translation_m": 0.03128464242771278,
|
||||
"open3d_small_rotation_deg": 0.3921882672789548,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 22,
|
||||
"j": 23,
|
||||
"open3d_small_translation_m": 0.01156826970365892,
|
||||
"open3d_small_rotation_deg": 0.061357836393243825,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 22,
|
||||
"j": 25,
|
||||
"open3d_small_translation_m": 0.04101882834929893,
|
||||
"open3d_small_rotation_deg": 0.420339642399271,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 23,
|
||||
"j": 24,
|
||||
"open3d_small_translation_m": 0.009191025382465435,
|
||||
"open3d_small_rotation_deg": 0.5691342384946491,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 26,
|
||||
"open3d_small_translation_m": 0.02091454005252894,
|
||||
"open3d_small_rotation_deg": 0.4875072590330582,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 27,
|
||||
"open3d_small_translation_m": 0.09632206662156642,
|
||||
"open3d_small_rotation_deg": 0.3699938120965201,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 28,
|
||||
"open3d_small_translation_m": 0.0301381128127144,
|
||||
"open3d_small_rotation_deg": 0.46587172800425497,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 27,
|
||||
"open3d_small_translation_m": 0.01010757207206556,
|
||||
"open3d_small_rotation_deg": 0.4201365669869023,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 28,
|
||||
"open3d_small_translation_m": 0.006243998078352694,
|
||||
"open3d_small_rotation_deg": 1.3246136424341457,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 29,
|
||||
"open3d_small_translation_m": 0.03617825286340023,
|
||||
"open3d_small_rotation_deg": 0.27823666091247784,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 27,
|
||||
"j": 28,
|
||||
"open3d_small_translation_m": 0.008540949499580083,
|
||||
"open3d_small_rotation_deg": 0.3676810414714511,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 27,
|
||||
"j": 29,
|
||||
"open3d_small_translation_m": 0.01399120240175791,
|
||||
"open3d_small_rotation_deg": 0.437290577300882,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 29,
|
||||
"open3d_small_translation_m": 0.12711262123542025,
|
||||
"open3d_small_rotation_deg": 0.9231252582756669,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 30,
|
||||
"open3d_small_translation_m": 0.030113115024945805,
|
||||
"open3d_small_rotation_deg": 0.33608547109429165,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 31,
|
||||
"accepted": false,
|
||||
"reason": "not_in_small_gicp_refined"
|
||||
},
|
||||
{
|
||||
"i": 30,
|
||||
"j": 31,
|
||||
"open3d_small_translation_m": 0.056474627862098614,
|
||||
"open3d_small_rotation_deg": 0.6666837905088073,
|
||||
"accepted": false,
|
||||
"reason": "backend_disagreement"
|
||||
},
|
||||
{
|
||||
"i": 30,
|
||||
"j": 32,
|
||||
"open3d_small_translation_m": 0.0152913302124184,
|
||||
"open3d_small_rotation_deg": 0.3092824993876474,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
},
|
||||
{
|
||||
"i": 31,
|
||||
"j": 32,
|
||||
"accepted": false,
|
||||
"reason": "not_in_small_gicp_refined"
|
||||
},
|
||||
{
|
||||
"i": 31,
|
||||
"j": 33,
|
||||
"open3d_small_translation_m": 0.013286737983556427,
|
||||
"open3d_small_rotation_deg": 0.37286489690414826,
|
||||
"accepted": true,
|
||||
"reason": ""
|
||||
}
|
||||
]
|
||||
}
|
||||
Binary file not shown.
+266
@@ -0,0 +1,266 @@
|
||||
{
|
||||
"schema_version": 1,
|
||||
"success": true,
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
||||
"frames": {
|
||||
"RTK": {
|
||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||
"x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver",
|
||||
"y_axis": "left",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
||||
},
|
||||
"LiDAR": "raw LiDAR sensor frame"
|
||||
},
|
||||
"backend": "consensus",
|
||||
"measured_lidar_extrinsic_used_as_initial": false,
|
||||
"body_heading_offset_used": false,
|
||||
"body_antenna_lever_xy_used": false,
|
||||
"translation_m": [
|
||||
1.6381793500373911,
|
||||
-0.24084479868828831,
|
||||
0.08448123595331278
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.8171674587248069,
|
||||
1.323288118779805,
|
||||
-22.104163317857477
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.004784711987091957,
|
||||
0.012700096588977744,
|
||||
-0.19160273666049008,
|
||||
0.9813787267829084
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.926254197701683,
|
||||
0.37594816689521215,
|
||||
0.026760737062740007,
|
||||
1.6381793500373911
|
||||
],
|
||||
[
|
||||
-0.3761912321127582,
|
||||
0.926530995670823,
|
||||
0.004524482591229066,
|
||||
-0.24084479868828831
|
||||
],
|
||||
[
|
||||
-0.02309368141930373,
|
||||
-0.014257975640431828,
|
||||
0.9996316281556624,
|
||||
0.08448123595331278
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"quality": {
|
||||
"stations": 34,
|
||||
"pairs": 25,
|
||||
"residuals": {
|
||||
"pairs": 25,
|
||||
"translation_m": {
|
||||
"rms": 0.10020667268070801,
|
||||
"median": 0.06249704966098745,
|
||||
"p90": 0.1210855900297508,
|
||||
"p95": 0.12306740757559503,
|
||||
"max": 0.35253378022021187
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.2527941187072538,
|
||||
"median": 0.7469173100535645,
|
||||
"p90": 1.8215127226094046,
|
||||
"p95": 1.973738680622839,
|
||||
"max": 4.325671952515919
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.04167906865173614,
|
||||
"rotation_deg": 0.7044680397451787
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.05434432733915097,
|
||||
"rotation_deg": 0.3949090486960162
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.10468985225969502,
|
||||
"rotation_deg": 0.2966790038612149
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.064702341381345,
|
||||
"rotation_deg": 0.5538785486937366
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.06403624975051968,
|
||||
"rotation_deg": 0.7469173100535645
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.019589736031301614,
|
||||
"rotation_deg": 0.7060778312055135
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.04563086081489213,
|
||||
"rotation_deg": 0.8701331718844921
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.05643960299219297,
|
||||
"rotation_deg": 1.8737737246943913
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.1234266519889254,
|
||||
"rotation_deg": 1.9987299196049513
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.06753558655646176,
|
||||
"rotation_deg": 0.2674085857580904
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.057937366822662255,
|
||||
"rotation_deg": 0.339735283375759
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.03457457850935045,
|
||||
"rotation_deg": 0.5158041285368985
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.06294442058649205,
|
||||
"rotation_deg": 0.6210132605220049
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.04800822576662163,
|
||||
"rotation_deg": 1.7431212194819241
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.06249704966098745,
|
||||
"rotation_deg": 0.8651715760380532
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.12026833019096655,
|
||||
"rotation_deg": 4.325671952515919
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.04916024916487916,
|
||||
"rotation_deg": 0.9331159567430728
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.02802786916783814,
|
||||
"rotation_deg": 0.8755591753060336
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.054001671602238746,
|
||||
"rotation_deg": 0.48777111889991587
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.10122861599246656,
|
||||
"rotation_deg": 0.9367994244405716
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.023164324685695653,
|
||||
"rotation_deg": 0.2827650802657415
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.06461070522327692,
|
||||
"rotation_deg": 1.1522606437088527
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.35253378022021187,
|
||||
"rotation_deg": 0.7805430662091233
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.11095170748867093,
|
||||
"rotation_deg": 0.8561868088481429
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.12163042992227363,
|
||||
"rotation_deg": 0.17586872890763125
|
||||
}
|
||||
]
|
||||
},
|
||||
"weighted_jacobian_condition_number": 7.739413195936781,
|
||||
"linearized_one_sigma": {
|
||||
"translation_m": [
|
||||
0.008936804232414386,
|
||||
0.009199590403303341,
|
||||
0.004827611183429192
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.08042474849565617,
|
||||
0.07825800357808266,
|
||||
0.1694976028248396
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 200,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.004007472116212317,
|
||||
0.00447369016783699,
|
||||
0.003161844455893403,
|
||||
0.10272261218646783,
|
||||
0.09618742156951016,
|
||||
0.14509879375374413
|
||||
],
|
||||
"p025": [
|
||||
1.6309949623104336,
|
||||
-0.2470615833911814,
|
||||
0.07875765037498306,
|
||||
-0.9881676010783537,
|
||||
1.1135179371917805,
|
||||
-22.3851229846878
|
||||
],
|
||||
"p975": [
|
||||
1.6456999445482652,
|
||||
-0.22784916633142768,
|
||||
0.09114084095723367,
|
||||
-0.5808877804701225,
|
||||
1.4821798187931374,
|
||||
-21.82296828752347
|
||||
]
|
||||
}
|
||||
},
|
||||
"z_constraint": {
|
||||
"observable_from_planar_AX_XB": false,
|
||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
||||
"rtk_reference_height_above_ground_m": 0.8535,
|
||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
||||
},
|
||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification"
|
||||
}
|
||||
@@ -0,0 +1,300 @@
|
||||
{
|
||||
"schema_version": 1,
|
||||
"success": true,
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
||||
"frames": {
|
||||
"RTK": {
|
||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||
"x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver",
|
||||
"y_axis": "left",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
||||
},
|
||||
"LiDAR": "raw LiDAR sensor frame"
|
||||
},
|
||||
"backend": "consensus",
|
||||
"measured_lidar_extrinsic_used_as_initial": false,
|
||||
"body_heading_offset_used": false,
|
||||
"body_antenna_lever_xy_used": false,
|
||||
"translation_m": [
|
||||
1.6381793500373911,
|
||||
-0.24084479868828831,
|
||||
0.08448123595331278
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.8171674587248069,
|
||||
1.323288118779805,
|
||||
-22.104163317857477
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.004784711987091957,
|
||||
0.012700096588977744,
|
||||
-0.19160273666049008,
|
||||
0.9813787267829084
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.926254197701683,
|
||||
0.37594816689521215,
|
||||
0.026760737062740007,
|
||||
1.6381793500373911
|
||||
],
|
||||
[
|
||||
-0.3761912321127582,
|
||||
0.926530995670823,
|
||||
0.004524482591229066,
|
||||
-0.24084479868828831
|
||||
],
|
||||
[
|
||||
-0.02309368141930373,
|
||||
-0.014257975640431828,
|
||||
0.9996316281556624,
|
||||
0.08448123595331278
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"quality": {
|
||||
"stations": 34,
|
||||
"pairs": 25,
|
||||
"residuals": {
|
||||
"pairs": 25,
|
||||
"translation_m": {
|
||||
"rms": 0.10020667268070801,
|
||||
"median": 0.06249704966098745,
|
||||
"p90": 0.1210855900297508,
|
||||
"p95": 0.12306740757559503,
|
||||
"max": 0.35253378022021187
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.2527941187072538,
|
||||
"median": 0.7469173100535645,
|
||||
"p90": 1.8215127226094046,
|
||||
"p95": 1.973738680622839,
|
||||
"max": 4.325671952515919
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.04167906865173614,
|
||||
"rotation_deg": 0.7044680397451787
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.05434432733915097,
|
||||
"rotation_deg": 0.3949090486960162
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.10468985225969502,
|
||||
"rotation_deg": 0.2966790038612149
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.064702341381345,
|
||||
"rotation_deg": 0.5538785486937366
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.06403624975051968,
|
||||
"rotation_deg": 0.7469173100535645
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.019589736031301614,
|
||||
"rotation_deg": 0.7060778312055135
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.04563086081489213,
|
||||
"rotation_deg": 0.8701331718844921
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.05643960299219297,
|
||||
"rotation_deg": 1.8737737246943913
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.1234266519889254,
|
||||
"rotation_deg": 1.9987299196049513
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.06753558655646176,
|
||||
"rotation_deg": 0.2674085857580904
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.057937366822662255,
|
||||
"rotation_deg": 0.339735283375759
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.03457457850935045,
|
||||
"rotation_deg": 0.5158041285368985
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.06294442058649205,
|
||||
"rotation_deg": 0.6210132605220049
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.04800822576662163,
|
||||
"rotation_deg": 1.7431212194819241
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.06249704966098745,
|
||||
"rotation_deg": 0.8651715760380532
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.12026833019096655,
|
||||
"rotation_deg": 4.325671952515919
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.04916024916487916,
|
||||
"rotation_deg": 0.9331159567430728
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.02802786916783814,
|
||||
"rotation_deg": 0.8755591753060336
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.054001671602238746,
|
||||
"rotation_deg": 0.48777111889991587
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.10122861599246656,
|
||||
"rotation_deg": 0.9367994244405716
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.023164324685695653,
|
||||
"rotation_deg": 0.2827650802657415
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.06461070522327692,
|
||||
"rotation_deg": 1.1522606437088527
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.35253378022021187,
|
||||
"rotation_deg": 0.7805430662091233
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.11095170748867093,
|
||||
"rotation_deg": 0.8561868088481429
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.12163042992227363,
|
||||
"rotation_deg": 0.17586872890763125
|
||||
}
|
||||
]
|
||||
},
|
||||
"weighted_jacobian_condition_number": 7.739413195936781,
|
||||
"linearized_one_sigma": {
|
||||
"translation_m": [
|
||||
0.008936804232414386,
|
||||
0.009199590403303341,
|
||||
0.004827611183429192
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.08042474849565617,
|
||||
0.07825800357808266,
|
||||
0.1694976028248396
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 200,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.004007472116212317,
|
||||
0.00447369016783699,
|
||||
0.003161844455893403,
|
||||
0.10272261218646783,
|
||||
0.09618742156951016,
|
||||
0.14509879375374413
|
||||
],
|
||||
"p025": [
|
||||
1.6309949623104336,
|
||||
-0.2470615833911814,
|
||||
0.07875765037498306,
|
||||
-0.9881676010783537,
|
||||
1.1135179371917805,
|
||||
-22.3851229846878
|
||||
],
|
||||
"p975": [
|
||||
1.6456999445482652,
|
||||
-0.22784916633142768,
|
||||
0.09114084095723367,
|
||||
-0.5808877804701225,
|
||||
1.4821798187931374,
|
||||
-21.82296828752347
|
||||
]
|
||||
}
|
||||
},
|
||||
"z_constraint": {
|
||||
"observable_from_planar_AX_XB": false,
|
||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
||||
"rtk_reference_height_above_ground_m": 0.8535,
|
||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
||||
},
|
||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
|
||||
"selection": {
|
||||
"recommended": true,
|
||||
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
|
||||
"open3d_vs_small_gicp": {
|
||||
"translation_m": 0.003889255293759414,
|
||||
"rotation_deg": 0.1884307130161592,
|
||||
"delta_matrix_4x4": [
|
||||
[
|
||||
0.9999968771376496,
|
||||
0.0024968749848742764,
|
||||
-0.00010644368722136346,
|
||||
0.0008526003522697501
|
||||
],
|
||||
[
|
||||
-0.0024970968314049877,
|
||||
0.9999945974960792,
|
||||
-0.0021376356258303525,
|
||||
-0.003658904827736509
|
||||
],
|
||||
[
|
||||
0.00010110570323801577,
|
||||
0.0021378947504826257,
|
||||
0.9999977095892133,
|
||||
0.0010058801324768218
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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.6721594124489447,24.171297449440814,0.8061657032755298,0.10961296014103396,6,2.7038608113687213,0.004225163540003575,0.1530449067720668,1.0,True,
|
||||
0,2,2.0412175279332088,80.09074797031303,0.7489394523717702,0.116305716193008,6,3.0720058333957327,0.02073003109723684,0.12418344306819311,1.0,True,
|
||||
0,3,6.30529961936688,79.9158388329924,0.6310283235519265,0.12470979645173097,6,5.235632990817998,0.02154775870989652,0.25399044979598373,1.0,True,
|
||||
1,2,1.2843386040405174,55.9194505208722,0.7867383512544803,0.11079021393934946,6,3.282529873989144,0.00768232673944143,0.0508043300468843,1.0,True,
|
||||
1,3,5.433888607887495,55.744541383551606,0.6794562317367552,0.11907169138096609,6,4.183386002322132,0.024832409186708038,0.29364088503444125,1.0,True,
|
||||
1,4,1.5299424710613851,106.08652205569952,0.6786112833230006,0.1125501582315264,6,3.2941312581877567,0.0077657602443488094,0.07274574571529673,1.0,True,
|
||||
2,3,4.340252832276203,0.1749091373206093,0.7586776859504132,0.11541610277862546,6,3.730803369806122,0.007879904085790266,0.11659483159927261,1.0,True,
|
||||
2,4,0.2520257253555564,50.1670715348273,0.7854572527608884,0.1098649389241602,6,2.641287751481567,0.017042953274276868,0.09383247792992644,1.0,True,
|
||||
2,5,5.8286926576588955,8.735318060700322,0.7074574574574575,0.11912871456224486,6,3.8527093190832513,0.016640250279170064,0.24773491621516538,1.0,True,
|
||||
3,4,4.1070657333447205,50.34198067214791,0.6974624291697462,0.11727630888949149,6,3.5010457716923216,0.0121487212194689,0.20793086843827258,1.0,True,
|
||||
3,5,2.2886212019715484,8.910227198020936,0.7962985964476462,0.10646082199215787,6,3.037745853837991,0.011008298723512349,0.0821707761041294,1.0,True,
|
||||
3,6,2.618668779147775,47.63555775102663,0.8376509054325956,0.10956583416752531,6,3.1930663579156175,0.001222821038315667,0.092075215117626,1.0,True,
|
||||
4,5,5.5767898078953735,41.431753474126985,0.6652516676773802,0.12322094568315027,6,4.985227704290953,0.005399786589871835,0.13893507775898076,0.0,False,multistart_instability
|
||||
4,6,6.68811709459501,97.97753842317455,0.04910385465259023,0.15664415071522125,6,4.835889397197473,2.574198069897065,12.647326063758534,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
4,7,6.153247042777442,123.28910472998353,0.020756115641215715,0.16997351387143192,6,17.054474046975617,5.52282870653845,6.186988956437115,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
5,6,2.0562758992403367,56.545784949047565,0.7526921648718901,0.10924852668609375,6,3.289802074469562,0.012374747586500019,0.14461856068328793,1.0,True,
|
||||
5,7,0.5812996709001959,81.85735125585653,0.7833561729164071,0.11683079277948678,6,2.8807765869032624,0.013689874812447942,0.20006562776472953,1.0,True,
|
||||
5,8,2.6887856969568644,172.47951556359513,0.3979730564825114,0.13180114830799514,6,4.4867592164383865,2.902311115345869,2.1073169352638135,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
6,7,1.9723544820714844,25.311566306808967,0.8497729566094854,0.10415909908074775,6,3.3759361297847534,0.008986805974950147,0.13099479506412062,1.0,True,
|
||||
6,8,0.7288530799256238,115.93373061454484,0.7678928928928929,0.1116672507978127,6,3.260920573387359,0.010085391730567652,0.1175283699615622,1.0,True,
|
||||
6,9,7.898758206937296,103.90638727582184,0.6071384156199477,0.12594282886521943,6,6.882498483502542,2.7065494720876333,9.167174886516003,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
7,8,2.6747845281541447,90.62216430773587,0.8299748110831234,0.10748525688830211,6,3.0071179226782405,0.00710646197885058,0.1069872847368705,1.0,True,
|
||||
7,9,8.06955581661561,78.59482096901287,0.6188509200150206,0.12713205078393502,6,6.716979637303372,6.104542218165768,6.596313811797356,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
7,10,8.088675434881791,134.6003195530505,0.04729478766868887,0.16509079956796627,6,8.903210909129099,5.698690438550839,24.43813017288333,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
8,9,7.73033999229505,12.027343338722995,0.6326834719980131,0.12084265385763364,6,5.53968988049318,2.4349995045990127,17.158034002952295,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
8,10,8.378411682322204,43.97815524531458,0.6163861933423412,0.1293602846396598,6,4.789005902863083,0.011327540721531722,0.17059480981566605,0.0,False,multistart_instability
|
||||
8,11,11.214115106391473,22.60670813095403,0.035782503501846426,0.17440177580299876,6,9.744579276034784,1.861364410474017,5.099464012971126,1.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
||||
9,10,1.9264207261313253,56.00549858403757,0.6543345543345543,0.10729272360686744,6,3.3788563349731624,1.6360232143180424,5.55466982065639,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
9,11,4.747620352849464,34.63405146967702,0.5818780055682106,0.1171768781510077,6,3.555986532172078,0.00981302583568424,0.06035460198954135,1.0,True,
|
||||
9,12,7.208566899019793,16.356227217122623,0.5379123584441162,0.12645695585785705,6,4.882558096197047,0.008862930161051686,0.11566409877698092,1.0,True,
|
||||
10,11,3.0731501091601263,21.371447114360553,0.7118898623279099,0.1192865608074921,6,3.220203471557625,0.0029529340190141227,0.004176908093440082,1.0,True,
|
||||
10,12,6.011368280631378,39.649271366914945,0.6120311738918656,0.12525652128126136,6,4.691686416856208,0.005686910809247203,0.10204044727299268,1.0,True,
|
||||
10,13,9.76425648692991,72.41150002956132,0.516551290119572,0.1354903305714048,6,7.346543684414896,0.013565042721589003,0.32552304340992394,1.0,True,
|
||||
11,12,3.3258193587172027,18.277824252554396,0.650555275113579,0.12104882943540898,6,4.612247786063477,0.003583199374507776,0.03300207707174117,1.0,True,
|
||||
11,13,7.195203402213438,51.04005291520078,0.5698054068172914,0.1285866274322162,6,7.539783468898048,0.016001435752898144,0.11059625579950419,1.0,True,
|
||||
11,14,3.634158560526847,20.548768889074672,0.6420881321982974,0.12414062948335593,6,4.952265047266808,0.012369101516230316,0.01870506040605898,1.0,True,
|
||||
12,13,3.8697507070400543,32.762228662646386,0.6972966112450819,0.1168089621311632,6,4.28224799374136,0.01141023876783206,0.04779713993428384,1.0,True,
|
||||
12,14,0.9871080784265185,2.2709446365202766,0.8749086479902558,0.09521299965540625,6,3.309695139564422,0.006159472215773367,0.014929948455569534,1.0,True,
|
||||
12,15,4.171948395051693,25.86371030092923,0.7022030893897189,0.11797995277580106,6,4.277232786772136,0.008495008365046321,0.10278299144703042,1.0,True,
|
||||
13,14,3.7992314627329202,30.491284026126113,0.6955810147299509,0.11455956122705321,6,3.350289886810734,0.01022866463344607,0.03515966054944392,1.0,True,
|
||||
13,15,0.9105848166450461,6.898518361717151,0.868300353819945,0.1045421356808481,6,3.2437357133954023,0.002375025382773949,0.01072504764419793,1.0,True,
|
||||
13,16,3.4957081323467,18.94489979461447,0.7265456392027422,0.1096252966406241,6,3.5227514244456217,0.008958927594996346,0.03041438512426757,1.0,True,
|
||||
14,15,3.8816793634199405,23.592765664408958,0.7120070334086913,0.11868441290330703,6,4.62059246950246,0.002571958018980028,0.055069197511519646,1.0,True,
|
||||
14,16,7.29101265002769,49.43618382074057,0.5918615984405458,0.12229328437386515,6,7.149509813179227,0.014273957859025563,0.25325650727957555,1.0,True,
|
||||
14,17,5.915950814087913,0.8461207481731609,0.6694009445687298,0.12443900216431925,6,5.1577414296960065,0.0178952010004604,0.10920228290609924,1.0,True,
|
||||
15,16,3.579287497246505,25.843418156331627,0.702887537993921,0.11495230769293859,6,3.5402899763525375,0.013545291843396808,0.03346625178333291,1.0,True,
|
||||
15,17,2.2400625117908257,24.438886412582114,0.7429531936901991,0.11679524427533863,6,3.5266643942801554,0.009894110027911674,0.07786907370565768,1.0,True,
|
||||
15,18,4.6956742726068,3.452521908779405,0.7209645010046886,0.11716134583909138,6,4.125231895423439,0.01165472931184747,0.13683586564190106,1.0,True,
|
||||
16,17,2.956793995513645,50.28230456891372,0.618922305764411,0.11254196340940027,6,4.068632188828398,0.031047860213503222,0.10375098145236057,1.0,True,
|
||||
16,18,3.369822545690391,22.390896247552213,0.6890156918687589,0.11084024896736888,6,4.421167108842175,0.01556225520373068,0.02495881796885156,1.0,True,
|
||||
16,19,2.313038703742191,30.035090485266096,0.8685060899826,0.10135543575024479,6,2.9200722330018793,0.0029522513838272538,0.02753369989558306,1.0,True,
|
||||
17,18,2.517968959880004,27.89140832136152,0.7697708305735859,0.10648049893472189,6,3.792822356453168,0.010582276181446961,0.03959905194910384,1.0,True,
|
||||
17,19,3.518310045065406,80.31739505417983,0.6293759512937596,0.10954497717502036,6,3.648306929393189,0.012240937390578075,0.06030618467886912,1.0,True,
|
||||
17,20,3.4199679241812992,152.98392843416642,0.6014520938674964,0.12300562605352787,6,4.719447385686111,0.03231013197809958,0.12856862709639913,0.0,False,multistart_instability
|
||||
18,19,2.0416624616211574,52.42598673281832,0.6827314510833881,0.10834806615100022,6,3.5073857685652805,0.012418496053909043,0.11905018881087433,1.0,True,
|
||||
18,20,5.836864764777489,125.0925201128049,0.5756313809779688,0.1265759478434111,6,5.389095141151799,0.012917057356044254,0.2404237301134517,0.0,False,multistart_instability
|
||||
18,21,10.84206419743439,175.70238585457497,0.2352252017703723,0.15182541744787395,6,9.65894418804011,0.2039286327273425,0.13690658217533308,0.0,False,heldout_inlier_ratio;forward_reverse_translation;multistart_instability
|
||||
19,20,6.120105723142265,72.6665333799865,0.6234734541714874,0.1277530580405749,6,5.958603794025071,0.009237066767190974,0.22540488888103255,1.0,True,
|
||||
19,21,11.201089261967727,123.27639912174082,0.3788200074840963,0.1468780339612994,6,10.170069582593085,4.607025297377655,2.696881779819613,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
19,22,13.962230939038237,128.85294276465584,0.17798277982779828,0.15985830060250797,6,11.806596815871064,2.332292389085354,2.4044448240559984,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
20,21,5.091648376409225,50.60986574175432,0.6596992097884272,0.12147955429086157,6,4.082032735955382,0.014725537493639118,0.19884224722867958,1.0,True,
|
||||
20,22,7.842914217245693,56.186409384669375,0.5739414499308958,0.13255415786946775,6,5.3284863413522086,0.006040617548523686,0.19706632354686843,1.0,True,
|
||||
20,23,4.953146569484805,70.79178325235414,0.6456945156330087,0.12075756038041646,6,3.9642702950866346,0.02294901101348451,0.19112109479244785,1.0,True,
|
||||
21,22,2.836151129858731,5.576543642915048,0.7716237647919971,0.1163227135854156,6,2.7962015303291894,0.009963578798274064,0.1533289219532281,1.0,True,
|
||||
21,23,1.4635995041292513,20.181917510599828,0.8576224819696593,0.09865676260631218,6,3.036795069384516,0.00392257332509571,0.00898487649366286,1.0,True,
|
||||
21,24,2.7001854883863183,54.59467208208592,0.7715940569126165,0.11361504553552869,6,2.7681582493333527,0.007475673605592584,0.04227769001294981,1.0,True,
|
||||
22,23,3.806551883906871,14.605373867684776,0.7396689147762109,0.11702962848624102,6,3.414804725088913,0.018140159434305195,0.1184732181610567,1.0,True,
|
||||
22,24,4.999470711928798,49.01812843917085,0.6983240223463687,0.11898873157361631,6,3.633465593686572,1.9214516862996405,12.118772315210817,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
22,25,2.281002791409386,12.391903814042275,0.736861094407697,0.1158639471011007,6,2.383007054117406,0.01855955252477569,0.06452602797902846,1.0,True,
|
||||
23,24,1.2663665558774873,34.41275457148608,0.7853164556962026,0.11287254423109305,6,2.4099218288471635,0.002195759849349955,0.03211295915781923,1.0,True,
|
||||
23,25,5.050865141821003,2.213470053642503,0.6881127450980392,0.12303206700403985,6,2.936372721803798,0.004939188019097873,0.12964064637099942,1.0,True,
|
||||
23,26,5.595299803053147,40.730927532824325,0.6852618757612667,0.12040544972155913,6,3.059006509397719,0.006032445250251169,0.14039335223022864,1.0,True,
|
||||
24,25,6.316196707646632,36.626224625128586,0.677667493796526,0.1234631580581415,6,3.6286524357748364,0.023479226891170584,0.16273859629355136,1.0,True,
|
||||
24,26,6.840027061556236,6.318172961338242,0.6530209617755857,0.12710147612984235,6,3.5331859775372023,0.013311199903813952,0.18193579850150715,1.0,True,
|
||||
24,27,7.477875812552711,31.60254826458195,0.6649014778325123,0.12481159614427853,6,4.10006355979974,0.01787282120544869,0.1680556511623414,1.0,True,
|
||||
25,26,1.433673687807028,42.944397586466835,0.9127837514934289,0.09581429165893823,6,2.9017658682436958,0.0035341488401767693,0.018440169300164816,1.0,True,
|
||||
25,27,1.973107678535245,68.22877288971054,0.8510739856801909,0.10418150333394123,6,2.3682534170899983,0.006103232696003387,0.13366555345654282,1.0,True,
|
||||
25,28,2.578633669986193,88.09106909546726,0.8853518429870751,0.10761251229651997,6,2.6372860976794636,0.0034832316659127254,0.03697744201993421,1.0,True,
|
||||
26,27,0.6711664435459649,25.284375303243706,0.9183867141162515,0.09074909570650827,6,2.8142315359282533,0.00040199505815399084,0.011554314408123986,1.0,True,
|
||||
26,28,1.202247282991071,45.146671509000434,0.8853200095170116,0.1060418493965508,6,2.65640202778952,0.007320240476184076,0.12930335868606002,1.0,True,
|
||||
26,29,0.8313560685788559,87.41573662125148,0.7880466815984911,0.10871274240898261,6,3.111957466886598,0.006395243061058376,0.039517035902872893,1.0,True,
|
||||
27,28,0.6147316450225401,19.862296205756735,0.9289448669201521,0.08505095515326752,6,2.9632528684698194,0.005157655135593023,0.02266680787731125,1.0,True,
|
||||
27,29,0.7540837235696874,62.13136131800778,0.7872365477452019,0.1045551346483567,6,2.9621627623005304,0.011650639607012715,0.16247191340775555,1.0,True,
|
||||
27,30,5.07252652550922,152.0939255621477,0.03871268656716418,0.16956979514477974,6,145.85449900829832,4.5477385995336626,37.97511303619642,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
28,29,1.3242039147826599,42.26906511225104,0.8000944621560987,0.10865475473581254,6,3.146622410005858,0.001139416496730335,0.02119627748722763,1.0,True,
|
||||
28,30,5.091487440029177,132.23162935639098,0.7721000935453695,0.10815845248211022,6,2.292141242686861,0.004462522533166182,0.03583195432667579,1.0,True,
|
||||
28,31,6.538757407908195,158.84212980112872,0.7465330381074466,0.10669666534392452,6,2.40259836682247,0.0063576490595817345,0.04001661521442668,1.0,True,
|
||||
29,30,4.767831371266539,89.96256424413991,0.7248812145092132,0.10870831469083345,6,2.947122380473709,0.004656385377159087,0.12465763189228557,0.0,False,multistart_instability
|
||||
29,31,5.715598450796842,116.57306468887764,0.7243012243012243,0.11138751941762652,6,2.7645021322697088,0.0042639063218924715,0.04663739608639213,0.0,False,multistart_instability
|
||||
29,32,5.281749147864957,159.39493219688646,0.32491640724086246,0.14745892800230104,6,2.8081088899077167,4.8118414680497805,1.8382424276135385,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
30,31,2.604154101624297,26.610500444737717,0.8185562292643862,0.09615795732951272,6,2.899558208444641,0.0049991634555749285,0.021384795873435915,1.0,True,
|
||||
30,32,1.69105635082049,69.43236795274656,0.8041343079031521,0.09953343153684206,6,2.614616874224757,0.0031713764951448154,0.023767377748422268,1.0,True,
|
||||
30,33,3.2411277385703925,160.7145717128097,0.05469213429825602,0.15997514255683,6,6.323554164510002,5.32763716297551,13.705863888332022,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
31,32,0.9137201114724802,42.82186750800884,0.8157085941946499,0.09925944770258255,6,3.048541140076824,0.004719596234885736,0.06146741991683861,1.0,True,
|
||||
31,33,1.4965271484681508,134.10407126807198,0.760345235280208,0.09698073659477859,6,2.550150787416455,0.0019366659831763946,0.026538404789073603,1.0,True,
|
||||
32,33,1.9169426499676907,91.28220376006315,0.7569928006609229,0.10008740880870787,6,3.6611044282667184,2.9881933505092046,2.6964434945984053,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
|
Binary file not shown.
+889
@@ -0,0 +1,889 @@
|
||||
{
|
||||
"selection_is_X_independent": true,
|
||||
"criteria": {
|
||||
"min_inlier_ratio": 0.7,
|
||||
"max_inlier_rmse_m": 0.13,
|
||||
"max_rotation_invariant_error_deg": 0.75,
|
||||
"reverse_translation_tolerance_m": 0.05,
|
||||
"reverse_rotation_tolerance_deg": 0.5
|
||||
},
|
||||
"input_pairs": 73,
|
||||
"accepted_pairs": 41,
|
||||
"pairs": [
|
||||
{
|
||||
"i": 0,
|
||||
"j": 1,
|
||||
"heldout_inlier_ratio": 0.8061657032755298,
|
||||
"heldout_inlier_rmse_m": 0.10961296014103396,
|
||||
"rotation_invariant_error_deg": 0.021953694767642418,
|
||||
"reverse_translation_m": 0.004225163540003575,
|
||||
"reverse_rotation_deg": 0.1530449067720668,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 0,
|
||||
"j": 2,
|
||||
"heldout_inlier_ratio": 0.7489394523717702,
|
||||
"heldout_inlier_rmse_m": 0.116305716193008,
|
||||
"rotation_invariant_error_deg": 0.672251363597141,
|
||||
"reverse_translation_m": 0.02073003109723684,
|
||||
"reverse_rotation_deg": 0.12418344306819311,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 0,
|
||||
"j": 3,
|
||||
"heldout_inlier_ratio": 0.6310283235519265,
|
||||
"heldout_inlier_rmse_m": 0.12470979645173097,
|
||||
"rotation_invariant_error_deg": 0.3195502906704917,
|
||||
"reverse_translation_m": 0.02154775870989652,
|
||||
"reverse_rotation_deg": 0.25399044979598373,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 1,
|
||||
"j": 2,
|
||||
"heldout_inlier_ratio": 0.7867383512544803,
|
||||
"heldout_inlier_rmse_m": 0.11079021393934946,
|
||||
"rotation_invariant_error_deg": 0.6532823847336218,
|
||||
"reverse_translation_m": 0.00768232673944143,
|
||||
"reverse_rotation_deg": 0.0508043300468843,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 1,
|
||||
"j": 3,
|
||||
"heldout_inlier_ratio": 0.6794562317367552,
|
||||
"heldout_inlier_rmse_m": 0.11907169138096609,
|
||||
"rotation_invariant_error_deg": 0.28968149037613955,
|
||||
"reverse_translation_m": 0.024832409186708038,
|
||||
"reverse_rotation_deg": 0.29364088503444125,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 1,
|
||||
"j": 4,
|
||||
"heldout_inlier_ratio": 0.6786112833230006,
|
||||
"heldout_inlier_rmse_m": 0.1125501582315264,
|
||||
"rotation_invariant_error_deg": 0.23509206217087808,
|
||||
"reverse_translation_m": 0.0077657602443488094,
|
||||
"reverse_rotation_deg": 0.07274574571529673,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 2,
|
||||
"j": 3,
|
||||
"heldout_inlier_ratio": 0.7586776859504132,
|
||||
"heldout_inlier_rmse_m": 0.11541610277862546,
|
||||
"rotation_invariant_error_deg": 0.06385020919199427,
|
||||
"reverse_translation_m": 0.007879904085790266,
|
||||
"reverse_rotation_deg": 0.11659483159927261,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 2,
|
||||
"j": 4,
|
||||
"heldout_inlier_ratio": 0.7854572527608884,
|
||||
"heldout_inlier_rmse_m": 0.1098649389241602,
|
||||
"rotation_invariant_error_deg": 0.8714733340459802,
|
||||
"reverse_translation_m": 0.017042953274276868,
|
||||
"reverse_rotation_deg": 0.09383247792992644,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 2,
|
||||
"j": 5,
|
||||
"heldout_inlier_ratio": 0.7074574574574575,
|
||||
"heldout_inlier_rmse_m": 0.11912871456224486,
|
||||
"rotation_invariant_error_deg": 0.18237928888532906,
|
||||
"reverse_translation_m": 0.016640250279170064,
|
||||
"reverse_rotation_deg": 0.24773491621516538,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 3,
|
||||
"j": 4,
|
||||
"heldout_inlier_ratio": 0.6974624291697462,
|
||||
"heldout_inlier_rmse_m": 0.11727630888949149,
|
||||
"rotation_invariant_error_deg": 0.4902436735823201,
|
||||
"reverse_translation_m": 0.0121487212194689,
|
||||
"reverse_rotation_deg": 0.20793086843827258,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 3,
|
||||
"j": 5,
|
||||
"heldout_inlier_ratio": 0.7962985964476462,
|
||||
"heldout_inlier_rmse_m": 0.10646082199215787,
|
||||
"rotation_invariant_error_deg": 0.5522737825084345,
|
||||
"reverse_translation_m": 0.011008298723512349,
|
||||
"reverse_rotation_deg": 0.0821707761041294,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 3,
|
||||
"j": 6,
|
||||
"heldout_inlier_ratio": 0.8376509054325956,
|
||||
"heldout_inlier_rmse_m": 0.10956583416752531,
|
||||
"rotation_invariant_error_deg": 0.24026261153986894,
|
||||
"reverse_translation_m": 0.001222821038315667,
|
||||
"reverse_rotation_deg": 0.092075215117626,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 5,
|
||||
"j": 6,
|
||||
"heldout_inlier_ratio": 0.7526921648718901,
|
||||
"heldout_inlier_rmse_m": 0.10924852668609375,
|
||||
"rotation_invariant_error_deg": 0.8049600839827491,
|
||||
"reverse_translation_m": 0.012374747586500019,
|
||||
"reverse_rotation_deg": 0.14461856068328793,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 5,
|
||||
"j": 7,
|
||||
"heldout_inlier_ratio": 0.7833561729164071,
|
||||
"heldout_inlier_rmse_m": 0.11683079277948678,
|
||||
"rotation_invariant_error_deg": 0.7937418989476015,
|
||||
"reverse_translation_m": 0.013689874812447942,
|
||||
"reverse_rotation_deg": 0.20006562776472953,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 6,
|
||||
"j": 7,
|
||||
"heldout_inlier_ratio": 0.8497729566094854,
|
||||
"heldout_inlier_rmse_m": 0.10415909908074775,
|
||||
"rotation_invariant_error_deg": 0.026617733421641532,
|
||||
"reverse_translation_m": 0.008986805974950147,
|
||||
"reverse_rotation_deg": 0.13099479506412062,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 6,
|
||||
"j": 8,
|
||||
"heldout_inlier_ratio": 0.7678928928928929,
|
||||
"heldout_inlier_rmse_m": 0.1116672507978127,
|
||||
"rotation_invariant_error_deg": 0.32815442351946444,
|
||||
"reverse_translation_m": 0.010085391730567652,
|
||||
"reverse_rotation_deg": 0.1175283699615622,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 7,
|
||||
"j": 8,
|
||||
"heldout_inlier_ratio": 0.8299748110831234,
|
||||
"heldout_inlier_rmse_m": 0.10748525688830211,
|
||||
"rotation_invariant_error_deg": 0.29848794137558343,
|
||||
"reverse_translation_m": 0.00710646197885058,
|
||||
"reverse_rotation_deg": 0.1069872847368705,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 9,
|
||||
"j": 11,
|
||||
"heldout_inlier_ratio": 0.5818780055682106,
|
||||
"heldout_inlier_rmse_m": 0.1171768781510077,
|
||||
"rotation_invariant_error_deg": 0.2635280280577561,
|
||||
"reverse_translation_m": 0.00981302583568424,
|
||||
"reverse_rotation_deg": 0.06035460198954135,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 9,
|
||||
"j": 12,
|
||||
"heldout_inlier_ratio": 0.5379123584441162,
|
||||
"heldout_inlier_rmse_m": 0.12645695585785705,
|
||||
"rotation_invariant_error_deg": 0.5448949372769505,
|
||||
"reverse_translation_m": 0.008862930161051686,
|
||||
"reverse_rotation_deg": 0.11566409877698092,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 10,
|
||||
"j": 11,
|
||||
"heldout_inlier_ratio": 0.7118898623279099,
|
||||
"heldout_inlier_rmse_m": 0.1192865608074921,
|
||||
"rotation_invariant_error_deg": 0.037876295004018345,
|
||||
"reverse_translation_m": 0.0029529340190141227,
|
||||
"reverse_rotation_deg": 0.004176908093440082,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 10,
|
||||
"j": 12,
|
||||
"heldout_inlier_ratio": 0.6120311738918656,
|
||||
"heldout_inlier_rmse_m": 0.12525652128126136,
|
||||
"rotation_invariant_error_deg": 0.22694248875488654,
|
||||
"reverse_translation_m": 0.005686910809247203,
|
||||
"reverse_rotation_deg": 0.10204044727299268,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 10,
|
||||
"j": 13,
|
||||
"heldout_inlier_ratio": 0.516551290119572,
|
||||
"heldout_inlier_rmse_m": 0.1354903305714048,
|
||||
"rotation_invariant_error_deg": 0.7336960316225287,
|
||||
"reverse_translation_m": 0.013565042721589003,
|
||||
"reverse_rotation_deg": 0.32552304340992394,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"heldout_rmse"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 11,
|
||||
"j": 12,
|
||||
"heldout_inlier_ratio": 0.650555275113579,
|
||||
"heldout_inlier_rmse_m": 0.12104882943540898,
|
||||
"rotation_invariant_error_deg": 0.16828520325654495,
|
||||
"reverse_translation_m": 0.003583199374507776,
|
||||
"reverse_rotation_deg": 0.03300207707174117,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 11,
|
||||
"j": 13,
|
||||
"heldout_inlier_ratio": 0.5698054068172914,
|
||||
"heldout_inlier_rmse_m": 0.1285866274322162,
|
||||
"rotation_invariant_error_deg": 0.7985328447719269,
|
||||
"reverse_translation_m": 0.016001435752898144,
|
||||
"reverse_rotation_deg": 0.11059625579950419,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 11,
|
||||
"j": 14,
|
||||
"heldout_inlier_ratio": 0.6420881321982974,
|
||||
"heldout_inlier_rmse_m": 0.12414062948335593,
|
||||
"rotation_invariant_error_deg": 0.9364144322988963,
|
||||
"reverse_translation_m": 0.012369101516230316,
|
||||
"reverse_rotation_deg": 0.01870506040605898,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 12,
|
||||
"j": 13,
|
||||
"heldout_inlier_ratio": 0.6972966112450819,
|
||||
"heldout_inlier_rmse_m": 0.1168089621311632,
|
||||
"rotation_invariant_error_deg": 0.9873386907551662,
|
||||
"reverse_translation_m": 0.01141023876783206,
|
||||
"reverse_rotation_deg": 0.04779713993428384,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 12,
|
||||
"j": 14,
|
||||
"heldout_inlier_ratio": 0.8749086479902558,
|
||||
"heldout_inlier_rmse_m": 0.09521299965540625,
|
||||
"rotation_invariant_error_deg": 0.7228372421049902,
|
||||
"reverse_translation_m": 0.006159472215773367,
|
||||
"reverse_rotation_deg": 0.014929948455569534,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 12,
|
||||
"j": 15,
|
||||
"heldout_inlier_ratio": 0.7022030893897189,
|
||||
"heldout_inlier_rmse_m": 0.11797995277580106,
|
||||
"rotation_invariant_error_deg": 0.34088274652411243,
|
||||
"reverse_translation_m": 0.008495008365046321,
|
||||
"reverse_rotation_deg": 0.10278299144703042,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 13,
|
||||
"j": 14,
|
||||
"heldout_inlier_ratio": 0.6955810147299509,
|
||||
"heldout_inlier_rmse_m": 0.11455956122705321,
|
||||
"rotation_invariant_error_deg": 1.7562842773469676,
|
||||
"reverse_translation_m": 0.01022866463344607,
|
||||
"reverse_rotation_deg": 0.03515966054944392,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 13,
|
||||
"j": 15,
|
||||
"heldout_inlier_ratio": 0.868300353819945,
|
||||
"heldout_inlier_rmse_m": 0.1045421356808481,
|
||||
"rotation_invariant_error_deg": 0.6514660625063202,
|
||||
"reverse_translation_m": 0.002375025382773949,
|
||||
"reverse_rotation_deg": 0.01072504764419793,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 13,
|
||||
"j": 16,
|
||||
"heldout_inlier_ratio": 0.7265456392027422,
|
||||
"heldout_inlier_rmse_m": 0.1096252966406241,
|
||||
"rotation_invariant_error_deg": 0.48470120712195097,
|
||||
"reverse_translation_m": 0.008958927594996346,
|
||||
"reverse_rotation_deg": 0.03041438512426757,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 14,
|
||||
"j": 15,
|
||||
"heldout_inlier_ratio": 0.7120070334086913,
|
||||
"heldout_inlier_rmse_m": 0.11868441290330703,
|
||||
"rotation_invariant_error_deg": 1.1046128571505562,
|
||||
"reverse_translation_m": 0.002571958018980028,
|
||||
"reverse_rotation_deg": 0.055069197511519646,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 14,
|
||||
"j": 16,
|
||||
"heldout_inlier_ratio": 0.5918615984405458,
|
||||
"heldout_inlier_rmse_m": 0.12229328437386515,
|
||||
"rotation_invariant_error_deg": 1.240484202449963,
|
||||
"reverse_translation_m": 0.014273957859025563,
|
||||
"reverse_rotation_deg": 0.25325650727957555,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 14,
|
||||
"j": 17,
|
||||
"heldout_inlier_ratio": 0.6694009445687298,
|
||||
"heldout_inlier_rmse_m": 0.12443900216431925,
|
||||
"rotation_invariant_error_deg": 0.4822313558358869,
|
||||
"reverse_translation_m": 0.0178952010004604,
|
||||
"reverse_rotation_deg": 0.10920228290609924,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 16,
|
||||
"heldout_inlier_ratio": 0.702887537993921,
|
||||
"heldout_inlier_rmse_m": 0.11495230769293859,
|
||||
"rotation_invariant_error_deg": 0.16340661916523302,
|
||||
"reverse_translation_m": 0.013545291843396808,
|
||||
"reverse_rotation_deg": 0.03346625178333291,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 17,
|
||||
"heldout_inlier_ratio": 0.7429531936901991,
|
||||
"heldout_inlier_rmse_m": 0.11679524427533863,
|
||||
"rotation_invariant_error_deg": 0.050851974711708436,
|
||||
"reverse_translation_m": 0.009894110027911674,
|
||||
"reverse_rotation_deg": 0.07786907370565768,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 15,
|
||||
"j": 18,
|
||||
"heldout_inlier_ratio": 0.7209645010046886,
|
||||
"heldout_inlier_rmse_m": 0.11716134583909138,
|
||||
"rotation_invariant_error_deg": 0.26477152026833295,
|
||||
"reverse_translation_m": 0.01165472931184747,
|
||||
"reverse_rotation_deg": 0.13683586564190106,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 16,
|
||||
"j": 17,
|
||||
"heldout_inlier_ratio": 0.618922305764411,
|
||||
"heldout_inlier_rmse_m": 0.11254196340940027,
|
||||
"rotation_invariant_error_deg": 0.05549924909794868,
|
||||
"reverse_translation_m": 0.031047860213503222,
|
||||
"reverse_rotation_deg": 0.10375098145236057,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 16,
|
||||
"j": 18,
|
||||
"heldout_inlier_ratio": 0.6890156918687589,
|
||||
"heldout_inlier_rmse_m": 0.11084024896736888,
|
||||
"rotation_invariant_error_deg": 0.06628387310017914,
|
||||
"reverse_translation_m": 0.01556225520373068,
|
||||
"reverse_rotation_deg": 0.02495881796885156,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 16,
|
||||
"j": 19,
|
||||
"heldout_inlier_ratio": 0.8685060899826,
|
||||
"heldout_inlier_rmse_m": 0.10135543575024479,
|
||||
"rotation_invariant_error_deg": 0.3152201090321931,
|
||||
"reverse_translation_m": 0.0029522513838272538,
|
||||
"reverse_rotation_deg": 0.02753369989558306,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 17,
|
||||
"j": 18,
|
||||
"heldout_inlier_ratio": 0.7697708305735859,
|
||||
"heldout_inlier_rmse_m": 0.10648049893472189,
|
||||
"rotation_invariant_error_deg": 0.2007356143443495,
|
||||
"reverse_translation_m": 0.010582276181446961,
|
||||
"reverse_rotation_deg": 0.03959905194910384,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 17,
|
||||
"j": 19,
|
||||
"heldout_inlier_ratio": 0.6293759512937596,
|
||||
"heldout_inlier_rmse_m": 0.10954497717502036,
|
||||
"rotation_invariant_error_deg": 0.25455213438726787,
|
||||
"reverse_translation_m": 0.012240937390578075,
|
||||
"reverse_rotation_deg": 0.06030618467886912,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 18,
|
||||
"j": 19,
|
||||
"heldout_inlier_ratio": 0.6827314510833881,
|
||||
"heldout_inlier_rmse_m": 0.10834806615100022,
|
||||
"rotation_invariant_error_deg": 0.4543752098998439,
|
||||
"reverse_translation_m": 0.012418496053909043,
|
||||
"reverse_rotation_deg": 0.11905018881087433,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 19,
|
||||
"j": 20,
|
||||
"heldout_inlier_ratio": 0.6234734541714874,
|
||||
"heldout_inlier_rmse_m": 0.1277530580405749,
|
||||
"rotation_invariant_error_deg": 0.038058886117710244,
|
||||
"reverse_translation_m": 0.009237066767190974,
|
||||
"reverse_rotation_deg": 0.22540488888103255,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 20,
|
||||
"j": 21,
|
||||
"heldout_inlier_ratio": 0.6596992097884272,
|
||||
"heldout_inlier_rmse_m": 0.12147955429086157,
|
||||
"rotation_invariant_error_deg": 0.33308699073501913,
|
||||
"reverse_translation_m": 0.014725537493639118,
|
||||
"reverse_rotation_deg": 0.19884224722867958,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 20,
|
||||
"j": 22,
|
||||
"heldout_inlier_ratio": 0.5739414499308958,
|
||||
"heldout_inlier_rmse_m": 0.13255415786946775,
|
||||
"rotation_invariant_error_deg": 0.49226728584466883,
|
||||
"reverse_translation_m": 0.006040617548523686,
|
||||
"reverse_rotation_deg": 0.19706632354686843,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"heldout_rmse"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 20,
|
||||
"j": 23,
|
||||
"heldout_inlier_ratio": 0.6456945156330087,
|
||||
"heldout_inlier_rmse_m": 0.12075756038041646,
|
||||
"rotation_invariant_error_deg": 0.4097857444953803,
|
||||
"reverse_translation_m": 0.02294901101348451,
|
||||
"reverse_rotation_deg": 0.19112109479244785,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 22,
|
||||
"heldout_inlier_ratio": 0.7716237647919971,
|
||||
"heldout_inlier_rmse_m": 0.1163227135854156,
|
||||
"rotation_invariant_error_deg": 0.20299476740399935,
|
||||
"reverse_translation_m": 0.009963578798274064,
|
||||
"reverse_rotation_deg": 0.1533289219532281,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 23,
|
||||
"heldout_inlier_ratio": 0.8576224819696593,
|
||||
"heldout_inlier_rmse_m": 0.09865676260631218,
|
||||
"rotation_invariant_error_deg": 0.08196486868098773,
|
||||
"reverse_translation_m": 0.00392257332509571,
|
||||
"reverse_rotation_deg": 0.00898487649366286,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 21,
|
||||
"j": 24,
|
||||
"heldout_inlier_ratio": 0.7715940569126165,
|
||||
"heldout_inlier_rmse_m": 0.11361504553552869,
|
||||
"rotation_invariant_error_deg": 0.4941752812908575,
|
||||
"reverse_translation_m": 0.007475673605592584,
|
||||
"reverse_rotation_deg": 0.04227769001294981,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 22,
|
||||
"j": 23,
|
||||
"heldout_inlier_ratio": 0.7396689147762109,
|
||||
"heldout_inlier_rmse_m": 0.11702962848624102,
|
||||
"rotation_invariant_error_deg": 0.07221906147914936,
|
||||
"reverse_translation_m": 0.018140159434305195,
|
||||
"reverse_rotation_deg": 0.1184732181610567,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 22,
|
||||
"j": 25,
|
||||
"heldout_inlier_ratio": 0.736861094407697,
|
||||
"heldout_inlier_rmse_m": 0.1158639471011007,
|
||||
"rotation_invariant_error_deg": 0.19913456977445776,
|
||||
"reverse_translation_m": 0.01855955252477569,
|
||||
"reverse_rotation_deg": 0.06452602797902846,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 23,
|
||||
"j": 24,
|
||||
"heldout_inlier_ratio": 0.7853164556962026,
|
||||
"heldout_inlier_rmse_m": 0.11287254423109305,
|
||||
"rotation_invariant_error_deg": 0.5709657470221714,
|
||||
"reverse_translation_m": 0.002195759849349955,
|
||||
"reverse_rotation_deg": 0.03211295915781923,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 23,
|
||||
"j": 25,
|
||||
"heldout_inlier_ratio": 0.6881127450980392,
|
||||
"heldout_inlier_rmse_m": 0.12303206700403985,
|
||||
"rotation_invariant_error_deg": 2.3734594630006685,
|
||||
"reverse_translation_m": 0.004939188019097873,
|
||||
"reverse_rotation_deg": 0.12964064637099942,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio",
|
||||
"rotation_conjugacy_invariant"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 23,
|
||||
"j": 26,
|
||||
"heldout_inlier_ratio": 0.6852618757612667,
|
||||
"heldout_inlier_rmse_m": 0.12040544972155913,
|
||||
"rotation_invariant_error_deg": 0.15748268923838538,
|
||||
"reverse_translation_m": 0.006032445250251169,
|
||||
"reverse_rotation_deg": 0.14039335223022864,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 24,
|
||||
"j": 25,
|
||||
"heldout_inlier_ratio": 0.677667493796526,
|
||||
"heldout_inlier_rmse_m": 0.1234631580581415,
|
||||
"rotation_invariant_error_deg": 0.10001483417664048,
|
||||
"reverse_translation_m": 0.023479226891170584,
|
||||
"reverse_rotation_deg": 0.16273859629355136,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 24,
|
||||
"j": 26,
|
||||
"heldout_inlier_ratio": 0.6530209617755857,
|
||||
"heldout_inlier_rmse_m": 0.12710147612984235,
|
||||
"rotation_invariant_error_deg": 0.7149536911782421,
|
||||
"reverse_translation_m": 0.013311199903813952,
|
||||
"reverse_rotation_deg": 0.18193579850150715,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 24,
|
||||
"j": 27,
|
||||
"heldout_inlier_ratio": 0.6649014778325123,
|
||||
"heldout_inlier_rmse_m": 0.12481159614427853,
|
||||
"rotation_invariant_error_deg": 0.5126580224867077,
|
||||
"reverse_translation_m": 0.01787282120544869,
|
||||
"reverse_rotation_deg": 0.1680556511623414,
|
||||
"accepted": false,
|
||||
"rejection_reasons": [
|
||||
"overlap_ratio"
|
||||
]
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 26,
|
||||
"heldout_inlier_ratio": 0.9127837514934289,
|
||||
"heldout_inlier_rmse_m": 0.09581429165893823,
|
||||
"rotation_invariant_error_deg": 0.0540123863768045,
|
||||
"reverse_translation_m": 0.0035341488401767693,
|
||||
"reverse_rotation_deg": 0.018440169300164816,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 27,
|
||||
"heldout_inlier_ratio": 0.8510739856801909,
|
||||
"heldout_inlier_rmse_m": 0.10418150333394123,
|
||||
"rotation_invariant_error_deg": 0.24124343828050598,
|
||||
"reverse_translation_m": 0.006103232696003387,
|
||||
"reverse_rotation_deg": 0.13366555345654282,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 25,
|
||||
"j": 28,
|
||||
"heldout_inlier_ratio": 0.8853518429870751,
|
||||
"heldout_inlier_rmse_m": 0.10761251229651997,
|
||||
"rotation_invariant_error_deg": 0.31032271091986274,
|
||||
"reverse_translation_m": 0.0034832316659127254,
|
||||
"reverse_rotation_deg": 0.03697744201993421,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 27,
|
||||
"heldout_inlier_ratio": 0.9183867141162515,
|
||||
"heldout_inlier_rmse_m": 0.09074909570650827,
|
||||
"rotation_invariant_error_deg": 0.1549242409220426,
|
||||
"reverse_translation_m": 0.00040199505815399084,
|
||||
"reverse_rotation_deg": 0.011554314408123986,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 28,
|
||||
"heldout_inlier_ratio": 0.8853200095170116,
|
||||
"heldout_inlier_rmse_m": 0.1060418493965508,
|
||||
"rotation_invariant_error_deg": 0.2535175622408232,
|
||||
"reverse_translation_m": 0.007320240476184076,
|
||||
"reverse_rotation_deg": 0.12930335868606002,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 26,
|
||||
"j": 29,
|
||||
"heldout_inlier_ratio": 0.7880466815984911,
|
||||
"heldout_inlier_rmse_m": 0.10871274240898261,
|
||||
"rotation_invariant_error_deg": 0.6273264323501451,
|
||||
"reverse_translation_m": 0.006395243061058376,
|
||||
"reverse_rotation_deg": 0.039517035902872893,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 27,
|
||||
"j": 28,
|
||||
"heldout_inlier_ratio": 0.9289448669201521,
|
||||
"heldout_inlier_rmse_m": 0.08505095515326752,
|
||||
"rotation_invariant_error_deg": 0.06642597183531862,
|
||||
"reverse_translation_m": 0.005157655135593023,
|
||||
"reverse_rotation_deg": 0.02266680787731125,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 27,
|
||||
"j": 29,
|
||||
"heldout_inlier_ratio": 0.7872365477452019,
|
||||
"heldout_inlier_rmse_m": 0.1045551346483567,
|
||||
"rotation_invariant_error_deg": 0.46617350279205994,
|
||||
"reverse_translation_m": 0.011650639607012715,
|
||||
"reverse_rotation_deg": 0.16247191340775555,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 29,
|
||||
"heldout_inlier_ratio": 0.8000944621560987,
|
||||
"heldout_inlier_rmse_m": 0.10865475473581254,
|
||||
"rotation_invariant_error_deg": 0.3806425437120282,
|
||||
"reverse_translation_m": 0.001139416496730335,
|
||||
"reverse_rotation_deg": 0.02119627748722763,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 30,
|
||||
"heldout_inlier_ratio": 0.7721000935453695,
|
||||
"heldout_inlier_rmse_m": 0.10815845248211022,
|
||||
"rotation_invariant_error_deg": 0.37744105649966286,
|
||||
"reverse_translation_m": 0.004462522533166182,
|
||||
"reverse_rotation_deg": 0.03583195432667579,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 28,
|
||||
"j": 31,
|
||||
"heldout_inlier_ratio": 0.7465330381074466,
|
||||
"heldout_inlier_rmse_m": 0.10669666534392452,
|
||||
"rotation_invariant_error_deg": 0.22580828656680296,
|
||||
"reverse_translation_m": 0.0063576490595817345,
|
||||
"reverse_rotation_deg": 0.04001661521442668,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 30,
|
||||
"j": 31,
|
||||
"heldout_inlier_ratio": 0.8185562292643862,
|
||||
"heldout_inlier_rmse_m": 0.09615795732951272,
|
||||
"rotation_invariant_error_deg": 0.608920703223724,
|
||||
"reverse_translation_m": 0.0049991634555749285,
|
||||
"reverse_rotation_deg": 0.021384795873435915,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 30,
|
||||
"j": 32,
|
||||
"heldout_inlier_ratio": 0.8041343079031521,
|
||||
"heldout_inlier_rmse_m": 0.09953343153684206,
|
||||
"rotation_invariant_error_deg": 0.7410181887106262,
|
||||
"reverse_translation_m": 0.0031713764951448154,
|
||||
"reverse_rotation_deg": 0.023767377748422268,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 31,
|
||||
"j": 32,
|
||||
"heldout_inlier_ratio": 0.8157085941946499,
|
||||
"heldout_inlier_rmse_m": 0.09925944770258255,
|
||||
"rotation_invariant_error_deg": 0.1261576421594981,
|
||||
"reverse_translation_m": 0.004719596234885736,
|
||||
"reverse_rotation_deg": 0.06146741991683861,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
},
|
||||
{
|
||||
"i": 31,
|
||||
"j": 33,
|
||||
"heldout_inlier_ratio": 0.760345235280208,
|
||||
"heldout_inlier_rmse_m": 0.09698073659477859,
|
||||
"rotation_invariant_error_deg": 0.11718235773108177,
|
||||
"reverse_translation_m": 0.0019366659831763946,
|
||||
"reverse_rotation_deg": 0.026538404789073603,
|
||||
"accepted": true,
|
||||
"rejection_reasons": []
|
||||
}
|
||||
]
|
||||
}
|
||||
+346
@@ -0,0 +1,346 @@
|
||||
{
|
||||
"schema_version": 1,
|
||||
"success": true,
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
||||
"frames": {
|
||||
"RTK": {
|
||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||
"x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver",
|
||||
"y_axis": "left",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
||||
},
|
||||
"LiDAR": "raw LiDAR sensor frame"
|
||||
},
|
||||
"backend": "open3d_gicp",
|
||||
"measured_lidar_extrinsic_used_as_initial": false,
|
||||
"body_heading_offset_used": false,
|
||||
"body_antenna_lever_xy_used": false,
|
||||
"translation_m": [
|
||||
1.6368478986289665,
|
||||
-0.2424214446279586,
|
||||
0.08408840390370595
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.8529068506201997,
|
||||
1.3285763243955087,
|
||||
-22.150129507458946
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.005076802391609619,
|
||||
0.012807178841486907,
|
||||
-0.19199197181185435,
|
||||
0.9812997936448346
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9259501178598366,
|
||||
0.3766733256085306,
|
||||
0.027084774511975725,
|
||||
1.6368478986289665
|
||||
],
|
||||
[
|
||||
-0.3769334036732196,
|
||||
0.9262266176745453,
|
||||
0.005045979240275979,
|
||||
-0.2424214446279586
|
||||
],
|
||||
[
|
||||
-0.023185953305318648,
|
||||
-0.014881481316772506,
|
||||
0.9996204044951974,
|
||||
0.08408840390370595
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"quality": {
|
||||
"stations": 34,
|
||||
"pairs": 41,
|
||||
"residuals": {
|
||||
"pairs": 41,
|
||||
"translation_m": {
|
||||
"rms": 0.11160362192890017,
|
||||
"median": 0.06165368731012592,
|
||||
"p90": 0.12167778728549836,
|
||||
"p95": 0.12315870952465978,
|
||||
"max": 0.45797693508387505
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.082945057501201,
|
||||
"median": 0.744916302248719,
|
||||
"p90": 1.523342607477641,
|
||||
"p95": 1.864505235125524,
|
||||
"max": 4.332008025818138
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.03696060713715898,
|
||||
"rotation_deg": 0.7820683861381595
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.039028552931951836,
|
||||
"rotation_deg": 0.6949685743890126
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.01732676228513485,
|
||||
"rotation_deg": 1.0438060827136844
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.0574978038017053,
|
||||
"rotation_deg": 0.394946532754576
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.10801668349596734,
|
||||
"rotation_deg": 0.29336064351197005
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.06577881887454434,
|
||||
"rotation_deg": 0.5542732613092561
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.06363940755916121,
|
||||
"rotation_deg": 0.5890545170693674
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.0640679503495012,
|
||||
"rotation_deg": 0.746120063911557
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.021009631897429052,
|
||||
"rotation_deg": 0.6663196620233134
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.08609573827254784,
|
||||
"rotation_deg": 0.3687772861174946
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.02732304440983888,
|
||||
"rotation_deg": 0.5222061823280917
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.04489317903985948,
|
||||
"rotation_deg": 0.8700136243621963
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.07260882213085644,
|
||||
"rotation_deg": 0.463182802330913
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.08831772542234817,
|
||||
"rotation_deg": 0.659009021415606
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.053804778572191785,
|
||||
"rotation_deg": 1.864505235125524
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.12315870952465978,
|
||||
"rotation_deg": 1.9856861904193648
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.06880647181572622,
|
||||
"rotation_deg": 0.27331219092746517
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.059582300078982915,
|
||||
"rotation_deg": 0.3410200031868246
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.034895300030885576,
|
||||
"rotation_deg": 0.5283410202190479
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.002366788176226253,
|
||||
"rotation_deg": 0.3852038090000981
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.06233663305653708,
|
||||
"rotation_deg": 0.6208000110952433
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.012626372544105475,
|
||||
"rotation_deg": 0.38292733656142575
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.047494831014178666,
|
||||
"rotation_deg": 1.7712490998072288
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.06165368731012592,
|
||||
"rotation_deg": 0.8670376638275039
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.12172160687143056,
|
||||
"rotation_deg": 4.332008025818138
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.06185808030991236,
|
||||
"rotation_deg": 1.523342607477641
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.049450564075379046,
|
||||
"rotation_deg": 0.9499016191505171
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.016599648000378425,
|
||||
"rotation_deg": 0.7685476074882036
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.028501035078080036,
|
||||
"rotation_deg": 0.9107236146504971
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.05418062512497841,
|
||||
"rotation_deg": 0.49604339332524483
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.02534272431328254,
|
||||
"rotation_deg": 0.2798196345939902
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.1006162178003358,
|
||||
"rotation_deg": 0.9066113005741254
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.023348232889945957,
|
||||
"rotation_deg": 0.29316028754024664
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.06352851658442724,
|
||||
"rotation_deg": 1.123276625138155
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.06685972566531029,
|
||||
"rotation_deg": 0.7607492800980359
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.3534873096430149,
|
||||
"rotation_deg": 0.744916302248719
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.45797693508387505,
|
||||
"rotation_deg": 0.7684054290628693
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.10414608850682983,
|
||||
"rotation_deg": 0.8325040370564112
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.11058068261532474,
|
||||
"rotation_deg": 0.8695456609442757
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.03405996353456899,
|
||||
"rotation_deg": 0.7913171839743647
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.12167778728549836,
|
||||
"rotation_deg": 0.16247416554150054
|
||||
}
|
||||
]
|
||||
},
|
||||
"weighted_jacobian_condition_number": 7.006062559882809,
|
||||
"linearized_one_sigma": {
|
||||
"translation_m": [
|
||||
0.007129000573825617,
|
||||
0.007213998960440989,
|
||||
0.0046717381822970125
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.06695429523124989,
|
||||
0.06503891988826449,
|
||||
0.14115095959507304
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.002608526149658222,
|
||||
0.0028276716521100863,
|
||||
0.0023368456176917846,
|
||||
0.0795227773764157,
|
||||
0.07300189168975164,
|
||||
0.10947642324382982
|
||||
],
|
||||
"p025": [
|
||||
1.6327841703535608,
|
||||
-0.24653273132861378,
|
||||
0.07991981050403614,
|
||||
-0.9872072161381827,
|
||||
1.1864367848117605,
|
||||
-22.364052444549895
|
||||
],
|
||||
"p975": [
|
||||
1.6431149050874008,
|
||||
-0.2355422823589496,
|
||||
0.08894657837563841,
|
||||
-0.6759368885907463,
|
||||
1.4669998309298398,
|
||||
-21.953029139092944
|
||||
]
|
||||
}
|
||||
},
|
||||
"z_constraint": {
|
||||
"observable_from_planar_AX_XB": false,
|
||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
||||
"rtk_reference_height_above_ground_m": 0.8535,
|
||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
||||
},
|
||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification"
|
||||
}
|
||||
@@ -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.6721594124489447,24.171297449440814,0.8193962748876044,0.11049675306366954,6,14.013392649694936,0.026679665410762447,0.12399936190197167,1.0,True,
|
||||
0,2,2.0412175279332088,80.09074797031303,0.7525388867463684,0.11492533799491883,6,14.962108606929117,0.0018464756299783867,0.03093241101186373,1.0,True,
|
||||
0,3,6.30529961936688,79.9158388329924,0.628093901505486,0.12365050970311502,6,23.490589415548122,0.010524333328230958,0.23898233567764737,0.5,True,
|
||||
0,4,2.238422471863255,130.25781950514033,0.693351593625498,0.11485738628517483,6,16.305124521706706,0.003627491377787396,0.0495829610363469,1.0,True,
|
||||
0,5,7.5269579262553155,88.82606603101335,0.6015065913370998,0.12959859333012305,6,26.27293180169831,0.013710015050868782,0.26025123892797375,1.0,True,
|
||||
1,2,1.2843386040405174,55.9194505208722,0.7981310803891449,0.11167434332282765,6,13.484382276710306,0.0507423684848027,0.517099810570953,1.0,False,forward_reverse_rotation
|
||||
1,3,5.433888607887495,55.744541383551606,0.678820988438572,0.12109867185657658,6,27.478714016210855,0.008324315958294127,0.2568985217684905,1.0,True,
|
||||
1,4,1.5299424710613851,106.08652205569952,0.6772473651580905,0.1115749218038809,6,13.250326799303036,3.109603769112268,6.665689673243039,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
1,5,7.072560501394692,64.65476858157254,0.618779694923731,0.1278696355679149,6,21.376356908960656,0.012044684321696756,0.5185219918090542,1.0,False,forward_reverse_rotation
|
||||
1,6,8.049825623399226,8.10898363252498,0.02911760982402836,0.16814699881028172,6,51.15739724954147,2.3526085660101135,12.54864815902537,0.0,False,backend_not_converged;heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
2,3,4.340252832276203,0.1749091373206093,0.7609663064208518,0.11583878636902087,6,13.299472485717942,0.007842434748069874,0.017111535671962216,1.0,True,
|
||||
2,4,0.2520257253555564,50.1670715348273,0.796748976299789,0.1148434235904791,6,12.041666425070192,0.011210942709506708,0.11462542679279858,1.0,True,
|
||||
2,5,5.8286926576588955,8.735318060700322,0.7112112112112112,0.12001318292058727,6,15.618628103418056,0.011299298862678088,0.02158353743310138,1.0,True,
|
||||
2,6,6.928094074716812,47.810466888347236,0.058659571772456606,0.1689308471089219,6,22.346184538451386,2.376212271528816,4.191297741726165,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
2,7,6.405228405426323,73.1220331951562,0.05777324320877439,0.16763553481687163,6,11.228331216247241,2.3764975144091216,1.7142235592052535,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
3,4,4.1070657333447205,50.34198067214791,0.6957378664695738,0.11619002437046365,6,17.76967737811838,3.222808271854619,15.590678196925943,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
3,5,2.2886212019715484,8.910227198020936,0.7989069680784996,0.10775905778143813,6,12.151555874812614,0.013532537604982006,0.06485728954506689,1.0,True,
|
||||
3,6,2.618668779147775,47.63555775102663,0.8435613682092555,0.11222309863990189,6,13.914901775514537,0.00951276519885504,0.09743636874086448,1.0,True,
|
||||
3,7,2.7963089245830335,72.9471240578356,0.8651898734177215,0.11184568072686091,6,12.975777248765162,0.010827690226297664,0.12520280777371842,1.0,True,
|
||||
3,8,2.6983089912812726,163.5692883655715,0.825590155700653,0.1078798726707026,6,12.959410142765158,0.005608807919239624,0.021536601402144962,1.0,True,
|
||||
4,5,5.5767898078953735,41.431753474126985,0.6652516676773802,0.12444359189600171,6,17.028714587649738,0.0031552835887398907,0.05824661275042354,1.0,True,
|
||||
4,6,6.68811709459501,97.97753842317455,0.03891480481217775,0.16130442983218543,6,53.12725754116488,5.386834276571879,3.248443974085464,1.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
||||
4,7,6.153247042777442,123.28910472998353,0.01717321472695824,0.16597934102353687,6,197.50040966862915,1.8946664283973234,13.38395621317346,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
4,8,6.802787549686365,146.08873096228075,0.7129198332924737,0.11349036082807593,6,18.616925126379055,4.094017521116411,1.0439112418969816,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
4,9,3.018117089902297,158.11607430100386,0.3323991714390155,0.13218238008205907,6,82.91588951856485,0.17404817666776692,0.7491904391974432,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
5,6,2.0562758992403367,56.545784949047565,0.7604901596732269,0.11101745020162715,6,16.790595774247244,0.01028831511886355,0.031120237309440944,1.0,True,
|
||||
5,7,0.5812996709001959,81.85735125585653,0.8092687180764918,0.10777871969807898,6,15.203386410549202,0.010579733674272045,0.033334626234333836,1.0,True,
|
||||
5,8,2.6887856969568644,172.47951556359513,0.40909652700531457,0.13021927164196687,6,88.03642906190348,2.282274682790372,1.1841285568948536,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
5,9,7.501939677383991,160.45217222486954,0.3361179361179361,0.13367797847230906,6,83.15270070161475,5.255898884183195,7.177742907146106,0.5,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
5,10,7.507648391453363,143.54232919109307,0.5442391832766165,0.13173165083201846,6,26.337222871823244,4.175333583746659,14.054946454556381,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
6,7,1.9723544820714844,25.311566306808967,0.8539354187689203,0.10462253085152279,6,12.50291076661203,0.0028433142784691904,0.028424505435071433,1.0,True,
|
||||
6,8,0.7288530799256238,115.93373061454484,0.7757757757757757,0.11316400028465075,6,15.521747346102574,0.007224674181692249,0.10395594559619384,1.0,True,
|
||||
6,9,7.898758206937296,103.90638727582184,0.6009202835468226,0.1259448851291812,6,28.795189977892598,4.270523553799638,1.1342776748452013,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
||||
6,10,8.382165572419371,159.91188585985944,0.048837495386886455,0.16976022756819517,6,33.03889916791793,8.842801960353667,9.008466157678757,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
6,11,11.12084001821623,138.54043874549885,0.026144624410151765,0.1725705028585339,6,71.57462790292871,2.4153309235424008,10.273490365819672,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
7,8,2.6747845281541447,90.62216430773587,0.8340050377833753,0.11132245152738934,6,16.17035077702852,0.004806949653405576,0.020340398656013788,1.0,True,
|
||||
7,9,8.06955581661561,78.59482096901287,0.6160971335586432,0.12637042722943573,6,24.178664977065605,4.367245914434154,12.796681469446304,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
7,10,8.088675434881791,134.6003195530505,0.04890429614956048,0.17569950505457485,6,17.12714594442953,9.4426170895353,17.952544168886714,0.0,False,backend_not_converged;heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
7,11,10.488793105910844,113.2288724386899,0.03723199383746309,0.16334679446297104,6,88.61274561425562,7.5280305092863244,55.909263646297305,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
7,12,13.79542539595065,94.95104818613552,0.023342903507676944,0.1706723033660903,6,71.17833596146563,5.060797550499477,27.755426788516992,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
8,9,7.73033999229505,12.027343338722995,0.6407549981373402,0.12189583104066957,6,28.06713348388492,2.3076573667891433,3.487290408239611,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
8,10,8.378411682322204,43.97815524531458,0.6075420709986488,0.12933255488441212,6,20.429633989572718,4.865121579871581,2.927465091074779,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
8,11,11.214115106391473,22.60670813095403,0.03922067999490641,0.16246238376196148,6,54.700772476792416,2.594876837596059,7.9735312612345846,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
8,12,14.373410961212507,4.328883878399634,0.023529411764705882,0.1707990278738782,6,65.92814703722017,0.7170106443431138,1.1046762967347212,0.5,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
||||
8,13,18.141079267476005,28.43334478424675,0.020491803278688523,0.1759614106055459,6,128.98300559552638,1.9608884223852157,1.569223825596656,0.0,False,backend_not_converged;heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
9,10,1.9264207261313253,56.00549858403757,0.6576312576312576,0.1069384415347781,6,15.95767235456931,1.6379095873833018,4.808769928108965,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
9,11,4.747620352849464,34.63405146967702,0.5883320678309288,0.11815838246331258,6,21.526672921842795,2.059598786777497,11.82652375960602,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
9,12,7.208566899019793,16.356227217122623,0.5413589364844904,0.12469816852190199,6,38.03547459177405,1.1412129496099401,4.794166723137469,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
9,13,10.706998136562337,16.406001445523756,0.015145729922362225,0.16400783300033744,6,309.6688821904962,3.999785287368469,12.657822343539058,0.5,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation
|
||||
9,14,7.911071381405571,14.085282580602351,0.5416463116756228,0.12859551377809345,6,26.721469573230685,0.011377143482079926,0.04588334980819398,1.0,True,
|
||||
10,11,3.0731501091601263,21.371447114360553,0.7131414267834794,0.12095106901516853,6,13.404202373771934,0.006897671932216771,0.18540056788520015,1.0,True,
|
||||
10,12,6.011368280631378,39.649271366914945,0.6117876278616659,0.1250022412120491,6,19.806559061723252,0.006509808900810373,0.0674238161099294,1.0,True,
|
||||
10,13,9.76425648692991,72.41150002956132,0.5244808055380743,0.1368926377190841,6,36.182352720806286,0.004651842966001154,0.17931102925270942,1.0,True,
|
||||
10,14,6.554187411792988,41.92021600343522,0.5996858385693572,0.12955691635611982,6,17.916357473627126,0.009593578345611885,0.0575378323241282,1.0,True,
|
||||
10,15,10.1706917218607,65.51298166784419,0.5048970366649924,0.13966622273060353,6,30.173625507677606,0.006074999657627903,0.048675594115713976,1.0,True,
|
||||
11,12,3.3258193587172027,18.277824252554396,0.6508076728924785,0.12017753597340275,6,15.211760062254436,0.016302173123952872,0.05141023051579196,1.0,True,
|
||||
11,13,7.195203402213438,51.04005291520078,0.5666710199817161,0.12966061077144855,6,26.1420797484443,0.010250004424021028,0.06307533415790957,1.0,True,
|
||||
11,14,3.634158560526847,20.548768889074672,0.64271407110666,0.12109534664165013,6,14.045896850674039,0.0076106764286992265,0.053501024445426364,1.0,True,
|
||||
11,15,7.446972831184529,44.14153455348363,0.570479416362689,0.12836105648415896,6,18.857231663145765,0.006898635427401595,0.028532809464236104,1.0,True,
|
||||
11,16,10.651790397923806,69.98495270981525,0.47336531178995206,0.13436934972977357,6,42.57049752296035,0.03569532774909474,0.4066994385898772,1.0,True,
|
||||
12,13,3.8697507070400543,32.762228662646386,0.7056733087955325,0.1168985924651875,6,14.509757354686162,0.0020816591736723326,0.08889045468170857,1.0,True,
|
||||
12,14,0.9871080784265185,2.2709446365202766,0.8745432399512789,0.09766070609034913,6,11.857755748379178,0.0024736851957500175,0.01806957610594206,1.0,True,
|
||||
12,15,4.171948395051693,25.86371030092923,0.7033426183844012,0.1210390118956012,6,11.859397045728281,0.02278023379659275,0.04927694758545221,1.0,True,
|
||||
12,16,7.331699780686257,51.70712845726085,0.5820235756385069,0.1245813395619955,6,32.351864267271324,0.009898398498331785,0.03808519306435069,1.0,True,
|
||||
12,17,6.344245780495681,1.4248238883471156,0.6542219994988725,0.12658997017247905,6,11.229930872030522,0.019742666743374927,0.07899239993213694,1.0,True,
|
||||
13,14,3.7992314627329202,30.491284026126113,0.6984766461034874,0.11406275275916469,6,13.924716870947337,0.012533601308866885,0.10861598809330086,1.0,True,
|
||||
13,15,0.9105848166450461,6.898518361717151,0.8794391298650243,0.0990320912382964,6,10.514819201987361,0.006638809913640662,0.04266354358349535,1.0,True,
|
||||
13,16,3.4957081323467,18.94489979461447,0.7273073505141552,0.10958532316684444,6,19.405056504086563,0.005441055528599314,0.12519365495729431,1.0,True,
|
||||
13,17,2.9366461487378266,31.337404774299262,0.7130265716137395,0.11779895987026272,6,12.816390530620648,0.013263327701592529,0.16221305155705523,1.0,True,
|
||||
13,18,5.248032013425982,3.445996452937746,0.7019876443728176,0.11940768524727288,6,25.985486690964827,0.008995185112868311,0.05198334816510605,1.0,True,
|
||||
14,15,3.8816793634199405,23.592765664408958,0.7089927153981411,0.11692319124843933,6,10.742828632896593,0.09073157715426228,0.8466374858545868,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
||||
14,16,7.29101265002769,49.43618382074057,0.5923489278752436,0.12241125802320883,6,33.653658158107916,0.003104270324855819,0.13386021564092,1.0,True,
|
||||
14,17,5.915950814087913,0.8461207481731609,0.6687795177728063,0.12452036369781098,6,8.953051466023156,0.011592867275090568,0.10132037554601482,1.0,True,
|
||||
14,18,8.433581718971825,27.04528757318836,0.6196476790536196,0.12845389972151766,6,17.805501350543512,0.010071755472376367,0.13041952825792016,1.0,True,
|
||||
14,19,9.056542482242314,79.47127430600666,0.5845660749506904,0.12584132662356765,6,30.335272352492893,0.016830803029543952,0.25747616717579747,1.0,True,
|
||||
15,16,3.579287497246505,25.843418156331627,0.7032674772036475,0.1160221689285562,6,22.696890954829914,0.0009988867448377137,0.0903616813411077,1.0,True,
|
||||
15,17,2.2400625117908257,24.438886412582114,0.7489009568140678,0.11923375870790395,6,6.944925131742929,0.02582458487951321,0.09503968088936432,1.0,True,
|
||||
15,18,4.6956742726068,3.452521908779405,0.7165438713998661,0.1178392296206422,6,19.334165264362177,0.009354386824150452,0.1747840133793935,1.0,True,
|
||||
15,19,5.176369821469625,55.87850864159772,0.6924358974358974,0.11506895717743917,6,20.417126084614868,0.012497900854286311,0.3255615001296683,1.0,True,
|
||||
15,20,1.1877416357686716,128.54504202158432,0.6751867872591427,0.11901811643083532,6,27.381712286843683,2.670174543103617,1.9448445706434312,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
16,17,2.956793995513645,50.28230456891372,0.6284461152882206,0.11136186275509914,6,25.18428705575399,0.015372505619716304,0.0751987172187524,1.0,True,
|
||||
16,18,3.369822545690391,22.390896247552213,0.6921281286473868,0.10919235768364494,6,39.42922894015897,0.005355462743716019,0.036543030274398446,1.0,True,
|
||||
16,19,2.313038703742191,30.035090485266096,0.8880188913745961,0.09683468613705355,6,11.709094307553842,0.006745004702224827,0.04851926363284082,1.0,True,
|
||||
16,20,4.242518045976368,102.70162386525263,0.36698412698412697,0.13430062225326117,6,102.05441227788522,1.6194077849835204,3.5461435258834046,1.0,False,forward_reverse_translation;forward_reverse_rotation
|
||||
16,21,9.189957911290794,153.31148960700713,0.23764328854924197,0.14863424676851908,6,75.99451631707726,0.01721606559516444,0.561012451825117,0.5,False,heldout_inlier_ratio;forward_reverse_rotation
|
||||
17,18,2.517968959880004,27.89140832136152,0.7665916015366274,0.11462271017395576,6,9.759734204828526,0.003677259621955875,0.03455865038654393,1.0,True,
|
||||
17,19,3.518310045065406,80.31739505417983,0.645738203957382,0.11467441399973677,6,29.935027493013713,0.002657916871055147,0.03996637099866608,1.0,True,
|
||||
17,20,3.4199679241812992,152.98392843416642,0.2749902761571373,0.1378092692558536,6,36.98799912625142,0.9962499249666478,0.8583377913930432,0.5,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
17,21,8.339875108212771,156.4062058240796,0.4672368255565338,0.13751533768592306,6,32.19730139309397,3.7101311942981123,2.4525134629880094,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
17,22,10.944066586184825,150.8296621811644,0.4255952380952381,0.14178517078123323,6,38.71138397815585,1.8838258929727458,2.643696557684808,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
18,19,2.0416624616211574,52.42598673281832,0.6999343401181878,0.10876407223187459,6,36.91910298018587,0.0029722527816906435,0.028553700929610345,1.0,True,
|
||||
18,20,5.836864764777489,125.0925201128049,0.5796614723267061,0.12778176089815013,6,32.46060078153021,0.08496447343578362,0.24110896992060832,0.5,False,forward_reverse_translation
|
||||
18,21,10.84206419743439,175.70238585457497,0.23118979432439468,0.15492240575842026,6,95.57603256530417,0.3127879752995947,1.8359225701448998,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
18,22,13.461930516546937,178.72107050264088,0.20872354073123797,0.1539528469442395,6,158.96979855906298,5.509854679183967,5.400016950581102,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
18,23,10.787990019880349,164.1156966348418,0.3967277486910995,0.14073241205403234,6,67.81571155690442,0.037998014107336324,0.09667096210109852,1.0,True,
|
||||
19,20,6.120105723142265,72.6665333799865,0.6260444787247719,0.1266849163783949,6,25.979821491778758,0.02278172414929769,0.1983479736758361,1.0,True,
|
||||
19,21,11.201089261967727,123.27639912174082,0.22215292503430212,0.15090955425182226,6,96.8936157485445,2.289236102137041,0.9775110057344923,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
19,22,13.962230939038237,128.85294276465584,0.1883148831488315,0.1538013519108862,6,137.2212791172918,1.512900854675742,1.7779489198947585,1.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
19,23,10.877481155608614,143.45831663234054,0.37768025078369905,0.1432975121556297,6,38.93505210462377,2.6484748594978824,0.9762972275431752,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
19,24,10.384369483528566,177.87107120381796,0.43504761904761907,0.1392444656116938,6,41.9366060613255,0.04258350776081601,0.7879276702809289,1.0,False,forward_reverse_rotation
|
||||
20,21,5.091648376409225,50.60986574175432,0.6607188376242672,0.12214671257866083,6,14.082798146569486,0.014717307768564562,0.07480162341419787,1.0,True,
|
||||
20,22,7.842914217245693,56.186409384669375,0.576328684508104,0.13245799460807808,6,16.034011252152304,0.021651469263481868,0.4612054468064915,1.0,True,
|
||||
20,23,4.953146569484805,70.79178325235414,0.6451819579702717,0.1210226317867539,6,12.51146899612653,0.018249896901540018,0.12115759970964086,1.0,True,
|
||||
20,24,4.806112840058592,105.20453782384023,0.7383177570093458,0.11441422046802803,6,12.152737785424252,0.009011280440944078,0.06430292826078875,1.0,True,
|
||||
20,25,7.7432318481763245,68.57831319871164,0.6182822702159718,0.12791257682460658,6,29.104161441720713,0.01470353026057929,0.3205553574328468,1.0,True,
|
||||
21,22,2.836151129858731,5.576543642915048,0.7740636818348177,0.11564789267022943,6,12.209471497858695,0.006518431057241462,0.06991931948827856,1.0,True,
|
||||
21,23,1.4635995041292513,20.181917510599828,0.862223327530465,0.10307112004551743,6,11.124777032517395,0.002678668765812511,0.01879593375546071,1.0,True,
|
||||
21,24,2.7001854883863183,54.59467208208592,0.7795265676152102,0.11182158240581809,6,15.934013426145448,0.003491995501354943,0.037425651095358885,1.0,True,
|
||||
21,25,3.6513937480713023,17.968447456957325,0.7340892465252378,0.12002239056891176,6,16.572162980052227,0.03773466739435234,0.28781074838303833,1.0,True,
|
||||
21,26,4.368847767445087,60.912845043424156,0.7147358216190014,0.11997311573294335,6,17.3723156532691,0.01216183417643751,0.10312122071404906,1.0,True,
|
||||
22,23,3.806551883906871,14.605373867684776,0.7408951563458002,0.1172672510975272,6,12.610213323576234,0.009206244303296198,0.0960198600148152,1.0,True,
|
||||
22,24,4.999470711928798,49.01812843917085,0.6936064556176288,0.11914624513148228,6,13.20338761495324,0.006090737153725476,0.02755713841749006,1.0,True,
|
||||
22,25,2.281002791409386,12.391903814042275,0.7356584485868911,0.11474070552638106,6,16.673633938013026,0.001961709159757097,0.011643770804742994,1.0,True,
|
||||
22,26,1.990287035474152,55.33630140050909,0.7422594142259414,0.11765221626900067,6,18.880420396501982,0.007814937371704422,0.03934255356487579,1.0,True,
|
||||
22,27,2.5532406896142295,80.6206767037528,0.7254925373134329,0.11870181965154772,6,21.309389349405333,0.018241823604788293,0.08279236858581901,1.0,True,
|
||||
23,24,1.2663665558774873,34.41275457148608,0.7884810126582279,0.10662565692629541,6,10.611199681165658,0.0018823381482244372,0.020239211377623904,1.0,True,
|
||||
23,25,5.050865141821003,2.213470053642503,0.6843137254901961,0.1217109193489842,6,15.15080906413716,0.01463985673592735,0.20460048921133533,1.0,True,
|
||||
23,26,5.595299803053147,40.730927532824325,0.6772228989037758,0.12237954766026346,6,16.382334072569808,0.04518647006837217,0.20367965681897174,1.0,True,
|
||||
23,27,6.240759831138249,66.01530283606803,0.6637469586374696,0.12340951265232125,6,20.64383986204704,0.010626193556947943,1.1817079481882706,1.0,False,forward_reverse_rotation
|
||||
23,28,6.598772106458927,85.87759904182478,0.6720351390922401,0.12122778564083768,6,19.206570679040215,0.043540468662592216,0.2605147805386839,0.5,True,
|
||||
24,25,6.316196707646632,36.626224625128586,0.6764267990074442,0.12367636388755723,6,21.50176872604184,0.03581640576644142,0.20465043373191275,1.0,True,
|
||||
24,26,6.840027061556236,6.318172961338242,0.6524044389642417,0.12740222503880924,6,21.75476709306229,0.0620474433075452,0.12014838295938772,1.0,True,
|
||||
24,27,7.477875812552711,31.60254826458195,0.6546798029556651,0.12425657449629156,6,26.295019203914542,0.05403266260961897,0.2126374478532295,1.0,True,
|
||||
24,28,7.81303641449596,51.464844470338676,0.6614377470355731,0.12010155595807544,6,20.985000954928537,0.04949067679791638,0.25138526875322614,0.5,True,
|
||||
24,29,7.4949096209288655,93.73390958258972,0.047106325706594884,0.16491171897379944,6,28.651559198797667,0.8785910837751835,4.6775880176920355,0.0,False,heldout_inlier_ratio;heldout_inlier_rmse;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
25,26,1.433673687807028,42.944397586466835,0.9137395459976105,0.08148335762110498,6,16.56285514245561,0.003236736725553251,0.006670817258604747,1.0,True,
|
||||
25,27,1.973107678535245,68.22877288971054,0.8596658711217183,0.10961458820309757,6,22.188153380460914,0.010624789877375612,0.04475651255675051,1.0,True,
|
||||
25,28,2.578633669986193,88.09106909546726,0.8839157491622786,0.10490699814192775,6,16.172179483480598,0.0028341913749953818,0.01585308444597317,1.0,True,
|
||||
25,29,1.4869736566208207,130.3601342077183,0.7857227558401518,0.10954358768244278,6,20.132087187715292,0.0032477187428175502,0.016542353808297643,1.0,True,
|
||||
25,30,5.843711828111692,139.6773015481418,0.05061061531235322,0.15827452276344428,6,186.52424080569762,2.4401001990983646,2.630476332375907,0.0,False,heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
26,27,0.6711664435459649,25.284375303243706,0.9188612099644128,0.07520973241900301,6,20.799051531446725,0.00099208733077853,0.005485007138530622,1.0,True,
|
||||
26,28,1.202247282991071,45.146671509000434,0.9182726623840114,0.07611768518866213,6,21.995419584098137,0.004984978187528897,0.011735070988964648,1.0,True,
|
||||
26,29,0.8313560685788559,87.41573662125148,0.7856890251090416,0.1085139306178217,6,24.280602674778585,0.002586732426994246,0.12205883610742861,1.0,True,
|
||||
26,30,5.554376083336843,177.37830086539105,0.7634835395750642,0.10495724850674515,6,34.80647378548566,0.008489213184549637,0.021733210949119494,1.0,True,
|
||||
26,31,6.5424104132673175,156.01119868987084,0.7374054682955207,0.10737521663044328,6,38.310555401782864,0.007807741115783734,0.04372865220115068,0.5,True,
|
||||
27,28,0.6147316450225401,19.862296205756735,0.9281131178707225,0.07220941715117642,6,20.092791877654474,0.002300778353404822,0.001589714984734129,1.0,True,
|
||||
27,29,0.7540837235696874,62.13136131800778,0.7871188037207112,0.10966659884725233,6,24.009650087098127,0.005134403698946511,0.024348440992038003,1.0,True,
|
||||
27,30,5.07252652550922,152.0939255621477,0.7922108208955224,0.10147035321534549,6,32.52026042532519,0.007488026390185518,0.03860522829819172,1.0,True,
|
||||
27,31,6.285022243874859,178.7044260068885,0.7592097617664149,0.10539218018667616,6,48.70669958381935,0.0020937297862771895,0.027401753528281184,1.0,True,
|
||||
27,32,5.778763736289045,138.47370648510574,0.7480278422273782,0.10408734715846861,6,42.21510199826032,0.004407463145301957,0.03490543724835993,0.5,True,
|
||||
28,29,1.3242039147826599,42.26906511225104,0.787814381863266,0.10901449493832827,6,25.9136254196751,0.01570175790237293,0.035696604981368125,1.0,True,
|
||||
28,30,5.091487440029177,132.23162935639098,0.7791159962581852,0.1043856075500982,6,36.50167792578953,0.011701496185342901,0.047047156803733815,1.0,True,
|
||||
28,31,6.538757407908195,158.84212980112872,0.7449015266285981,0.10668967013687278,6,48.04636971250768,0.02442137437725171,0.6810283060803413,1.0,False,forward_reverse_rotation
|
||||
28,32,5.963604730984572,158.33600269086236,0.7252210330386226,0.11213192172123396,6,59.78207706090093,0.01795092616246746,0.05811390678775167,0.0,False,multistart_instability
|
||||
28,33,5.80807626014008,67.05379893079936,0.6692465836255895,0.10871806386783625,6,54.67376255043743,0.132593515882677,0.6593919290562373,0.5,False,forward_reverse_translation;forward_reverse_rotation
|
||||
29,30,4.767831371266539,89.96256424413991,0.7320662880982732,0.10929719051930432,6,30.745355096911858,0.005463604154020641,0.01858348348785812,1.0,True,
|
||||
29,31,5.715598450796842,116.57306468887764,0.7254562254562255,0.11323336570178014,6,49.09678922599784,0.005077799585122041,0.07265894091769737,1.0,True,
|
||||
29,32,5.281749147864957,159.39493219688646,0.33356393404819557,0.1449978595558761,6,111.7227195582132,0.3588603464467423,0.15715476414253352,1.0,False,heldout_inlier_ratio;forward_reverse_translation
|
||||
29,33,4.779166013738703,109.3228640430504,0.2850467289719626,0.13990657628540273,6,178.4302458902832,0.39012209563155037,0.27153598748575447,0.0,False,backend_not_converged;heldout_inlier_ratio;forward_reverse_translation;multistart_instability
|
||||
30,31,2.604154101624297,26.610500444737717,0.8286237272623269,0.09646596945131831,6,41.03513305025278,0.018905314487389895,0.08632503464138006,1.0,True,
|
||||
30,32,1.69105635082049,69.43236795274656,0.8065326633165829,0.09815063400019147,6,35.25635856167186,0.007341509941520377,0.023973741904830165,1.0,True,
|
||||
30,33,3.2411277385703925,160.7145717128097,0.09253766757622493,0.15464713396746754,6,31.157195533215592,1.0417523955000538,7.5233256615684665,1.0,False,backend_not_converged;heldout_inlier_ratio;forward_reverse_translation;forward_reverse_rotation
|
||||
31,32,0.9137201114724802,42.82186750800884,0.8146841206602162,0.09914628416022428,6,34.3317318149268,0.0693687705829607,0.45703051311645604,1.0,True,
|
||||
31,33,1.4965271484681508,134.10407126807198,0.7551430598250177,0.1003246191461096,6,28.537349904719445,0.005559091155809675,0.033005318250111694,1.0,True,
|
||||
32,33,1.9169426499676907,91.28220376006315,0.7585270860380031,0.0987597723287551,6,32.396623704761396,2.8694889670610495,5.798276157700327,0.0,False,forward_reverse_translation;forward_reverse_rotation;multistart_instability
|
||||
|
Binary file not shown.
+1236
File diff suppressed because it is too large
Load Diff
+416
@@ -0,0 +1,416 @@
|
||||
{
|
||||
"schema_version": 1,
|
||||
"success": true,
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
|
||||
"equation": "A_RTK_ij X = X B_LiDAR_ij",
|
||||
"frames": {
|
||||
"RTK": {
|
||||
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
|
||||
"x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver",
|
||||
"y_axis": "left",
|
||||
"z_axis": "up",
|
||||
"yaw_enu_deg": "90 - rawHeadingDeg"
|
||||
},
|
||||
"LiDAR": "raw LiDAR sensor frame"
|
||||
},
|
||||
"backend": "small_gicp",
|
||||
"measured_lidar_extrinsic_used_as_initial": false,
|
||||
"body_heading_offset_used": false,
|
||||
"body_antenna_lever_xy_used": false,
|
||||
"translation_m": [
|
||||
1.6362863962126635,
|
||||
-0.2461267175734333,
|
||||
0.0851285837804841
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.7337275596198988,
|
||||
1.3206501867905613,
|
||||
-22.293138670917568
|
||||
],
|
||||
"quaternion_xyzw": [
|
||||
-0.004053851502643108,
|
||||
0.012544688899275593,
|
||||
-0.19323028574532955,
|
||||
0.9810648570503344
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[
|
||||
0.9250093749023974,
|
||||
0.37904117671318505,
|
||||
0.026180960611867178,
|
||||
1.6362863962126635
|
||||
],
|
||||
[
|
||||
-0.3792445939369631,
|
||||
0.9252912459175456,
|
||||
0.0031061548487011197,
|
||||
-0.2461267175734333
|
||||
],
|
||||
[
|
||||
-0.023047653074967735,
|
||||
-0.01280221013107426,
|
||||
0.9996523941368298,
|
||||
0.0851285837804841
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"quality": {
|
||||
"stations": 34,
|
||||
"pairs": 55,
|
||||
"residuals": {
|
||||
"pairs": 55,
|
||||
"translation_m": {
|
||||
"rms": 0.17789650760700804,
|
||||
"median": 0.07286630775287825,
|
||||
"p90": 0.3720138200025907,
|
||||
"p95": 0.3991391086271845,
|
||||
"max": 0.5307044931342686
|
||||
},
|
||||
"rotation_deg": {
|
||||
"rms": 1.6408745761259504,
|
||||
"median": 0.7722797079864339,
|
||||
"p90": 2.06911907015371,
|
||||
"p95": 4.199030922414834,
|
||||
"max": 4.936245135060849
|
||||
},
|
||||
"per_pair": [
|
||||
{
|
||||
"pair_index": 0,
|
||||
"translation_m": 0.0467393979562504,
|
||||
"rotation_deg": 0.7657431692138433
|
||||
},
|
||||
{
|
||||
"pair_index": 1,
|
||||
"translation_m": 0.05689017243056396,
|
||||
"rotation_deg": 0.808177090866613
|
||||
},
|
||||
{
|
||||
"pair_index": 2,
|
||||
"translation_m": 0.06397107958463491,
|
||||
"rotation_deg": 0.3801068497009622
|
||||
},
|
||||
{
|
||||
"pair_index": 3,
|
||||
"translation_m": 0.1107952950892596,
|
||||
"rotation_deg": 0.22993541870282783
|
||||
},
|
||||
{
|
||||
"pair_index": 4,
|
||||
"translation_m": 0.025928748934271984,
|
||||
"rotation_deg": 0.6046131588115704
|
||||
},
|
||||
{
|
||||
"pair_index": 5,
|
||||
"translation_m": 0.07562027851221909,
|
||||
"rotation_deg": 0.6309936647228139
|
||||
},
|
||||
{
|
||||
"pair_index": 6,
|
||||
"translation_m": 0.18916050340904586,
|
||||
"rotation_deg": 0.26477854089271946
|
||||
},
|
||||
{
|
||||
"pair_index": 7,
|
||||
"translation_m": 0.043365771415170416,
|
||||
"rotation_deg": 0.10315887875456749
|
||||
},
|
||||
{
|
||||
"pair_index": 8,
|
||||
"translation_m": 0.06454458096453627,
|
||||
"rotation_deg": 0.8075022372885624
|
||||
},
|
||||
{
|
||||
"pair_index": 9,
|
||||
"translation_m": 0.05200877185535076,
|
||||
"rotation_deg": 0.5226003188288877
|
||||
},
|
||||
{
|
||||
"pair_index": 10,
|
||||
"translation_m": 0.08066933230596428,
|
||||
"rotation_deg": 0.6774188851574047
|
||||
},
|
||||
{
|
||||
"pair_index": 11,
|
||||
"translation_m": 0.02679009789021938,
|
||||
"rotation_deg": 0.5921424571930466
|
||||
},
|
||||
{
|
||||
"pair_index": 12,
|
||||
"translation_m": 0.033564310488394096,
|
||||
"rotation_deg": 1.0085274567011704
|
||||
},
|
||||
{
|
||||
"pair_index": 13,
|
||||
"translation_m": 0.11139219509163403,
|
||||
"rotation_deg": 0.596821136461582
|
||||
},
|
||||
{
|
||||
"pair_index": 14,
|
||||
"translation_m": 0.06985048714143455,
|
||||
"rotation_deg": 0.8031764622169965
|
||||
},
|
||||
{
|
||||
"pair_index": 15,
|
||||
"translation_m": 0.05926582195242711,
|
||||
"rotation_deg": 2.1146215228832355
|
||||
},
|
||||
{
|
||||
"pair_index": 16,
|
||||
"translation_m": 0.053822686491612724,
|
||||
"rotation_deg": 0.5853967911734774
|
||||
},
|
||||
{
|
||||
"pair_index": 17,
|
||||
"translation_m": 0.06095128937580921,
|
||||
"rotation_deg": 0.44465865069314414
|
||||
},
|
||||
{
|
||||
"pair_index": 18,
|
||||
"translation_m": 0.1465573557185661,
|
||||
"rotation_deg": 2.0008653910594214
|
||||
},
|
||||
{
|
||||
"pair_index": 19,
|
||||
"translation_m": 0.10402727477620027,
|
||||
"rotation_deg": 0.1402969578428506
|
||||
},
|
||||
{
|
||||
"pair_index": 20,
|
||||
"translation_m": 0.06790583241641802,
|
||||
"rotation_deg": 0.31916987926311186
|
||||
},
|
||||
{
|
||||
"pair_index": 21,
|
||||
"translation_m": 0.037917260718891004,
|
||||
"rotation_deg": 0.6351484564817018
|
||||
},
|
||||
{
|
||||
"pair_index": 22,
|
||||
"translation_m": 0.03171219066433979,
|
||||
"rotation_deg": 0.7611907317100989
|
||||
},
|
||||
{
|
||||
"pair_index": 23,
|
||||
"translation_m": 0.08117084986905364,
|
||||
"rotation_deg": 1.5477927872017732
|
||||
},
|
||||
{
|
||||
"pair_index": 24,
|
||||
"translation_m": 0.07160138896826257,
|
||||
"rotation_deg": 0.47596695875013556
|
||||
},
|
||||
{
|
||||
"pair_index": 25,
|
||||
"translation_m": 0.01792823137987938,
|
||||
"rotation_deg": 0.3736004835939083
|
||||
},
|
||||
{
|
||||
"pair_index": 26,
|
||||
"translation_m": 0.07286630775287825,
|
||||
"rotation_deg": 1.800095203725142
|
||||
},
|
||||
{
|
||||
"pair_index": 27,
|
||||
"translation_m": 0.18109166588563658,
|
||||
"rotation_deg": 3.9699177938767174
|
||||
},
|
||||
{
|
||||
"pair_index": 28,
|
||||
"translation_m": 0.24825286214410192,
|
||||
"rotation_deg": 4.619204763488191
|
||||
},
|
||||
{
|
||||
"pair_index": 29,
|
||||
"translation_m": 0.06588464658827484,
|
||||
"rotation_deg": 0.8156362703186536
|
||||
},
|
||||
{
|
||||
"pair_index": 30,
|
||||
"translation_m": 0.07851282552634996,
|
||||
"rotation_deg": 4.018956419097684
|
||||
},
|
||||
{
|
||||
"pair_index": 31,
|
||||
"translation_m": 0.16064262686816105,
|
||||
"rotation_deg": 4.8460088207546645
|
||||
},
|
||||
{
|
||||
"pair_index": 32,
|
||||
"translation_m": 0.1352330525899733,
|
||||
"rotation_deg": 4.936245135060849
|
||||
},
|
||||
{
|
||||
"pair_index": 33,
|
||||
"translation_m": 0.0649121457474399,
|
||||
"rotation_deg": 1.99675397761927
|
||||
},
|
||||
{
|
||||
"pair_index": 34,
|
||||
"translation_m": 0.0460266060768811,
|
||||
"rotation_deg": 1.2713317047431354
|
||||
},
|
||||
{
|
||||
"pair_index": 35,
|
||||
"translation_m": 0.11269690515103371,
|
||||
"rotation_deg": 0.5444667992697017
|
||||
},
|
||||
{
|
||||
"pair_index": 36,
|
||||
"translation_m": 0.05570004024342942,
|
||||
"rotation_deg": 0.6200055232178192
|
||||
},
|
||||
{
|
||||
"pair_index": 37,
|
||||
"translation_m": 0.006720345812259607,
|
||||
"rotation_deg": 1.1919322914717754
|
||||
},
|
||||
{
|
||||
"pair_index": 38,
|
||||
"translation_m": 0.05571822171452726,
|
||||
"rotation_deg": 0.8100733260702963
|
||||
},
|
||||
{
|
||||
"pair_index": 39,
|
||||
"translation_m": 0.03156268585174415,
|
||||
"rotation_deg": 1.1994412303651207
|
||||
},
|
||||
{
|
||||
"pair_index": 40,
|
||||
"translation_m": 0.1396566575693052,
|
||||
"rotation_deg": 1.219665033277036
|
||||
},
|
||||
{
|
||||
"pair_index": 41,
|
||||
"translation_m": 0.42067987248571403,
|
||||
"rotation_deg": 1.1901837024442368
|
||||
},
|
||||
{
|
||||
"pair_index": 42,
|
||||
"translation_m": 0.5307044931342686,
|
||||
"rotation_deg": 1.3118186035265211
|
||||
},
|
||||
{
|
||||
"pair_index": 43,
|
||||
"translation_m": 0.0278193588287066,
|
||||
"rotation_deg": 0.5463366974423033
|
||||
},
|
||||
{
|
||||
"pair_index": 44,
|
||||
"translation_m": 0.08083543060888142,
|
||||
"rotation_deg": 1.597876847929754
|
||||
},
|
||||
{
|
||||
"pair_index": 45,
|
||||
"translation_m": 0.3628770836511988,
|
||||
"rotation_deg": 0.7452934588576191
|
||||
},
|
||||
{
|
||||
"pair_index": 46,
|
||||
"translation_m": 0.40429181316256,
|
||||
"rotation_deg": 1.3531101551442906
|
||||
},
|
||||
{
|
||||
"pair_index": 47,
|
||||
"translation_m": 0.3857777439966788,
|
||||
"rotation_deg": 0.596976162782461
|
||||
},
|
||||
{
|
||||
"pair_index": 48,
|
||||
"translation_m": 0.18867103421064077,
|
||||
"rotation_deg": 0.5750764247339077
|
||||
},
|
||||
{
|
||||
"pair_index": 49,
|
||||
"translation_m": 0.3781049775701853,
|
||||
"rotation_deg": 1.067294104551723
|
||||
},
|
||||
{
|
||||
"pair_index": 50,
|
||||
"translation_m": 0.33164608192922734,
|
||||
"rotation_deg": 0.4502803262827393
|
||||
},
|
||||
{
|
||||
"pair_index": 51,
|
||||
"translation_m": 0.39693080668345215,
|
||||
"rotation_deg": 0.7722797079864339
|
||||
},
|
||||
{
|
||||
"pair_index": 52,
|
||||
"translation_m": 0.05933929949217937,
|
||||
"rotation_deg": 0.9106743636918464
|
||||
},
|
||||
{
|
||||
"pair_index": 53,
|
||||
"translation_m": 0.09592936224298491,
|
||||
"rotation_deg": 0.8770147335571411
|
||||
},
|
||||
{
|
||||
"pair_index": 54,
|
||||
"translation_m": 0.10907903367697035,
|
||||
"rotation_deg": 0.527094410495204
|
||||
}
|
||||
]
|
||||
},
|
||||
"weighted_jacobian_condition_number": 5.739204995062189,
|
||||
"linearized_one_sigma": {
|
||||
"translation_m": [
|
||||
0.006273860396200483,
|
||||
0.006271739194739563,
|
||||
0.005766938056265489
|
||||
],
|
||||
"rotation_deg": [
|
||||
0.06648607826531162,
|
||||
0.07036185324485841,
|
||||
0.1461990497815168
|
||||
],
|
||||
"warning": "conditional local estimate; bootstrap is the primary stability check"
|
||||
},
|
||||
"bootstrap": {
|
||||
"runs": 100,
|
||||
"order": [
|
||||
"x_m",
|
||||
"y_m",
|
||||
"z_m",
|
||||
"roll_deg",
|
||||
"pitch_deg",
|
||||
"yaw_deg"
|
||||
],
|
||||
"std": [
|
||||
0.002418391924231912,
|
||||
0.0018782458573924745,
|
||||
0.0024162629680468473,
|
||||
0.09973116386354614,
|
||||
0.0717005619911583,
|
||||
0.09954619224880032
|
||||
],
|
||||
"p025": [
|
||||
1.6323740665381714,
|
||||
-0.24963297512571514,
|
||||
0.08157494287763181,
|
||||
-0.9175278935375253,
|
||||
1.1732724086153574,
|
||||
-22.44062221365902
|
||||
],
|
||||
"p975": [
|
||||
1.6411098578257144,
|
||||
-0.24215579438024784,
|
||||
0.09016027720271011,
|
||||
-0.5264292351234366,
|
||||
1.4226725074325792,
|
||||
-22.072092788653528
|
||||
]
|
||||
}
|
||||
},
|
||||
"z_constraint": {
|
||||
"observable_from_planar_AX_XB": false,
|
||||
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
|
||||
"rtk_reference_height_above_ground_m": 0.8535,
|
||||
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion"
|
||||
},
|
||||
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification"
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
{
|
||||
"final": {
|
||||
"translation_m": [
|
||||
1.6381793500373911,
|
||||
-0.24084479868828831,
|
||||
0.08448123595331278
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
-0.8171674587248069,
|
||||
1.323288118779805,
|
||||
-22.104163317857477
|
||||
],
|
||||
"pairs": 25,
|
||||
"translation_rms_m": 0.10020667268070801,
|
||||
"rotation_rms_deg": 1.2527941187072538,
|
||||
"condition_number": 7.739413195936781
|
||||
},
|
||||
"backend_difference": {
|
||||
"translation_m": 0.003889255293759414,
|
||||
"rotation_deg": 0.1884307130161592,
|
||||
"delta_matrix_4x4": [
|
||||
[
|
||||
0.9999968771376496,
|
||||
0.0024968749848742764,
|
||||
-0.00010644368722136346,
|
||||
0.0008526003522697501
|
||||
],
|
||||
[
|
||||
-0.0024970968314049877,
|
||||
0.9999945974960792,
|
||||
-0.0021376356258303525,
|
||||
-0.003658904827736509
|
||||
],
|
||||
[
|
||||
0.00010110570323801577,
|
||||
0.0021378947504826257,
|
||||
0.9999977095892133,
|
||||
0.0010058801324768218
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
# run目录
|
||||
|
||||
根README包含完整复现命令；这里仅列入口职责。
|
||||
|
||||
| 脚本 | 用途 |
|
||||
|---|---|
|
||||
| `run_full_pipeline.ps1` | 从逐站LiDAR dlog、RTK rscap、IMU rscap一直运行到最终`T_RTK_lidar` |
|
||||
| `export_multisensor_stations.ps1` | 解析原始三传感器数据并按LiDAR帧生成combined NPZ |
|
||||
| `prepare_multisensor_dataset.ps1` | 每站选一帧，生成yaw-only RTK参考轨迹和`frames_all` |
|
||||
| `run_direct_rtk_lidar.ps1` | 从combined数据运行RTK直接标定和最终结果封装 |
|
||||
| `run_single_dataset.ps1` | 执行地面、两个GICP后端、精筛、共识和AX=XB求解 |
|
||||
| `view_result.ps1` | 打开3D运动对对比并打印数值增量 |
|
||||
|
||||
所有路径均为命令行参数；默认生成目录`work/`和`outputs/`不会提交Git。
|
||||
@@ -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,22 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$CombinedRoot,
|
||||
[Parameter(Mandatory = $true)][string]$Output,
|
||||
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
|
||||
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
|
||||
[string]$PoseName = "rtk_gga_raw_heading",
|
||||
[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, "--pose-name", $PoseName, "--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" }
|
||||
@@ -0,0 +1,36 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$CombinedRoot,
|
||||
[string]$OutputRoot = "",
|
||||
[string]$WorkRoot = "",
|
||||
[double]$RtkReferenceHeightAboveGroundM = 0.8535,
|
||||
[int]$ExpectedStations = 34,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 200
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Repo = Split-Path -Parent $PSScriptRoot
|
||||
if ([string]::IsNullOrWhiteSpace($OutputRoot)) { $OutputRoot = Join-Path $Repo "outputs\rtk_lidar_calibration" }
|
||||
if ([string]::IsNullOrWhiteSpace($WorkRoot)) { $WorkRoot = Join-Path $Repo "work\prepared_rtk_direct" }
|
||||
$Prepared = $WorkRoot
|
||||
|
||||
& (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") `
|
||||
-CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg 0 `
|
||||
-AntennaLever @(0.0,0.0,0.0) -PoseName "rtk_gga_raw_heading" -MinStations 30 -ExpectedStations $ExpectedStations -Overwrite
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }
|
||||
|
||||
& (Join-Path $Repo "run\run_single_dataset.ps1") `
|
||||
-Prepared $Prepared -OutputRoot $OutputRoot `
|
||||
-ReferencePoseFile "reference_poses_rtk_gga_raw_heading.csv" `
|
||||
-ReferenceHeight $RtkReferenceHeightAboveGroundM -MinPairs $MinPairs -Bootstrap $Bootstrap
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct calibration failed" }
|
||||
|
||||
$Finalize = @(
|
||||
(Join-Path $Repo "code\finalize_direct_rtk_lidar.py"),
|
||||
"--result-root", $OutputRoot,
|
||||
"--reference-height", "$RtkReferenceHeightAboveGroundM"
|
||||
)
|
||||
& python @Finalize
|
||||
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
|
||||
|
||||
Write-Host "Final T_RTK_lidar: $(Join-Path $OutputRoot 'final_T_RTK_lidar.json')"
|
||||
@@ -0,0 +1,32 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$DataRoot,
|
||||
[Parameter(Mandatory = $true)][string]$RtkCapture,
|
||||
[Parameter(Mandatory = $true)][string]$ImuCapture,
|
||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
||||
[string]$LidarObject = "frontlidar",
|
||||
[string]$Timezone = "+08:00",
|
||||
[double]$RtkReferenceHeightAboveGroundM = 0.8535,
|
||||
[int]$ExpectedStations = 34,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 200
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$ExportRoot = Join-Path $OutputRoot "exported"
|
||||
$PreparedRoot = Join-Path $OutputRoot "prepared_rtk_direct"
|
||||
$CalibrationRoot = Join-Path $OutputRoot "calibration"
|
||||
|
||||
& (Join-Path $PSScriptRoot "export_multisensor_stations.ps1") `
|
||||
-DataRoot $DataRoot -RtkCapture $RtkCapture -ImuCapture $ImuCapture `
|
||||
-OutputRoot $ExportRoot -LidarObject $LidarObject -Timezone $Timezone
|
||||
if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" }
|
||||
|
||||
& (Join-Path $PSScriptRoot "run_direct_rtk_lidar.ps1") `
|
||||
-CombinedRoot (Join-Path $ExportRoot "combined") `
|
||||
-WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
|
||||
-RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM `
|
||||
-ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" }
|
||||
|
||||
Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')"
|
||||
Write-Host "Prepared frames: $(Join-Path $PreparedRoot 'frames_all')"
|
||||
@@ -0,0 +1,70 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$Prepared,
|
||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
||||
[string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
|
||||
[double]$ReferenceHeight = 0.8535,
|
||||
[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"
|
||||
$ReferencePoses = Join-Path $Prepared $ReferencePoseFile
|
||||
$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, $ReferencePoses)) {
|
||||
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, "--reference-poses", $ReferencePoses,
|
||||
"--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, "--reference-height", "$ReferenceHeight",
|
||||
"--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,
|
||||
"--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
|
||||
"--output", (Join-Path $ConsensusOut "extrinsic.json")
|
||||
)
|
||||
|
||||
Write-Host "Calibration results: $OutputRoot"
|
||||
@@ -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" }
|
||||
@@ -0,0 +1,11 @@
|
||||
# tools目录
|
||||
|
||||
| 文件 | 输入→输出 |
|
||||
|---|---|
|
||||
| `frontlidar_dlog_export.py` | LiDAR dlog → 逐帧原始点云NPZ；时间来自DObject post tick |
|
||||
| `rscap_v2/parse_rtk_imu_v2.py` | RTK/IMU rscap → JSONL，保存校验状态、主机时间、GNSS/IMU设备字段和原始报文 |
|
||||
| `rscap_v2/audit_capture_v2.py` | 检查rscap结构、时间范围和记录统计 |
|
||||
| `build_multisensor_npz.py` | 按LiDAR帧最近邻关联GGA/heading，并附加IMU时间窗 → combined NPZ |
|
||||
| `prepare_multisensor_station_dataset.py` | combined NPZ → 每站一帧`frames_all`和`reference_poses_*.csv` |
|
||||
|
||||
当前标定只使用LiDAR和RTK；IMU保持原始传感器坐标，不参与点云去畸变或外参求解。prepared阶段对站内有效RTK取平均、对heading取圆均值，并选择有效帧序列的中间LiDAR帧。
|
||||
@@ -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())
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,170 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare one static LiDAR frame and one yaw-only RTK reference 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="rtk_gga_raw_heading")
|
||||
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)
|
||||
reference_position = antenna - yaw_rotation(yaw) @ lever
|
||||
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, 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"reference_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",
|
||||
"reference_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