NODEDC_MISSION_CORE/tests/test_mask_grounding_semantics.py

from __future__ import annotations

import hashlib
from pathlib import Path

import numpy as np

from k1link.perception.contracts import (
    EvidenceBasis,
    EvidenceCurrentness,
    MetricGeometry,
    ObstacleObservation,
)
from k1link.perception.geometry_math import ProjectedPointCloud
from k1link.perception.mask_grounding_semantics import (
    MASK_GROUNDING_EVIDENCE_SCHEMA,
    MaskBindingResolution,
    MaskGroundingDetection,
    MaskLabelResolution,
    bind_mask_instances_to_geometry,
    cluster_mask_instances,
    load_mask_grounding_evidence,
    resolve_mask_instance_label,
)


def _observation(ordinal: int, source_ids: tuple[int, ...]) -> ObstacleObservation:
    return ObstacleObservation(
        observation_id=f"frame-000253:geometry:{ordinal}",
        occupancy_key=f"frame-000253:occupancy:{ordinal}",
        source_id="RAVNOVES00",
        frame_id="frame-000253",
        evidence_time_ns=253,
        basis=EvidenceBasis.LIDAR,
        currentness=EvidenceCurrentness.CURRENT,
        occupied_support=True,
        source_point_ids=source_ids,
        metric_geometry=MetricGeometry(
            coordinate_frame="map",
            centroid_xyz_m=(2.0 + ordinal, 0.0, 0.5),
            range_m=2.0 + ordinal,
            covariance_diagonal_m2=(0.1, 0.1, 0.1),
        ),
        proposal_ids=(),
        semantic_hint=None,
        reason_codes=("test-geometry-only",),
    )


def _projected() -> ProjectedPointCloud:
    return ProjectedPointCloud(
        pixels_xy=np.asarray(
            (
                (10.0, 10.0),
                (11.0, 10.0),
                (10.0, 11.0),
                (11.0, 11.0),
                (30.0, 30.0),
                (31.0, 30.0),
                (30.0, 31.0),
                (31.0, 31.0),
            ),
            dtype=np.float64,
        ),
        depths_m=np.ones(8, dtype=np.float64),
        source_indices=np.arange(8, dtype=np.int64),
        source_point_count=8,
        camera_front_point_count=8,
    )


def _detection(
    detection_id: str,
    class_name: str,
    confidence: float,
    mask: np.ndarray,
) -> MaskGroundingDetection:
    packed = mask.astype(np.uint8)
    return MaskGroundingDetection(
        detection_id=detection_id,
        prompt_set_id="urban-static/v0",
        class_id=0,
        class_name=class_name,
        confidence=confidence,
        box_xyxy=(1.0, 1.0, 40.0, 40.0),
        mask_sha256=hashlib.sha256(packed.tobytes()).hexdigest(),
        mask=mask,
    )


def test_empty_early_v0_ledger_is_read_without_inventing_boxes(tmp_path: Path) -> None:
    path = tmp_path / "frame-000253.npz"
    np.savez_compressed(
        path,
        schema_version=np.asarray(MASK_GROUNDING_EVIDENCE_SCHEMA),
        source_file_sha256=np.asarray("a" * 64),
        source_pixel_sha256=np.asarray("b" * 64),
        class_ids=np.empty(0, dtype=np.int16),
        class_names=np.empty(0, dtype="U128"),
        scores=np.empty(0, dtype=np.float32),
        boxes_xyxy=np.empty(0, dtype=np.float32),
        masks=np.empty((0, 600, 800), dtype=np.uint8),
    )

    evidence = load_mask_grounding_evidence(path, prompt_set_id="urban-agents/v0")

    assert evidence.detections == ()
    assert evidence.source_file_sha256 == "a" * 64


def test_same_mask_becomes_one_instance_and_conflicting_name_stays_ambiguous() -> None:
    mask = np.zeros((600, 800), dtype=np.bool_)
    mask[5:20, 5:20] = True
    instances = cluster_mask_instances(
        (
            _detection("trash", "trash bin", 0.43, mask),
            _detection("cart", "shopping cart", 0.41, mask),
        )
    )

    bindings = bind_mask_instances_to_geometry(
        instances,
        observations=(_observation(0, (0, 1, 2, 3)), _observation(1, (4, 5, 6, 7))),
        projected=_projected(),
    )
    label = resolve_mask_instance_label(instances[0])

    assert len(instances) == 1
    assert bindings[0].resolution is MaskBindingResolution.SELECTED
    assert bindings[0].selected_observation_id == "frame-000253:geometry:0"
    assert label.resolution is MaskLabelResolution.AMBIGUOUS
    assert label.selected_label is None


def test_mask_covering_two_obstacles_does_not_claim_either() -> None:
    mask = np.zeros((600, 800), dtype=np.bool_)
    mask[5:40, 5:40] = True
    instance = cluster_mask_instances((_detection("wide", "trash bin", 0.7, mask),))

    binding = bind_mask_instances_to_geometry(
        instance,
        observations=(_observation(0, (0, 1, 2, 3)), _observation(1, (4, 5, 6, 7))),
        projected=_projected(),
    )[0]

    assert binding.resolution is MaskBindingResolution.AMBIGUOUS
    assert binding.selected_observation_id is None
    assert binding.reason_code == "mask-covers-multiple-geometry-observations"


def test_two_distinct_masks_cannot_claim_one_geometry_observation() -> None:
    narrow = np.zeros((600, 800), dtype=np.bool_)
    narrow[9:13, 9:13] = True
    wide = np.zeros((600, 800), dtype=np.bool_)
    wide[1:25, 1:25] = True
    instances = cluster_mask_instances(
        (
            _detection("narrow", "bollard", 0.8, narrow),
            _detection("wide", "post", 0.8, wide),
        )
    )

    bindings = bind_mask_instances_to_geometry(
        instances,
        observations=(_observation(0, (0, 1, 2, 3)),),
        projected=_projected(),
    )

    assert len(instances) == 2
    assert all(item.resolution is MaskBindingResolution.AMBIGUOUS for item in bindings)
    assert all(item.selected_observation_id is None for item in bindings)
    assert all(
        item.reason_code == "observation-claimed-by-multiple-mask-instances"
        for item in bindings
    )