添加 LiDAR-IMU 外参标定流水线与说明文档

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

imu_lidar/keyframes.py

@@ -0,0 +1,49 @@
+"""LiDAR keyframe selection."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .contracts import LidarFrame
+from .registration import register_lidar_pair
+
+
+@dataclass(frozen=True)
+class KeyframeSet:
+    indices: tuple[int, ...]
+    frames: tuple[LidarFrame, ...]
+
+
+def build_keyframes(
+    frames: list[LidarFrame],
+    *,
+    min_translation_m: float = 0.3,
+    min_rotation_deg: float = 3.0,
+    max_frame_gap: int = 8,
+) -> KeyframeSet:
+    """Select keyframes with enough relative motion for hand-eye pairs."""
+
+    if not frames:
+        return KeyframeSet((), ())
+
+    selected = [0]
+    last = 0
+    for index in range(1, len(frames)):
+        if index - last > max_frame_gap:
+            selected.append(index)
+            last = index
+            continue
+        result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
+        if not result.ok:
+            continue
+        if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
+            selected.append(index)
+            last = index
+
+    if selected[-1] != len(frames) - 1 and len(frames) > 1:
+        selected.append(len(frames) - 1)
+
+    unique = tuple(dict.fromkeys(selected))
+    return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))