src/perception.py - master - koda/FiVI

src/perception.py
781 строка31 KB
koda
Prepare production-ready grasp planner
18 июл 2026, 09:39
18 июл 2026, 09:39f540458
from __future__ import annotations

from dataclasses import dataclass
from enum import Enum
import logging
from math import tan
from time import perf_counter_ns
from typing import cast

import mujoco  # type: ignore[import-untyped]
import numpy as np
import numpy.typing as npt
from scipy.spatial import cKDTree
from scipy.spatial import ConvexHull, QhullError

from .ik import FloatArray
from .runtime import Runtime
from .scene import Scene
from .surface import Shape


_LOGGER = logging.getLogger(__name__)


def _immutable(value: npt.ArrayLike, shape: tuple[int, ...], name: str) -> FloatArray:
    result = np.array(value, dtype=np.float64, copy=True)
    if result.shape != shape or not bool(np.all(np.isfinite(result))):
        raise ValueError(f"{name} must be finite with shape {shape}")
    result.flags.writeable = False
    return result


def _direction(value: npt.ArrayLike) -> FloatArray:
    result = np.array(value, dtype=np.float64, copy=True)
    length = float(np.linalg.norm(result))
    if result.shape != (3,) or not np.isfinite(length) or length <= 1.0e-12:
        raise ValueError("direction must be a finite non-zero vector")
    result /= length
    pivot = int(np.argmax(np.abs(result)))
    if result[pivot] < 0.0:
        result *= -1.0
    return result


class Segmentation(str, Enum):
    """Camera-derived mask backend used for one observation."""

    MUJOCO_SEGMENTATION_RENDER = "mujoco_segmentation_render"


@dataclass(frozen=True, slots=True)
class PerceptionConfig:
    """Thresholds controlling segmentation, fitting confidence, and uncertainty."""

    maximum_points: int = 6000
    minimum_segmented_pixels: int = 16
    visible_fraction_scale: float = 0.5
    confidence_residual_scale: float = 0.05
    uncertainty_base_m: float = 0.0003
    uncertainty_residual_weight: float = 2.5
    uncertainty_confidence_weight_m: float = 0.001
    uncertainty_visibility_weight_m: float = 0.0002
    cloud_residual_ratio: float = 0.02

    def __post_init__(self) -> None:
        if (
            isinstance(self.maximum_points, bool)
            or not isinstance(self.maximum_points, int)
            or isinstance(self.minimum_segmented_pixels, bool)
            or not isinstance(self.minimum_segmented_pixels, int)
        ):
            raise ValueError("point limits must be integers")
        values = np.array(
            (
                self.visible_fraction_scale,
                self.confidence_residual_scale,
                self.uncertainty_base_m,
                self.uncertainty_residual_weight,
                self.uncertainty_confidence_weight_m,
                self.uncertainty_visibility_weight_m,
                self.cloud_residual_ratio,
            ),
            dtype=np.float64,
        )
        if self.maximum_points < 32 or self.minimum_segmented_pixels < 16:
            raise ValueError("point limits must be at least 32 and 16 respectively")
        if not bool(np.all(np.isfinite(values))) or bool(np.any(values <= 0.0)):
            raise ValueError("perception thresholds must be finite and positive")
        if self.visible_fraction_scale > 1.0:
            raise ValueError("visible_fraction_scale must not exceed one")


@dataclass(frozen=True, slots=True)
class Observation:
    """Immutable object estimate containing only deployable planner inputs."""

    timestamp_s: float
    camera_name: str
    point_cloud_camera: FloatArray
    point_cloud_hand: FloatArray
    shape: Shape
    position_hand: FloatArray
    orientation_hand: FloatArray
    camera_position_hand: FloatArray
    dimensions_m: FloatArray
    visible_surface_fraction: float
    fit_residual_m: float
    confidence: float
    uncertainty_margin_m: float
    segmentation: Segmentation

    def __post_init__(self) -> None:
        if not isinstance(self.camera_name, str) or not self.camera_name.strip():
            raise ValueError("camera_name must be a non-empty string")
        if not isinstance(self.shape, Shape):
            raise ValueError("shape must be a Shape")
        if not isinstance(self.segmentation, Segmentation):
            raise ValueError("segmentation must be a Segmentation")
        camera = np.array(self.point_cloud_camera, dtype=np.float64, copy=True)
        hand = np.array(self.point_cloud_hand, dtype=np.float64, copy=True)
        if (
            camera.ndim != 2
            or camera.shape[1:] != (3,)
            or hand.shape != camera.shape
            or camera.shape[0] < 16
            or not bool(np.all(np.isfinite(camera)))
            or not bool(np.all(np.isfinite(hand)))
        ):
            raise ValueError("point clouds must be matching finite arrays with shape (N, 3)")
        camera.flags.writeable = False
        hand.flags.writeable = False
        object.__setattr__(self, "point_cloud_camera", camera)
        object.__setattr__(self, "point_cloud_hand", hand)
        object.__setattr__(self, "position_hand", _immutable(self.position_hand, (3,), "position_hand"))
        object.__setattr__(self, "camera_position_hand", _immutable(self.camera_position_hand, (3,), "camera_position_hand"))
        orientation = _immutable(self.orientation_hand, (3, 3), "orientation_hand")
        if not np.allclose(orientation.T @ orientation, np.eye(3), atol=1.0e-6):
            raise ValueError("orientation_hand must be orthonormal")
        if not np.isclose(float(np.linalg.det(orientation)), 1.0, atol=1.0e-6):
            raise ValueError("orientation_hand must be a proper rotation")
        object.__setattr__(self, "orientation_hand", orientation)
        dimensions = _immutable(self.dimensions_m, (3,), "dimensions_m")
        if bool(np.any(dimensions <= 0.0)):
            raise ValueError("dimensions_m must be positive")
        object.__setattr__(self, "dimensions_m", dimensions)
        scalars = np.array(
            (
                self.timestamp_s,
                self.visible_surface_fraction,
                self.fit_residual_m,
                self.confidence,
                self.uncertainty_margin_m,
            )
        )
        if not bool(np.all(np.isfinite(scalars))) or self.timestamp_s < 0.0:
            raise ValueError("observation scalars must be finite")
        if not 0.0 <= self.visible_surface_fraction <= 1.0:
            raise ValueError("visible_surface_fraction must be in [0, 1]")
        if not 0.0 <= self.confidence <= 1.0:
            raise ValueError("confidence must be in [0, 1]")
        if self.fit_residual_m < 0.0 or self.uncertainty_margin_m < 0.0:
            raise ValueError("residual and uncertainty must be non-negative")


@dataclass(frozen=True, slots=True)
class _PrimitiveFit:
    shape: Shape
    position: FloatArray
    orientation: FloatArray
    dimensions: FloatArray
    residual: float


def depth_to_point_cloud(
    depth_m: npt.ArrayLike,
    mask: npt.ArrayLike,
    vertical_fov_deg: float,
    *,
    maximum_points: int = 6000,
    minimum_points: int = 16,
) -> FloatArray:
    """Back-project masked metric depth into the MuJoCo camera frame."""

    depth = np.asarray(depth_m, dtype=np.float64)
    selected = np.asarray(mask, dtype=np.bool_)
    if depth.ndim != 2 or selected.shape != depth.shape:
        raise ValueError("depth and mask must have matching image shapes")
    if not np.isfinite(vertical_fov_deg) or not 1.0 < vertical_fov_deg < 179.0:
        raise ValueError("vertical_fov_deg must be in (1, 179)")
    if (
        isinstance(maximum_points, bool)
        or not isinstance(maximum_points, int)
        or isinstance(minimum_points, bool)
        or not isinstance(minimum_points, int)
    ):
        raise ValueError("point count limits must be integers")
    rows, columns = np.nonzero(selected & np.isfinite(depth) & (depth > 0.0))
    if maximum_points < 32 or minimum_points < 16 or maximum_points < minimum_points:
        raise ValueError("point count limits are invalid")
    if rows.size < minimum_points:
        raise ValueError(
            f"OBJECT_NOT_VISIBLE: fewer than {minimum_points} segmented depth pixels"
        )
    if rows.size > maximum_points:
        indices = np.linspace(0, rows.size - 1, maximum_points, dtype=np.intp)
        rows = rows[indices]
        columns = columns[indices]
    height, width = depth.shape
    focal = 0.5 * height / tan(0.5 * np.deg2rad(vertical_fov_deg))
    z = depth[rows, columns]
    x = (columns.astype(np.float64) + 0.5 - 0.5 * width) * z / focal
    y = (0.5 * height - rows.astype(np.float64) - 0.5) * z / focal
    return np.column_stack((x, y, -z))


def _proper_axes(eigenvectors: FloatArray) -> FloatArray:
    axes = np.asarray(eigenvectors, dtype=np.float64)
    if axes.shape != (3, 3) or not bool(np.all(np.isfinite(axes))):
        raise ValueError("axes must be finite with shape (3, 3)")
    first = _direction(axes[:, 0])
    second_value = axes[:, 1] - float(axes[:, 1] @ first) * first
    second = _direction(second_value)
    third = _direction(np.cross(first, second))
    result = np.column_stack((first, second, third))
    if float(np.linalg.det(result)) < 0.0:
        result[:, 2] *= -1.0
    return result


def _fit_sphere(points: FloatArray) -> _PrimitiveFit:
    matrix = np.column_stack((2.0 * points, np.ones(points.shape[0])))
    target = np.einsum("ij,ij->i", points, points)
    solution, _, _, _ = np.linalg.lstsq(matrix, target, rcond=None)
    center = solution[:3]
    distances = np.linalg.norm(points - center, axis=1)
    radius = float(np.median(distances))
    if not np.isfinite(radius) or radius <= 1.0e-9:
        raise ValueError("sphere fit has no valid radius")
    residual = float(np.median(np.abs(distances - radius)))
    return _PrimitiveFit(
        Shape.SPHERE,
        center,
        np.eye(3),
        np.full(3, 2.0 * radius),
        residual,
    )


def _pca(points: FloatArray) -> tuple[FloatArray, FloatArray, FloatArray]:
    center = np.mean(points, axis=0)
    covariance = (points - center).T @ (points - center) / points.shape[0]
    values, vectors = np.linalg.eigh(covariance)
    order = np.argsort(-values, kind="stable")
    return values[order], _proper_axes(vectors[:, order]), center


def _local_normals(points: FloatArray, maximum_samples: int = 1500) -> tuple[FloatArray, FloatArray]:
    stride = max(points.shape[0] // maximum_samples, 1)
    sample = points[::stride]
    tree = cKDTree(points)
    _, neighbor_indices = tree.query(sample, k=min(12, points.shape[0]))
    neighborhoods = points[neighbor_indices]
    centered = neighborhoods - np.mean(neighborhoods, axis=1, keepdims=True)
    covariance = np.einsum("nki,nkj->nij", centered, centered)
    _, vectors = np.linalg.eigh(covariance)
    return sample, vectors[:, :, 0]


def _box_axes(points: FloatArray, fallback: FloatArray) -> FloatArray:
    _, normals = _local_normals(points)
    normal_lengths = np.linalg.norm(normals, axis=1)
    valid = np.isfinite(normal_lengths) & (normal_lengths > 1.0e-12)
    if int(np.count_nonzero(valid)) < 8:
        return fallback
    directions = normals[valid] / normal_lengths[valid, None]
    canonical = np.stack(tuple(_direction(item) for item in directions))
    alignment = np.abs(canonical @ canonical.T)
    support = np.sum(alignment >= 0.985, axis=1)
    order = sorted(
        range(canonical.shape[0]),
        key=lambda index: (
            -int(support[index]),
            tuple(float(value) for value in np.round(canonical[index], 12)),
        ),
    )
    first_index = order[0]
    first_group = directions[alignment[first_index] >= 0.97]
    first_values, first_vectors = np.linalg.eigh(first_group.T @ first_group)
    first = _direction(first_vectors[:, int(np.argmax(first_values))])
    second_options = tuple(
        index for index in order if abs(float(canonical[index] @ first)) <= 0.25
    )
    if not second_options:
        return fallback
    second_index = second_options[0]
    second_group = directions[np.abs(directions @ canonical[second_index]) >= 0.97]
    second_values, second_vectors = np.linalg.eigh(second_group.T @ second_group)
    second_value = second_vectors[:, int(np.argmax(second_values))]
    second_value -= float(second_value @ first) * first
    if float(np.linalg.norm(second_value)) <= 1.0e-12:
        return fallback
    second = _direction(second_value)
    return _proper_axes(np.column_stack((first, second, np.cross(first, second))))


def _fit_box(points: FloatArray, camera_position: FloatArray) -> _PrimitiveFit:
    _, fallback_axes, centroid = _pca(points)
    axes = _box_axes(points, fallback_axes)
    try:
        hull = ConvexHull(points)
        directions: list[FloatArray] = []
        for value in np.asarray(hull.equations[:, :3], dtype=np.float64):
            normal = value / np.linalg.norm(value)
            if normal[int(np.argmax(np.abs(normal)))] < 0.0:
                normal = -normal
            if all(abs(float(np.dot(normal, item))) < 0.98 for item in directions):
                directions.append(normal)
            if len(directions) >= 24:
                break
        best_error = np.inf
        best_volume = np.inf
        initial_local = (points - centroid) @ axes
        initial_lower = np.percentile(initial_local, 0.5, axis=0)
        initial_upper = np.percentile(initial_local, 99.5, axis=0)
        initial_midpoint = 0.5 * (initial_lower + initial_upper)
        initial_dimensions = initial_upper - initial_lower
        initial_centered = initial_local - initial_midpoint
        best_error = float(
            np.median(
                np.min(
                    np.abs(np.abs(initial_centered) - 0.5 * initial_dimensions),
                    axis=1,
                )
            )
        )
        best_volume = float(np.prod(initial_dimensions))
        for first_index, first in enumerate(directions):
            for second in directions[first_index + 1 :]:
                if abs(float(np.dot(first, second))) > 0.25:
                    continue
                third = np.cross(first, second)
                third /= np.linalg.norm(third)
                second_orthogonal = np.cross(third, first)
                candidate = _proper_axes(
                    np.column_stack((first, second_orthogonal, third))
                )
                coordinates = (points - centroid) @ candidate
                lower = np.percentile(coordinates, 0.5, axis=0)
                upper = np.percentile(coordinates, 99.5, axis=0)
                extent = upper - lower
                volume = float(np.prod(extent))
                midpoint = 0.5 * (lower + upper)
                error = float(
                    np.median(
                        np.min(
                            np.abs(np.abs(coordinates - midpoint) - 0.5 * extent),
                            axis=1,
                        )
                    )
                )
                if (error, volume) < (best_error, best_volume):
                    best_error = error
                    best_volume = volume
                    axes = candidate
    except QhullError:
        pass
    local = (points - centroid) @ axes
    lower = np.percentile(local, 0.5, axis=0)
    upper = np.percentile(local, 99.5, axis=0)
    dimensions = upper - lower
    midpoint = 0.5 * (lower + upper)
    thin_axis = int(np.argmin(dimensions))
    other = np.delete(dimensions, thin_axis)
    if dimensions[thin_axis] < 0.25 * float(np.median(other)):
        inferred = float(np.median(other))
        view = centroid - camera_position
        sign = 1.0 if float(np.dot(view, axes[:, thin_axis])) >= 0.0 else -1.0
        midpoint[thin_axis] += 0.5 * sign * inferred
        dimensions[thin_axis] = inferred
    center = centroid + axes @ midpoint
    centered = (points - center) @ axes
    face_error = np.min(np.abs(np.abs(centered) - 0.5 * dimensions), axis=1)
    residual = float(np.median(face_error))
    return _PrimitiveFit(Shape.BOX, center, axes, dimensions, residual)


def _fit_cylinder(points: FloatArray) -> _PrimitiveFit:
    _, normals = _local_normals(points)
    _, point_axes, _ = _pca(points)
    normal_covariance = normals.T @ normals / normals.shape[0]
    _, normal_axes = np.linalg.eigh(normal_covariance)
    alignment_counts = np.sum(np.abs(normals @ normals.T) > 0.985, axis=1)
    candidate_axes: list[FloatArray] = [
        normal_axes[:, index] for index in range(3)
    ] + [point_axes[:, index] for index in range(3)]
    for index in np.argsort(-alignment_counts, kind="stable"):
        candidate = normals[int(index)]
        if all(abs(float(np.dot(candidate, axis))) < 0.95 for axis in candidate_axes):
            candidate_axes.append(candidate)
        if len(candidate_axes) >= 12:
            break
    try:
        hull_normals = np.asarray(
            ConvexHull(points).equations[:, :3], dtype=np.float64
        )
    except QhullError:
        hull_normals = np.empty((0, 3), dtype=np.float64)
    initial_axes = tuple(candidate_axes)
    for reference in initial_axes:
        if hull_normals.shape[0] == 0:
            break
        best = hull_normals[
            int(np.argmax(np.abs(hull_normals @ reference)))
        ]
        best = best / np.linalg.norm(best)
        if all(
            abs(float(np.dot(best, axis))) < 0.9995
            for axis in candidate_axes
        ):
            candidate_axes.append(best)
    for candidate in hull_normals:
        candidate = candidate / np.linalg.norm(candidate)
        if all(abs(float(np.dot(candidate, axis))) < 0.98 for axis in candidate_axes):
            candidate_axes.append(candidate)
        if len(candidate_axes) >= 24:
            break
    candidates: list[tuple[float, _PrimitiveFit]] = []
    scale = max(float(np.linalg.norm(np.ptp(points, axis=0))), 1.0e-6)
    for axis_value in candidate_axes:
        axis = axis_value / np.linalg.norm(axis_value)
        helper = np.array((1.0, 0.0, 0.0))
        if abs(float(np.dot(axis, helper))) > 0.9:
            helper = np.array((0.0, 1.0, 0.0))
        first = helper - float(np.dot(helper, axis)) * axis
        first /= np.linalg.norm(first)
        second = np.cross(axis, first)
        tangent = np.column_stack((first, second))
        radial_coordinates = points @ tangent
        axial = points @ axis
        axial_low, axial_high = np.percentile(axial, (0.01, 99.99))
        axial_span = float(axial_high - axial_low)
        axial_inset = max(0.01 * axial_span, 1.0e-5)
        side_mask = (axial > axial_low + axial_inset) & (
            axial < axial_high - axial_inset
        )
        if int(np.count_nonzero(side_mask)) < 32:
            central_low, central_high = np.percentile(axial, (20.0, 80.0))
            side_mask = (axial >= central_low) & (axial <= central_high)
        side_coordinates = radial_coordinates[side_mask]
        radial_matrix = np.column_stack(
            (2.0 * side_coordinates, np.ones(side_coordinates.shape[0]))
        )
        radial_target = np.einsum("ij,ij->i", side_coordinates, side_coordinates)
        circle, _, _, _ = np.linalg.lstsq(radial_matrix, radial_target, rcond=None)
        radial_center = circle[:2]
        radial = np.linalg.norm(radial_coordinates - radial_center, axis=1)
        radius_squared = float(circle[2] + radial_center @ radial_center)
        if radius_squared <= 0.0:
            continue
        observed_diameter = float(
            np.max(np.ptp(radial_coordinates[side_mask], axis=0))
        )
        radius = max(
            float(np.sqrt(radius_squared)),
            float(np.median(radial[side_mask])),
            0.5 * observed_diameter,
        )
        radial = np.linalg.norm(radial_coordinates - radial_center, axis=1)
        axial_center = 0.5 * float(axial_low + axial_high)
        half_height = 0.5 * axial_span
        side_error = np.abs(radial - radius)
        cap_error = np.minimum(
            np.abs(axial - axial_low), np.abs(axial - axial_high)
        )
        residual = float(np.median(np.minimum(side_error, cap_error)))
        rotation = _proper_axes(np.column_stack((first, second, axis)))
        center = tangent @ radial_center + axial_center * axis
        normal_alignment = np.abs(normals @ axis)
        normal_error = float(
            np.median(np.minimum(normal_alignment, np.abs(1.0 - normal_alignment)))
        )
        candidates.append(
            (normal_error + residual / scale,
            _PrimitiveFit(
                Shape.CYLINDER,
                center,
                rotation,
                np.array((2.0 * radius, 2.0 * radius, 2.0 * half_height)),
                residual,
            ))
        )
    if not candidates:
        raise ValueError("cylinder fit has no valid radius")
    return min(candidates, key=lambda item: (item[0], item[1].residual))[1]


def _surface_errors(points: FloatArray, fit: _PrimitiveFit) -> FloatArray:
    local = (points - fit.position) @ fit.orientation
    if fit.shape is Shape.BOX:
        delta = np.abs(local) - 0.5 * fit.dimensions
        signed = np.linalg.norm(np.maximum(delta, 0.0), axis=1) + np.minimum(
            np.max(delta, axis=1),
            0.0,
        )
        return cast(FloatArray, np.abs(signed))
    if fit.shape is Shape.SPHERE:
        return cast(
            FloatArray,
            np.abs(np.linalg.norm(local, axis=1) - 0.5 * fit.dimensions[0]),
        )
    radius = 0.25 * float(fit.dimensions[0] + fit.dimensions[1])
    half_height = 0.5 * float(fit.dimensions[2])
    delta = np.column_stack(
        (
            np.linalg.norm(local[:, :2], axis=1) - radius,
            np.abs(local[:, 2]) - half_height,
        )
    )
    signed = np.linalg.norm(np.maximum(delta, 0.0), axis=1) + np.minimum(
        np.max(delta, axis=1),
        0.0,
    )
    return cast(FloatArray, np.abs(signed))


def _robust_fit(points: FloatArray, fit: _PrimitiveFit) -> _PrimitiveFit:
    residual = float(np.percentile(_surface_errors(points, fit), 80.0))
    return _PrimitiveFit(
        fit.shape,
        fit.position,
        fit.orientation,
        fit.dimensions,
        residual,
    )


def _fit_candidates(
    points: FloatArray,
    camera: FloatArray,
) -> tuple[tuple[float, int, _PrimitiveFit], ...]:
    box = _robust_fit(points, _fit_box(points, camera))
    sphere = _robust_fit(points, _fit_sphere(points))
    try:
        cylinder = _robust_fit(points, _fit_cylinder(points))
    except ValueError:
        cylinder = None

    scale = max(float(np.linalg.norm(np.ptp(points, axis=0))), 1.0e-6)
    normal_points, normals = _local_normals(points)
    normals = normals / np.maximum(np.linalg.norm(normals, axis=1, keepdims=True), 1.0e-12)
    box_alignment = np.max(np.abs(normals @ box.orientation), axis=1)
    box_normal_error = float(np.median(1.0 - box_alignment))
    radial = normal_points - sphere.position
    radial /= np.maximum(np.linalg.norm(radial, axis=1, keepdims=True), 1.0e-12)
    sphere_normal_error = float(np.median(1.0 - np.abs(np.sum(normals * radial, axis=1))))
    candidates: list[tuple[float, int, _PrimitiveFit]] = [
        (box.residual / scale + 0.08 * box_normal_error, 0, box),
        (sphere.residual / scale + 0.08 * sphere_normal_error, 1, sphere),
    ]
    if cylinder is not None:
        cylinder_axis = cylinder.orientation[:, 2]
        cylinder_alignment = np.abs(normals @ cylinder_axis)
        cylinder_normal_error = float(
            np.median(
                np.minimum(cylinder_alignment, np.abs(1.0 - cylinder_alignment))
            )
        )
        candidates.append(
            (
                cylinder.residual / scale + 0.08 * cylinder_normal_error,
                2,
                cylinder,
            )
        )
    return tuple(candidates)


def fit_observed_primitive(
    point_cloud_hand: npt.ArrayLike,
    camera_position_hand: npt.ArrayLike,
    cloud_residual_ratio: float = 0.02,
) -> _PrimitiveFit:
    """Fit a primitive or return a generic cloud model for irregular data."""

    points = np.asarray(point_cloud_hand, dtype=np.float64)
    camera = _immutable(camera_position_hand, (3,), "camera_position_hand")
    if points.ndim != 2 or points.shape[1:] != (3,) or points.shape[0] < 32:
        raise ValueError("INSUFFICIENT_DEPTH_OBSERVATION")
    if not bool(np.all(np.isfinite(points))):
        raise ValueError("DEPTH_OBSERVATION_NOT_FINITE")
    if not np.isfinite(cloud_residual_ratio) or cloud_residual_ratio <= 0.0:
        raise ValueError("cloud_residual_ratio must be finite and positive")
    centered = points - np.mean(points, axis=0)
    singular_values = np.linalg.svd(centered, compute_uv=False)
    scale = max(float(singular_values[0]), 1.0e-12)
    if int(np.count_nonzero(singular_values > scale * 1.0e-8)) < 2:
        raise ValueError("DEGENERATE_DEPTH_OBSERVATION")
    candidates = _fit_candidates(points, camera)
    score, _, selected = min(candidates, key=lambda item: (item[0], item[1]))

    if score <= cloud_residual_ratio:
        return selected

    center = np.mean(points, axis=0)
    dimensions = np.maximum(np.ptp(points, axis=0), 1.0e-6)
    tree = cKDTree(points)
    neighbor_distance, _ = tree.query(points[:: max(1, points.shape[0] // 1000)], k=2)
    distances = np.asarray(neighbor_distance, dtype=np.float64)
    noise = float(np.median(distances[:, 1]))
    return _PrimitiveFit(Shape.CLOUD, center, np.eye(3), dimensions, noise)


class Observer:

    def __init__(
        self,
        runtime: Runtime,
        scene: Scene,
        config: PerceptionConfig = PerceptionConfig(),
    ) -> None:

        self._runtime = runtime
        self._scene = scene
        self._config = config
        camera_id = int(
            mujoco.mj_name2id(
                runtime.model,
                mujoco.mjtObj.mjOBJ_CAMERA,
                scene.runtime.camera_name,
            )
        )
        if camera_id < 0:
            raise ValueError("camera unavailable")
        self._camera_id = camera_id
        geom_ids = tuple(
            int(mujoco.mj_name2id(runtime.model, mujoco.mjtObj.mjOBJ_GEOM, name))
            for name in scene.geom_names
        )
        if any(geom_id < 0 for geom_id in geom_ids):
            raise ValueError("object segmentation geoms are unavailable")
        self._object_geom_ids = np.array(geom_ids, dtype=np.int32)

    def observe(self) -> Observation:
        """Render, segment, back-project, and fit one primitive observation."""

        started = perf_counter_ns()
        rgb, depth = self._runtime.get_camera_image()
        rendered = perf_counter_ns()
        if rgb.shape[:2] != depth.shape:
            raise ValueError("CAMERA_UNAVAILABLE: RGB and depth shapes differ")
        segmentation = self._runtime.get_camera_segmentation()
        segmented = perf_counter_ns()
        mask = np.isin(segmentation[:, :, 0], self._object_geom_ids) & (
            segmentation[:, :, 1] == int(mujoco.mjtObj.mjOBJ_GEOM)
        )
        segmented_count = int(np.count_nonzero(mask))
        if segmented_count == 0:
            raise ValueError("SEGMENTATION_FAILED: object has no camera pixels")
        rows, columns = np.nonzero(mask)
        valid_depth_count = int(
            np.count_nonzero(mask & np.isfinite(depth) & (depth > 0.0))
        )
        camera_points = depth_to_point_cloud(
            depth,
            mask,
            float(self._runtime.model.cam_fovy[self._camera_id]),
            maximum_points=self._config.maximum_points,
            minimum_points=self._config.minimum_segmented_pixels,
        )
        camera_rotation = np.asarray(
            self._runtime.data.cam_xmat[self._camera_id], dtype=np.float64
        ).reshape(3, 3)
        camera_position = np.asarray(
            self._runtime.data.cam_xpos[self._camera_id], dtype=np.float64
        )
        world_points = camera_points @ camera_rotation.T + camera_position
        palm_id = int(
            mujoco.mj_name2id(
                self._runtime.model,
                mujoco.mjtObj.mjOBJ_BODY,
                "palm",
            )
        )
        if palm_id < 0:
            raise ValueError("palm frame is unavailable")
        palm_rotation = np.asarray(
            self._runtime.data.xmat[palm_id], dtype=np.float64
        ).reshape(3, 3)
        palm_position = np.asarray(self._runtime.data.xpos[palm_id], dtype=np.float64)
        hand_points = (world_points - palm_position) @ palm_rotation
        camera_position_hand = (camera_position - palm_position) @ palm_rotation
        reconstructed = perf_counter_ns()
        try:
            fit = fit_observed_primitive(
                hand_points,
                camera_position_hand,
                self._config.cloud_residual_ratio,
            )
        except ValueError as error:
            if "INSUFFICIENT_DEPTH_OBSERVATION" in str(error):
                raise
            raise ValueError(f"SHAPE_FIT_FAILED: {error}") from error
        fitted = perf_counter_ns()
        bounding_area = max(
            (int(np.max(rows)) - int(np.min(rows)) + 1)
            * (int(np.max(columns)) - int(np.min(columns)) + 1),
            1,
        )
        mask_density = float(rows.size / bounding_area)
        valid_depth_fraction = float(valid_depth_count / max(segmented_count, 1))
        visible_fraction = float(
            np.clip(
                self._config.visible_fraction_scale
                * mask_density
                * valid_depth_fraction,
                0.0,
                self._config.visible_fraction_scale,
            )
        )
        characteristic = max(float(np.min(fit.dimensions)), 1.0e-6)
        confidence = float(
            np.clip(
                visible_fraction
                * np.exp(
                    -fit.residual
                    / (self._config.confidence_residual_scale * characteristic)
                ),
                0.0,
                1.0,
            )
        )
        uncertainty = (
            self._config.uncertainty_base_m
            + self._config.uncertainty_residual_weight * fit.residual
            + self._config.uncertainty_confidence_weight_m * (1.0 - confidence)
            + self._config.uncertainty_visibility_weight_m * (1.0 - visible_fraction)
        )
        _LOGGER.debug(
            "RGB-D observation: valid_depth=%d segmented=%d points=%d "
            "visible_fraction=%.6f fit_residual_m=%.9f confidence=%.6f",
            valid_depth_count,
            segmented_count,
            camera_points.shape[0],
            visible_fraction,
            fit.residual,
            confidence,
        )
        _LOGGER.debug(
            "RGB-D timing: render_ms=%.3f segmentation_ms=%.3f "
            "point_cloud_ms=%.3f fitting_ms=%.3f",
            (rendered - started) / 1.0e6,
            (segmented - rendered) / 1.0e6,
            (reconstructed - segmented) / 1.0e6,
            (fitted - reconstructed) / 1.0e6,
        )
        return Observation(
            timestamp_s=float(self._runtime.data.time),
            camera_name=self._scene.runtime.camera_name,
            point_cloud_camera=camera_points,
            point_cloud_hand=hand_points,
            shape=fit.shape,
            position_hand=fit.position,
            orientation_hand=fit.orientation,
            camera_position_hand=camera_position_hand,
            dimensions_m=fit.dimensions,
            visible_surface_fraction=visible_fraction,
            fit_residual_m=fit.residual,
            confidence=confidence,
            uncertainty_margin_m=uncertainty,
            segmentation=Segmentation.MUJOCO_SEGMENTATION_RENDER,
        )


__all__ = [
    "Observation",
    "Observer",
    "PerceptionConfig",
    "Segmentation",
    "depth_to_point_cloud",
    "fit_observed_primitive",
]