295 lines
10 KiB
Python
295 lines
10 KiB
Python
#!/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()
|