可视化按键对齐雷达-IMU：N/]/[/]切换运动对

Co-authored-by: Cursor <cursoragent@cursor.com>

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结果对比
 

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"{name}: B^-1*motion  "
-        f"[{tx:+.4f}, {ty:+.4f}, {tz:+.4f}] cm; "
+        f"t_xyz=[{tx:+.3f}, {ty:+.3f}, {tz:+.3f}] cm  "
-        f"rpy xyz = [{roll:+.4f}, {pitch:+.4f}, {yaw:+.4f}] deg; "
+        f"rpy=[{roll:+.3f}, {pitch:+.3f}, {yaw:+.3f}] deg  "
-        f"norm = {item['translation_norm_cm']:.4f} cm / "
+        f"|t|={item['translation_norm_cm']:.3f} cm  "
-        f"{item['rotation_angle_deg']:.6f} deg"
+        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"]):
+        pairs_a = np.asarray(data["A"], float)
-            raise IndexError(
+        pairs_b = np.asarray(data["B"], float)
-                f"pair-index {args.pair_index} outside [0,{len(data['A']) - 1}]"
+        pairs_meta = np.asarray(data["meta"])
-            )
+        n_pairs = len(pairs_a)
-        a_ij = np.asarray(data["A"][args.pair_index], float)
+        if not 0 <= args.pair_index < n_pairs:
-        b_gicp = np.asarray(data["B"][args.pair_index], float)
+            raise IndexError(f"pair-index {args.pair_index} outside [0,{n_pairs - 1}]")
-        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
+    left_rpy = np.asarray(args.left_rpy_deg, float)
 
-    transforms = {
+    pair_index = int(args.pair_index)
-        "1 raw": np.eye(4),
+    i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair(
-        "2 RTK initial (X0=I)": a_ij,
+        stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy
-        "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)
+    viewer.create_window("RTK–LiDAR registration inspection", 1400, 900)
-    target_cloud = cloud(o3d, target, COLORS["target"], args.voxel)
+    target_cloud = cloud(o3d, stations[i][3], COLORS["target"], args.voxel)
-    source_cloud = cloud(o3d, source, COLORS["source"], 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(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
+
+    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()
 

雷达与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` **多看几对**，不要只挑视觉最好的一对。
 
 ---