diff --git a/LiDAR_RTK_Direct_Calibration/.gitignore b/LiDAR_RTK_Direct_Calibration/.gitignore new file mode 100644 index 0000000..f833587 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/.gitignore @@ -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/ diff --git a/LiDAR_RTK_Direct_Calibration/README.md b/LiDAR_RTK_Direct_Calibration/README.md new file mode 100644 index 0000000..95ff8cf --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/README.md @@ -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)。 diff --git a/LiDAR_RTK_Direct_Calibration/code/README.md b/LiDAR_RTK_Direct_Calibration/code/README.md new file mode 100644 index 0000000..6f038d9 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/code/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帧。 diff --git a/LiDAR_RTK_Direct_Calibration/code/compare_extrinsics.py b/LiDAR_RTK_Direct_Calibration/code/compare_extrinsics.py new file mode 100644 index 0000000..b407738 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/code/compare_extrinsics.py @@ -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()) diff --git a/LiDAR_RTK_Direct_Calibration/code/cross_backend_filter.py b/LiDAR_RTK_Direct_Calibration/code/cross_backend_filter.py new file mode 100644 index 0000000..95436ad --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/code/cross_backend_filter.py @@ -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() diff --git a/LiDAR_RTK_Direct_Calibration/code/finalize_direct_rtk_lidar.py b/LiDAR_RTK_Direct_Calibration/code/finalize_direct_rtk_lidar.py new file mode 100644 index 0000000..a694cc0 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/code/finalize_direct_rtk_lidar.py @@ -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() diff --git a/LiDAR_RTK_Direct_Calibration/code/refine_pairs.py b/LiDAR_RTK_Direct_Calibration/code/refine_pairs.py new file mode 100644 index 0000000..f460d36 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/code/refine_pairs.py @@ -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() diff --git a/LiDAR_RTK_Direct_Calibration/code/rigorous_calibration.py b/LiDAR_RTK_Direct_Calibration/code/rigorous_calibration.py new file mode 100644 index 0000000..0974795 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/code/rigorous_calibration.py @@ -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() diff --git a/LiDAR_RTK_Direct_Calibration/code/visualize_pair_3d.py b/LiDAR_RTK_Direct_Calibration/code/visualize_pair_3d.py new file mode 100644 index 0000000..3aa71a0 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/code/visualize_pair_3d.py @@ -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() diff --git a/LiDAR_RTK_Direct_Calibration/data/README.md b/LiDAR_RTK_Direct_Calibration/data/README.md new file mode 100644 index 0000000..7aadaba --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/data/README.md @@ -0,0 +1,5 @@ +# 数据说明 + +原始LiDAR dlog、RTK/IMU rscap、逐帧NPZ和prepared点云体积较大，不进入Git。请从项目云盘取得数据，并按根README中的目录示例放置；实际路径通过命令参数传入。 + +公开数据包应同时提供：采集日期、车辆/传感器安装版本、站点数量、ANT1/ANT2接线、rawHeading方向、RTK参考点离地高度及其测量方法。 diff --git a/LiDAR_RTK_Direct_Calibration/requirements.txt b/LiDAR_RTK_Direct_Calibration/requirements.txt new file mode 100644 index 0000000..9f8e5e8 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/requirements.txt @@ -0,0 +1,4 @@ +numpy>=1.26 +scipy>=1.11 +open3d>=0.18 +small-gicp diff --git a/LiDAR_RTK_Direct_Calibration/results/README.md b/LiDAR_RTK_Direct_Calibration/results/README.md new file mode 100644 index 0000000..e43ace6 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/README.md @@ -0,0 +1,5 @@ +# results目录 + +`reference_data4/`是本仓库附带的精简参考结果。新的运行结果应写到仓库外目录或`outputs/`，不要覆盖参考结果。 + +参考结果保留最终矩阵、共识B、两后端精筛B、逐对CSV/筛选审计和地面平面；未保留原始点云、逐帧combined数据、冗长的初筛JSON和带本机绝对路径的过程文件。 diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/README.md b/LiDAR_RTK_Direct_Calibration/results/reference_data4/README.md new file mode 100644 index 0000000..5a263f5 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/README.md @@ -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。 diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/common/ground_planes.csv b/LiDAR_RTK_Direct_Calibration/results/reference_data4/common/ground_planes.csv new file mode 100644 index 0000000..9933132 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/common/ground_planes.csv @@ -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 diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/B_consensus.consensus.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/B_consensus.consensus.json new file mode 100644 index 0000000..2de830a --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/B_consensus.consensus.json @@ -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": "" + } + ] +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/B_consensus.npz b/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/B_consensus.npz new file mode 100644 index 0000000..d32b2fc Binary files /dev/null and b/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/B_consensus.npz differ diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/extrinsic_rtk_lidar.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/extrinsic_rtk_lidar.json new file mode 100644 index 0000000..112b091 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/consensus/extrinsic_rtk_lidar.json @@ -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" +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/final_T_RTK_lidar.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/final_T_RTK_lidar.json new file mode 100644 index 0000000..81eb18c --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/final_T_RTK_lidar.json @@ -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 + ] + ] + } + } +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_quality.csv b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_quality.csv new file mode 100644 index 0000000..d970019 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_quality.csv @@ -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 diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_refined.npz b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_refined.npz new file mode 100644 index 0000000..cda1418 Binary files /dev/null and b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_refined.npz differ diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_refined.refinement.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_refined.refinement.json new file mode 100644 index 0000000..63800ee --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/B_refined.refinement.json @@ -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": [] + } + ] +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/extrinsic_rtk_lidar.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/extrinsic_rtk_lidar.json new file mode 100644 index 0000000..b225cdb --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/open3d_gicp/extrinsic_rtk_lidar.json @@ -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" +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_quality.csv b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_quality.csv new file mode 100644 index 0000000..90e32d2 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_quality.csv @@ -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 diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_refined.npz b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_refined.npz new file mode 100644 index 0000000..c55022b Binary files /dev/null and b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_refined.npz differ diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_refined.refinement.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_refined.refinement.json new file mode 100644 index 0000000..e3822c8 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/B_refined.refinement.json @@ -0,0 +1,1236 @@ +{ + "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": 101, + "accepted_pairs": 55, + "pairs": [ + { + "i": 0, + "j": 1, + "heldout_inlier_ratio": 0.8193962748876044, + "heldout_inlier_rmse_m": 0.11049675306366954, + "rotation_invariant_error_deg": 0.03734197192313715, + "reverse_translation_m": 0.026679665410762447, + "reverse_rotation_deg": 0.12399936190197167, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 0, + "j": 2, + "heldout_inlier_ratio": 0.7525388867463684, + "heldout_inlier_rmse_m": 0.11492533799491883, + "rotation_invariant_error_deg": 0.6760112300786716, + "reverse_translation_m": 0.0018464756299783867, + "reverse_rotation_deg": 0.03093241101186373, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 0, + "j": 3, + "heldout_inlier_ratio": 0.628093901505486, + "heldout_inlier_rmse_m": 0.12365050970311502, + "rotation_invariant_error_deg": 0.3329548009851777, + "reverse_translation_m": 0.010524333328230958, + "reverse_rotation_deg": 0.23898233567764737, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 0, + "j": 4, + "heldout_inlier_ratio": 0.693351593625498, + "heldout_inlier_rmse_m": 0.11485738628517483, + "rotation_invariant_error_deg": 0.1961722863909472, + "reverse_translation_m": 0.003627491377787396, + "reverse_rotation_deg": 0.0495829610363469, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 0, + "j": 5, + "heldout_inlier_ratio": 0.6015065913370998, + "heldout_inlier_rmse_m": 0.12959859333012305, + "rotation_invariant_error_deg": 0.8874623539854838, + "reverse_translation_m": 0.013710015050868782, + "reverse_rotation_deg": 0.26025123892797375, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 1, + "j": 3, + "heldout_inlier_ratio": 0.678820988438572, + "heldout_inlier_rmse_m": 0.12109867185657658, + "rotation_invariant_error_deg": 0.28201354980988214, + "reverse_translation_m": 0.008324315958294127, + "reverse_rotation_deg": 0.2568985217684905, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 2, + "j": 3, + "heldout_inlier_ratio": 0.7609663064208518, + "heldout_inlier_rmse_m": 0.11583878636902087, + "rotation_invariant_error_deg": 0.03805484899912012, + "reverse_translation_m": 0.007842434748069874, + "reverse_rotation_deg": 0.017111535671962216, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 2, + "j": 4, + "heldout_inlier_ratio": 0.796748976299789, + "heldout_inlier_rmse_m": 0.1148434235904791, + "rotation_invariant_error_deg": 0.8619802082204089, + "reverse_translation_m": 0.011210942709506708, + "reverse_rotation_deg": 0.11462542679279858, + "accepted": false, + "rejection_reasons": [ + "rotation_conjugacy_invariant" + ] + }, + { + "i": 2, + "j": 5, + "heldout_inlier_ratio": 0.7112112112112112, + "heldout_inlier_rmse_m": 0.12001318292058727, + "rotation_invariant_error_deg": 0.193055344183378, + "reverse_translation_m": 0.011299298862678088, + "reverse_rotation_deg": 0.02158353743310138, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 3, + "j": 5, + "heldout_inlier_ratio": 0.7989069680784996, + "heldout_inlier_rmse_m": 0.10775905778143813, + "rotation_invariant_error_deg": 0.5566178981223633, + "reverse_translation_m": 0.013532537604982006, + "reverse_rotation_deg": 0.06485728954506689, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 3, + "j": 6, + "heldout_inlier_ratio": 0.8435613682092555, + "heldout_inlier_rmse_m": 0.11222309863990189, + "rotation_invariant_error_deg": 0.2427940986524746, + "reverse_translation_m": 0.00951276519885504, + "reverse_rotation_deg": 0.09743636874086448, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 3, + "j": 7, + "heldout_inlier_ratio": 0.8651898734177215, + "heldout_inlier_rmse_m": 0.11184568072686091, + "rotation_invariant_error_deg": 0.2337168435335002, + "reverse_translation_m": 0.010827690226297664, + "reverse_rotation_deg": 0.12520280777371842, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 3, + "j": 8, + "heldout_inlier_ratio": 0.825590155700653, + "heldout_inlier_rmse_m": 0.1078798726707026, + "rotation_invariant_error_deg": 0.07359256633802147, + "reverse_translation_m": 0.005608807919239624, + "reverse_rotation_deg": 0.021536601402144962, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 4, + "j": 5, + "heldout_inlier_ratio": 0.6652516676773802, + "heldout_inlier_rmse_m": 0.12444359189600171, + "rotation_invariant_error_deg": 1.0678084485756258, + "reverse_translation_m": 0.0031552835887398907, + "reverse_rotation_deg": 0.05824661275042354, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 5, + "j": 6, + "heldout_inlier_ratio": 0.7604901596732269, + "heldout_inlier_rmse_m": 0.11101745020162715, + "rotation_invariant_error_deg": 0.8151406131512573, + "reverse_translation_m": 0.01028831511886355, + "reverse_rotation_deg": 0.031120237309440944, + "accepted": false, + "rejection_reasons": [ + "rotation_conjugacy_invariant" + ] + }, + { + "i": 5, + "j": 7, + "heldout_inlier_ratio": 0.8092687180764918, + "heldout_inlier_rmse_m": 0.10777871969807898, + "rotation_invariant_error_deg": 0.8022146383376452, + "reverse_translation_m": 0.010579733674272045, + "reverse_rotation_deg": 0.033334626234333836, + "accepted": false, + "rejection_reasons": [ + "rotation_conjugacy_invariant" + ] + }, + { + "i": 6, + "j": 7, + "heldout_inlier_ratio": 0.8539354187689203, + "heldout_inlier_rmse_m": 0.10462253085152279, + "rotation_invariant_error_deg": 0.0251639045456713, + "reverse_translation_m": 0.0028433142784691904, + "reverse_rotation_deg": 0.028424505435071433, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 6, + "j": 8, + "heldout_inlier_ratio": 0.7757757757757757, + "heldout_inlier_rmse_m": 0.11316400028465075, + "rotation_invariant_error_deg": 0.31912673762975885, + "reverse_translation_m": 0.007224674181692249, + "reverse_rotation_deg": 0.10395594559619384, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 7, + "j": 8, + "heldout_inlier_ratio": 0.8340050377833753, + "heldout_inlier_rmse_m": 0.11132245152738934, + "rotation_invariant_error_deg": 0.2998850033683311, + "reverse_translation_m": 0.004806949653405576, + "reverse_rotation_deg": 0.020340398656013788, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 9, + "j": 14, + "heldout_inlier_ratio": 0.5416463116756228, + "heldout_inlier_rmse_m": 0.12859551377809345, + "rotation_invariant_error_deg": 1.229967831664748, + "reverse_translation_m": 0.011377143482079926, + "reverse_rotation_deg": 0.04588334980819398, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 10, + "j": 11, + "heldout_inlier_ratio": 0.7131414267834794, + "heldout_inlier_rmse_m": 0.12095106901516853, + "rotation_invariant_error_deg": 0.021248756295900506, + "reverse_translation_m": 0.006897671932216771, + "reverse_rotation_deg": 0.18540056788520015, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 10, + "j": 12, + "heldout_inlier_ratio": 0.6117876278616659, + "heldout_inlier_rmse_m": 0.1250022412120491, + "rotation_invariant_error_deg": 0.22627625875819746, + "reverse_translation_m": 0.006509808900810373, + "reverse_rotation_deg": 0.0674238161099294, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 10, + "j": 13, + "heldout_inlier_ratio": 0.5244808055380743, + "heldout_inlier_rmse_m": 0.1368926377190841, + "rotation_invariant_error_deg": 0.7458516771815482, + "reverse_translation_m": 0.004651842966001154, + "reverse_rotation_deg": 0.17931102925270942, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "heldout_rmse" + ] + }, + { + "i": 10, + "j": 14, + "heldout_inlier_ratio": 0.5996858385693572, + "heldout_inlier_rmse_m": 0.12955691635611982, + "rotation_invariant_error_deg": 0.9907110909418364, + "reverse_translation_m": 0.009593578345611885, + "reverse_rotation_deg": 0.0575378323241282, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 10, + "j": 15, + "heldout_inlier_ratio": 0.5048970366649924, + "heldout_inlier_rmse_m": 0.13966622273060353, + "rotation_invariant_error_deg": 0.081650080653759, + "reverse_translation_m": 0.006074999657627903, + "reverse_rotation_deg": 0.048675594115713976, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "heldout_rmse" + ] + }, + { + "i": 11, + "j": 12, + "heldout_inlier_ratio": 0.6508076728924785, + "heldout_inlier_rmse_m": 0.12017753597340275, + "rotation_invariant_error_deg": 0.15490315710381708, + "reverse_translation_m": 0.016302173123952872, + "reverse_rotation_deg": 0.05141023051579196, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 11, + "j": 13, + "heldout_inlier_ratio": 0.5666710199817161, + "heldout_inlier_rmse_m": 0.12966061077144855, + "rotation_invariant_error_deg": 0.8179501999282053, + "reverse_translation_m": 0.010250004424021028, + "reverse_rotation_deg": 0.06307533415790957, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 11, + "j": 14, + "heldout_inlier_ratio": 0.64271407110666, + "heldout_inlier_rmse_m": 0.12109534664165013, + "rotation_invariant_error_deg": 0.9336352575787998, + "reverse_translation_m": 0.0076106764286992265, + "reverse_rotation_deg": 0.053501024445426364, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 11, + "j": 15, + "heldout_inlier_ratio": 0.570479416362689, + "heldout_inlier_rmse_m": 0.12836105648415896, + "rotation_invariant_error_deg": 0.14451133298980778, + "reverse_translation_m": 0.006898635427401595, + "reverse_rotation_deg": 0.028532809464236104, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 11, + "j": 16, + "heldout_inlier_ratio": 0.47336531178995206, + "heldout_inlier_rmse_m": 0.13436934972977357, + "rotation_invariant_error_deg": 0.24187723526689808, + "reverse_translation_m": 0.03569532774909474, + "reverse_rotation_deg": 0.4066994385898772, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "heldout_rmse" + ] + }, + { + "i": 12, + "j": 13, + "heldout_inlier_ratio": 0.7056733087955325, + "heldout_inlier_rmse_m": 0.1168985924651875, + "rotation_invariant_error_deg": 0.9871558772761304, + "reverse_translation_m": 0.0020816591736723326, + "reverse_rotation_deg": 0.08889045468170857, + "accepted": false, + "rejection_reasons": [ + "rotation_conjugacy_invariant" + ] + }, + { + "i": 12, + "j": 14, + "heldout_inlier_ratio": 0.8745432399512789, + "heldout_inlier_rmse_m": 0.09766070609034913, + "rotation_invariant_error_deg": 0.6209742090469781, + "reverse_translation_m": 0.0024736851957500175, + "reverse_rotation_deg": 0.01806957610594206, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 12, + "j": 15, + "heldout_inlier_ratio": 0.7033426183844012, + "heldout_inlier_rmse_m": 0.1210390118956012, + "rotation_invariant_error_deg": 0.3460892559833617, + "reverse_translation_m": 0.02278023379659275, + "reverse_rotation_deg": 0.04927694758545221, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 12, + "j": 16, + "heldout_inlier_ratio": 0.5820235756385069, + "heldout_inlier_rmse_m": 0.1245813395619955, + "rotation_invariant_error_deg": 0.5019661911966864, + "reverse_translation_m": 0.009898398498331785, + "reverse_rotation_deg": 0.03808519306435069, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 12, + "j": 17, + "heldout_inlier_ratio": 0.6542219994988725, + "heldout_inlier_rmse_m": 0.12658997017247905, + "rotation_invariant_error_deg": 0.4304077384691065, + "reverse_translation_m": 0.019742666743374927, + "reverse_rotation_deg": 0.07899239993213694, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 13, + "j": 14, + "heldout_inlier_ratio": 0.6984766461034874, + "heldout_inlier_rmse_m": 0.11406275275916469, + "rotation_invariant_error_deg": 1.758070291968199, + "reverse_translation_m": 0.012533601308866885, + "reverse_rotation_deg": 0.10861598809330086, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 13, + "j": 15, + "heldout_inlier_ratio": 0.8794391298650243, + "heldout_inlier_rmse_m": 0.0990320912382964, + "rotation_invariant_error_deg": 0.647826919954313, + "reverse_translation_m": 0.006638809913640662, + "reverse_rotation_deg": 0.04266354358349535, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 13, + "j": 16, + "heldout_inlier_ratio": 0.7273073505141552, + "heldout_inlier_rmse_m": 0.10958532316684444, + "rotation_invariant_error_deg": 0.4561367476009046, + "reverse_translation_m": 0.005441055528599314, + "reverse_rotation_deg": 0.12519365495729431, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 13, + "j": 17, + "heldout_inlier_ratio": 0.7130265716137395, + "heldout_inlier_rmse_m": 0.11779895987026272, + "rotation_invariant_error_deg": 0.5739060705989587, + "reverse_translation_m": 0.013263327701592529, + "reverse_rotation_deg": 0.16221305155705523, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 13, + "j": 18, + "heldout_inlier_ratio": 0.7019876443728176, + "heldout_inlier_rmse_m": 0.11940768524727288, + "rotation_invariant_error_deg": 0.3921371405111893, + "reverse_translation_m": 0.008995185112868311, + "reverse_rotation_deg": 0.05198334816510605, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 14, + "j": 16, + "heldout_inlier_ratio": 0.5923489278752436, + "heldout_inlier_rmse_m": 0.12241125802320883, + "rotation_invariant_error_deg": 1.2462518377866232, + "reverse_translation_m": 0.003104270324855819, + "reverse_rotation_deg": 0.13386021564092, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 14, + "j": 17, + "heldout_inlier_ratio": 0.6687795177728063, + "heldout_inlier_rmse_m": 0.12452036369781098, + "rotation_invariant_error_deg": 0.46701481462250366, + "reverse_translation_m": 0.011592867275090568, + "reverse_rotation_deg": 0.10132037554601482, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 14, + "j": 18, + "heldout_inlier_ratio": 0.6196476790536196, + "heldout_inlier_rmse_m": 0.12845389972151766, + "rotation_invariant_error_deg": 1.3569621038916289, + "reverse_translation_m": 0.010071755472376367, + "reverse_rotation_deg": 0.13041952825792016, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 14, + "j": 19, + "heldout_inlier_ratio": 0.5845660749506904, + "heldout_inlier_rmse_m": 0.12584132662356765, + "rotation_invariant_error_deg": 0.8859663897253682, + "reverse_translation_m": 0.016830803029543952, + "reverse_rotation_deg": 0.25747616717579747, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 15, + "j": 16, + "heldout_inlier_ratio": 0.7032674772036475, + "heldout_inlier_rmse_m": 0.1160221689285562, + "rotation_invariant_error_deg": 0.16691786777400353, + "reverse_translation_m": 0.0009988867448377137, + "reverse_rotation_deg": 0.0903616813411077, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 15, + "j": 17, + "heldout_inlier_ratio": 0.7489009568140678, + "heldout_inlier_rmse_m": 0.11923375870790395, + "rotation_invariant_error_deg": 0.0571702091132984, + "reverse_translation_m": 0.02582458487951321, + "reverse_rotation_deg": 0.09503968088936432, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 15, + "j": 18, + "heldout_inlier_ratio": 0.7165438713998661, + "heldout_inlier_rmse_m": 0.1178392296206422, + "rotation_invariant_error_deg": 0.2517513583303437, + "reverse_translation_m": 0.009354386824150452, + "reverse_rotation_deg": 0.1747840133793935, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 15, + "j": 19, + "heldout_inlier_ratio": 0.6924358974358974, + "heldout_inlier_rmse_m": 0.11506895717743917, + "rotation_invariant_error_deg": 0.19972369514616872, + "reverse_translation_m": 0.012497900854286311, + "reverse_rotation_deg": 0.3255615001296683, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 16, + "j": 17, + "heldout_inlier_ratio": 0.6284461152882206, + "heldout_inlier_rmse_m": 0.11136186275509914, + "rotation_invariant_error_deg": 0.05019974250597414, + "reverse_translation_m": 0.015372505619716304, + "reverse_rotation_deg": 0.0751987172187524, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 16, + "j": 18, + "heldout_inlier_ratio": 0.6921281286473868, + "heldout_inlier_rmse_m": 0.10919235768364494, + "rotation_invariant_error_deg": 0.055849228377191196, + "reverse_translation_m": 0.005355462743716019, + "reverse_rotation_deg": 0.036543030274398446, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 16, + "j": 19, + "heldout_inlier_ratio": 0.8880188913745961, + "heldout_inlier_rmse_m": 0.09683468613705355, + "rotation_invariant_error_deg": 0.31232814527145436, + "reverse_translation_m": 0.006745004702224827, + "reverse_rotation_deg": 0.04851926363284082, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 17, + "j": 18, + "heldout_inlier_ratio": 0.7665916015366274, + "heldout_inlier_rmse_m": 0.11462271017395576, + "rotation_invariant_error_deg": 0.21682013544613454, + "reverse_translation_m": 0.003677259621955875, + "reverse_rotation_deg": 0.03455865038654393, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 17, + "j": 19, + "heldout_inlier_ratio": 0.645738203957382, + "heldout_inlier_rmse_m": 0.11467441399973677, + "rotation_invariant_error_deg": 0.2814841354799569, + "reverse_translation_m": 0.002657916871055147, + "reverse_rotation_deg": 0.03996637099866608, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 18, + "j": 19, + "heldout_inlier_ratio": 0.6999343401181878, + "heldout_inlier_rmse_m": 0.10876407223187459, + "rotation_invariant_error_deg": 0.4582992256862468, + "reverse_translation_m": 0.0029722527816906435, + "reverse_rotation_deg": 0.028553700929610345, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 18, + "j": 23, + "heldout_inlier_ratio": 0.3967277486910995, + "heldout_inlier_rmse_m": 0.14073241205403234, + "rotation_invariant_error_deg": 0.09592166015420389, + "reverse_translation_m": 0.037998014107336324, + "reverse_rotation_deg": 0.09667096210109852, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "heldout_rmse" + ] + }, + { + "i": 19, + "j": 20, + "heldout_inlier_ratio": 0.6260444787247719, + "heldout_inlier_rmse_m": 0.1266849163783949, + "rotation_invariant_error_deg": 0.046353745268874036, + "reverse_translation_m": 0.02278172414929769, + "reverse_rotation_deg": 0.1983479736758361, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 20, + "j": 21, + "heldout_inlier_ratio": 0.6607188376242672, + "heldout_inlier_rmse_m": 0.12214671257866083, + "rotation_invariant_error_deg": 0.3384536988680438, + "reverse_translation_m": 0.014717307768564562, + "reverse_rotation_deg": 0.07480162341419787, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 20, + "j": 22, + "heldout_inlier_ratio": 0.576328684508104, + "heldout_inlier_rmse_m": 0.13245799460807808, + "rotation_invariant_error_deg": 0.5030331090557638, + "reverse_translation_m": 0.021651469263481868, + "reverse_rotation_deg": 0.4612054468064915, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "heldout_rmse" + ] + }, + { + "i": 20, + "j": 23, + "heldout_inlier_ratio": 0.6451819579702717, + "heldout_inlier_rmse_m": 0.1210226317867539, + "rotation_invariant_error_deg": 0.4119360838682553, + "reverse_translation_m": 0.018249896901540018, + "reverse_rotation_deg": 0.12115759970964086, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 20, + "j": 24, + "heldout_inlier_ratio": 0.7383177570093458, + "heldout_inlier_rmse_m": 0.11441422046802803, + "rotation_invariant_error_deg": 0.19247038033003605, + "reverse_translation_m": 0.009011280440944078, + "reverse_rotation_deg": 0.06430292826078875, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 20, + "j": 25, + "heldout_inlier_ratio": 0.6182822702159718, + "heldout_inlier_rmse_m": 0.12791257682460658, + "rotation_invariant_error_deg": 0.06528383884479183, + "reverse_translation_m": 0.01470353026057929, + "reverse_rotation_deg": 0.3205553574328468, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 21, + "j": 22, + "heldout_inlier_ratio": 0.7740636818348177, + "heldout_inlier_rmse_m": 0.11564789267022943, + "rotation_invariant_error_deg": 0.19149128351628697, + "reverse_translation_m": 0.006518431057241462, + "reverse_rotation_deg": 0.06991931948827856, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 21, + "j": 23, + "heldout_inlier_ratio": 0.862223327530465, + "heldout_inlier_rmse_m": 0.10307112004551743, + "rotation_invariant_error_deg": 0.07805073858411404, + "reverse_translation_m": 0.002678668765812511, + "reverse_rotation_deg": 0.01879593375546071, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 21, + "j": 24, + "heldout_inlier_ratio": 0.7795265676152102, + "heldout_inlier_rmse_m": 0.11182158240581809, + "rotation_invariant_error_deg": 0.5019012138072725, + "reverse_translation_m": 0.003491995501354943, + "reverse_rotation_deg": 0.037425651095358885, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 21, + "j": 25, + "heldout_inlier_ratio": 0.7340892465252378, + "heldout_inlier_rmse_m": 0.12002239056891176, + "rotation_invariant_error_deg": 0.07617121764256396, + "reverse_translation_m": 0.03773466739435234, + "reverse_rotation_deg": 0.28781074838303833, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 21, + "j": 26, + "heldout_inlier_ratio": 0.7147358216190014, + "heldout_inlier_rmse_m": 0.11997311573294335, + "rotation_invariant_error_deg": 0.12059196585352083, + "reverse_translation_m": 0.01216183417643751, + "reverse_rotation_deg": 0.10312122071404906, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 22, + "j": 23, + "heldout_inlier_ratio": 0.7408951563458002, + "heldout_inlier_rmse_m": 0.1172672510975272, + "rotation_invariant_error_deg": 0.06785708786323497, + "reverse_translation_m": 0.009206244303296198, + "reverse_rotation_deg": 0.0960198600148152, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 22, + "j": 24, + "heldout_inlier_ratio": 0.6936064556176288, + "heldout_inlier_rmse_m": 0.11914624513148228, + "rotation_invariant_error_deg": 0.6464246277119656, + "reverse_translation_m": 0.006090737153725476, + "reverse_rotation_deg": 0.02755713841749006, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 22, + "j": 25, + "heldout_inlier_ratio": 0.7356584485868911, + "heldout_inlier_rmse_m": 0.11474070552638106, + "rotation_invariant_error_deg": 0.09992812876469337, + "reverse_translation_m": 0.001961709159757097, + "reverse_rotation_deg": 0.011643770804742994, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 22, + "j": 26, + "heldout_inlier_ratio": 0.7422594142259414, + "heldout_inlier_rmse_m": 0.11765221626900067, + "rotation_invariant_error_deg": 0.28585419533643375, + "reverse_translation_m": 0.007814937371704422, + "reverse_rotation_deg": 0.03934255356487579, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 22, + "j": 27, + "heldout_inlier_ratio": 0.7254925373134329, + "heldout_inlier_rmse_m": 0.11870181965154772, + "rotation_invariant_error_deg": 0.17656741069014004, + "reverse_translation_m": 0.018241823604788293, + "reverse_rotation_deg": 0.08279236858581901, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 23, + "j": 24, + "heldout_inlier_ratio": 0.7884810126582279, + "heldout_inlier_rmse_m": 0.10662565692629541, + "rotation_invariant_error_deg": 0.5489092886683338, + "reverse_translation_m": 0.0018823381482244372, + "reverse_rotation_deg": 0.020239211377623904, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 23, + "j": 25, + "heldout_inlier_ratio": 0.6843137254901961, + "heldout_inlier_rmse_m": 0.1217109193489842, + "rotation_invariant_error_deg": 2.3712361046355452, + "reverse_translation_m": 0.01463985673592735, + "reverse_rotation_deg": 0.20460048921133533, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant" + ] + }, + { + "i": 23, + "j": 26, + "heldout_inlier_ratio": 0.6772228989037758, + "heldout_inlier_rmse_m": 0.12237954766026346, + "rotation_invariant_error_deg": 0.1059647647472346, + "reverse_translation_m": 0.04518647006837217, + "reverse_rotation_deg": 0.20367965681897174, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 23, + "j": 28, + "heldout_inlier_ratio": 0.6720351390922401, + "heldout_inlier_rmse_m": 0.12122778564083768, + "rotation_invariant_error_deg": 0.024721954757012554, + "reverse_translation_m": 0.043540468662592216, + "reverse_rotation_deg": 0.2605147805386839, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 24, + "j": 25, + "heldout_inlier_ratio": 0.6764267990074442, + "heldout_inlier_rmse_m": 0.12367636388755723, + "rotation_invariant_error_deg": 0.08555695772261629, + "reverse_translation_m": 0.03581640576644142, + "reverse_rotation_deg": 0.20465043373191275, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 24, + "j": 26, + "heldout_inlier_ratio": 0.6524044389642417, + "heldout_inlier_rmse_m": 0.12740222503880924, + "rotation_invariant_error_deg": 0.9095954626203522, + "reverse_translation_m": 0.0620474433075452, + "reverse_rotation_deg": 0.12014838295938772, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "rotation_conjugacy_invariant", + "forward_reverse_translation" + ] + }, + { + "i": 24, + "j": 27, + "heldout_inlier_ratio": 0.6546798029556651, + "heldout_inlier_rmse_m": 0.12425657449629156, + "rotation_invariant_error_deg": 0.564599224941059, + "reverse_translation_m": 0.05403266260961897, + "reverse_rotation_deg": 0.2126374478532295, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio", + "forward_reverse_translation" + ] + }, + { + "i": 24, + "j": 28, + "heldout_inlier_ratio": 0.6614377470355731, + "heldout_inlier_rmse_m": 0.12010155595807544, + "rotation_invariant_error_deg": 0.5530413756685064, + "reverse_translation_m": 0.04949067679791638, + "reverse_rotation_deg": 0.25138526875322614, + "accepted": false, + "rejection_reasons": [ + "overlap_ratio" + ] + }, + { + "i": 25, + "j": 26, + "heldout_inlier_ratio": 0.9137395459976105, + "heldout_inlier_rmse_m": 0.08148335762110498, + "rotation_invariant_error_deg": 0.05413176275047249, + "reverse_translation_m": 0.003236736725553251, + "reverse_rotation_deg": 0.006670817258604747, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 25, + "j": 27, + "heldout_inlier_ratio": 0.8596658711217183, + "heldout_inlier_rmse_m": 0.10961458820309757, + "rotation_invariant_error_deg": 0.2507254665718506, + "reverse_translation_m": 0.010624789877375612, + "reverse_rotation_deg": 0.04475651255675051, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 25, + "j": 28, + "heldout_inlier_ratio": 0.8839157491622786, + "heldout_inlier_rmse_m": 0.10490699814192775, + "rotation_invariant_error_deg": 0.32277369370385145, + "reverse_translation_m": 0.0028341913749953818, + "reverse_rotation_deg": 0.01585308444597317, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 25, + "j": 29, + "heldout_inlier_ratio": 0.7857227558401518, + "heldout_inlier_rmse_m": 0.10954358768244278, + "rotation_invariant_error_deg": 0.6270920954002577, + "reverse_translation_m": 0.0032477187428175502, + "reverse_rotation_deg": 0.016542353808297643, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 26, + "j": 27, + "heldout_inlier_ratio": 0.9188612099644128, + "heldout_inlier_rmse_m": 0.07520973241900301, + "rotation_invariant_error_deg": 0.1438602933497286, + "reverse_translation_m": 0.00099208733077853, + "reverse_rotation_deg": 0.005485007138530622, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 26, + "j": 28, + "heldout_inlier_ratio": 0.9182726623840114, + "heldout_inlier_rmse_m": 0.07611768518866213, + "rotation_invariant_error_deg": 0.19922486670869688, + "reverse_translation_m": 0.004984978187528897, + "reverse_rotation_deg": 0.011735070988964648, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 26, + "j": 29, + "heldout_inlier_ratio": 0.7856890251090416, + "heldout_inlier_rmse_m": 0.1085139306178217, + "rotation_invariant_error_deg": 0.6529919473115342, + "reverse_translation_m": 0.002586732426994246, + "reverse_rotation_deg": 0.12205883610742861, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 26, + "j": 30, + "heldout_inlier_ratio": 0.7634835395750642, + "heldout_inlier_rmse_m": 0.10495724850674515, + "rotation_invariant_error_deg": 0.5694300136306936, + "reverse_translation_m": 0.008489213184549637, + "reverse_rotation_deg": 0.021733210949119494, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 26, + "j": 31, + "heldout_inlier_ratio": 0.7374054682955207, + "heldout_inlier_rmse_m": 0.10737521663044328, + "rotation_invariant_error_deg": 0.031357796447281316, + "reverse_translation_m": 0.007807741115783734, + "reverse_rotation_deg": 0.04372865220115068, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 27, + "j": 28, + "heldout_inlier_ratio": 0.9281131178707225, + "heldout_inlier_rmse_m": 0.07220941715117642, + "rotation_invariant_error_deg": 0.05880245510741844, + "reverse_translation_m": 0.002300778353404822, + "reverse_rotation_deg": 0.001589714984734129, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 27, + "j": 29, + "heldout_inlier_ratio": 0.7871188037207112, + "heldout_inlier_rmse_m": 0.10966659884725233, + "rotation_invariant_error_deg": 0.5032080902871527, + "reverse_translation_m": 0.005134403698946511, + "reverse_rotation_deg": 0.024348440992038003, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 27, + "j": 30, + "heldout_inlier_ratio": 0.7922108208955224, + "heldout_inlier_rmse_m": 0.10147035321534549, + "rotation_invariant_error_deg": 0.43965382907569506, + "reverse_translation_m": 0.007488026390185518, + "reverse_rotation_deg": 0.03860522829819172, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 27, + "j": 31, + "heldout_inlier_ratio": 0.7592097617664149, + "heldout_inlier_rmse_m": 0.10539218018667616, + "rotation_invariant_error_deg": 0.1790519739515446, + "reverse_translation_m": 0.0020937297862771895, + "reverse_rotation_deg": 0.027401753528281184, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 27, + "j": 32, + "heldout_inlier_ratio": 0.7480278422273782, + "heldout_inlier_rmse_m": 0.10408734715846861, + "rotation_invariant_error_deg": 0.2846642642131201, + "reverse_translation_m": 0.004407463145301957, + "reverse_rotation_deg": 0.03490543724835993, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 28, + "j": 29, + "heldout_inlier_ratio": 0.787814381863266, + "heldout_inlier_rmse_m": 0.10901449493832827, + "rotation_invariant_error_deg": 0.32455907844826015, + "reverse_translation_m": 0.01570175790237293, + "reverse_rotation_deg": 0.035696604981368125, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 28, + "j": 30, + "heldout_inlier_ratio": 0.7791159962581852, + "heldout_inlier_rmse_m": 0.1043856075500982, + "rotation_invariant_error_deg": 0.40289749313456014, + "reverse_translation_m": 0.011701496185342901, + "reverse_rotation_deg": 0.047047156803733815, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 29, + "j": 30, + "heldout_inlier_ratio": 0.7320662880982732, + "heldout_inlier_rmse_m": 0.10929719051930432, + "rotation_invariant_error_deg": 0.013598924391132527, + "reverse_translation_m": 0.005463604154020641, + "reverse_rotation_deg": 0.01858348348785812, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 29, + "j": 31, + "heldout_inlier_ratio": 0.7254562254562255, + "heldout_inlier_rmse_m": 0.11323336570178014, + "rotation_invariant_error_deg": 0.5964723771180616, + "reverse_translation_m": 0.005077799585122041, + "reverse_rotation_deg": 0.07265894091769737, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 30, + "j": 31, + "heldout_inlier_ratio": 0.8286237272623269, + "heldout_inlier_rmse_m": 0.09646596945131831, + "rotation_invariant_error_deg": 0.63029837626231, + "reverse_translation_m": 0.018905314487389895, + "reverse_rotation_deg": 0.08632503464138006, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 30, + "j": 32, + "heldout_inlier_ratio": 0.8065326633165829, + "heldout_inlier_rmse_m": 0.09815063400019147, + "rotation_invariant_error_deg": 0.7279843595666335, + "reverse_translation_m": 0.007341509941520377, + "reverse_rotation_deg": 0.023973741904830165, + "accepted": true, + "rejection_reasons": [] + }, + { + "i": 31, + "j": 32, + "heldout_inlier_ratio": 0.8146841206602162, + "heldout_inlier_rmse_m": 0.09914628416022428, + "rotation_invariant_error_deg": 0.117332543973383, + "reverse_translation_m": 0.0693687705829607, + "reverse_rotation_deg": 0.45703051311645604, + "accepted": false, + "rejection_reasons": [ + "forward_reverse_translation" + ] + }, + { + "i": 31, + "j": 33, + "heldout_inlier_ratio": 0.7551430598250177, + "heldout_inlier_rmse_m": 0.1003246191461096, + "rotation_invariant_error_deg": 0.09234543926339711, + "reverse_translation_m": 0.005559091155809675, + "reverse_rotation_deg": 0.033005318250111694, + "accepted": true, + "rejection_reasons": [] + } + ] +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/extrinsic_rtk_lidar.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/extrinsic_rtk_lidar.json new file mode 100644 index 0000000..a3902b5 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/small_gicp/extrinsic_rtk_lidar.json @@ -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" +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/results/reference_data4/summary.json b/LiDAR_RTK_Direct_Calibration/results/reference_data4/summary.json new file mode 100644 index 0000000..43f583b --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/results/reference_data4/summary.json @@ -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 + ] + ] + } +} \ No newline at end of file diff --git a/LiDAR_RTK_Direct_Calibration/run/README.md b/LiDAR_RTK_Direct_Calibration/run/README.md new file mode 100644 index 0000000..120bf1c --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/run/README.md @@ -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。 diff --git a/LiDAR_RTK_Direct_Calibration/run/export_multisensor_stations.ps1 b/LiDAR_RTK_Direct_Calibration/run/export_multisensor_stations.ps1 new file mode 100644 index 0000000..e96be10 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/run/export_multisensor_stations.ps1 @@ -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" diff --git a/LiDAR_RTK_Direct_Calibration/run/prepare_multisensor_dataset.ps1 b/LiDAR_RTK_Direct_Calibration/run/prepare_multisensor_dataset.ps1 new file mode 100644 index 0000000..a5b07a9 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/run/prepare_multisensor_dataset.ps1 @@ -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" } diff --git a/LiDAR_RTK_Direct_Calibration/run/run_direct_rtk_lidar.ps1 b/LiDAR_RTK_Direct_Calibration/run/run_direct_rtk_lidar.ps1 new file mode 100644 index 0000000..7c837b2 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/run/run_direct_rtk_lidar.ps1 @@ -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')" diff --git a/LiDAR_RTK_Direct_Calibration/run/run_full_pipeline.ps1 b/LiDAR_RTK_Direct_Calibration/run/run_full_pipeline.ps1 new file mode 100644 index 0000000..0ad9f17 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/run/run_full_pipeline.ps1 @@ -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')" diff --git a/LiDAR_RTK_Direct_Calibration/run/run_single_dataset.ps1 b/LiDAR_RTK_Direct_Calibration/run/run_single_dataset.ps1 new file mode 100644 index 0000000..3bf3e53 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/run/run_single_dataset.ps1 @@ -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" diff --git a/LiDAR_RTK_Direct_Calibration/run/view_result.ps1 b/LiDAR_RTK_Direct_Calibration/run/view_result.ps1 new file mode 100644 index 0000000..e513e61 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/run/view_result.ps1 @@ -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" } diff --git a/LiDAR_RTK_Direct_Calibration/tools/README.md b/LiDAR_RTK_Direct_Calibration/tools/README.md new file mode 100644 index 0000000..c670433 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/README.md @@ -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帧。 diff --git a/LiDAR_RTK_Direct_Calibration/tools/build_multisensor_npz.py b/LiDAR_RTK_Direct_Calibration/tools/build_multisensor_npz.py new file mode 100644 index 0000000..2da2043 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/build_multisensor_npz.py @@ -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()) diff --git a/LiDAR_RTK_Direct_Calibration/tools/frontlidar_dlog_export.py b/LiDAR_RTK_Direct_Calibration/tools/frontlidar_dlog_export.py new file mode 100644 index 0000000..9bfd2fa --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/frontlidar_dlog_export.py @@ -0,0 +1,1963 @@ +#!/usr/bin/env python3 +"""Export Medulla DObject lidar + RTK recordings to sharded NumPy NPZ / pickle. + +The default NPZ path uses only the Python standard library; NumPy is required +only when consuming the exported files, not while exporting them. +""" + +from __future__ import annotations + +import argparse +import array +import bisect +import csv +import io +import json +import math +import os +import pickle +import re +import struct +import sys +import time +import zipfile +from contextlib import nullcontext +from dataclasses import asdict, dataclass +from datetime import datetime, timedelta, timezone +from pathlib import Path +from typing import BinaryIO, Iterable, Iterator, Sequence + + +RECORD_RE = re.compile( + r"^\[(?P[^]]+)\].*?DObject `(?P[^`]+)` post " + r"len=(?P\d+)B, id:(?P[0-9A-Fa-f]+), tic:(?P\d+), " + r"@(?P[^:]+):(?P\d+)" +) +GPS_TEXT_RE = re.compile( + r"^\[(?P[^]]+)\].*?\$GPS-POST-Z>text\)>(?P.+)$" +) +POINT_STRUCT = struct.Struct("<5f") +DOTNET_UNIX_EPOCH_TICKS = 621355968000000000 +TICKS_PER_SECOND = 10_000_000 +TICKS_PER_DAY = 864_000_000_000 +NS_PER_TICK = 100 +FORMAT_VERSION = "medulla-lidar3d-rtk-v2" +RTK_OBJECT_NAMES = ("GPS-POST-Z", "rtk", "GPS-POST") + + +@dataclass(frozen=True) +class RecordRef: + sequence: int + object_name: str + log_time: str + source_log: str + source_dorec: str + source_offset: int + payload_length: int + log_record_id: str + dotnet_ticks: int + + +@dataclass +class RtkSample: + index: int + source: str + object_name: str + dotnet_ticks: int + timestamp_iso_local: str + unix_time_ns: int + log_time: str + record_id: str + source_log: str + source_dorec: str + source_offset: int + payload_length: int + counter: int | None = None + lat: float | None = None + lon: float | None = None + alt_m: float | None = None + raw_heading_deg: float | None = None + vehicle_heading_deg: float | None = None + fix: int | None = None + sat: int | None = None + position_valid: bool | None = None + heading_valid: bool | None = None + heading_solution: str | None = None + position_time: str | None = None + heading_time: str | None = None + last_line: str | None = None + device_name: str | None = None + device_stamp_hex: str | None = None + raw_text: str | None = None + parse_error: str | None = None + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Export Medulla frontlidar dlog records with matched RTK/GPS-POST-Z " + "to per-frame NPZ or pickle files." + ) + ) + parser.add_argument("--dlog", required=True, help="Directory containing dobject/ and dobject_recording/.") + parser.add_argument("--out", required=True, help="Output dataset directory.") + parser.add_argument("--object", default="frontlidar", help="Lidar DObject name; default: frontlidar.") + parser.add_argument("--format", choices=("npz", "pickle"), default="npz") + parser.add_argument("--timezone", default="+08:00", help="Fixed offset used to interpret DateTime.Now.Ticks.") + parser.add_argument("--stride", type=int, default=1, help="Export every Nth lidar frame.") + parser.add_argument("--max-frames", type=int, default=0, help="0 exports all selected frames.") + parser.add_argument("--resume", action="store_true", help="Keep already exported frame files.") + parser.add_argument("--compress", action="store_true", help="Use ZIP deflate level 1 for NPZ files.") + parser.add_argument( + "--include-xyz", + choices=("none", "sensor", "cart", "both"), + default="none", + help="Optionally generate XYZ arrays. Raw N x 5 data is always exported.", + ) + parser.add_argument("--x", type=float, default=736.0, help="Lidar X in vehicle frame, mm.") + parser.add_argument("--y", type=float, default=0.0, help="Lidar Y in vehicle frame, mm.") + parser.add_argument("--z", type=float, default=0.0, help="Lidar Z in vehicle frame, mm.") + parser.add_argument("--yaw", type=float, default=0.0, help="Yaw in degrees.") + parser.add_argument("--pitch", type=float, default=0.0, help="Pitch in degrees.") + parser.add_argument("--roll", type=float, default=0.0, help="Roll in degrees.") + parser.add_argument( + "--rtk-max-dt-ms", + type=float, + default=1000.0, + help=( + "Fail rtk_time_alignment when an interior frame's nearest |dt| exceeds this many " + "milliseconds. Edge frames before the first / after the last RTK are reported " + "separately and do not fail the check." + ), + ) + parser.add_argument( + "--skip-rtk", + action="store_true", + help="Export lidar only (no per-frame RTK matching).", + ) + parser.add_argument( + "--rtk-sidecars", + action="store_true", + help=( + "Also write rtk/ sidecars (gps_post_z / rtk_binary / gps_post / text). " + "Default is off; per-frame NPZ still embeds matched RTK fields." + ), + ) + parser.add_argument( + "--write-reports", + action="store_true", + help=( + "Write audit files under reports/ (metadata.json, manifest.csv, " + "rtk_match.csv, validation_report.json). Default package only has " + "frames/ and README.md." + ), + ) + args = parser.parse_args() + if args.stride < 1: + parser.error("--stride must be >= 1") + if args.max_frames < 0: + parser.error("--max-frames must be >= 0") + if args.rtk_max_dt_ms < 0: + parser.error("--rtk-max-dt-ms must be >= 0") + if args.skip_rtk and args.rtk_sidecars: + parser.error("--rtk-sidecars cannot be used with --skip-rtk") + parse_timezone(args.timezone) + return args + + +def resolve_dlog_root(value: str) -> Path: + root = Path(value).expanduser().resolve() + if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir(): + return root + child = root / "dlog" + if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir(): + return child + raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording") + + +def parse_timezone(text: str) -> timezone: + match = re.fullmatch(r"([+-])(\d{2}):(\d{2})", text.strip()) + if not match: + raise ValueError(f"invalid timezone offset: {text!r}") + minutes = int(match.group(2)) * 60 + int(match.group(3)) + if match.group(1) == "-": + minutes = -minutes + return timezone(timedelta(minutes=minutes)) + + +def timezone_minutes(tz: timezone) -> int: + return int(tz.utcoffset(None).total_seconds() // 60) + + +def dotnet_ticks_to_values(ticks: int, tz: timezone) -> tuple[str, int]: + days, remainder = divmod(ticks, TICKS_PER_DAY) + seconds, subsecond_ticks = divmod(remainder, TICKS_PER_SECOND) + local_dt = datetime(1, 1, 1) + timedelta(days=days, seconds=seconds) + fraction = f"{subsecond_ticks:07d}" + offset = tz.utcoffset(None) + sign = "+" if offset >= timedelta(0) else "-" + total_minutes = abs(int(offset.total_seconds() // 60)) + iso = f"{local_dt:%Y-%m-%dT%H:%M:%S}.{fraction}{sign}{total_minutes // 60:02d}:{total_minutes % 60:02d}" + offset_ticks = int(offset.total_seconds()) * TICKS_PER_SECOND + unix_ns = (ticks - DOTNET_UNIX_EPOCH_TICKS - offset_ticks) * NS_PER_TICK + return iso, unix_ns + + +def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]: + pending: list[tuple[str, str, str, int, int, str, int, str]] = [] + for log_path in sorted((dlog_root / "dobject").rglob("*.log")): + relative_log = log_path.relative_to(dlog_root).as_posix() + with log_path.open("r", encoding="utf-8", errors="replace") as stream: + for line in stream: + match = RECORD_RE.search(line) + if not match or match.group("name").casefold() != object_name.casefold(): + continue + pending.append( + ( + match.group("name"), + match.group("log_time"), + relative_log, + int(match.group("offset")), + int(match.group("len")), + match.group("id").upper(), + int(match.group("tic")), + match.group("file"), + ) + ) + pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3])) + seen: set[tuple[str, int, int]] = set() + records: list[RecordRef] = [] + for item in pending: + key = (item[7].casefold(), item[3], item[6]) + if key in seen: + continue + seen.add(key) + records.append( + RecordRef( + sequence=len(records), + object_name=item[0], + log_time=item[1], + source_log=item[2], + source_dorec=item[7], + source_offset=item[3], + payload_length=item[4], + log_record_id=item[5], + dotnet_ticks=item[6], + ) + ) + return records + + +def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]: + result: dict[str, list[Path]] = {} + for path in (dlog_root / "dobject_recording").rglob("*.dorec"): + result.setdefault(path.name.casefold(), []).append(path) + return result + + +def choose_dorec(index: dict[str, list[Path]], name: str) -> Path: + matches = index.get(Path(name).name.casefold(), []) + if not matches: + raise FileNotFoundError(f"missing recording file: {name}") + if len(matches) > 1: + raise RuntimeError(f"ambiguous recording file {name}: {matches}") + return matches[0] + + +def read_exact(stream: BinaryIO, size: int) -> bytes: + data = stream.read(size) + if len(data) != size: + raise EOFError(f"expected {size} bytes, got {len(data)}") + return data + + +def read_record(path: Path, record: RecordRef) -> tuple[dict[str, object], bytes]: + with path.open("rb") as stream: + stream.seek(record.source_offset) + name_length = read_exact(stream, 1)[0] + name = read_exact(stream, name_length).decode("ascii") + ticks = struct.unpack(" tuple[int, int, bytes]: + if len(payload) < 8: + raise ValueError(f"payload too short: {len(payload)}") + frame_counter, point_count = struct.unpack_from(" tuple[str, int]: + if pos >= len(data): + raise ValueError("truncated length-prefixed string") + length = data[pos] + pos += 1 + end = pos + length + if end > len(data): + raise ValueError("truncated length-prefixed string body") + return data[pos:end].decode("ascii", errors="replace"), end + + +def parse_bool_text(value: str) -> bool | None: + lowered = value.strip().casefold() + if lowered in ("true", "1", "yes"): + return True + if lowered in ("false", "0", "no"): + return False + return None + + +def parse_kv_payload(text: str, separators: Sequence[str] = ("=", ":")) -> dict[str, str]: + result: dict[str, str] = {} + for part in text.replace("\r", "").replace("\n", ",").split(","): + part = part.strip() + if not part: + continue + key = value = None + for sep in separators: + if sep in part: + key, value = part.split(sep, 1) + break + if key is None or value is None: + continue + result[key.strip()] = value.strip() + return result + + +def coerce_int(value: str | None) -> int | None: + if value is None or value == "": + return None + try: + return int(float(value)) + except ValueError: + return None + + +def coerce_float(value: str | None) -> float | None: + if value is None or value == "": + return None + try: + return float(value) + except ValueError: + return None + + +def parse_gps_post_z_text(text: str) -> dict[str, object]: + fields = parse_kv_payload(text, separators=("=", ":")) + alt = fields.get("alt", fields.get("h")) + vehicle_heading = fields.get("vehicleHeading", fields.get("th")) + return { + "counter": coerce_int(fields.get("counter")), + "lat": coerce_float(fields.get("lat")), + "lon": coerce_float(fields.get("lon")), + "alt_m": coerce_float(alt), + "raw_heading_deg": coerce_float(fields.get("rawHeading")), + "vehicle_heading_deg": coerce_float(vehicle_heading), + "fix": coerce_int(fields.get("fix")), + "sat": coerce_int(fields.get("sat")), + "position_valid": parse_bool_text(fields.get("positionValid", "")), + "heading_valid": parse_bool_text(fields.get("headingValid", "")), + "heading_solution": fields.get("headingSolution"), + "position_time": fields.get("positionTime"), + "heading_time": fields.get("headingTime"), + "last_line": fields.get("lastLine"), + "time_field": fields.get("time"), + "raw_fields": fields, + } + + +def parse_rtk_binary_payload(payload: bytes) -> dict[str, object]: + pos = 0 + device_name, pos = read_len_prefixed_ascii(payload, pos) + version, counter, unknown0 = struct.unpack_from("= len(payload): + raise ValueError("rtk payload truncated before validity flags") + position_valid = bool(payload[pos]) + pos += 1 + raw_heading, vehicle_heading = struct.unpack_from("= len(payload): + raise ValueError("rtk payload truncated before heading validity") + heading_valid = bool(payload[pos]) + pos += 1 + heading_solution, pos = read_len_prefixed_ascii(payload, pos) + has_nmea = bool(payload[pos]) if pos < len(payload) else False + if pos < len(payload): + pos += 1 + last_line = None + if has_nmea and pos < len(payload): + last_line, pos = read_len_prefixed_ascii(payload, pos) + return { + "device_name": device_name, + "version": version, + "counter": counter, + "unknown0": unknown0, + "device_stamp_hex": device_stamp.hex().upper(), + "position_time": position_time, + "heading_time": heading_time, + "lat": lat, + "lon": lon, + "alt_m": alt, + "reserved0": reserved0, + "reserved1": reserved1, + "fix": fix, + "sat": sat, + "position_valid": position_valid, + "raw_heading_deg": raw_heading, + "vehicle_heading_deg": vehicle_heading, + "heading_valid": heading_valid, + "heading_solution": heading_solution, + "last_line": last_line, + "bytes_consumed": pos, + "payload_length": len(payload), + } + + +def parse_gps_post_payload(payload: bytes) -> dict[str, object]: + pos = 0 + name, pos = read_len_prefixed_ascii(payload, pos) + if pos + 20 > len(payload): + raise ValueError("GPS-POST payload too short") + counter, unknown0 = struct.unpack_from(" tuple[float, ...]: + yaw, pitch, roll = map(math.radians, (yaw_deg, pitch_deg, roll_deg)) + cy, sy = math.cos(yaw), math.sin(yaw) + cp, sp = math.cos(pitch), math.sin(pitch) + cr, sr = math.cos(roll), math.sin(roll) + return ( + cy * cp, + -sy * cr + cy * sp * sr, + sy * sr + cy * sp * cr, + sy * cp, + cy * cr + sy * sp * sr, + -cy * sr + sy * sp * cr, + -sp, + cp * sr, + cp * cr, + ) + + +def generate_xyz( + raw_points: bytes, + mode: str, + translation: tuple[float, float, float], + rotation: tuple[float, ...], +) -> tuple[bytes | None, bytes | None]: + need_sensor = mode in ("sensor", "both") + need_cart = mode in ("cart", "both") + sensor = array.array("f") if need_sensor else None + cart = array.array("f") if need_cart else None + tx, ty, tz = translation + r = rotation + for distance, azimuth, altitude, _intensity, _progression in POINT_STRUCT.iter_unpack(raw_points): + alt = math.radians(altitude) + azi = math.radians(azimuth) + cos_alt = math.cos(alt) + sx = distance * cos_alt * math.cos(azi) + sy = distance * cos_alt * math.sin(azi) + sz = distance * math.sin(alt) + if sensor is not None: + sensor.extend((sx, sy, sz)) + if cart is not None: + cart.extend( + ( + r[0] * sx + r[1] * sy + r[2] * sz + tx, + r[3] * sx + r[4] * sy + r[5] * sz + ty, + r[6] * sx + r[7] * sy + r[8] * sz + tz, + ) + ) + if sys.byteorder != "little": + if sensor is not None: + sensor.byteswap() + if cart is not None: + cart.byteswap() + return (sensor.tobytes() if sensor is not None else None, cart.tobytes() if cart is not None else None) + + +def numpy_header(descr: str, shape: Sequence[int]) -> bytes: + shape_text = "(" + ", ".join(str(value) for value in shape) + if len(shape) == 1: + shape_text += "," + shape_text += ")" + header = f"{{'descr': '{descr}', 'fortran_order': False, 'shape': {shape_text}, }}" + encoded = header.encode("latin-1") + padding = (-((10 + len(encoded) + 1) % 16)) % 16 + encoded += b" " * padding + b"\n" + if len(encoded) > 65535: + raise ValueError("NPY v1 header is too long") + return b"\x93NUMPY\x01\x00" + struct.pack(" None: + with archive.open(name + ".npy", "w", force_zip64=True) as entry: + entry.write(numpy_header(descr, shape)) + entry.write(data) + + +def scalar_bytes(fmt: str, value: int | float) -> bytes: + return struct.pack(fmt, value) + + +def open_npz_writer(path: Path, compress: bool) -> zipfile.ZipFile: + compression = zipfile.ZIP_DEFLATED if compress else zipfile.ZIP_STORED + kwargs: dict[str, object] = {"compression": compression, "allowZip64": True} + if compress: + kwargs["compresslevel"] = 1 + return zipfile.ZipFile(path, "w", **kwargs) + + +def pack_f64_array(values: Sequence[float]) -> bytes: + data = array.array("d", values) + if sys.byteorder != "little": + data.byteswap() + return data.tobytes() + + +def pack_i8_array(values: Sequence[int]) -> bytes: + data = array.array("q", values) + if sys.byteorder != "little": + data.byteswap() + return data.tobytes() + + +def pack_i4_array(values: Sequence[int]) -> bytes: + data = array.array("i", values) + if sys.byteorder != "little": + data.byteswap() + return data.tobytes() + + +def pack_u1_bools(values: Sequence[bool | None]) -> bytes: + return bytes(1 if value else 0 for value in values) + + +def export_npz( + path: Path, + metadata: dict[str, object], + raw_points: bytes, + point_count: int, + xyz_sensor: bytes | None, + xyz_cart: bytes | None, + compress: bool, + rtk_arrays: dict[str, tuple[str, Sequence[int], bytes]] | None = None, +) -> None: + temp = path.with_suffix(path.suffix + ".tmp") + with open_npz_writer(temp, compress) as archive: + write_npy_entry(archive, "points_raw", " None: + points = array.array("f") + points.frombytes(raw_points) + if sys.byteorder != "little": + points.byteswap() + frame: dict[str, object] = { + "metadata": metadata, + "point_columns": ("d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"), + "points_raw_flat_f32": points, + "points_raw_shape": (point_count, 5), + } + for key, raw in (("xyz_sensor_mm_flat_f32", xyz_sensor), ("xyz_cart_mm_flat_f32", xyz_cart)): + if raw is not None: + values = array.array("f") + values.frombytes(raw) + if sys.byteorder != "little": + values.byteswap() + frame[key] = values + frame[key.replace("_flat_f32", "_shape")] = (point_count, 3) + temp = path.with_suffix(path.suffix + ".tmp") + with temp.open("wb") as stream: + pickle.dump(frame, stream, protocol=5) + os.replace(temp, path) + + +def frame_filename(record: RecordRef, frame_counter: int, extension: str) -> str: + return f"{record.object_name}_{record.sequence:06d}_{record.dotnet_ticks}_frame{frame_counter:010d}.{extension}" + + +def manifest_fields(include_rtk: bool) -> list[str]: + fields = [ + "sequence", + "status", + "object_name", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "record_id_bytes_hex", + "frame_counter", + "point_count", + "payload_length", + "source_log", + "source_dorec", + "source_offset", + "output_file", + "error", + ] + if include_rtk: + fields.extend( + [ + "rtk_matched", + "rtk_nearest_index", + "rtk_prev_index", + "rtk_next_index", + "rtk_dt_ns", + "rtk_prev_dt_ns", + "rtk_next_dt_ns", + "rtk_lat", + "rtk_lon", + "rtk_alt_m", + "rtk_vehicle_heading_deg", + "rtk_raw_heading_deg", + "rtk_fix", + "rtk_sat", + "rtk_position_valid", + "rtk_heading_valid", + "rtk_heading_solution", + ] + ) + return fields + + +def nan_if_none(value: float | None) -> float: + return float("nan") if value is None else float(value) + + +def int_or_sentinel(value: int | None, sentinel: int = -1) -> int: + return sentinel if value is None else int(value) + + +def bool_or_false(value: bool | None) -> bool: + return bool(value) + + +def sample_to_public_dict(sample: RtkSample) -> dict[str, object]: + data = asdict(sample) + return data + + +def write_jsonl(path: Path, rows: Iterable[dict[str, object]]) -> int: + count = 0 + with path.open("w", encoding="utf-8") as stream: + for row in rows: + stream.write(json.dumps(row, ensure_ascii=False, separators=(",", ":"))) + stream.write("\n") + count += 1 + return count + + +def write_csv_rows(path: Path, fieldnames: Sequence[str], rows: Iterable[dict[str, object]]) -> int: + count = 0 + with path.open("w", newline="", encoding="utf-8-sig") as stream: + writer = csv.DictWriter(stream, fieldnames=list(fieldnames), extrasaction="ignore") + writer.writeheader() + for row in rows: + writer.writerow({key: row.get(key, "") for key in fieldnames}) + count += 1 + return count + + +def export_rtk_table_npz(path: Path, samples: Sequence[RtkSample], compress: bool) -> None: + n = len(samples) + temp = path.with_suffix(path.suffix + ".tmp") + with open_npz_writer(temp, compress) as archive: + write_npy_entry(archive, "index", " tuple[list[RtkSample], dict[str, int]]: + records = discover_records(root, "GPS-POST-Z") + samples: list[RtkSample] = [] + stats = { + "discovered": len(records), + "parsed_ok": 0, + "parse_errors": 0, + "read_errors": 0, + } + for record in records: + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + sample = RtkSample( + index=len(samples), + source="dobject", + object_name=record.object_name, + dotnet_ticks=record.dotnet_ticks, + timestamp_iso_local=timestamp_iso, + unix_time_ns=unix_ns, + log_time=record.log_time, + record_id=record.log_record_id, + source_log=record.source_log, + source_dorec=record.source_dorec, + source_offset=record.source_offset, + payload_length=record.payload_length, + ) + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + sample.record_id = str(record_meta["record_id"]) + text = payload.decode("utf-8", errors="replace") + sample.raw_text = text + parsed = parse_gps_post_z_text(text) + sample.counter = parsed["counter"] # type: ignore[assignment] + sample.lat = parsed["lat"] # type: ignore[assignment] + sample.lon = parsed["lon"] # type: ignore[assignment] + sample.alt_m = parsed["alt_m"] # type: ignore[assignment] + sample.raw_heading_deg = parsed["raw_heading_deg"] # type: ignore[assignment] + sample.vehicle_heading_deg = parsed["vehicle_heading_deg"] # type: ignore[assignment] + sample.fix = parsed["fix"] # type: ignore[assignment] + sample.sat = parsed["sat"] # type: ignore[assignment] + sample.position_valid = parsed["position_valid"] # type: ignore[assignment] + sample.heading_valid = parsed["heading_valid"] # type: ignore[assignment] + sample.heading_solution = parsed["heading_solution"] # type: ignore[assignment] + sample.position_time = parsed["position_time"] # type: ignore[assignment] + sample.heading_time = parsed["heading_time"] # type: ignore[assignment] + sample.last_line = parsed["last_line"] # type: ignore[assignment] + stats["parsed_ok"] += 1 + except Exception as exc: + sample.parse_error = f"{type(exc).__name__}: {exc}" + if "read" in type(exc).__name__.casefold() or "mismatch" in str(exc).casefold(): + stats["read_errors"] += 1 + else: + stats["parse_errors"] += 1 + samples.append(sample) + _ = timezone_text + return samples, stats + + +def load_rtk_binary_samples( + root: Path, + dorec_index: dict[str, list[Path]], + tz: timezone, +) -> tuple[list[RtkSample], dict[str, int]]: + records = discover_records(root, "rtk") + samples: list[RtkSample] = [] + stats = {"discovered": len(records), "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + for record in records: + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + sample = RtkSample( + index=len(samples), + source="dobject", + object_name=record.object_name, + dotnet_ticks=record.dotnet_ticks, + timestamp_iso_local=timestamp_iso, + unix_time_ns=unix_ns, + log_time=record.log_time, + record_id=record.log_record_id, + source_log=record.source_log, + source_dorec=record.source_dorec, + source_offset=record.source_offset, + payload_length=record.payload_length, + ) + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + sample.record_id = str(record_meta["record_id"]) + parsed = parse_rtk_binary_payload(payload) + sample.device_name = str(parsed["device_name"]) + sample.device_stamp_hex = str(parsed["device_stamp_hex"]) + sample.counter = int(parsed["counter"]) # type: ignore[arg-type] + sample.lat = float(parsed["lat"]) # type: ignore[arg-type] + sample.lon = float(parsed["lon"]) # type: ignore[arg-type] + sample.alt_m = float(parsed["alt_m"]) # type: ignore[arg-type] + sample.raw_heading_deg = float(parsed["raw_heading_deg"]) # type: ignore[arg-type] + sample.vehicle_heading_deg = float(parsed["vehicle_heading_deg"]) # type: ignore[arg-type] + sample.fix = int(parsed["fix"]) # type: ignore[arg-type] + sample.sat = int(parsed["sat"]) # type: ignore[arg-type] + sample.position_valid = bool(parsed["position_valid"]) + sample.heading_valid = bool(parsed["heading_valid"]) + sample.heading_solution = str(parsed["heading_solution"]) + sample.position_time = str(parsed["position_time"]) + sample.heading_time = str(parsed["heading_time"]) + sample.last_line = parsed["last_line"] # type: ignore[assignment] + sample.raw_text = json.dumps(parsed, ensure_ascii=False, separators=(",", ":")) + stats["parsed_ok"] += 1 + except Exception as exc: + sample.parse_error = f"{type(exc).__name__}: {exc}" + stats["parse_errors"] += 1 + samples.append(sample) + return samples, stats + + +def load_gps_post_samples( + root: Path, + dorec_index: dict[str, list[Path]], + tz: timezone, +) -> tuple[list[dict[str, object]], dict[str, int]]: + records = discover_records(root, "GPS-POST") + rows: list[dict[str, object]] = [] + stats = {"discovered": len(records), "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + for index, record in enumerate(records): + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + row: dict[str, object] = { + "index": index, + "object_name": record.object_name, + "dotnet_ticks": record.dotnet_ticks, + "timestamp_iso_local": timestamp_iso, + "unix_time_ns": unix_ns, + "log_time": record.log_time, + "record_id": record.log_record_id, + "source_log": record.source_log, + "source_dorec": record.source_dorec, + "source_offset": record.source_offset, + "payload_length": record.payload_length, + "parse_error": "", + } + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + row["record_id"] = record_meta["record_id"] + parsed = parse_gps_post_payload(payload) + row.update(parsed) + stats["parsed_ok"] += 1 + except Exception as exc: + row["parse_error"] = f"{type(exc).__name__}: {exc}" + stats["parse_errors"] += 1 + rows.append(row) + return rows, stats + + +def load_gps_post_z_text_logs(root: Path) -> tuple[list[dict[str, object]], dict[str, int]]: + rows: list[dict[str, object]] = [] + stats = {"files": 0, "lines": 0, "parsed_ok": 0, "parse_errors": 0} + text_root = root / "GPS-POST-Z" + if not text_root.is_dir(): + return rows, stats + for log_path in sorted(text_root.rglob("*.log")): + stats["files"] += 1 + relative = log_path.relative_to(root).as_posix() + with log_path.open("r", encoding="utf-8", errors="replace") as stream: + for line_no, line in enumerate(stream, start=1): + match = GPS_TEXT_RE.search(line) + if not match: + continue + stats["lines"] += 1 + body = match.group("body").strip() + row: dict[str, object] = { + "index": len(rows), + "source": "GPS-POST-Z-text-log", + "source_log": relative, + "line_no": line_no, + "log_time": match.group("log_time"), + "raw_text": body, + "parse_error": "", + } + try: + parsed = parse_gps_post_z_text(body) + row.update( + { + "lat": parsed["lat"], + "lon": parsed["lon"], + "alt_m": parsed["alt_m"], + "raw_heading_deg": parsed["raw_heading_deg"], + "vehicle_heading_deg": parsed["vehicle_heading_deg"], + "fix": parsed["fix"], + "sat": parsed["sat"], + "position_valid": parsed["position_valid"], + "heading_valid": parsed["heading_valid"], + "heading_solution": parsed["heading_solution"], + "position_time": parsed["position_time"], + "heading_time": parsed["heading_time"], + } + ) + stats["parsed_ok"] += 1 + except Exception as exc: + row["parse_error"] = f"{type(exc).__name__}: {exc}" + stats["parse_errors"] += 1 + rows.append(row) + return rows, stats + + +def match_rtk(unix_time_ns: int, samples: Sequence[RtkSample]) -> dict[str, object]: + if not samples: + return { + "matched": False, + "nearest_index": -1, + "prev_index": -1, + "next_index": -1, + "dt_ns": None, + "prev_dt_ns": None, + "next_dt_ns": None, + "sample": None, + } + ticks = [sample.unix_time_ns for sample in samples] + pos = bisect.bisect_left(ticks, unix_time_ns) + if pos < len(samples) and ticks[pos] == unix_time_ns: + nearest = prev_index = next_index = pos + else: + candidates: list[int] = [] + if pos < len(samples): + candidates.append(pos) + if pos > 0: + candidates.append(pos - 1) + nearest = min(candidates, key=lambda idx: (abs(ticks[idx] - unix_time_ns), idx)) + prev_index = pos - 1 if pos > 0 else -1 + next_index = pos if pos < len(samples) else -1 + sample = samples[nearest] + prev_dt = None if prev_index < 0 else unix_time_ns - samples[prev_index].unix_time_ns + next_dt = None if next_index < 0 else samples[next_index].unix_time_ns - unix_time_ns + return { + "matched": True, + "nearest_index": nearest, + "prev_index": prev_index, + "next_index": next_index, + "dt_ns": unix_time_ns - sample.unix_time_ns, + "prev_dt_ns": prev_dt, + "next_dt_ns": next_dt, + "sample": sample, + } + + +def rtk_frame_arrays(match: dict[str, object]) -> dict[str, tuple[str, Sequence[int], bytes]]: + sample: RtkSample | None = match["sample"] # type: ignore[assignment] + return { + "rtk_nearest_index": (" dict[str, object]: + rtk_dir = output / "rtk" + rtk_dir.mkdir(exist_ok=True) + written: dict[str, object] = {"directory": "rtk"} + + export_rtk_table_npz(rtk_dir / "gps_post_z.npz", gps_post_z, compress) + gps_dicts = [sample_to_public_dict(sample) for sample in gps_post_z] + write_jsonl(rtk_dir / "gps_post_z.jsonl", gps_dicts) + write_csv_rows( + rtk_dir / "gps_post_z.csv", + [ + "index", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "counter", + "lat", + "lon", + "alt_m", + "raw_heading_deg", + "vehicle_heading_deg", + "fix", + "sat", + "position_valid", + "heading_valid", + "heading_solution", + "position_time", + "heading_time", + "last_line", + "source_log", + "source_dorec", + "source_offset", + "payload_length", + "parse_error", + "raw_text", + ], + gps_dicts, + ) + written["gps_post_z"] = { + "npz": "rtk/gps_post_z.npz", + "csv": "rtk/gps_post_z.csv", + "jsonl": "rtk/gps_post_z.jsonl", + "count": len(gps_post_z), + } + + export_rtk_table_npz(rtk_dir / "rtk_binary.npz", rtk_binary, compress) + rtk_dicts = [sample_to_public_dict(sample) for sample in rtk_binary] + write_jsonl(rtk_dir / "rtk_binary.jsonl", rtk_dicts) + write_csv_rows( + rtk_dir / "rtk_binary.csv", + [ + "index", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "device_name", + "device_stamp_hex", + "counter", + "lat", + "lon", + "alt_m", + "raw_heading_deg", + "vehicle_heading_deg", + "fix", + "sat", + "position_valid", + "heading_valid", + "heading_solution", + "position_time", + "heading_time", + "last_line", + "source_log", + "source_dorec", + "source_offset", + "payload_length", + "parse_error", + ], + rtk_dicts, + ) + written["rtk_binary"] = { + "npz": "rtk/rtk_binary.npz", + "csv": "rtk/rtk_binary.csv", + "jsonl": "rtk/rtk_binary.jsonl", + "count": len(rtk_binary), + } + + write_jsonl(rtk_dir / "gps_post.jsonl", gps_post_rows) + write_csv_rows( + rtk_dir / "gps_post.csv", + [ + "index", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "device_name", + "counter", + "unknown0", + "flags_hex", + "device_stamp_hex", + "source_log", + "source_dorec", + "source_offset", + "payload_length", + "payload_hex", + "parse_error", + ], + gps_post_rows, + ) + # Compact NPZ for GPS-POST counters / stamps only. + temp = (rtk_dir / "gps_post.npz").with_suffix(".npz.tmp") + with open_npz_writer(temp, compress) as archive: + n = len(gps_post_rows) + write_npy_entry( + archive, + "index", + " dict[str, object]: + abs_dts = [abs(value) for value in match_dts_ns] + abs_interior = [abs(value) for value in interior_dts_ns] + threshold_ns = rtk_max_dt_ms * 1_000_000.0 + over_threshold = sum(1 for value in abs_interior if value > threshold_ns) + checks: list[dict[str, object]] = [ + { + "name": "lidar_payload_length", + "ok": lidar_stats.get("frames_with_errors", 0) == 0, + "detail": "Each lidar payload must equal 8 + point_count * 20.", + }, + { + "name": "gps_post_z_parse", + "ok": gps_post_z_stats.get("parse_errors", 0) == 0 + and gps_post_z_stats.get("read_errors", 0) == 0, + "detail": "All GPS-POST-Z dobject payloads should parse.", + }, + { + "name": "lidar_rtk_coverage", + "ok": unmatched_frames == 0, + "detail": "Every exported lidar frame should have a nearest GPS-POST-Z sample.", + }, + { + "name": "rtk_time_alignment", + "ok": over_threshold == 0, + "detail": ( + f"Interior frames (with both prev/next RTK) should have nearest |dt| " + f"<= {rtk_max_dt_ms} ms." + ), + }, + ] + if rtk_sidecars: + checks.insert( + 2, + { + "name": "rtk_binary_parse", + "ok": rtk_binary_stats.get("parse_errors", 0) == 0, + "detail": "All rtk binary payloads should parse as UNICORE_N4_RTK_V1.", + }, + ) + report = { + "format_version": FORMAT_VERSION, + "generated_at": datetime.now().astimezone().isoformat(), + "lidar": lidar_stats, + "rtk_sidecars": rtk_sidecars, + "rtk_sources": { + "GPS-POST-Z_dobject": gps_post_z_stats, + "rtk_dobject": rtk_binary_stats if rtk_sidecars else {"skipped": True}, + "GPS-POST_dobject": gps_post_stats if rtk_sidecars else {"skipped": True}, + "GPS-POST-Z_text_log": text_stats if rtk_sidecars else {"skipped": True}, + }, + "matching": { + "primary_source": "GPS-POST-Z", + "matched_frames": matched_frames, + "unmatched_frames": unmatched_frames, + "edge_frames_outside_rtk_span": edge_frames, + "rtk_max_dt_ms_threshold": rtk_max_dt_ms, + "interior_frames_over_threshold": over_threshold, + "dt_ns": { + "count": len(abs_dts), + "min": min(match_dts_ns) if match_dts_ns else None, + "max": max(match_dts_ns) if match_dts_ns else None, + "abs_min": min(abs_dts) if abs_dts else None, + "abs_max": max(abs_dts) if abs_dts else None, + "abs_mean": (sum(abs_dts) / len(abs_dts)) if abs_dts else None, + }, + "interior_dt_ns": { + "count": len(abs_interior), + "abs_min": min(abs_interior) if abs_interior else None, + "abs_max": max(abs_interior) if abs_interior else None, + "abs_mean": (sum(abs_interior) / len(abs_interior)) if abs_interior else None, + }, + }, + "checks": checks, + } + report["ok"] = all(bool(check["ok"]) for check in checks) + return report + + +def write_package_readme( + path: Path, + *, + format_version: str, + frame_count: int, + include_rtk: bool, + timezone_text: str, +) -> None: + rtk_note = ( + "每帧 NPZ 已嵌入最近 RTK：`rtk_lat` / `rtk_lon` / `rtk_alt_m` / " + "`rtk_vehicle_heading_deg` / `rtk_fix` / `rtk_sat` 等。\n" + if include_rtk + else "本包未嵌入 RTK（导出时使用了 `--skip-rtk`）。\n" + ) + text = """# FrontLidar 数据集 + +格式版本：`@@FORMAT_VERSION@@` +帧数：`@@FRAME_COUNT@@` +时区：`@@TIMEZONE@@` + +## 目录 + +```text +. + README.md # 本说明 + frames/ # 逐帧 NPZ + frontlidar___frame.npz +``` + +直接打包本目录即可分发。 + +## 依赖 + +```powershell +pip install numpy +``` + +## 快速读取 + +```python +import json +from pathlib import Path +import numpy as np + +root = Path(__file__).resolve().parent # 或改成数据集路径 +frame_path = next(sorted((root / "frames").glob("*.npz"))) + +with np.load(frame_path, allow_pickle=False) as f: + points = f["points_raw"] # float32 (N, 5) + unix_ns = int(f["unix_time_ns"][0]) + meta = json.loads(f["metadata_json_utf8"].tobytes().decode("utf-8")) + + # 列: d_mm, azimuth_deg, altitude_deg, intensity, progression + d_mm, az, alt, intensity, prog = (points[:, i] for i in range(5)) + + # RTK（若导出时未 --skip-rtk） + if "rtk_lat" in f.files: + lat = float(f["rtk_lat"][0]) + lon = float(f["rtk_lon"][0]) + heading = float(f["rtk_vehicle_heading_deg"][0]) + dt_ms = int(f["rtk_dt_ns"][0]) / 1e6 + print(lat, lon, heading, dt_ms, meta.get("rtk", {}).get("heading_solution")) +``` + +@@RTK_NOTE@@ +## 点云列 + +| 列 | 字段 | 单位 | +|---:|---|---| +| 0 | d_mm | mm | +| 1 | azimuth_deg | ° | +| 2 | altitude_deg | ° | +| 3 | intensity | 设备定义 | +| 4 | progression | 0–1 | + +极坐标转传感器 XYZ（mm）： + +```python +az = np.deg2rad(points[:, 1]) +alt = np.deg2rad(points[:, 2]) +d = points[:, 0] +xyz = np.column_stack(( + d * np.cos(alt) * np.cos(az), + d * np.cos(alt) * np.sin(az), + d * np.sin(alt), +)).astype(np.float32) +``` + +## 时间戳 + +- `dotnet_ticks`：原始权威时间 +- `unix_time_ns`：按导出时区转换的 Unix 纳秒 +- 标量字段均为 shape `(1,)`，用 `[0]` 取出 + +## 批量遍历 + +```python +for path in sorted((root / "frames").glob("*.npz")): + with np.load(path, allow_pickle=False) as f: + pts = f["points_raw"] + # ... +``` + +更多字段说明见导出仓库中的 `FRONTLIDAR_NPZ_READ.md`（若一并提供）。 +""" + text = ( + text.replace("@@FORMAT_VERSION@@", format_version) + .replace("@@FRAME_COUNT@@", str(frame_count)) + .replace("@@TIMEZONE@@", timezone_text) + .replace("@@RTK_NOTE@@", rtk_note) + ) + path.write_text(text, encoding="utf-8") + + +def export_dataset(args: argparse.Namespace) -> int: + started = time.time() + root = resolve_dlog_root(args.dlog) + output = Path(args.out).expanduser().resolve() + output.mkdir(parents=True, exist_ok=True) + frames_dir = output / "frames" + frames_dir.mkdir(exist_ok=True) + tz = parse_timezone(args.timezone) + records = discover_records(root, args.object) + selected = records[:: args.stride] + if args.max_frames: + selected = selected[: args.max_frames] + if not selected: + raise RuntimeError(f"no DObject records named {args.object!r} found under {root}") + dorec_index = index_dorec_files(root) + rotation = rotation_matrix(args.yaw, args.pitch, args.roll) + translation = (args.x, args.y, args.z) + extension = "npz" if args.format == "npz" else "pkl" + include_rtk = not args.skip_rtk + rtk_sidecars = bool(args.rtk_sidecars) + + gps_post_z: list[RtkSample] = [] + rtk_binary: list[RtkSample] = [] + gps_post_rows: list[dict[str, object]] = [] + text_rows: list[dict[str, object]] = [] + gps_post_z_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + rtk_binary_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + gps_post_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + text_stats = {"files": 0, "lines": 0, "parsed_ok": 0, "parse_errors": 0} + rtk_outputs: dict[str, object] = {} + + if include_rtk: + print("Loading GPS-POST-Z for per-frame RTK matching...", flush=True) + gps_post_z, gps_post_z_stats = load_gps_post_z_samples(root, dorec_index, tz, args.timezone) + if rtk_sidecars: + print("Loading RTK sidecars...", flush=True) + rtk_binary, rtk_binary_stats = load_rtk_binary_samples(root, dorec_index, tz) + gps_post_rows, gps_post_stats = load_gps_post_samples(root, dorec_index, tz) + text_rows, text_stats = load_gps_post_z_text_logs(root) + rtk_outputs = export_rtk_sidecars( + output, + bool(args.compress and args.format == "npz"), + gps_post_z, + rtk_binary, + gps_post_rows, + text_rows, + ) + print( + f"RTK loaded: GPS-POST-Z={len(gps_post_z)} rtk={len(rtk_binary)} " + f"GPS-POST={len(gps_post_rows)} text={len(text_rows)} (sidecars on)", + flush=True, + ) + else: + print( + f"RTK loaded: GPS-POST-Z={len(gps_post_z)} (sidecars off; use --rtk-sidecars to write rtk/)", + flush=True, + ) + + write_reports = bool(args.write_reports) + reports_dir = output / "reports" + if write_reports: + reports_dir.mkdir(exist_ok=True) + manifest_tmp = reports_dir / "manifest.partial.csv" + manifest_final = reports_dir / "manifest.csv" + match_tmp = reports_dir / "rtk_match.partial.csv" + match_final = reports_dir / "rtk_match.csv" + manifest_stream_cm: object = manifest_tmp.open("w", newline="", encoding="utf-8-sig") + match_stream = ( + match_tmp.open("w", newline="", encoding="utf-8-sig") if include_rtk else None + ) + else: + manifest_tmp = manifest_final = match_tmp = match_final = None + manifest_stream_cm = nullcontext(io.StringIO()) + match_stream = io.StringIO() if include_rtk else None + + exported = skipped = resumed = total_points = 0 + matched_frames = unmatched_frames = edge_frames = 0 + match_dts_ns: list[int] = [] + interior_dts_ns: list[int] = [] + match_fieldnames = [ + "sequence", + "lidar_unix_time_ns", + "lidar_dotnet_ticks", + "output_file", + "rtk_matched", + "rtk_nearest_index", + "rtk_prev_index", + "rtk_next_index", + "rtk_dt_ns", + "rtk_prev_dt_ns", + "rtk_next_dt_ns", + "rtk_dotnet_ticks", + "rtk_unix_time_ns", + "rtk_lat", + "rtk_lon", + "rtk_alt_m", + "rtk_raw_heading_deg", + "rtk_vehicle_heading_deg", + "rtk_fix", + "rtk_sat", + "rtk_position_valid", + "rtk_heading_valid", + "rtk_heading_solution", + "rtk_position_time", + "rtk_heading_time", + "rtk_last_line", + "rtk_source_dorec", + "rtk_source_offset", + ] + + match_writer = csv.DictWriter(match_stream, fieldnames=match_fieldnames) if match_stream else None + if match_writer: + match_writer.writeheader() + + with manifest_stream_cm as manifest_stream: + writer = csv.DictWriter(manifest_stream, fieldnames=manifest_fields(include_rtk)) + writer.writeheader() + for selected_index, record in enumerate(selected): + row: dict[str, object] = { + "sequence": record.sequence, + "status": "error", + "object_name": record.object_name, + "dotnet_ticks": record.dotnet_ticks, + "timestamp_iso_local": "", + "unix_time_ns": "", + "log_time": record.log_time, + "record_id": record.log_record_id, + "record_id_bytes_hex": "", + "frame_counter": "", + "point_count": "", + "payload_length": record.payload_length, + "source_log": record.source_log, + "source_dorec": record.source_dorec, + "source_offset": record.source_offset, + "output_file": "", + "error": "", + } + if include_rtk: + row.update( + { + "rtk_matched": False, + "rtk_nearest_index": -1, + "rtk_prev_index": -1, + "rtk_next_index": -1, + "rtk_dt_ns": "", + "rtk_prev_dt_ns": "", + "rtk_next_dt_ns": "", + "rtk_lat": "", + "rtk_lon": "", + "rtk_alt_m": "", + "rtk_vehicle_heading_deg": "", + "rtk_raw_heading_deg": "", + "rtk_fix": "", + "rtk_sat": "", + "rtk_position_valid": "", + "rtk_heading_valid": "", + "rtk_heading_solution": "", + } + ) + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + frame_counter, point_count, raw_points = parse_lidar_payload(payload) + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + filename = frame_filename(record, frame_counter, extension) + relative_output = (Path("frames") / filename).as_posix() + frame_path = output / relative_output + metadata: dict[str, object] = { + "format_version": FORMAT_VERSION, + "sequence": record.sequence, + "object_name": record.object_name, + "dotnet_ticks": record.dotnet_ticks, + "timestamp_iso_local": timestamp_iso, + "unix_time_ns": unix_ns, + "timezone": args.timezone, + "log_time": record.log_time, + "record_id": record_meta["record_id"], + "record_id_bytes_hex": record_meta["record_id_bytes_hex"], + "frame_counter": frame_counter, + "point_count": point_count, + "payload_length": record_meta["payload_length"], + "source_log": record.source_log, + "source_dorec": record.source_dorec, + "source_offset": record.source_offset, + "point_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"], + "extrinsic": { + "translation_mm": list(translation), + "yaw_pitch_roll_deg": [args.yaw, args.pitch, args.roll], + "rotation_row_major": list(rotation), + }, + } + rtk_arrays = None + match_info: dict[str, object] | None = None + if include_rtk: + match_info = match_rtk(unix_ns, gps_post_z) + sample: RtkSample | None = match_info["sample"] # type: ignore[assignment] + if match_info["matched"]: + matched_frames += 1 + dt_ns = int(match_info["dt_ns"]) + match_dts_ns.append(dt_ns) + prev_i = int(match_info["prev_index"]) + next_i = int(match_info["next_index"]) + if prev_i < 0 or next_i < 0: + edge_frames += 1 + else: + interior_dts_ns.append(dt_ns) + else: + unmatched_frames += 1 + rtk_meta = { + "matched": bool(match_info["matched"]), + "nearest_index": match_info["nearest_index"], + "prev_index": match_info["prev_index"], + "next_index": match_info["next_index"], + "dt_ns": match_info["dt_ns"], + "prev_dt_ns": match_info["prev_dt_ns"], + "next_dt_ns": match_info["next_dt_ns"], + "primary_source": "GPS-POST-Z", + } + if sample is not None: + rtk_meta.update( + { + "dotnet_ticks": sample.dotnet_ticks, + "unix_time_ns": sample.unix_time_ns, + "timestamp_iso_local": sample.timestamp_iso_local, + "lat": sample.lat, + "lon": sample.lon, + "alt_m": sample.alt_m, + "raw_heading_deg": sample.raw_heading_deg, + "vehicle_heading_deg": sample.vehicle_heading_deg, + "fix": sample.fix, + "sat": sample.sat, + "position_valid": sample.position_valid, + "heading_valid": sample.heading_valid, + "heading_solution": sample.heading_solution, + "position_time": sample.position_time, + "heading_time": sample.heading_time, + "last_line": sample.last_line, + "counter": sample.counter, + "record_id": sample.record_id, + "source_dorec": sample.source_dorec, + "source_offset": sample.source_offset, + } + ) + metadata["rtk"] = rtk_meta + rtk_arrays = rtk_frame_arrays(match_info) + row.update( + { + "rtk_matched": bool(match_info["matched"]), + "rtk_nearest_index": match_info["nearest_index"], + "rtk_prev_index": match_info["prev_index"], + "rtk_next_index": match_info["next_index"], + "rtk_dt_ns": "" if match_info["dt_ns"] is None else match_info["dt_ns"], + "rtk_prev_dt_ns": "" if match_info["prev_dt_ns"] is None else match_info["prev_dt_ns"], + "rtk_next_dt_ns": "" if match_info["next_dt_ns"] is None else match_info["next_dt_ns"], + "rtk_lat": "" if sample is None or sample.lat is None else sample.lat, + "rtk_lon": "" if sample is None or sample.lon is None else sample.lon, + "rtk_alt_m": "" if sample is None or sample.alt_m is None else sample.alt_m, + "rtk_vehicle_heading_deg": ( + "" if sample is None or sample.vehicle_heading_deg is None else sample.vehicle_heading_deg + ), + "rtk_raw_heading_deg": ( + "" if sample is None or sample.raw_heading_deg is None else sample.raw_heading_deg + ), + "rtk_fix": "" if sample is None or sample.fix is None else sample.fix, + "rtk_sat": "" if sample is None or sample.sat is None else sample.sat, + "rtk_position_valid": "" if sample is None else sample.position_valid, + "rtk_heading_valid": "" if sample is None else sample.heading_valid, + "rtk_heading_solution": "" if sample is None else sample.heading_solution, + } + ) + if match_writer is not None: + match_writer.writerow( + { + "sequence": record.sequence, + "lidar_unix_time_ns": unix_ns, + "lidar_dotnet_ticks": record.dotnet_ticks, + "output_file": relative_output, + "rtk_matched": bool(match_info["matched"]), + "rtk_nearest_index": match_info["nearest_index"], + "rtk_prev_index": match_info["prev_index"], + "rtk_next_index": match_info["next_index"], + "rtk_dt_ns": "" if match_info["dt_ns"] is None else match_info["dt_ns"], + "rtk_prev_dt_ns": "" if match_info["prev_dt_ns"] is None else match_info["prev_dt_ns"], + "rtk_next_dt_ns": "" if match_info["next_dt_ns"] is None else match_info["next_dt_ns"], + "rtk_dotnet_ticks": "" if sample is None else sample.dotnet_ticks, + "rtk_unix_time_ns": "" if sample is None else sample.unix_time_ns, + "rtk_lat": "" if sample is None else sample.lat, + "rtk_lon": "" if sample is None else sample.lon, + "rtk_alt_m": "" if sample is None else sample.alt_m, + "rtk_raw_heading_deg": "" if sample is None else sample.raw_heading_deg, + "rtk_vehicle_heading_deg": "" if sample is None else sample.vehicle_heading_deg, + "rtk_fix": "" if sample is None else sample.fix, + "rtk_sat": "" if sample is None else sample.sat, + "rtk_position_valid": "" if sample is None else sample.position_valid, + "rtk_heading_valid": "" if sample is None else sample.heading_valid, + "rtk_heading_solution": "" if sample is None else sample.heading_solution, + "rtk_position_time": "" if sample is None else sample.position_time, + "rtk_heading_time": "" if sample is None else sample.heading_time, + "rtk_last_line": "" if sample is None else sample.last_line, + "rtk_source_dorec": "" if sample is None else sample.source_dorec, + "rtk_source_offset": "" if sample is None else sample.source_offset, + } + ) + + row.update( + { + "timestamp_iso_local": timestamp_iso, + "unix_time_ns": unix_ns, + "record_id": record_meta["record_id"], + "record_id_bytes_hex": record_meta["record_id_bytes_hex"], + "frame_counter": frame_counter, + "point_count": point_count, + "output_file": relative_output, + } + ) + if args.resume and frame_path.exists(): + row["status"] = "resumed" + resumed += 1 + else: + xyz_sensor, xyz_cart = generate_xyz(raw_points, args.include_xyz, translation, rotation) + if args.format == "npz": + export_npz( + frame_path, + metadata, + raw_points, + point_count, + xyz_sensor, + xyz_cart, + args.compress, + rtk_arrays=rtk_arrays, + ) + else: + export_pickle(frame_path, metadata, raw_points, point_count, xyz_sensor, xyz_cart) + row["status"] = "exported" + exported += 1 + total_points += point_count + except Exception as exc: + skipped += 1 + row["error"] = f"{type(exc).__name__}: {exc}" + writer.writerow(row) + if (selected_index + 1) % 25 == 0 or selected_index + 1 == len(selected): + manifest_stream.flush() + if match_stream is not None: + match_stream.flush() + print( + f"[{selected_index + 1}/{len(selected)}] exported={exported} resumed={resumed} " + f"errors={skipped} points={total_points}", + flush=True, + ) + + if match_stream is not None: + match_stream.close() + if write_reports and match_tmp is not None and match_final is not None: + os.replace(match_tmp, match_final) + if write_reports and manifest_tmp is not None and manifest_final is not None: + os.replace(manifest_tmp, manifest_final) + + lidar_stats = { + "records_discovered": len(records), + "records_selected": len(selected), + "frames_exported": exported, + "frames_resumed": resumed, + "frames_with_errors": skipped, + "total_points_in_manifest": total_points, + } + validation = build_validation_report( + lidar_stats=lidar_stats, + gps_post_z_stats=gps_post_z_stats, + rtk_binary_stats=rtk_binary_stats, + gps_post_stats=gps_post_stats, + text_stats=text_stats, + match_dts_ns=match_dts_ns, + interior_dts_ns=interior_dts_ns, + edge_frames=edge_frames, + matched_frames=matched_frames, + unmatched_frames=unmatched_frames, + rtk_max_dt_ms=args.rtk_max_dt_ms, + rtk_sidecars=rtk_sidecars, + ) + + write_package_readme( + output / "README.md", + format_version=FORMAT_VERSION, + frame_count=exported + resumed, + include_rtk=include_rtk, + timezone_text=args.timezone, + ) + + metadata = { + "format_version": FORMAT_VERSION, + "generated_at": datetime.now().astimezone().isoformat(), + "source_dlog": str(root), + "object_name": args.object, + "output_format": args.format, + "npz_compressed": bool(args.compress and args.format == "npz"), + "include_xyz": args.include_xyz, + "include_rtk": include_rtk, + "rtk_sidecars": rtk_sidecars, + "write_reports": write_reports, + "timezone": args.timezone, + "timezone_offset_minutes": timezone_minutes(tz), + "rtk_max_dt_ms": args.rtk_max_dt_ms, + "rtk_objects": list(RTK_OBJECT_NAMES), + "rtk_outputs": rtk_outputs if rtk_sidecars else None, + "package_contents": ["README.md", "frames/"] + + (["rtk/"] if rtk_sidecars else []) + + (["reports/"] if write_reports else []), + **lidar_stats, + "point_columns": [ + {"name": "d_mm", "dtype": "float32", "unit": "mm"}, + {"name": "azimuth_deg", "dtype": "float32", "unit": "degree"}, + {"name": "altitude_deg", "dtype": "float32", "unit": "degree"}, + {"name": "intensity", "dtype": "float32", "unit": "device-specific"}, + {"name": "progression", "dtype": "float32", "unit": "scan fraction"}, + ], + "extrinsic": { + "translation_mm": list(translation), + "yaw_pitch_roll_deg": [args.yaw, args.pitch, args.roll], + "rotation_row_major": list(rotation), + }, + "validation_ok": validation["ok"], + "duration_seconds": round(time.time() - started, 3), + } + if write_reports: + with (reports_dir / "metadata.json").open("w", encoding="utf-8") as stream: + json.dump(metadata, stream, ensure_ascii=False, indent=2) + stream.write("\n") + with (reports_dir / "validation_report.json").open("w", encoding="utf-8") as stream: + json.dump(validation, stream, ensure_ascii=False, indent=2) + stream.write("\n") + + print(json.dumps(metadata, ensure_ascii=False, indent=2), flush=True) + print( + json.dumps( + {"validation_ok": validation["ok"], "checks": validation["checks"]}, + ensure_ascii=False, + indent=2, + ), + flush=True, + ) + if skipped != 0: + return 1 + if include_rtk and not validation["ok"]: + return 1 + return 0 + + +def main() -> int: + try: + return export_dataset(parse_args()) + except Exception as exc: + print(f"ERROR: {type(exc).__name__}: {exc}", file=sys.stderr) + return 2 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/LiDAR_RTK_Direct_Calibration/tools/prepare_multisensor_station_dataset.py b/LiDAR_RTK_Direct_Calibration/tools/prepare_multisensor_station_dataset.py new file mode 100644 index 0000000..c47fdc5 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/prepare_multisensor_station_dataset.py @@ -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()) diff --git a/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/audit_capture_v2.py b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/audit_capture_v2.py new file mode 100644 index 0000000..4d8bb77 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/audit_capture_v2.py @@ -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() diff --git a/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/capture_format_v2.py b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/capture_format_v2.py new file mode 100644 index 0000000..3812980 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/capture_format_v2.py @@ -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(" int: + raw = stream.read(8) + if len(raw) != 8: + raise EOFError("truncated int64") + return struct.unpack(" int: + raw = stream.read(4) + if len(raw) != 4: + raise EOFError("truncated uint32") + return struct.unpack(" 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(" 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 + diff --git a/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/parse_rtk_imu_v2.py b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/parse_rtk_imu_v2.py new file mode 100644 index 0000000..d30a115 --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/parse_rtk_imu_v2.py @@ -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() diff --git a/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/pipeline_common.py b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/pipeline_common.py new file mode 100644 index 0000000..3f733ce --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/pipeline_common.py @@ -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(" dict: + i16 = lambda i: struct.unpack_from(" 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()] diff --git a/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/pipeline_common_corrected.py b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/pipeline_common_corrected.py new file mode 100644 index 0000000..35b37ef --- /dev/null +++ b/LiDAR_RTK_Direct_Calibration/tools/rscap_v2/pipeline_common_corrected.py @@ -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