From 8477935ad2cccc0fdf658feaf907459c4d1d2520 Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Mon, 3 Aug 2026 16:16:14 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=AF=E8=A7=86=E5=8C=96=E6=8C=89=E9=94=AE?= =?UTF-8?q?=E5=AF=B9=E9=BD=90=E9=9B=B7=E8=BE=BE-IMU=EF=BC=9AN/]/[/]?= =?UTF-8?q?=E5=88=87=E6=8D=A2=E8=BF=90=E5=8A=A8=E5=AF=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- README.md | 6 +- code/visualize_pair_3d.py | 279 +++++++++++++++++++++++++++----------- 雷达与RTK标定说明书.md | 4 +- 3 files changed, 209 insertions(+), 80 deletions(-) diff --git a/README.md b/README.md index 50dac36..ea6ae25 100644 --- a/README.md +++ b/README.md @@ -165,9 +165,11 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.p - `2`：RTK运动A直接作为初值； - `3`：GICP测得的B； - `4`：最终外参预测的 `X^-1 A X`； -- `Q/Esc`：退出。 +- `N` / `]`：下一运动对； +- `P` / `[`：上一运动对； +- `Q` / `Esc`：退出。 -模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应查看多对，不能只挑视觉效果最好的一对。 +模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应用 `N`/`P` 多看几对，不能只挑视觉效果最好的一对。 ## 7. data4与data4+data5结果对比 diff --git a/code/visualize_pair_3d.py b/code/visualize_pair_3d.py index 3aa71a0..462e404 100644 --- a/code/visualize_pair_3d.py +++ b/code/visualize_pair_3d.py @@ -1,5 +1,21 @@ #!/usr/bin/env python3 -"""Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion.""" +"""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 @@ -7,7 +23,10 @@ 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, + inverse_transform, + load_stations, + rotation_angle_deg, + rpy_deg, ) @@ -16,15 +35,29 @@ COLORS = { "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) - item = item.voxel_down_sample(voxel) + 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 @@ -50,15 +83,75 @@ def print_delta(name, 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" + 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 @@ -66,10 +159,13 @@ def main(): 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("--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], + "--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", ) @@ -82,85 +178,114 @@ def main(): 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) + 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) - b_calibrated = inverse_transform(x) @ a_ij @ x + left_rpy = np.asarray(args.left_rpy_deg, float) - 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." + 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 ) - 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.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) - 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.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() diff --git a/雷达与RTK标定说明书.md b/雷达与RTK标定说明书.md index 98bbab4..6a56506 100644 --- a/雷达与RTK标定说明书.md +++ b/雷达与RTK标定说明书.md @@ -176,9 +176,11 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.p | `2` | 仅用 RTK 运动作初值 | | `3` | GICP 测得的 B | | `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） | +| `N` / `]` | 下一运动对 | +| `P` / `[` | 上一运动对 | | `Q` / `Esc` | 退出 | -蓝 = 目标站 i，橙 = 源站 j。重点看模式 **3 与 4**：墙面、立柱、路缘、地面应基本重合。**多看几对**，不要只挑视觉最好的一对。 +蓝 = 目标站 i，橙 = 源站 j。重点看模式 **3 与 4**：墙面、立柱、路缘、地面应基本重合。用 `N`/`P` **多看几对**，不要只挑视觉最好的一对。 ---