51 lines
1.4 KiB
Python
51 lines
1.4 KiB
Python
"""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,
|
|
min_registration_fitness: float = 0.5,
|
|
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 or result.fitness < min_registration_fitness:
|
|
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))
|