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