307 lines
11 KiB
Python
307 lines
11 KiB
Python
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=(),
|
|
)
|