From ac1dc7d9307ca3435a36ee541c7c2baf11bd14e1 Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Wed, 26 Aug 2026 16:09:32 +0300 Subject: [PATCH] fix(lab): show only actionable obstacle boxes --- .../src/core/laboratory/m4ReplayThreat.ts | 4 +- .../laboratory/M4ReplayThreatVisual.tsx | 2 +- .../laboratory/m4StaticObstacleBoxes.ts | 73 +++++++--- .../test/m4ReplayThreat.test.mjs | 77 ++++++++++- src/k1link/perception/m48s_replay_timeline.py | 128 ++++++++++++++++-- tests/test_m48s_replay_timeline.py | 53 ++++++++ 6 files changed, 306 insertions(+), 31 deletions(-) create mode 100644 tests/test_m48s_replay_timeline.py diff --git a/apps/control-station/src/core/laboratory/m4ReplayThreat.ts b/apps/control-station/src/core/laboratory/m4ReplayThreat.ts index 0305673..5075f27 100644 --- a/apps/control-station/src/core/laboratory/m4ReplayThreat.ts +++ b/apps/control-station/src/core/laboratory/m4ReplayThreat.ts @@ -186,7 +186,7 @@ export interface M4ThreatTimeline { cameraPointSampleLimit: number; worldStateDelivery: "source-paced-latest-wins" | null; occupancyProvenanceDelivery: "baseline-versus-additive-component-diff" | null; - cameraObstacleProjectionDelivery: "factory-kb4-occupied-voxel-bounds" | null; + cameraObstacleProjectionDelivery: "factory-kb4-actionable-added-corridor-bounds" | null; worldStateFrameCount: number; supersededFrameCount: number; sourceRepresentationId: "registered-map-increment-v1"; @@ -747,7 +747,7 @@ export async function fetchM4ThreatTimeline( ? null : exact( payload.camera_obstacle_projection_delivery, - "factory-kb4-occupied-voxel-bounds", + "factory-kb4-actionable-added-corridor-bounds", "M4.8R3 camera obstacle projection delivery", ), worldStateFrameCount: payload.world_state_frame_count === undefined diff --git a/apps/control-station/src/workspaces/laboratory/M4ReplayThreatVisual.tsx b/apps/control-station/src/workspaces/laboratory/M4ReplayThreatVisual.tsx index a7130cf..7ed68d0 100644 --- a/apps/control-station/src/workspaces/laboratory/M4ReplayThreatVisual.tsx +++ b/apps/control-station/src/workspaces/laboratory/M4ReplayThreatVisual.tsx @@ -513,7 +513,7 @@ export function M4ReplayThreatVisual({ shape="pill" variant={showStaticObstacles ? "primary" : "secondary"} aria-pressed={showStaticObstacles} - title="Автоматические рамки занятых LiDAR-компонентов · без ручной разметки" + title="Только LOW-STEP LiDAR-препятствия с решением threat · без ручной разметки" onClick={() => setShowStaticObstacles((visible) => !visible)} > OBSTACLES diff --git a/apps/control-station/src/workspaces/laboratory/m4StaticObstacleBoxes.ts b/apps/control-station/src/workspaces/laboratory/m4StaticObstacleBoxes.ts index c47815b..2141d88 100644 --- a/apps/control-station/src/workspaces/laboratory/m4StaticObstacleBoxes.ts +++ b/apps/control-station/src/workspaces/laboratory/m4StaticObstacleBoxes.ts @@ -2,6 +2,12 @@ import type { RecordedEvidenceBox } from "../../components/laboratory/RecordedEv import type { M4ThreatMetricVisual } from "../../core/laboratory/m4ReplayThreat"; const MINIMUM_BOX_SIZE_PX = 12; +const RETAINED_CURRENT_OVERLAP_LIMIT = 0.5; + +interface ObstacleBoxCandidate { + readonly box: RecordedEvidenceBox; + readonly state: M4ThreatMetricVisual["state"]; +} function shortComponentId(componentId: string): string { const finalSegment = componentId.split(/[-_]/).pop(); @@ -37,26 +43,42 @@ function visibleBox( return [left, top, right, bottom]; } -function tone(obstacle: M4ThreatMetricVisual): RecordedEvidenceBox["tone"] { - if (obstacle.assessment.decision === "threat") return "danger"; - if (obstacle.assessment.decision === "not-threat") return "success"; - return "warning"; +function overlapFractionOfSmaller( + left: readonly [number, number, number, number], + right: readonly [number, number, number, number], +): number { + const intersectionWidth = Math.max( + 0, + Math.min(left[2], right[2]) - Math.max(left[0], right[0]), + ); + const intersectionHeight = Math.max( + 0, + Math.min(left[3], right[3]) - Math.max(left[1], right[1]), + ); + const intersectionArea = intersectionWidth * intersectionHeight; + const leftArea = (left[2] - left[0]) * (left[3] - left[1]); + const rightArea = (right[2] - right[0]) * (right[3] - right[1]); + const smallerArea = Math.min(leftArea, rightArea); + return smallerArea > 0 ? intersectionArea / smallerArea : 0; } /** - * Build camera evidence from the same world-state components shown in 3D. - * No image detector or manual review extent participates in these boxes. + * Build actionable camera evidence from the same world-state shown in 3D. + * Unknown and clear components remain in world-state, but do not compete with + * actual corridor threats for the operator's attention. */ export function buildM4StaticObstacleBoxes( obstacles: readonly M4ThreatMetricVisual[], imageWidth: number, imageHeight: number, ): readonly RecordedEvidenceBox[] { - const result: RecordedEvidenceBox[] = []; + const candidates: ObstacleBoxCandidate[] = []; for (const obstacle of obstacles) { if ( obstacle.occupancySource === "baseline" || (obstacle.state !== "current" && obstacle.state !== "retained") + || obstacle.assessment.decision !== "threat" + || obstacle.assessment.corridorIntersection !== "intersects" || obstacle.cameraProjection === null ) continue; const projection = obstacle.cameraProjection; @@ -65,16 +87,33 @@ export function buildM4StaticObstacleBoxes( const depth = projection.nearestDepthM.toLocaleString("ru-RU", { maximumFractionDigits: 1, }); - result.push({ - boxXyxy, - label: `OBS #${shortComponentId(obstacle.componentId)} · ${depth} м`, - tone: tone(obstacle), - dashed: false, + candidates.push({ + state: obstacle.state, + box: { + boxXyxy, + label: `OBS #${shortComponentId(obstacle.componentId)} · ${depth} м`, + tone: "danger", + dashed: false, + }, }); } - return result.sort((left, right) => { - const leftArea = (left.boxXyxy[2] - left.boxXyxy[0]) * (left.boxXyxy[3] - left.boxXyxy[1]); - const rightArea = (right.boxXyxy[2] - right.boxXyxy[0]) * (right.boxXyxy[3] - right.boxXyxy[1]); - return rightArea - leftArea; - }); + const currentBoxes = candidates + .filter((candidate) => candidate.state === "current") + .map((candidate) => candidate.box.boxXyxy); + return candidates + .filter((candidate) => ( + candidate.state !== "retained" + || !currentBoxes.some((currentBox) => ( + overlapFractionOfSmaller(candidate.box.boxXyxy, currentBox) + >= RETAINED_CURRENT_OVERLAP_LIMIT + )) + )) + .map((candidate) => candidate.box) + .sort((left, right) => { + const leftArea = (left.boxXyxy[2] - left.boxXyxy[0]) + * (left.boxXyxy[3] - left.boxXyxy[1]); + const rightArea = (right.boxXyxy[2] - right.boxXyxy[0]) + * (right.boxXyxy[3] - right.boxXyxy[1]); + return rightArea - leftArea; + }); } diff --git a/apps/control-station/test/m4ReplayThreat.test.mjs b/apps/control-station/test/m4ReplayThreat.test.mjs index 8818a45..5b1ec47 100644 --- a/apps/control-station/test/m4ReplayThreat.test.mjs +++ b/apps/control-station/test/m4ReplayThreat.test.mjs @@ -370,7 +370,7 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint" camera_point_sample_limit: 20000, world_state_delivery: "source-paced-latest-wins", occupancy_provenance_delivery: "baseline-versus-additive-component-diff", - camera_obstacle_projection_delivery: "factory-kb4-occupied-voxel-bounds", + camera_obstacle_projection_delivery: "factory-kb4-actionable-added-corridor-bounds", world_state_frame_count: 4481, superseded_frame_count: 8, local_surface_visualization: { @@ -400,7 +400,7 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint" assert.equal(timeline.supersededFrameCount, 8); assert.equal( timeline.cameraObstacleProjectionDelivery, - "factory-kb4-occupied-voxel-bounds", + "factory-kb4-actionable-added-corridor-bounds", ); const chunk = await fetchM4ThreatTimelineChunk(replayResultId, 1, 1, { @@ -455,7 +455,7 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint" assert.match(requested, new RegExp(`^${endpointRoot}/${replayResultId}/timeline/chunk`)); assert.match( requested, - /obstacle_projection=factory-kb4-occupied-voxel-bounds/, + /obstacle_projection=factory-kb4-actionable-added-corridor-bounds/, ); assert.equal(chunk.frames[0].worldStateAvailable, false); assert.equal(chunk.frames[0].terminalOutcome, "superseded"); @@ -525,6 +525,24 @@ test("M4.8R3 turns active low-step components into native fisheye obstacle boxes obstacle, { ...obstacle, componentId: "baseline", occupancySource: "baseline" }, { ...obstacle, componentId: "held", state: "held" }, + { + ...obstacle, + componentId: "unknown", + assessment: { + ...obstacle.assessment, + decision: "unknown", + corridorIntersection: "unknown", + }, + }, + { + ...obstacle, + componentId: "clear", + assessment: { + ...obstacle.assessment, + decision: "not-threat", + corridorIntersection: "clear", + }, + }, ], 800, 600); assert.equal(boxes.length, 1); assert.deepEqual(boxes[0].boxXyxy, [394, 294, 406, 306]); @@ -533,6 +551,59 @@ test("M4.8R3 turns active low-step components into native fisheye obstacle boxes assert.equal(boxes[0].dashed, false); }); +test("M4.8R3 keeps separate CURRENT threats and suppresses their spanning ROLLING box", () => { + const current = { + componentId: "temporal-left-1", + state: "current", + motion: "stationary", + centroidBodyXyzM: [3, 0, 0.3], + cellCentersBodyXyzM: [[3, 0, 0.3]], + occupancySource: "additive-low-step", + cameraProjection: { + bboxXyxy: [350, 260, 410, 310], + nearestDepthM: 3, + projectedCellCount: 2, + projection: "factory-kb4-occupied-voxel-bounds", + authority: "visual-derived", + }, + assessment: { + componentId: "temporal-left-1", + decision: "threat", + corridorIntersection: "intersects", + relativeSpeedMps: null, + closestApproachM: 2.5, + ttcSeconds: null, + reasonCodes: ["current-corridor-intersection"], + }, + }; + const boxes = buildM4StaticObstacleBoxes([ + current, + { + ...current, + componentId: "temporal-right-2", + cameraProjection: { + ...current.cameraProjection, + bboxXyxy: [405, 265, 445, 320], + }, + }, + { + ...current, + componentId: "rolling-spanning", + state: "retained", + cameraProjection: { + ...current.cameraProjection, + bboxXyxy: [340, 255, 450, 325], + }, + }, + ], 800, 600); + + assert.equal(boxes.length, 2); + assert.deepEqual(boxes.map((box) => box.label).sort(), [ + "OBS #1 · 3 м", + "OBS #2 · 3 м", + ]); +}); + test("M4.6 local SLAM surface reprojects registered increments into the active body frame", () => { const frames = [ timelineFrame(0, 10, { diff --git a/src/k1link/perception/m48s_replay_timeline.py b/src/k1link/perception/m48s_replay_timeline.py index bd73150..ad65dd0 100644 --- a/src/k1link/perception/m48s_replay_timeline.py +++ b/src/k1link/perception/m48s_replay_timeline.py @@ -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", diff --git a/tests/test_m48s_replay_timeline.py b/tests/test_m48s_replay_timeline.py new file mode 100644 index 0000000..0638a10 --- /dev/null +++ b/tests/test_m48s_replay_timeline.py @@ -0,0 +1,53 @@ +from __future__ import annotations + +from pathlib import Path + +from k1link.perception.m48s_replay_timeline import _actionable_camera_metric_rows +from k1link.perception.threat import ReplayBodyFrame, load_replay_threat_profile + +REPOSITORY_ROOT = Path(__file__).resolve().parents[1] + + +def test_camera_obstacles_keep_only_added_cells_inside_an_accepted_threat() -> None: + profile = load_replay_threat_profile( + REPOSITORY_ROOT / "config/perception/m4-replay-threat-v3.json" + ) + body_frame = ReplayBodyFrame( + frame_id="frame-000001", + origin_map_xyz_m=(0.0, 0.0, 0.0), + basis_map_from_body=((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)), + sensor_height_m=1.25, + surface_slope_deg=0.0, + forward_source="test", + camera_forward_alignment_deg=0.0, + ) + threat = { + "component_id": "rolling-actionable", + "cells": [ + {"x": 0, "y": 0, "z": 0}, + {"x": 1, "y": 4, "z": 0}, + {"x": 2, "y": 0, "z": 0}, + ], + "assessment": { + "decision": "threat", + "corridor_intersection": "intersects", + }, + } + unknown = { + **threat, + "component_id": "rolling-unknown", + "assessment": { + "decision": "unknown", + "corridor_intersection": "unknown", + }, + } + + selected = _actionable_camera_metric_rows( + [threat, unknown], + added_cells=frozenset({(0, 0, 0), (1, 4, 0)}), + body_frame=body_frame, + profile=profile, + ) + + assert [row["component_id"] for row in selected] == ["rolling-actionable"] + assert selected[0]["cells"] == [{"x": 0, "y": 0, "z": 0}]