feat(perception): add occupied-only low-step shadow

2026-08-26 12:39:54 +03:00
parent 5983883ffe
commit 343ddeac2d
8 changed files with 1124 additions and 7 deletions
@@ -0,0 +1,203 @@
+from __future__ import annotations
+
+import json
+from pathlib import Path
+
+import numpy as np
+
+from k1link.perception.contracts import (
+    ClockBasis,
+    ModalityOutcome,
+    ModalityStatus,
+    ObstacleObservation,
+    SourceEnvelope,
+    TimestampBundle,
+)
+from k1link.perception.geometry import GeometryFrame, RecordedGeometryStore
+from k1link.perception.geometry_math import Kb4ProjectionProfile, project_map_points_kb4
+from k1link.perception.m48_low_step_occupancy import (
+    M48AdditiveLowStepGeometryProvider,
+    load_m48_low_step_occupancy_profile,
+)
+from k1link.perception.providers import SourcePacket
+from k1link.perception.recorded_source import RecordedFrameReference
+
+REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
+PROFILE_PATH = (
+    REPOSITORY_ROOT
+    / "config/perception/m48r3-additive-low-step-occupancy-v1.json"
+)
+R2_CASES_PATH = (
+    REPOSITORY_ROOT
+    / ".runtime/compute-experiments/m48/static-occupancy-qualification-results"
+    / (
+        "m48-static-occupancy-qualification-"
+        "568024554cff011332ff19ca4739f70555a6232c0607dec2a71be6db408ea69a"
+    )
+    / "cases.jsonl"
+)
+
+
+class _Store:
+    def __init__(self, frame: GeometryFrame, step: np.ndarray) -> None:
+        self.profile = RecordedGeometryStore.from_repository(REPOSITORY_ROOT).profile
+        self._frame = frame
+        self._step = np.asarray(step, dtype=np.uint8)
+
+    def frame(self, _packet: SourcePacket) -> GeometryFrame:
+        return self._frame
+
+    def point_step_candidates_for_frame(self, _frame_index: int) -> np.ndarray:
+        return self._step
+
+
+def _status() -> ModalityStatus:
+    return ModalityStatus(True, ModalityOutcome.AVAILABLE, "test-available")
+
+
+def _packet(sequence: int = 0) -> SourcePacket:
+    profile = RecordedGeometryStore.from_repository(REPOSITORY_ROOT).profile
+    reference = RecordedFrameReference(profile.source_pack_id, sequence)
+    return SourcePacket(
+        envelope=SourceEnvelope(
+            source_id=profile.source_id,
+            session_id=profile.session_id,
+            frame_id=f"frame-{sequence:06d}",
+            sequence=sequence,
+            timestamps=TimestampBundle(
+                utc_ns=sequence + 1,
+                monotonic_ns=sequence + 2,
+                source_ns=sequence + 3,
+                clock_basis=ClockBasis.RECORDED_HOST,
+            ),
+            source_age_ns=0,
+            binding_reason="test-recorded-source",
+            calibration_id="camera-1-kb4-test",
+            representation_id="registered-map-increment-v1",
+            image=_status(),
+            registered_point_increment=_status(),
+            pose=_status(),
+        ),
+        image_payload="image",
+        registered_point_increment_payload=reference,
+        pose_payload=reference,
+    )
+
+
+def _frame(points: np.ndarray) -> GeometryFrame:
+    return GeometryFrame(
+        frame_index=0,
+        points_map=np.asarray(points, dtype=np.float64),
+        point_class=np.ones(points.shape[0], dtype=np.uint8),
+        sensor_position_map=np.zeros(3, dtype=np.float64),
+        sensor_orientation_xyzw=np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float64),
+        projection=Kb4ProjectionProfile(
+            width=100,
+            height=100,
+            intrinsic_fx_fy_cx_cy=(100.0, 100.0, 50.0, 50.0),
+            distortion_kb4=(0.0, 0.0, 0.0, 0.0),
+            t_camera_from_lidar=np.eye(4, dtype=np.float64),
+        ),
+        surface_valid=True,
+    )
+
+
+def test_wide_operator_region_cannot_bridge_two_spatial_components() -> None:
+    points = np.asarray(
+        (
+            (0.00, 0.0, 5.00),
+            (0.10, 0.0, 5.05),
+            (1.70, 0.0, 5.00),
+            (1.80, 0.0, 5.05),
+        ),
+        dtype=np.float64,
+    )
+    store = _Store(_frame(points), np.ones(4, dtype=np.uint8))
+    provider = M48AdditiveLowStepGeometryProvider(  # type: ignore[arg-type]
+        store=store,
+        profile=load_m48_low_step_occupancy_profile(PROFILE_PATH),
+    )
+
+    observations = provider.associate(_packet(), ())
+    additive = tuple(
+        item
+        for item in observations
+        if "additive-low-step-current-component" in item.reason_codes
+    )
+
+    assert len(additive) == 2
+    assert {item.source_point_ids for item in additive} == {(0, 1), (2, 3)}
+    assert all(item.semantic_hint is None for item in additive)
+    assert all("occupied-only-never-free" in item.reason_codes for item in additive)
+    snapshot = provider.snapshot()
+    assert snapshot.additive_observation_count == 2
+    assert snapshot.additive_voxel_count == 3
+    assert snapshot.failed_frames == 0
+
+
+def test_frame_1856_preserves_baseline_posts_and_splits_low_hemisphere_support() -> None:
+    store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT)
+    provider = M48AdditiveLowStepGeometryProvider(
+        store=store,
+        profile=load_m48_low_step_occupancy_profile(PROFILE_PATH),
+    )
+    frame = store.frame_for_index(1856)
+    assert frame is not None
+    observations = provider.associate(_packet(1856), ())
+    projected = project_map_points_kb4(
+        frame.points_map,
+        position_map_xyz=frame.sensor_position_map,
+        orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw,
+        profile=frame.projection,
+    )
+    source_rows = {
+        int(source_index): row
+        for row, source_index in enumerate(projected.source_indices)
+    }
+    cases = [
+        json.loads(line)
+        for line in R2_CASES_PATH.read_text("utf-8").splitlines()
+        if line.strip() and json.loads(line)["sequence"] == 1856
+    ]
+    by_anchor = {row["anchor_id"]: row for row in cases}
+    posts = by_anchor["anchor-5e6e2a81e0667bdd9faf2a9e"]
+    hemispheres = by_anchor["anchor-924a4623077fe5df18816b47"]
+
+    assert posts["accepted_graph"]["component_count"] >= 2
+    assert _component_hits(
+        observations,
+        hemispheres["extent_xyxy"],
+        projected.pixels_xy,
+        source_rows,
+        width=frame.projection.width,
+        height=frame.projection.height,
+    ) >= 2
+
+
+def _component_hits(
+    observations: tuple[ObstacleObservation, ...],
+    extent: list[float],
+    pixels: np.ndarray,
+    source_rows: dict[int, int],
+    *,
+    width: int,
+    height: int,
+) -> int:
+    x1, y1, x2, y2 = (
+        extent[0] * width,
+        extent[1] * height,
+        extent[2] * width,
+        extent[3] * height,
+    )
+    count = 0
+    for value in observations:
+        indices = value.source_point_ids
+        if not indices:
+            continue
+        rows = [source_rows[index] for index in indices if index in source_rows]
+        if any(
+            x1 <= pixels[row, 0] <= x2 and y1 <= pixels[row, 1] <= y2
+            for row in rows
+        ):
+            count += 1
+    return count
@@ -24,6 +24,10 @@ NATIVE_GRAPH_CONFIG = (
    REPOSITORY_ROOT
    / "config/perception/m48n-rf-detr-native-reference-graph-shadow-v0.json"
 )
+LOW_STEP_GRAPH_CONFIG = (
+    REPOSITORY_ROOT
+    / "config/perception/m48r3-native-low-step-reference-graph-shadow-v1.json"
+)


 def test_m48s_reference_graph_replaces_only_the_detector_pin() -> None:
@@ -116,6 +120,42 @@ def test_m48n_native_reference_graph_pins_every_profile_digest() -> None:
        assert pins[role].sha256 == hashlib.sha256(payload).hexdigest()


+def test_m48r3_graph_replaces_only_the_geometry_pin() -> None:
+    candidate = ReferencePerceptionGraphConfigV2.from_dict(
+        json.loads(LOW_STEP_GRAPH_CONFIG.read_text("utf-8"))
+    )
+    native = ReferencePerceptionGraphConfigV2.from_dict(
+        json.loads(NATIVE_GRAPH_CONFIG.read_text("utf-8"))
+    )
+    candidate_pins = {item.role: item for item in candidate.providers}
+    native_pins = {item.role: item for item in native.providers}
+
+    assert candidate.graph_id == native.graph_id == "reference-perception-graph/v2"
+    assert candidate.queues == native.queues
+    assert candidate.authority == native.authority
+    assert candidate_pins[ProviderRole.GEOMETRY].provider_id == (
+        "ravnoves00-additive-low-step-geometry/v1"
+    )
+    assert all(
+        candidate_pins[role] == native_pins[role]
+        for role in ProviderRole
+        if role is not ProviderRole.GEOMETRY
+    )
+
+
+def test_m48r3_graph_pins_the_additive_profile_digest() -> None:
+    config = ReferencePerceptionGraphConfigV2.from_dict(
+        json.loads(LOW_STEP_GRAPH_CONFIG.read_text("utf-8"))
+    )
+    pin = {item.role: item for item in config.providers}[ProviderRole.GEOMETRY]
+    payload = (
+        REPOSITORY_ROOT
+        / "config/perception/m48r3-additive-low-step-occupancy-v1.json"
+    ).read_bytes()
+
+    assert pin.sha256 == hashlib.sha256(payload).hexdigest()
+
+
 def test_m48s_advisory_policy_is_bounded_distinct_and_commandless() -> None:
    matrix = advisory_policy_matrix()