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

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

README.md

@@ -73,16 +73,25 @@ powershell -File tools\reproduce_synthetic.ps1
 证明：链路可跑通，能收回已知 yaw / δt。  
 不证明：实车安装精度、平移可交付。
 
-产物在 `examples/synthetic_session/out/`。叠点查看：
+产物在 `examples/synthetic_session/out/`（含 `summary.json` 与 `motion_pairs.json`）。叠点查看：
 
 ```powershell
+# 优先读取 summary 同目录的 motion_pairs.json，按需加载点云（无需重算配准）
 python tools\visualize_pair_3d.py `
   --lidar examples\synthetic_session\lidar `
-  --imu examples\synthetic_session\imu.csv `
   --summary examples\synthetic_session\out\summary.json `
   --pair-index 0
 ```
 
+旧标定目录若缺少缓存，可只补导出运动对（不重求解外参）：
+
+```powershell
+python tools\export_motion_pairs_for_viz.py `
+  --lidar path\to\lidar `
+  --imu path\to\imu.csv `
+  --summary path\to\out\summary.json
+```
+
 键 `1`–`4` 切换叠点模式；`N`/`P` 切换运动对。
 
 ---

imu_lidar/CHANGELOG.md

@@ -5,6 +5,31 @@
 
 ---
 
+## 2026-08-11 10:55 (UTC+8)
+
+### 运动对缓存：标定落盘，可视化直读
+
+- **原本**：`visualize_pair_3d` 每次启动都重新关键帧+配准+预积分，等同半次标定。
+- **改成**：
+  - 标定成功后写出 `motion_pairs.json`（`motion_pairs_io.py` / `finalize`）。
+  - 可视化优先读缓存并对点云懒加载；`--rebuild-pairs` 可回退旧路径。
+  - 旧结果可用 `tools/export_motion_pairs_for_viz.py` 只补导出运动对，无需重求解外参。
+
+---
+
+## 2026-08-11 08:55 (UTC+8)
+
+### 主机桥接后冻结 δt + 旋转先验软约束
+
+- **原本**：手眼后 signed δt 精修可在弱 MSE 下降下连走数步（最远约 0.5 s）；旋转手眼无 CAD 先验，平面运动下 yaw 易掉进低残差错解。
+- **改成**：
+  - CLI：`--fixed-time-offset-s`、`--no-signed-time-refine`、`--max-signed-refine-shift-s`。
+  - signed refine：默认 `|Δδt|≤0.05 s`，且要求 MSE 至少降约 2%。
+  - `rotation_handeye` 读取配置 `rotation_prior` 作初值/软约束。
+  - 主机 UTC 桥接会话建议：`--fixed-time-offset-s 0 --no-signed-time-refine`。
+
+---
+
 ## 2026-08-09 14:30 (UTC+8)
 
 ### 导出：HI13 IMU + recovered dlog zip + 墙钟切窗

imu_lidar/cli.py

@@ -52,6 +52,23 @@ def build_parser() -> argparse.ArgumentParser:
     )
     run.add_argument("--max-iterations", type=int, default=2)
     run.add_argument("--time-offset-search-s", type=float, default=1.0)
+    run.add_argument(
+        "--fixed-time-offset-s",
+        type=float,
+        default=None,
+        help="Skip |ω| δt search and use this constant (use 0 after host-UTC bridge)",
+    )
+    run.add_argument(
+        "--no-signed-time-refine",
+        action="store_true",
+        help="Disable signed 3-axis δt refine after hand-eye (recommended for host-bridged data)",
+    )
+    run.add_argument(
+        "--max-signed-refine-shift-s",
+        type=float,
+        default=0.05,
+        help="Max |Δδt| accepted by signed refine from the coarse estimate",
+    )
     run.add_argument("--min-pair-rotation-deg", type=float, default=3.0)
     run.add_argument("--min-pair-translation-m", type=float, default=0.3)
     return parser
@@ -102,6 +119,9 @@ def main(argv: list[str] | None = None) -> int:
             min_pair_rotation_deg=args.min_pair_rotation_deg,
             min_pair_translation_m=args.min_pair_translation_m,
             time_offset_search_s=args.time_offset_search_s,
+            fixed_time_offset_s=args.fixed_time_offset_s,
+            enable_signed_time_refine=not args.no_signed_time_refine,
+            max_signed_refine_shift_s=args.max_signed_refine_shift_s,
         )
         result = run_calibration(request)
         print(f"status: {result.status.value}")

imu_lidar/contracts.py

@@ -51,6 +51,12 @@ class CalibrationRequest:
     min_pair_rotation_deg: float = 3.0
     min_pair_translation_m: float = 0.3
     time_offset_search_s: float = 1.0
+    # If set, skip |ω| search and use this constant (host-UTC-bridged sessions: 0).
+    fixed_time_offset_s: float | None = None
+    # Signed 3-axis refine after hand-eye; disable for already-bridged timelines.
+    enable_signed_time_refine: bool = True
+    # Reject signed refine steps that walk farther than this from the coarse δt.
+    max_signed_refine_shift_s: float = 0.05
 
 
 @dataclass

imu_lidar/finalize.py

@@ -34,6 +34,7 @@ def finalize_result(
     T_IMU_lidar: np.ndarray | None = None,
     time_offset_s: float | None = None,
     output_directory: Path | None = None,
+    motion_pairs_payload: dict[str, Any] | None = None,
 ) -> CalibrationResult:
     """Build the result envelope and optionally write report files."""
 
@@ -72,5 +73,10 @@ def finalize_result(
                 json.dumps({"delta_t_s": time_offset_s, "definition": "t_imu = t_lidar + delta_t"}, indent=2),
                 encoding="utf-8",
             )
+        if motion_pairs_payload is not None:
+            from .motion_pairs_io import save_motion_pairs
+
+            save_motion_pairs(output_directory / "motion_pairs.json", motion_pairs_payload)
+            summary["motion_pairs_file"] = "motion_pairs.json"
         (output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
     return result

imu_lidar/lidar_io.py

@@ -19,10 +19,7 @@ import numpy as np
 from .contracts import LidarFrame
 
 
-def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
-    """Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
-
-    root = Path(path)
+def _read_frames_index(root: Path) -> tuple[np.ndarray, str]:
     index_path = root / "frames_index.csv"
     if not index_path.exists():
         raise FileNotFoundError(f"missing frames_index.csv under {root}")
@@ -38,6 +35,54 @@ def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
         raise ValueError(
             f"frames_index.csv must contain frame_id,{file_key}/filename,t_start,t_end; got {sorted(names)}"
         )
+    return rows, file_key
+
+
+def list_lidar_frame_entries(path: Path | str) -> list[tuple[str, float, float, Path]]:
+    """Return ``(frame_id, t_start, t_end, npz_path)`` sorted by mid time (same as ``load_lidar_frames``)."""
+
+    root = Path(path)
+    rows, file_key = _read_frames_index(root)
+    entries: list[tuple[str, float, float, Path]] = []
+    for row in rows:
+        t0 = float(row["t_start"])
+        t1 = float(row["t_end"])
+        entries.append((str(row["frame_id"]), t0, t1, root / str(row[file_key])))
+    entries.sort(key=lambda item: 0.5 * (item[1] + item[2]))
+    return entries
+
+
+def load_lidar_frame_at(root: Path | str, index: int) -> LidarFrame:
+    """Load one frame by index in mid-time-sorted order (matches motion-pair ``i``/``j``)."""
+
+    entries = list_lidar_frame_entries(root)
+    if index < 0 or index >= len(entries):
+        raise IndexError(f"frame index {index} outside [0, {len(entries) - 1}] for {root}")
+    frame_id, t0, t1, npz_path = entries[index]
+    with np.load(npz_path) as payload:
+        if "points" not in payload.files:
+            raise ValueError(f"{npz_path} must contain array 'points'")
+        points = np.asarray(payload["points"], dtype=float)
+    if points.ndim != 2 or points.shape[1] < 3:
+        raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
+    return LidarFrame(
+        frame_id=frame_id,
+        t_start_s=t0,
+        t_end_s=t1,
+        points_xyz=points[:, :3],
+        path=npz_path,
+    )
+
+
+def lidar_frame_count(path: Path | str) -> int:
+    return len(list_lidar_frame_entries(path))
+
+
+def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
+    """Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
+
+    root = Path(path)
+    rows, file_key = _read_frames_index(root)
 
     frames: list[LidarFrame] = []
     for row in rows:

imu_lidar/motion_pairs_io.py

@@ -0,0 +1,142 @@
+"""Serialize / deserialize motion pairs for fast visualization."""
+
+from __future__ import annotations
+
+import json
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from .contracts import MotionPair
+
+SCHEMA_VERSION = 1
+
+# Keep viz-relevant fields; drop large cov / Jacobians.
+_METADATA_KEEP = frozenset(
+    {
+        "backend",
+        "rotation_deg_A",
+        "rotation_deg_B",
+        "translation_m_B",
+        "weight",
+        "duration_s",
+        "mean_gyro_norm",
+        "preint_sigma_rad",
+        "t_i_imu_s",
+        "t_j_imu_s",
+        "modeling",
+    }
+)
+
+
+def _to_list(value: Any) -> Any:
+    if isinstance(value, np.ndarray):
+        return value.tolist()
+    if isinstance(value, (np.floating, np.integer, np.bool_)):
+        return value.item()
+    return value
+
+
+def pair_to_dict(pair: MotionPair) -> dict[str, Any]:
+    meta = {
+        str(k): _to_list(v)
+        for k, v in (pair.metadata or {}).items()
+        if str(k) in _METADATA_KEEP
+    }
+    return {
+        "session_id": pair.session_id,
+        "i": int(pair.i),
+        "j": int(pair.j),
+        "t_i_s": float(pair.t_i_s),
+        "t_j_s": float(pair.t_j_s),
+        "R_A": np.asarray(pair.R_A, dtype=float).reshape(3, 3).tolist(),
+        "R_B": np.asarray(pair.R_B, dtype=float).reshape(3, 3).tolist(),
+        "t_A_m": None if pair.t_A_m is None else np.asarray(pair.t_A_m, dtype=float).reshape(3).tolist(),
+        "t_B_m": None if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float).reshape(3).tolist(),
+        "fitness": float(pair.fitness),
+        "metadata": meta,
+    }
+
+
+def pair_from_dict(payload: dict[str, Any]) -> MotionPair:
+    t_a = payload.get("t_A_m")
+    t_b = payload.get("t_B_m")
+    return MotionPair(
+        session_id=str(payload.get("session_id", "")),
+        i=int(payload["i"]),
+        j=int(payload["j"]),
+        t_i_s=float(payload["t_i_s"]),
+        t_j_s=float(payload["t_j_s"]),
+        R_A=np.asarray(payload["R_A"], dtype=float).reshape(3, 3),
+        R_B=np.asarray(payload["R_B"], dtype=float).reshape(3, 3),
+        t_A_m=None if t_a is None else np.asarray(t_a, dtype=float).reshape(3),
+        t_B_m=None if t_b is None else np.asarray(t_b, dtype=float).reshape(3),
+        fitness=float(payload.get("fitness", 0.0)),
+        metadata=dict(payload.get("metadata") or {}),
+    )
+
+
+def build_motion_pairs_payload(
+    *,
+    prepared_sessions: list[dict[str, Any]],
+) -> dict[str, Any]:
+    """Build a JSON-serializable cache from pipeline ``prepared`` session dicts."""
+
+    sessions_out: list[dict[str, Any]] = []
+    for prep in prepared_sessions:
+        pairs = prep.get("pairs") or ()
+        sessions_out.append(
+            {
+                "session_id": prep.get("session_id"),
+                "delta_t_s": float(prep.get("time_offset_s", 0.0)),
+                "gyro_bias_rad_s": np.asarray(prep.get("gyro_bias_rad_s", np.zeros(3)), dtype=float)
+                .reshape(3)
+                .tolist(),
+                "pair_count": len(pairs),
+                "pairs": [pair_to_dict(pair) for pair in pairs],
+            }
+        )
+    return {
+        "schema_version": SCHEMA_VERSION,
+        "sessions": sessions_out,
+        "note": "Cached motion pairs for visualization; A=IMU preintegration, B=LiDAR registration",
+    }
+
+
+def save_motion_pairs(path: Path | str, payload: dict[str, Any]) -> Path:
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    destination.write_text(json.dumps(payload, indent=2), encoding="utf-8")
+    return destination
+
+
+def load_motion_pairs(path: Path | str) -> dict[str, Any]:
+    payload = json.loads(Path(path).read_text(encoding="utf-8"))
+    if int(payload.get("schema_version", 0)) != SCHEMA_VERSION:
+        raise ValueError(
+            f"unsupported motion_pairs schema_version={payload.get('schema_version')}; "
+            f"expected {SCHEMA_VERSION}"
+        )
+    return payload
+
+
+def pairs_for_session(payload: dict[str, Any], session_id: str | None = None) -> list[MotionPair]:
+    sessions = payload.get("sessions") or []
+    if not sessions:
+        return []
+    if session_id is None:
+        chosen = sessions[0]
+    else:
+        chosen = next((s for s in sessions if s.get("session_id") == session_id), None)
+        if chosen is None:
+            raise KeyError(f"session_id {session_id!r} not found in motion_pairs cache")
+    return [pair_from_dict(item) for item in chosen.get("pairs") or []]
+
+
+def resolve_motion_pairs_path(summary_path: Path | str) -> Path | None:
+    """Return ``motion_pairs.json`` next to a summary if it exists."""
+
+    summary = Path(summary_path)
+    candidate = summary.parent / "motion_pairs.json"
+    return candidate if candidate.is_file() else None

imu_lidar/pipeline.py

@@ -24,6 +24,7 @@ from .keyframes import build_keyframes
 from .lidar_deskew import deskew_lidar_frames
 from .lidar_io import load_lidar_frames
 from .motion_pairs import build_motion_pairs
+from .motion_pairs_io import build_motion_pairs_payload
 from .rotation_handeye import solve_rotation_handeye
 from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
 from .timestamp_audit import audit_timestamps
@@ -76,6 +77,8 @@ def _build_pairs_and_handeye(
     delta_t_s: float,
     gyro_bias_rad_s: np.ndarray,
     request: CalibrationRequest,
+    R_prior: np.ndarray | None = None,
+    prior_sigma_deg: float | None = None,
 ):
     keyframes = build_keyframes(
         working_frames,
@@ -92,7 +95,11 @@ def _build_pairs_and_handeye(
         min_rotation_deg=request.min_pair_rotation_deg,
         min_translation_m=request.min_pair_translation_m,
     )
-    handeye = solve_rotation_handeye(pair_set.pairs)
+    handeye = solve_rotation_handeye(
+        pair_set.pairs,
+        R_prior=R_prior,
+        prior_sigma_deg=prior_sigma_deg,
+    )
     return keyframes, pair_set, handeye
 
 
@@ -108,9 +115,24 @@ def _translation_prior_from_config(
     return np.asarray(tp["t_IMU_lidar_m"], dtype=float).reshape(3), tp.get("sigma_m", [0.05, 0.05, 0.05])
 
 
+def _rotation_prior_from_config(
+    vehicle_config: dict[str, Any] | None,
+) -> tuple[np.ndarray | None, float | None]:
+    if vehicle_config is None or not prior_enabled(vehicle_config, "rotation_prior"):
+        return None, None
+    init_cfg = vehicle_config.get("initialization") or {}
+    rp = init_cfg.get("rotation_prior") or {}
+    if rp.get("R_IMU_lidar") is None:
+        return None, None
+    return np.asarray(rp["R_IMU_lidar"], dtype=float).reshape(3, 3), float(rp.get("sigma_deg", 15.0))
+
+
 def _prepare_session_pairs(
     session: SessionInput,
     request: CalibrationRequest,
+    *,
+    R_prior: np.ndarray | None = None,
+    prior_sigma_deg: float | None = None,
 ) -> dict[str, Any]:
     """Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet."""
 
@@ -125,17 +147,30 @@ def _prepare_session_pairs(
     if not imu_report.ok:
         return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)}
 
-    offset = estimate_time_offset(
-        imu,
-        frames,
-        gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
-        search_s=request.time_offset_search_s,
-    )
-    if not offset.ok:
-        return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}
+    if request.fixed_time_offset_s is not None:
+        offset = TimeOffsetResult(
+            delta_t_s=float(request.fixed_time_offset_s),
+            correlation_peak=1.0,
+            search_s=0.0,
+            notes=(
+                f"fixed_time_offset_s={float(request.fixed_time_offset_s):.6f} "
+                "(skip |ω| search; intended for host-UTC-bridged sessions)",
+            ),
+            ok=True,
+        )
+    else:
+        offset = estimate_time_offset(
+            imu,
+            frames,
+            gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+            search_s=request.time_offset_search_s,
+        )
+        if not offset.ok:
+            return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}
 
+    coarse_delta_t = float(offset.delta_t_s)
     working_frames = frames
-    r_x = np.eye(3)
+    r_x = np.eye(3) if R_prior is None else np.asarray(R_prior, dtype=float).reshape(3, 3)
     handeye = None
     pair_set = None
     keyframes = None
@@ -158,6 +193,8 @@ def _prepare_session_pairs(
             delta_t_s=offset.delta_t_s,
             gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
             request=request,
+            R_prior=R_prior,
+            prior_sigma_deg=prior_sigma_deg,
         )
         pairs_notes = list(pair_set.notes)
         pair_count = len(pair_set.pairs)
@@ -176,6 +213,9 @@ def _prepare_session_pairs(
             }
         r_x = handeye.R_IMU_lidar
 
+        if not request.enable_signed_time_refine:
+            continue
+
         for _ in range(2):
             refined = refine_time_offset_signed(
                 imu,
@@ -184,7 +224,23 @@ def _prepare_session_pairs(
                 R_IMU_lidar=r_x,
                 gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
                 search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
+                max_shift_s=request.max_signed_refine_shift_s,
             )
+            # Also bound total walk away from the original coarse estimate.
+            if abs(refined.delta_t_s - coarse_delta_t) > request.max_signed_refine_shift_s:
+                refined = TimeOffsetResult(
+                    delta_t_s=float(offset.delta_t_s),
+                    correlation_peak=refined.correlation_peak,
+                    search_s=refined.search_s,
+                    notes=tuple(
+                        list(refined.notes)
+                        + [
+                            f"signed refine clamped: |δt-coarse| would exceed "
+                            f"{request.max_signed_refine_shift_s:.3f}s"
+                        ]
+                    ),
+                    ok=True,
+                )
             delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
             offset = _merge_time_offset(offset, refined)
             if delta_shift < 1e-3:
@@ -196,6 +252,8 @@ def _prepare_session_pairs(
                 delta_t_s=offset.delta_t_s,
                 gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
                 request=request,
+                R_prior=R_prior,
+                prior_sigma_deg=prior_sigma_deg,
             )
             pairs_notes = list(pair_set.notes)
             pair_count = len(pair_set.pairs)
@@ -292,9 +350,16 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
                 output_directory=request.output_directory,
             )
 
+    r_prior, prior_sigma_deg = _rotation_prior_from_config(vehicle_config)
+
     prepared: list[dict[str, Any]] = []
     for session in request.sessions:
-        prep = _prepare_session_pairs(session, request)
+        prep = _prepare_session_pairs(
+            session,
+            request,
+            R_prior=r_prior,
+            prior_sigma_deg=prior_sigma_deg,
+        )
         if not prep.get("ok"):
             return finalize_result(
                 status=CalibrationStatus.BLOCKED,
@@ -305,7 +370,11 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
         prepared.append(prep)
 
     all_pairs = _remap_pairs_for_joint(prepared)
-    handeye = solve_rotation_handeye(all_pairs)
+    handeye = solve_rotation_handeye(
+        all_pairs,
+        R_prior=r_prior,
+        prior_sigma_deg=prior_sigma_deg,
+    )
     if not handeye.ok:
         return finalize_result(
             status=CalibrationStatus.BLOCKED,
@@ -412,6 +481,7 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
         T_IMU_lidar=T,
         time_offset_s=delta_t,
         output_directory=request.output_directory,
+        motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared),
     )
 
 

imu_lidar/rotation_handeye.py

@@ -57,8 +57,24 @@ def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
     return float(np.degrees(np.linalg.norm(err)))
 
 
-def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> RotationHandeyeResult:
-    """Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement."""
+def _rms_deg(r_x: np.ndarray, pairs: list[MotionPair]) -> float:
+    if not pairs:
+        return 1e9
+    errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in pairs], dtype=float)
+    return float(np.sqrt(np.mean(errs**2)))
+
+
+def solve_rotation_handeye(
+    pairs: list[MotionPair] | tuple[MotionPair, ...],
+    *,
+    R_prior: np.ndarray | None = None,
+    prior_sigma_deg: float | None = None,
+) -> RotationHandeyeResult:
+    """Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement.
+
+    Optional CAD / installation ``R_prior`` soft-constrains the extrinsic yaw that
+    is weakly observable under near-planar motion.
+    """
 
     usable = [pair for pair in pairs if rotation_angle_deg(pair.R_A) > 1.0 and rotation_angle_deg(pair.R_B) > 1.0]
     notes: list[str] = []
@@ -73,6 +89,21 @@ def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) ->
         )
 
     r0 = _tsai_rotation_initial(usable)
+    r_prior = None
+    if R_prior is not None:
+        r_prior = orthonormalize_rotation(np.asarray(R_prior, dtype=float).reshape(3, 3))
+        rms_tsai = _rms_deg(r0, usable)
+        rms_prior = _rms_deg(r_prior, usable)
+        if rms_prior <= rms_tsai * 1.25:
+            r0 = r_prior
+            notes.append(
+                f"init from rotation prior (rms={rms_prior:.3f} deg vs Tsai {rms_tsai:.3f} deg)"
+            )
+        else:
+            notes.append(
+                f"init from Tsai (rms={rms_tsai:.3f} deg; prior {rms_prior:.3f} deg kept as soft constraint)"
+            )
+
     weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
     notes.append(
         f"weighted hand-eye: weight median={float(np.median(weights)):.3g}, "
@@ -85,12 +116,21 @@ def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) ->
     def unpack(vec: np.ndarray) -> np.ndarray:
         return orthonormalize_rotation(so3_exp(vec))
 
+    sigma = 15.0 if prior_sigma_deg is None else float(prior_sigma_deg)
+    prior_w = 0.0
+    if r_prior is not None and sigma > 1e-6:
+        # Scale prior to a few strong pairs so it regularizes yaw without dominating.
+        prior_w = float(np.sqrt(np.median(weights)) / np.deg2rad(sigma))
+        notes.append(f"rotation prior soft constraint sigma={sigma:.1f} deg, weight={prior_w:.3g}")
+
     def residual(vec: np.ndarray) -> np.ndarray:
         r_x = unpack(vec)
         residuals = []
         for pair, weight in zip(usable, weights):
             err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
             residuals.append(np.sqrt(weight) * err)
+        if r_prior is not None and prior_w > 0:
+            residuals.append(prior_w * so3_log(r_prior.T @ r_x))
         return np.concatenate(residuals)
 
     opt = least_squares(residual, pack(r0), loss="huber", f_scale=np.deg2rad(1.0), max_nfev=200)

imu_lidar/time_offset.py

@@ -190,6 +190,7 @@ def refine_time_offset_signed(
     gyro_bias_rad_s: np.ndarray | None = None,
     search_s: float = 0.08,
     sample_hz: float = 50.0,
+    max_shift_s: float | None = 0.05,
 ) -> TimeOffsetResult:
     """Refine ``δt`` with signed 3-axis rates using a known ``R_IMU_lidar``.
 
@@ -293,9 +294,23 @@ def refine_time_offset_signed(
         f"corr={best_corr:.3f}, mag_corr={mag_at_best:.3f} (coarse_mag={mag_at_coarse:.3f}), "
         f"search=±{half:.3f}s"
     )
+    shift = abs(best_delta - float(delta_t_s))
+    if max_shift_s is not None and shift > float(max_shift_s):
+        notes.append(
+            f"signed refine rejected: |Δδt|={shift:.4f}s exceeds max_shift={float(max_shift_s):.4f}s; "
+            "keeping previous delta_t"
+        )
+        return TimeOffsetResult(
+            delta_t_s=float(delta_t_s),
+            correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
+            search_s=search_s,
+            notes=tuple(notes),
+            ok=True,
+        )
+    # Require a meaningful MSE drop so tiny downhill noise cannot walk δt across iterations.
     improved = (
         np.isfinite(best_cost)
-        and best_cost < coarse_cost * 0.999
+        and best_cost < coarse_cost * 0.98
         # Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
         and mag_at_best + 1e-4 >= mag_at_coarse
     )

imu_lidar/文件职责说明.md

@@ -41,10 +41,11 @@ python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output
 | 5   | `lidar_deskew.py`       | 可选点云去畸变（低速可关）                 |
 | 6   | `imu_preintegration.py` | IMU 预积分（旋转及速度/位移增量、协方差、零偏雅可比） |
 | 6   | `motion_pairs.py`       | 构造运动对；手眼使用其中的旋转               |
+| 6   | `motion_pairs_io.py`    | 运动对 JSON 缓存读写（供可视化直读）          |
 | 7   | `rotation_handeye.py`   | 加权旋转手眼                        |
 | 8   | `observability.py`      | 旋转 / 平移可观性检查                  |
 | 8   | `joint_optimizer.py`    | 联合精修；完整模式下可估计平移、重力、速度与时变零偏    |
-| 9   | `finalize.py`           | 写出结果 JSON                     |
+| 9   | `finalize.py`           | 写出结果 JSON（含 `motion_pairs.json`） |
 | —   | `pipeline.py`           | 编排全流程                         |
 | —   | `cli.py`                | 命令行入口                         |
 | —   | `CHANGELOG.md`          | 改动记录                          |

tests/test_motion_pairs_io.py

@@ -0,0 +1,55 @@
+"""Tests for motion-pair cache IO."""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+
+from imu_lidar.contracts import MotionPair
+from imu_lidar.motion_pairs_io import (
+    build_motion_pairs_payload,
+    load_motion_pairs,
+    pair_from_dict,
+    pair_to_dict,
+    pairs_for_session,
+    save_motion_pairs,
+)
+
+
+def test_pair_roundtrip(tmp_path: Path) -> None:
+    pair = MotionPair(
+        session_id="s0",
+        i=1,
+        j=4,
+        t_i_s=1.0,
+        t_j_s=2.5,
+        R_A=np.eye(3),
+        R_B=np.eye(3),
+        t_A_m=np.array([0.1, 0.0, 0.0]),
+        t_B_m=np.array([0.1, 0.0, 0.0]),
+        fitness=0.8,
+        metadata={"weight": 12.0, "cov9": [[0.0] * 9] * 9, "backend": "test"},
+    )
+    encoded = pair_to_dict(pair)
+    assert "cov9" not in encoded["metadata"]
+    assert encoded["metadata"]["weight"] == 12.0
+    restored = pair_from_dict(encoded)
+    assert restored.i == 1 and restored.j == 4
+    np.testing.assert_allclose(restored.t_A_m, [0.1, 0.0, 0.0])
+
+    payload = build_motion_pairs_payload(
+        prepared_sessions=[
+            {
+                "session_id": "s0",
+                "time_offset_s": 0.0,
+                "gyro_bias_rad_s": np.zeros(3),
+                "pairs": (pair,),
+            }
+        ]
+    )
+    path = save_motion_pairs(tmp_path / "motion_pairs.json", payload)
+    loaded = load_motion_pairs(path)
+    pairs = pairs_for_session(loaded, "s0")
+    assert len(pairs) == 1
+    assert pairs[0].session_id == "s0"

tools/export_motion_pairs_for_viz.py

@@ -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())

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)