Class PoseQueryEngine

tool/benchmark/benchmark_mapping.py:96–265

Query system for timestamp-based pose lookup with SLERP interpolation for rotations.

class PoseQueryEngine:
    """
    Query system for timestamp-based pose lookup with SLERP interpolation for rotations.
    """

    def __init__(self):
        self.continuous_poses: Dict[int, np.ndarray] = (
            {}
        )  # timestamp_ns -> 4x4 pose matrix

    def load_continuous_poses(self, poses_file: str) -> bool:
        """
        Load continuous poses from a saved file.

        Args:
            poses_file: Path to the continuous poses .npy file

        Returns:
            True if loaded successfully, False otherwise
        """
        try:
            loaded_poses = np.load(poses_file, allow_pickle=True).item()
            self.continuous_poses.update(loaded_poses)
            print(f"Loaded {len(loaded_poses)} continuous poses from {poses_file}")
            return True
        except Exception as e:
            print(f"Failed to load continuous poses from {poses_file}: {e}")
            return False

    def find_closest_poses(
        self, target_timestamp: int, poses_dict: Dict[int, np.ndarray]
    ) -> Tuple[Optional[Tuple[int, np.ndarray]], Optional[Tuple[int, np.ndarray]]]:
        """
        Find the two closest poses (before and after) to the target timestamp.

        Args:
            target_timestamp: Target timestamp in nanoseconds
            poses_dict: Dictionary of timestamp -> pose matrix

        Returns:
            Tuple of (before_pose, after_pose) where each is (timestamp, pose_matrix) or None
        """
        if not poses_dict:
            return None, None

        timestamps = sorted(poses_dict.keys())

        # Find insertion point
        idx = np.searchsorted(timestamps, target_timestamp)

        before_pose = None
        after_pose = None

        if idx > 0:
            before_ts = timestamps[idx - 1]
            before_pose = (before_ts, poses_dict[before_ts])

        if idx < len(timestamps):