88 lines
4.0 KiB
Python
88 lines
4.0 KiB
Python
#!/usr/bin/env python3
|
|
"""Build a concise backend comparison and select the recommended result."""
|
|
import argparse
|
|
import json
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--open3d", required=True)
|
|
parser.add_argument("--small", required=True)
|
|
parser.add_argument("--open3d-quality", required=True)
|
|
parser.add_argument("--small-quality", required=True)
|
|
parser.add_argument("--open3d-check", required=True)
|
|
parser.add_argument("--small-check", required=True)
|
|
parser.add_argument("--output", required=True)
|
|
parser.add_argument("--recommended-output", required=True)
|
|
args = parser.parse_args()
|
|
|
|
open_result = json.loads(Path(args.open3d).read_text(encoding="utf-8-sig"))
|
|
small_result = json.loads(Path(args.small).read_text(encoding="utf-8-sig"))
|
|
open_quality = json.loads(Path(args.open3d_quality).read_text(encoding="utf-8-sig"))
|
|
small_quality = json.loads(Path(args.small_quality).read_text(encoding="utf-8-sig"))
|
|
open_check = json.loads(Path(args.open3d_check).read_text(encoding="utf-8-sig"))
|
|
small_check = json.loads(Path(args.small_check).read_text(encoding="utf-8-sig"))
|
|
x_open = np.asarray(open_result["matrix_4x4"], float)
|
|
x_small = np.asarray(small_result["matrix_4x4"], float)
|
|
delta = np.linalg.inv(x_open) @ x_small
|
|
|
|
def compact(result, quality, check):
|
|
estimate = result["estimation"]["residuals"]
|
|
auxiliary = check["metrics"]
|
|
return {
|
|
"translation_m": result["translation_m"],
|
|
"rotation_rpy_deg_xyz": result["rotation_rpy_deg_xyz"],
|
|
"estimation_pairs": estimate["pairs"],
|
|
"estimation_translation_rms_m": estimate["translation_m"]["rms"],
|
|
"estimation_rotation_rms_deg": estimate["rotation_deg"]["rms"],
|
|
"bootstrap_std": result["bootstrap"]["std"],
|
|
"initial_B_loop_closure": quality["accepted_loop_closure"],
|
|
"batch1_auxiliary_pairs": auxiliary["pairs"],
|
|
"batch1_auxiliary_translation_rms_m": auxiliary["translation_m"]["rms"],
|
|
"batch1_auxiliary_rotation_rms_deg": auxiliary["rotation_deg"]["rms"],
|
|
}
|
|
|
|
summary = {
|
|
"recommended_backend": "open3d_gicp",
|
|
"selection_reason": (
|
|
"The two X estimates agree closely; Open3D has lower second-batch AX residual, "
|
|
"better B loop closure, and lower first-batch auxiliary residual."
|
|
),
|
|
"coordinate_convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
|
|
"measured_extrinsic_used_as_initial": False,
|
|
"second_batch_role": "estimation (dense RTK)",
|
|
"first_batch_role": "auxiliary check only (sparse RTK)",
|
|
"backend_difference": {
|
|
"translation_m": float(np.linalg.norm(delta[:3, 3])),
|
|
"rotation_deg": float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude())),
|
|
},
|
|
"open3d_gicp": compact(open_result, open_quality, open_check),
|
|
"small_gicp": compact(small_result, small_quality, small_check),
|
|
"important_limit": (
|
|
"Backend agreement is strong, but AX rotation RMS remains about one degree. "
|
|
"This is not a centimetre-grade absolute certification."
|
|
),
|
|
}
|
|
output = Path(args.output)
|
|
output.parent.mkdir(parents=True, exist_ok=True)
|
|
output.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
recommended = dict(open_result)
|
|
recommended["selection"] = {
|
|
"recommended_backend": "open3d_gicp",
|
|
"comparison_summary": str(output.name),
|
|
"backend_difference": summary["backend_difference"],
|
|
"warning": summary["important_limit"],
|
|
}
|
|
Path(args.recommended_output).write_text(
|
|
json.dumps(recommended, ensure_ascii=False, indent=2), encoding="utf-8"
|
|
)
|
|
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|