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

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

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),
     )