支持主机桥接后固定δt与旋转先验，并落盘运动对供可视化直读。

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

tools/visualize_pair_3d.py

@@ -34,12 +34,39 @@ from imu_lidar.geometry import (
 from imu_lidar.imu_io import load_imu_samples
 from imu_lidar.imu_preintegration import preintegrate_imu
 from imu_lidar.keyframes import build_keyframes
-from imu_lidar.lidar_io import load_lidar_frames
+from imu_lidar.lidar_io import load_lidar_frame_at, load_lidar_frames
 from imu_lidar.motion_pairs import build_motion_pairs
+from imu_lidar.motion_pairs_io import (
+    load_motion_pairs,
+    pairs_for_session,
+    resolve_motion_pairs_path,
+)
 from imu_lidar.registration import register_lidar_pair
 from imu_lidar.time_offset import lidar_time_to_imu_time
 
 
+class _LazyFrameStore:
+    """Load NPZ frames on demand; indices match mid-time-sorted ``load_lidar_frames``."""
+
+    def __init__(self, lidar_dir: Path, *, max_cached: int = 16):
+        self.lidar_dir = Path(lidar_dir)
+        self.max_cached = max_cached
+        self._cache: dict[int, object] = {}
+        self._order: list[int] = []
+
+    def __getitem__(self, index: int):
+        index = int(index)
+        if index in self._cache:
+            return self._cache[index]
+        frame = load_lidar_frame_at(self.lidar_dir, index)
+        self._cache[index] = frame
+        self._order.append(index)
+        while len(self._order) > self.max_cached:
+            old = self._order.pop(0)
+            self._cache.pop(old, None)
+        return frame
+
+
 COLORS = {
     "target": [0.10, 0.65, 1.00],
     "source": [1.00, 0.35, 0.05],
@@ -357,13 +384,34 @@ def _run_gui(
 def main(argv: list[str] | None = None) -> int:
     parser = argparse.ArgumentParser(description=__doc__)
     parser.add_argument("--lidar", required=True, type=Path, help="LiDAR session directory")
-    parser.add_argument("--imu", required=True, type=Path, help="IMU CSV")
+    parser.add_argument(
+        "--imu",
+        type=Path,
+        default=None,
+        help="IMU CSV (only needed when rebuilding pairs without motion_pairs.json)",
+    )
     parser.add_argument(
         "--summary",
         required=True,
         type=Path,
         help="summary.json (or T_IMU_lidar.json) from a calibration run",
     )
+    parser.add_argument(
+        "--motion-pairs",
+        type=Path,
+        default=None,
+        help="Cached motion_pairs.json (default: next to --summary)",
+    )
+    parser.add_argument(
+        "--session-id",
+        default=None,
+        help="Session id inside multi-session motion_pairs.json",
+    )
+    parser.add_argument(
+        "--rebuild-pairs",
+        action="store_true",
+        help="Ignore cache and rebuild pairs from IMU/LiDAR (slow)",
+    )
     parser.add_argument("--pair-index", type=int, default=0, help="Starting motion-pair index")
     parser.add_argument("--frame-i", type=int, default=None, help="Optional explicit frame index i")
     parser.add_argument("--frame-j", type=int, default=None, help="Optional explicit frame index j")
@@ -384,42 +432,70 @@ def main(argv: list[str] | None = None) -> int:
     args = parser.parse_args(argv)
 
     x, delta_t_s, gyro_bias = _load_extrinsic(args.summary)
-    frames, imu, keyframes, pair_set = _build_pair_list(
-        lidar_dir=args.lidar,
-        imu_path=args.imu,
-        delta_t_s=delta_t_s,
-        gyro_bias=gyro_bias,
-        min_rotation_deg=args.min_pair_rotation_deg,
-        min_translation_m=args.min_pair_translation_m,
-    )
+    cache_path = args.motion_pairs or resolve_motion_pairs_path(args.summary)
+    use_cache = (not args.rebuild_pairs) and cache_path is not None and args.frame_i is None
 
+    frames = None
+    pairs: tuple = ()
     fixed_single_pair = None
-    if args.frame_i is not None and args.frame_j is not None:
-        frame_i, frame_j, a_ij, b_gicp = _pair_from_indices(
-            frames,
-            imu,
-            i=args.frame_i,
-            j=args.frame_j,
-            delta_t_s=delta_t_s,
-            gyro_bias=gyro_bias,
-        )
-        transforms = _transforms_for_pair(x, a_ij, b_gicp)
-        label = f"frames {args.frame_i} <- {args.frame_j}"
-        fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp)
-        pairs = ()
-    else:
-        if not pair_set.pairs:
-            raise SystemExit("no motion pairs rebuilt; loosen min-pair thresholds or check data")
-        if not 0 <= args.pair_index < len(pair_set.pairs):
+    label = ""
+    b_gicp = np.eye(4)
+    transforms: dict[str, np.ndarray] = {}
+    frame_i = frame_j = None
+
+    if use_cache:
+        payload = load_motion_pairs(cache_path)
+        pair_list = pairs_for_session(payload, args.session_id)
+        if not pair_list:
+            raise SystemExit(f"no pairs in cache: {cache_path}")
+        if not 0 <= args.pair_index < len(pair_list):
             raise SystemExit(
-                f"pair-index {args.pair_index} outside [0, {len(pair_set.pairs) - 1}] "
-                f"({len(pair_set.pairs)} pairs available)"
+                f"pair-index {args.pair_index} outside [0, {len(pair_list) - 1}] "
+                f"({len(pair_list)} pairs in cache)"
             )
-        pairs = pair_set.pairs
+        frames = _LazyFrameStore(args.lidar)
+        pairs = tuple(pair_list)
         frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
             frames, pairs, args.pair_index, x
         )
-        print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes")
+        print(f"loaded {len(pairs)} cached pairs from {cache_path}")
+    else:
+        if args.imu is None:
+            raise SystemExit("--imu is required when motion_pairs.json is missing (or use --rebuild-pairs with --imu)")
+        frames, imu, keyframes, pair_set = _build_pair_list(
+            lidar_dir=args.lidar,
+            imu_path=args.imu,
+            delta_t_s=delta_t_s,
+            gyro_bias=gyro_bias,
+            min_rotation_deg=args.min_pair_rotation_deg,
+            min_translation_m=args.min_pair_translation_m,
+        )
+        if args.frame_i is not None and args.frame_j is not None:
+            frame_i, frame_j, a_ij, b_gicp = _pair_from_indices(
+                frames,
+                imu,
+                i=args.frame_i,
+                j=args.frame_j,
+                delta_t_s=delta_t_s,
+                gyro_bias=gyro_bias,
+            )
+            transforms = _transforms_for_pair(x, a_ij, b_gicp)
+            label = f"frames {args.frame_i} <- {args.frame_j}"
+            fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp)
+            pairs = ()
+        else:
+            if not pair_set.pairs:
+                raise SystemExit("no motion pairs rebuilt; loosen min-pair thresholds or check data")
+            if not 0 <= args.pair_index < len(pair_set.pairs):
+                raise SystemExit(
+                    f"pair-index {args.pair_index} outside [0, {len(pair_set.pairs) - 1}] "
+                    f"({len(pair_set.pairs)} pairs available)"
+                )
+            pairs = pair_set.pairs
+            frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
+                frames, pairs, args.pair_index, x
+            )
+            print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes")
 
     if args.save_png is not None:
         _print_pair_header(label, b_gicp, transforms)