feat(perception): add diagnostic semantic SLAM replay

2026-08-06 11:26:37 +03:00
parent b7a51e26e6
commit 8eaa3ab497
15 changed files with 4554 additions and 6 deletions
@@ -0,0 +1,306 @@
+from __future__ import annotations
+
+from dataclasses import replace
+
+import numpy as np
+import pytest
+
+from k1link.perception.contracts import (
+    EvidenceBasis,
+    EvidenceCurrentness,
+    MetricGeometry,
+    ObstacleObservation,
+)
+from k1link.perception.geometry_math import ProjectedPointCloud
+from k1link.perception.semantic_fusion import (
+    NO_SEMANTIC_CLASS_ID,
+    SemanticClassDefinition,
+    SemanticClassDisposition,
+    SemanticEvidenceAuthority,
+    SemanticEvidenceStatus,
+    SemanticFusionError,
+    SemanticMask,
+    fuse_semantic_diagnostics,
+)
+
+
+def _classes() -> tuple[SemanticClassDefinition, ...]:
+    return (
+        SemanticClassDefinition(1, "road"),
+        SemanticClassDefinition(2, "car"),
+        SemanticClassDefinition(
+            255,
+            "void / uncertain",
+            SemanticClassDisposition.AMBIGUOUS,
+        ),
+    )
+
+
+def _mask(*, source_id: str = "RAVNOVES00", frame_id: str = "frame-000014") -> SemanticMask:
+    return SemanticMask(
+        source_id=source_id,
+        frame_id=frame_id,
+        provider_id="semantic-provider/v1",
+        model_id="semantic-model/v1",
+        preprocess_id="raw-kb4-semantic/v1",
+        labels=np.asarray(
+            [
+                [1, 2, 255, 1],
+                [1, 1, 1, 1],
+                [1, 1, 1, 1],
+            ],
+            dtype=np.uint8,
+        ),
+        classes=_classes(),
+    )
+
+
+def _projection() -> ProjectedPointCloud:
+    return ProjectedPointCloud(
+        pixels_xy=np.asarray(
+            [
+                [0.1, 0.1],
+                [1.2, 0.2],
+                [1.8, 0.8],
+                [2.1, 0.2],
+                [9.0, 9.0],
+            ],
+            dtype=np.float64,
+        ),
+        depths_m=np.asarray([2.0, 2.1, 2.2, 2.3, 2.4], dtype=np.float64),
+        source_indices=np.asarray([0, 1, 2, 3, 4], dtype=np.int64),
+        source_point_count=5,
+        camera_front_point_count=5,
+    )
+
+
+def _geometry_observation(
+    *point_ids: int,
+    observation_id: str = "geometry-observation-1",
+) -> ObstacleObservation:
+    return ObstacleObservation(
+        observation_id=observation_id,
+        occupancy_key=f"occupancy-{observation_id}",
+        source_id="RAVNOVES00",
+        frame_id="frame-000014",
+        evidence_time_ns=14_000_000_000,
+        basis=EvidenceBasis.LIDAR,
+        currentness=EvidenceCurrentness.CURRENT,
+        occupied_support=True,
+        source_point_ids=point_ids,
+        metric_geometry=MetricGeometry(
+            coordinate_frame="map",
+            centroid_xyz_m=(2.0, 0.0, 0.5),
+            range_m=2.0,
+            covariance_diagonal_m2=(0.1, 0.1, 0.1),
+        ),
+        proposal_ids=(),
+        semantic_hint=None,
+        reason_codes=("qualified-lidar-points",),
+    )
+
+
+def _camera_only_observation() -> ObstacleObservation:
+    return ObstacleObservation(
+        observation_id="camera-observation-1",
+        occupancy_key="occupancy-camera-observation-1",
+        source_id="RAVNOVES00",
+        frame_id="frame-000014",
+        evidence_time_ns=14_000_000_000,
+        basis=EvidenceBasis.CAMERA,
+        currentness=EvidenceCurrentness.CURRENT,
+        occupied_support=False,
+        source_point_ids=(),
+        metric_geometry=None,
+        proposal_ids=("proposal-1",),
+        semantic_hint=None,
+        reason_codes=("camera-only",),
+    )
+
+
+def test_semantic_mask_is_strict_source_bound_uint8_and_immutable() -> None:
+    labels = np.asarray([[1, 2]], dtype=np.uint8)
+    semantic = SemanticMask(
+        source_id="RAVNOVES00",
+        frame_id="frame-000014",
+        provider_id="semantic-provider/v1",
+        model_id="semantic-model/v1",
+        preprocess_id="raw-kb4-semantic/v1",
+        labels=labels,
+        classes=_classes(),
+    )
+    labels[0, 0] = 2
+    assert semantic.labels.tolist() == [[1, 2]]
+    assert semantic.labels.flags.writeable is False
+    with pytest.raises(ValueError):
+        semantic.labels[0, 0] = 2
+
+    with pytest.raises(SemanticFusionError, match="uint8 HxW"):
+        replace(semantic, labels=np.asarray([[1, 2]], dtype=np.int64))
+    with pytest.raises(SemanticFusionError, match="undeclared"):
+        replace(semantic, labels=np.asarray([[1, 7]], dtype=np.uint8))
+    with pytest.raises(SemanticFusionError, match="unique"):
+        replace(
+            semantic,
+            classes=(SemanticClassDefinition(1, "road"), SemanticClassDefinition(1, "other")),
+        )
+
+
+def test_mask_projection_keeps_absence_ambiguity_and_unprojected_separate() -> None:
+    result = fuse_semantic_diagnostics(
+        semantic_mask=_mask(),
+        projected=_projection(),
+        observations=(),
+    )
+    labels = result.point_labels
+    assert [labels.status_for(index) for index in range(5)] == [
+        SemanticEvidenceStatus.LABELED,
+        SemanticEvidenceStatus.LABELED,
+        SemanticEvidenceStatus.LABELED,
+        SemanticEvidenceStatus.AMBIGUOUS,
+        SemanticEvidenceStatus.UNPROJECTED,
+    ]
+    assert [labels.class_id_for(index) for index in range(5)] == [1, 2, 2, 255, None]
+    assert [labels.label_for(index) for index in range(5)] == [
+        "road",
+        "car",
+        "car",
+        "void / uncertain",
+        None,
+    ]
+    assert labels.class_ids.tolist() == [1, 2, 2, 255, NO_SEMANTIC_CLASS_ID]
+    assert labels.class_ids.flags.writeable is False
+    assert labels.status_codes.flags.writeable is False
+    assert result.authority is SemanticEvidenceAuthority.DIAGNOSTIC_ONLY
+
+
+def test_observation_aggregation_is_detached_from_geometry_and_safety_authority() -> None:
+    observation = _geometry_observation(0, 1, 2, 4)
+    before = observation.to_dict()
+    result = fuse_semantic_diagnostics(
+        semantic_mask=_mask(),
+        projected=_projection(),
+        observations=(observation,),
+    )
+    evidence = result.observation_evidence[0]
+    assert observation.to_dict() == before
+    assert evidence.observation_id == observation.observation_id
+    assert evidence.occupancy_key == observation.occupancy_identity
+    assert evidence.status is SemanticEvidenceStatus.LABELED
+    assert evidence.dominant_class_id == 2
+    assert evidence.dominant_label == "car"
+    assert evidence.dominant_fraction_of_labeled == pytest.approx(2 / 3)
+    assert evidence.labeled_point_count == 3
+    assert evidence.unprojected_point_count == 1
+    assert evidence.semantic_coverage_fraction == pytest.approx(0.75)
+    assert evidence.authority is SemanticEvidenceAuthority.DIAGNOSTIC_ONLY
+    assert not hasattr(evidence, "occupied_support")
+    assert not hasattr(evidence, "motion")
+    assert not hasattr(evidence, "threat")
+    assert not hasattr(evidence, "actuation_allowed")
+
+
+def test_tied_or_provider_ambiguous_labels_remain_ambiguous() -> None:
+    tied = _geometry_observation(0, 1, observation_id="geometry-tied")
+    provider_ambiguous = _geometry_observation(3, observation_id="geometry-void")
+    result = fuse_semantic_diagnostics(
+        semantic_mask=_mask(),
+        projected=_projection(),
+        observations=(tied, provider_ambiguous),
+    )
+    tie_evidence, void_evidence = result.observation_evidence
+    assert tie_evidence.status is SemanticEvidenceStatus.AMBIGUOUS
+    assert tie_evidence.reason_code == "semantic-label-majority-ambiguous"
+    assert tie_evidence.dominant_class_id is None
+    assert {item.label: item.point_count for item in tie_evidence.class_evidence} == {
+        "road": 1,
+        "car": 1,
+    }
+    assert void_evidence.status is SemanticEvidenceStatus.AMBIGUOUS
+    assert void_evidence.reason_code == "semantic-classes-ambiguous"
+    assert void_evidence.ambiguous_point_count == 1
+    assert void_evidence.class_evidence[0].disposition is SemanticClassDisposition.AMBIGUOUS
+
+    mixed = fuse_semantic_diagnostics(
+        semantic_mask=_mask(),
+        projected=_projection(),
+        observations=(_geometry_observation(1, 3, observation_id="geometry-mixed"),),
+    ).observation_evidence[0]
+    assert mixed.status is SemanticEvidenceStatus.AMBIGUOUS
+    assert mixed.reason_code == "semantic-label-majority-ambiguous"
+    assert mixed.dominant_class_id is None
+
+
+def test_missing_mask_and_pointless_geometry_have_distinct_outcomes() -> None:
+    observation = _geometry_observation(0, 1)
+    absent = fuse_semantic_diagnostics(
+        semantic_mask=None,
+        projected=_projection(),
+        observations=(observation,),
+    )
+    assert absent.mask_available is False
+    assert [absent.point_labels.status_for(index) for index in range(5)] == [
+        SemanticEvidenceStatus.ABSENT
+    ] * 5
+    assert absent.observation_evidence[0].status is SemanticEvidenceStatus.ABSENT
+    assert absent.observation_evidence[0].absent_point_count == 2
+
+    unprojected = fuse_semantic_diagnostics(
+        semantic_mask=_mask(),
+        projected=_projection(),
+        observations=(_camera_only_observation(),),
+    )
+    evidence = unprojected.observation_evidence[0]
+    assert evidence.status is SemanticEvidenceStatus.UNPROJECTED
+    assert evidence.reason_code == "observation-has-no-source-points"
+    assert evidence.source_point_count == 0
+
+
+def test_fusion_rejects_frame_escape_invalid_point_ids_and_duplicate_ownership() -> None:
+    observation = _geometry_observation(0)
+    with pytest.raises(SemanticFusionError, match="source frame"):
+        fuse_semantic_diagnostics(
+            semantic_mask=_mask(frame_id="frame-000015"),
+            projected=_projection(),
+            observations=(observation,),
+        )
+    with pytest.raises(SemanticFusionError, match="outside the source frame"):
+        fuse_semantic_diagnostics(
+            semantic_mask=_mask(),
+            projected=_projection(),
+            observations=(_geometry_observation(5),),
+        )
+    with pytest.raises(SemanticFusionError, match="duplicate observation ownership"):
+        fuse_semantic_diagnostics(
+            semantic_mask=_mask(),
+            projected=_projection(),
+            observations=(
+                observation,
+                _geometry_observation(0, observation_id="geometry-observation-2"),
+            ),
+        )
+    with pytest.raises(SemanticFusionError, match="escaped their source frame"):
+        fuse_semantic_diagnostics(
+            semantic_mask=None,
+            projected=_projection(),
+            observations=(
+                observation,
+                replace(
+                    _geometry_observation(1, observation_id="geometry-observation-2"),
+                    frame_id="frame-000015",
+                ),
+            ),
+        )
+
+
+def test_projection_validator_rejects_malformed_existing_contract_values() -> None:
+    malformed = replace(
+        _projection(),
+        source_indices=np.asarray([0, 1, 2, 3, 5], dtype=np.int64),
+    )
+    with pytest.raises(SemanticFusionError, match="outside the source frame"):
+        fuse_semantic_diagnostics(
+            semantic_mask=_mask(),
+            projected=malformed,
+            observations=(),
+        )