完善Phase-A会话级联合优化并修正雷达相位中心高度先验

imu_lidar/motion_pairs.py

@@ -2,7 +2,10 @@
 
 from __future__ import annotations
 
+from collections.abc import Callable
 from dataclasses import dataclass
+from time import perf_counter
+from typing import Any
 
 import numpy as np
 
@@ -30,7 +33,12 @@ def build_motion_pairs(
     acc_bias_m_s2: np.ndarray | None = None,
     min_rotation_deg: float = 3.0,
     min_translation_m: float = 0.3,
+    min_registration_fitness: float = 0.5,
+    max_imu_gap_s: float = 0.05,
+    max_lidar_gap_s: float = 1.0,
+    all_frame_times_s: np.ndarray | None = None,
     max_index_span: int = 4,
+    progress_callback: Callable[[dict[str, Any]], None] | None = None,
 ) -> MotionPairSet:
     """Create A/B motion pairs between nearby keyframes.
 
@@ -40,21 +48,74 @@ def build_motion_pairs(
 
     notes: list[str] = []
     pairs: list[MotionPair] = []
+    rejected_fitness = 0
+    rejected_imu_gap = 0
+    rejected_lidar_gap = 0
+    frame_times = (
+        None
+        if all_frame_times_s is None
+        else np.asarray(all_frame_times_s, dtype=float).reshape(-1)
+    )
     bias_g = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
     bias_a = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float)
     n = len(keyframes)
     if n < 2:
         return MotionPairSet((), ("need at least two keyframes",))
 
+    total_candidates = sum(max(n - span, 0) for span in range(1, max_index_span + 1))
+    processed_candidates = 0
+    started_at = perf_counter()
+    last_progress_at = started_at
+
+    def report_progress(*, event: str, span: int, force: bool = False) -> None:
+        nonlocal last_progress_at
+        if progress_callback is None:
+            return
+        now = perf_counter()
+        if not force and processed_candidates > 1 and now - last_progress_at < 10.0:
+            return
+        last_progress_at = now
+        progress_callback(
+            {
+                "event": event,
+                "processed_candidates": processed_candidates,
+                "total_candidates": total_candidates,
+                "progress_pct": 100.0 * processed_candidates / max(total_candidates, 1),
+                "current_span": span,
+                "max_span": max_index_span,
+                "accepted_pairs": len(pairs),
+                "rejected_fitness": rejected_fitness,
+                "rejected_imu_gap": rejected_imu_gap,
+                "rejected_lidar_gap": rejected_lidar_gap,
+                "elapsed_s": now - started_at,
+            }
+        )
+
+    report_progress(event="start", span=1, force=True)
+
     for span in range(1, max_index_span + 1):
         for start in range(0, n - span):
+            processed_candidates += 1
+            report_progress(event="running", span=span)
             i = start
             j = start + span
             frame_i = keyframes[i]
             frame_j = keyframes[j]
+            source_i = int(keyframe_indices[i])
+            source_j = int(keyframe_indices[j])
+            if frame_times is not None:
+                lo = min(source_i, source_j)
+                hi = max(source_i, source_j)
+                local_times = frame_times[lo : hi + 1]
+                if local_times.size >= 2 and np.any(np.diff(local_times) > max_lidar_gap_s):
+                    rejected_lidar_gap += 1
+                    continue
             reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
             if not reg.ok:
                 continue
+            if reg.fitness < min_registration_fitness:
+                rejected_fitness += 1
+                continue
             if reg.rotation_deg < min_rotation_deg and reg.translation_m < min_translation_m:
                 continue
 
@@ -64,6 +125,16 @@ def build_motion_pairs(
                 continue
             if t_i_imu < imu.t_s[0] or t_j_imu > imu.t_s[-1]:
                 continue
+            imu_lo = max(int(np.searchsorted(imu.t_s, t_i_imu, side="right")) - 1, 0)
+            imu_hi = min(
+                int(np.searchsorted(imu.t_s, t_j_imu, side="left")) + 1,
+                imu.t_s.size,
+            )
+            if imu_hi - imu_lo >= 2 and np.any(
+                np.diff(imu.t_s[imu_lo:imu_hi]) > max_imu_gap_s
+            ):
+                rejected_imu_gap += 1
+                continue
 
             preint = preintegrate_imu(
                 imu.t_s,
@@ -111,14 +182,27 @@ def build_motion_pairs(
                         "delta_p": preint.delta_p.tolist(),
                         "t_i_imu_s": t_i_imu,
                         "t_j_imu_s": t_j_imu,
+                        "gyro_bias0_rad_s": bias_g.tolist(),
+                        "accel_bias0_m_s2": bias_a.tolist(),
+                        "time_offset_s": float(delta_t_s),
+                        "keyframe_span": int(span),
+                        "is_consecutive": bool(span == 1),
                         "modeling": "imu_preintegration_factor_phase_c",
                     },
                 )
             )
 
+    report_progress(event="complete", span=max_index_span, force=True)
+
     notes.append(
         f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)"
     )
+    notes.append(
+        "quality rejects: "
+        f"fitness<{min_registration_fitness:.2f}: {rejected_fitness}, "
+        f"IMU gap>{max_imu_gap_s:.3f}s: {rejected_imu_gap}, "
+        f"LiDAR gap>{max_lidar_gap_s:.3f}s: {rejected_lidar_gap}"
+    )
     return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes))