#!/usr/bin/env python3 """Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion. Modes (keyboard), aligned with the LiDAR–IMU viewer: 1 raw source (no transform) 2 RTK prediction with X=I (B_pred = A) 3 LiDAR registration B (reference) 4 calibrated prediction B_pred = X^{-1} A X 5 optional body-left RPY test (only if --left-rpy-deg is non-zero) N / ] next motion pair P / [ previous motion pair Q / Esc exit Blue = target station i; orange = source station j after the selected transform. """ from __future__ import annotations 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, ) COLORS = { "target": [0.10, 0.65, 1.00], "source": [1.00, 0.35, 0.05], } MODE_NAMES = ( "1 raw", "2 RTK initial (X=I)", "3 GICP B", "4 calibrated X^-1 A X", ) def cloud(o3d, points, color, voxel): item = o3d.geometry.PointCloud() item.points = o3d.utility.Vector3dVector(points) if voxel > 0: item = item.voxel_down_sample(voxel) item.paint_uniform_color(color) return item def set_cloud_points(cloud_geom, points, color, voxel, o3d) -> None: tmp = cloud(o3d, points, color, voxel) cloud_geom.points = tmp.points cloud_geom.colors = tmp.colors 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 " f"t_xyz=[{tx:+.3f}, {ty:+.3f}, {tz:+.3f}] cm " f"rpy=[{roll:+.3f}, {pitch:+.3f}, {yaw:+.3f}] deg " f"|t|={item['translation_norm_cm']:.3f} cm " f"|R|={item['rotation_angle_deg']:.4f} deg" ) return item def transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg): b_calibrated = inverse_transform(x) @ a_ij @ x transforms = { MODE_NAMES[0]: np.eye(4), MODE_NAMES[1]: a_ij.copy(), MODE_NAMES[2]: b_gicp.copy(), MODE_NAMES[3]: b_calibrated, } correction = np.asarray(left_rpy_deg, float) test_name = None if np.any(np.abs(correction) > 0.0): x_test = body_left_rpy(x, correction) test_name = f"5 test body-left RPY {correction.tolist()} deg" transforms[test_name] = inverse_transform(x_test) @ a_ij @ x_test return transforms, test_name def resolve_pair(stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy_deg): a_ij = np.asarray(pairs_a[pair_index], float) b_gicp = np.asarray(pairs_b[pair_index], float) i, j = np.asarray(pairs_meta[pair_index, :2], int) transforms, test_name = transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg) label = ( f"pair {pair_index + 1}/{len(pairs_a)} " f"station {i} <- {j} " f"rotB={rotation_angle_deg(b_gicp[:3, :3]):.2f} deg " f"|tB|={float(np.linalg.norm(b_gicp[:3, 3])):.3f} m" ) return i, j, a_ij, b_gicp, transforms, test_name, label def print_pair_header(label, b_gicp, transforms, test_name, a_ij): print("-" * 72) print(label) print("blue=target i | orange=source j") mode_hint = "1-4" if test_name is not None: mode_hint = "1-5" print(f"{mode_hint}: overlay mode | N/]: next pair | P/[: prev pair | 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("mode4 minus mode3", b_gicp, transforms[MODE_NAMES[3]]) roll, pitch, yaw = np.abs(baseline["rotation_rpy_deg_xyz"]) if max(roll, pitch) > max(0.10, 2.0 * yaw): print("note: roll/pitch dominate yaw on this pair.") tx, ty, tz = np.abs(baseline["translation_xyz_cm"]) if tz > max(tx, ty): print("note: largest translation component is Z for this pair.") 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 test_name is not None: print_delta("mode5 minus mode3", b_gicp, transforms[test_name]) 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, help="Starting motion-pair index") 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'])}" ) pairs_a = np.asarray(data["A"], float) pairs_b = np.asarray(data["B"], float) pairs_meta = np.asarray(data["meta"]) n_pairs = len(pairs_a) if not 0 <= args.pair_index < n_pairs: raise IndexError(f"pair-index {args.pair_index} outside [0,{n_pairs - 1}]") with open(args.extrinsic, encoding="utf-8-sig") as stream: result = json.load(stream) x = np.asarray(result["matrix_4x4"], float) left_rpy = np.asarray(args.left_rpy_deg, float) pair_index = int(args.pair_index) i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair( stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy ) viewer = o3d.visualization.VisualizerWithKeyCallback() viewer.create_window("RTK–LiDAR registration inspection", 1400, 900) target_cloud = cloud(o3d, stations[i][3], COLORS["target"], args.voxel) source_cloud = cloud(o3d, stations[j][3], COLORS["source"], args.voxel) viewer.add_geometry(target_cloud) viewer.add_geometry(source_cloud) viewer.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0)) viewer.get_render_option().background_color = np.array([0.02, 0.02, 0.02]) viewer.get_render_option().point_size = 2.0 state = { "pair_index": pair_index, "mode_name": MODE_NAMES[3], "current": np.eye(4), "transforms": transforms, "b_gicp": b_gicp, "a_ij": a_ij, "test_name": test_name, } def apply_mode(vis, mode_name: str, *, announce: bool = True) -> None: desired = state["transforms"][mode_name] source_cloud.transform(desired @ inverse_transform(state["current"])) state["current"] = desired state["mode_name"] = mode_name vis.update_geometry(source_cloud) if announce: if mode_name == MODE_NAMES[2]: print(f"{mode_name}: registration reference; delta = 0") else: print_delta(mode_name + " minus mode3", state["b_gicp"], desired) def load_pair(vis, new_index: int) -> None: new_index = int(new_index) % n_pairs i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair( stations, pairs_a, pairs_b, pairs_meta, new_index, x, left_rpy ) state["pair_index"] = new_index state["transforms"] = transforms state["b_gicp"] = b_gicp state["a_ij"] = a_ij state["test_name"] = test_name state["current"] = np.eye(4) set_cloud_points(target_cloud, stations[i][3], COLORS["target"], args.voxel, o3d) set_cloud_points(source_cloud, stations[j][3], COLORS["source"], args.voxel, o3d) vis.update_geometry(target_cloud) vis.update_geometry(source_cloud) # Keep current mode if still available (mode 5 may vanish when correction is zero). mode_name = state["mode_name"] if mode_name not in transforms: mode_name = MODE_NAMES[3] print_pair_header(label, b_gicp, transforms, test_name, a_ij) apply_mode(vis, mode_name, announce=True) def make_mode_cb(mode_name: str): def callback(vis): if mode_name not in state["transforms"]: print(f"{mode_name}: unavailable (pass non-zero --left-rpy-deg for mode 5)") return False apply_mode(vis, mode_name, announce=True) return False return callback def next_pair(vis): load_pair(vis, state["pair_index"] + 1) return False def prev_pair(vis): load_pair(vis, state["pair_index"] - 1) return False print_pair_header(label, b_gicp, transforms, test_name, a_ij) for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4")), MODE_NAMES): viewer.register_key_callback(key, make_mode_cb(name)) def mode5(vis): name = state["test_name"] if name is None or name not in state["transforms"]: print("5: unavailable (pass non-zero --left-rpy-deg for mode 5)") return False apply_mode(vis, name, announce=True) return False viewer.register_key_callback(ord("5"), mode5) for key in (ord("N"), ord("n"), ord("]")): viewer.register_key_callback(key, next_pair) for key in (ord("P"), ord("p"), ord("[")): viewer.register_key_callback(key, prev_pair) apply_mode(viewer, MODE_NAMES[3], announce=False) viewer.run() viewer.destroy_window() if __name__ == "__main__": main()