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

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-11 10:57:11 +08:00
parent c2f99b94a2
commit 03fcee7e32
14 changed files with 674 additions and 53 deletions
@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+"""Build motion_pairs.json next to an existing summary without re-solving extrinsic.
+
+Use this once for older calibration outputs that predate automatic pair caching.
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import sys
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+    sys.path.insert(0, str(ROOT))
+
+from imu_lidar.imu_audit import audit_imu
+from imu_lidar.imu_io import load_imu_samples
+from imu_lidar.keyframes import build_keyframes
+from imu_lidar.lidar_io import load_lidar_frames
+from imu_lidar.motion_pairs import build_motion_pairs
+from imu_lidar.motion_pairs_io import build_motion_pairs_payload, save_motion_pairs
+
+
+def _load_summary_meta(summary_path: Path) -> tuple[float, np.ndarray, str]:
+    summary = json.loads(summary_path.read_text(encoding="utf-8"))
+    delta_t = float(summary.get("time_offset_s") or 0.0)
+    session = (summary.get("details") or {}).get("sessions", [{}])[0]
+    session_id = str(session.get("session_id") or summary_path.parent.name)
+    bias = np.asarray(
+        (session.get("imu_audit") or {}).get("gyro_bias_rad_s")
+        or (session.get("joint") or {}).get("gyro_bias_rad_s")
+        or [0.0, 0.0, 0.0],
+        dtype=float,
+    ).reshape(3)
+    return delta_t, bias, session_id
+
+
+def export_one(
+    *,
+    lidar: Path,
+    imu: Path,
+    summary: Path,
+    output: Path | None,
+    min_rotation_deg: float,
+    min_translation_m: float,
+) -> Path:
+    delta_t, bias_from_summary, session_id = _load_summary_meta(summary)
+    imu_series = load_imu_samples(imu)
+    # Prefer freshly audited bias if summary bias is missing/zeros.
+    if float(np.linalg.norm(bias_from_summary)) < 1e-12:
+        bias = audit_imu(imu_series).gyro_bias_rad_s
+    else:
+        bias = bias_from_summary
+
+    frames = load_lidar_frames(lidar)
+    keyframes = build_keyframes(
+        frames,
+        min_translation_m=min_translation_m,
+        min_rotation_deg=min_rotation_deg,
+    )
+    pair_set = build_motion_pairs(
+        session_id=session_id,
+        keyframes=list(keyframes.frames),
+        keyframe_indices=keyframes.indices,
+        imu=imu_series,
+        delta_t_s=delta_t,
+        gyro_bias_rad_s=bias,
+        min_rotation_deg=min_rotation_deg,
+        min_translation_m=min_translation_m,
+    )
+    prepared = [
+        {
+            "session_id": session_id,
+            "time_offset_s": delta_t,
+            "gyro_bias_rad_s": np.asarray(bias, dtype=float).reshape(3),
+            "pairs": pair_set.pairs,
+        }
+    ]
+    payload = build_motion_pairs_payload(prepared_sessions=prepared)
+    out = output or (summary.parent / "motion_pairs.json")
+    save_motion_pairs(out, payload)
+    print(f"wrote {out} ({len(pair_set.pairs)} pairs, session={session_id}, dt={delta_t:.6f})")
+    return out
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--lidar", type=Path, required=True)
+    parser.add_argument("--imu", type=Path, required=True)
+    parser.add_argument("--summary", type=Path, required=True)
+    parser.add_argument("--output", type=Path, default=None, help="Default: <summary_dir>/motion_pairs.json")
+    parser.add_argument("--min-pair-rotation-deg", type=float, default=2.0)
+    parser.add_argument("--min-pair-translation-m", type=float, default=0.3)
+    args = parser.parse_args()
+    export_one(
+        lidar=args.lidar,
+        imu=args.imu,
+        summary=args.summary,
+        output=args.output,
+        min_rotation_deg=args.min_pair_rotation_deg,
+        min_translation_m=args.min_pair_translation_m,
+    )
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
@@ -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)