fix(lab): show only actionable obstacle boxes

2026-08-26 16:09:32 +03:00
parent a98ad929d9
commit ac1dc7d930
6 changed files with 306 additions and 31 deletions
@@ -27,6 +27,8 @@ from .spatial_evidence import (
 from .threat import (
    DEFAULT_REPLAY_THREAT_PROFILE_PATH,
    RecordedReplayBodyFrameResolver,
+    ReplayBodyFrame,
+    ReplayThreatProfile,
    load_replay_threat_profile,
 )
 from .threat_timeline import (
@@ -49,8 +51,12 @@ EXPECTED_FRAME_COUNT: Final = 4_489
 CAMERA_ACCUMULATION_WINDOW_SECONDS: Final = 2.0
 CAMERA_ACCUMULATION_POINT_LIMIT: Final = 20_000
 CAMERA_POINT_OVERLAY_SCHEMA: Final = "missioncore.m48s-camera-point-overlay/v1"
+ACTIONABLE_CAMERA_OBSTACLE_PROJECTION: Final = (
+    "factory-kb4-actionable-added-corridor-bounds"
+)
 _SOURCE_ENVELOPE_MARKER: Final = b'"source_envelope":'
 _JSON_DECODER: Final = json.JSONDecoder()
+Cell = tuple[int, int, int]


 class M48sReplayTimelineError(RuntimeError):
@@ -62,6 +68,12 @@ class _LedgerIndex:
    offsets_by_sequence: dict[int, int]


+@dataclass(frozen=True, slots=True)
+class _FrameDiff:
+    component_provenance: dict[str, str]
+    added_cells: frozenset[Cell]
+
+
 class M48sReplayTimeline:
    """Read source-indexed chunks while preserving latest-wins world-state gaps."""

@@ -188,7 +200,7 @@ class M48sReplayTimeline:
                else None
            ),
            "camera_obstacle_projection_delivery": (
-                "factory-kb4-occupied-voxel-bounds"
+                ACTIONABLE_CAMERA_OBSTACLE_PROJECTION
                if self.frame_diff_path is not None
                else None
            ),
@@ -435,19 +447,31 @@ class M48sReplayTimeline:
                    body_frame,
                    occupied_voxel_size_m=self.profile.corridor.occupied_voxel_size_m,
                )
-                provenance = self._component_provenance(sequence)
+                frame_diff = self._frame_diff(sequence)
+                provenance = (
+                    {} if frame_diff is None else frame_diff.component_provenance
+                )
+                actionable_metric_rows = (
+                    []
+                    if frame_diff is None
+                    else _actionable_camera_metric_rows(
+                        metric_rows,
+                        added_cells=frame_diff.added_cells,
+                        body_frame=body_frame,
+                        profile=self.profile,
+                    )
+                )
                camera_projections = (
                    {}
-                    if frame is None or not provenance
+                    if frame is None or not actionable_metric_rows
                    else project_metric_obstacles_to_camera(
-                        metric_rows,
+                        actionable_metric_rows,
                        position_map_xyz=frame.sensor_position_map,
                        orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw,
                        profile=frame.projection,
                        occupied_voxel_size_m=(
                            self.profile.corridor.occupied_voxel_size_m
                        ),
-                        component_ids=set(provenance),
                    )
                )
                for visual in metric_visuals:
@@ -537,9 +561,9 @@ class M48sReplayTimeline:
            raise M48sReplayTimelineError("M4.8S frame row is invalid")
        return value

-    def _component_provenance(self, sequence: int) -> dict[str, str]:
+    def _frame_diff(self, sequence: int) -> _FrameDiff | None:
        if self.frame_diff_path is None:
-            return {}
+            return None
        offset = self.frame_diff_offsets.get(sequence)
        if offset is None:
            raise M48sReplayTimelineError("M4.8R3 frame diff is incomplete")
@@ -556,7 +580,88 @@ class M48sReplayTimeline:
            for key, item in provenance.items()
        ):
            raise M48sReplayTimelineError("M4.8R3 component provenance changed")
-        return provenance
+        raw_added_cells = value.get("added_cells")
+        if not isinstance(raw_added_cells, list):
+            raise M48sReplayTimelineError("M4.8R3 added cells changed")
+        added_cells: set[Cell] = set()
+        for raw_cell in raw_added_cells:
+            if (
+                not isinstance(raw_cell, list)
+                or len(raw_cell) != 3
+                or any(not isinstance(item, int) or isinstance(item, bool) for item in raw_cell)
+            ):
+                raise M48sReplayTimelineError("M4.8R3 added cell is invalid")
+            added_cells.add((raw_cell[0], raw_cell[1], raw_cell[2]))
+        return _FrameDiff(
+            component_provenance=provenance,
+            added_cells=frozenset(added_cells),
+        )
+
+
+def _actionable_camera_metric_rows(
+    metric_rows: list[dict[str, object]],
+    *,
+    added_cells: frozenset[Cell],
+    body_frame: ReplayBodyFrame,
+    profile: ReplayThreatProfile,
+) -> list[dict[str, object]]:
+    """Keep only newly added voxel cells that cause a current corridor threat.
+
+    A rolling component can join spatially distant baseline and LOW-STEP cells.
+    Projecting its complete envelope produces an honest component bound but not
+    an operator-usable obstacle box.  The camera layer therefore visualizes the
+    exact added cells that participate in the already accepted corridor
+    intersection; threat authority and the complete 3D component stay intact.
+    """
+
+    voxel_size_m = profile.corridor.occupied_voxel_size_m
+    expansion_m = voxel_size_m * math.sqrt(2) / 2
+    minimum_x = -(
+        profile.rig.body_length_m / 2
+        + profile.corridor.rear_margin_m
+        + expansion_m
+    )
+    maximum_x = (
+        profile.rig.body_length_m / 2
+        + profile.corridor.forward_length_m
+        + expansion_m
+    )
+    half_width = (
+        profile.rig.body_width_m / 2
+        + profile.corridor.lateral_clearance_m
+        + expansion_m
+    )
+    actionable: list[dict[str, object]] = []
+    for row in metric_rows:
+        assessment = _object(row.get("assessment"), "metric assessment")
+        if (
+            assessment.get("decision") != "threat"
+            or assessment.get("corridor_intersection") != "intersects"
+        ):
+            continue
+        raw_cells = row.get("cells")
+        if not isinstance(raw_cells, list):
+            raise M48sReplayTimelineError("M4.8R3 metric cells changed")
+        selected_cells: list[dict[str, object]] = []
+        for raw_cell in raw_cells:
+            cell = _object(raw_cell, "metric cell")
+            indices = (
+                _signed_integer(cell.get("x"), "metric cell x"),
+                _signed_integer(cell.get("y"), "metric cell y"),
+                _signed_integer(cell.get("z"), "metric cell z"),
+            )
+            if indices not in added_cells:
+                continue
+            center_map = tuple((index + 0.5) * voxel_size_m for index in indices)
+            center_body = body_frame.map_point_to_body(center_map)
+            if (
+                minimum_x <= center_body[0] <= maximum_x
+                and -half_width <= center_body[1] <= half_width
+            ):
+                selected_cells.append(cell)
+        if selected_cells:
+            actionable.append({**row, "cells": selected_cells})
+    return actionable


 def _index_ledger(
@@ -708,7 +813,14 @@ def _text(value: object, label: str) -> str:
    return value


+def _signed_integer(value: object, label: str) -> int:
+    if not isinstance(value, int) or isinstance(value, bool):
+        raise M48sReplayTimelineError(f"M4.8S {label} is invalid")
+    return value
+
+
 __all__ = [
+    "ACTIONABLE_CAMERA_OBSTACLE_PROJECTION",
    "CAMERA_ACCUMULATION_POINT_LIMIT",
    "CAMERA_ACCUMULATION_WINDOW_SECONDS",
    "CAMERA_POINT_OVERLAY_SCHEMA",