SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix(lab): show only actionable obstacle boxes

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-26 16:09:32 +03:00
parent a98ad929d9
commit ac1dc7d930
6 changed files with 306 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/control-station/src/core/laboratory/m4ReplayThreat.ts
+2 -2
View File
@@ -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
apps/control-station/src/workspaces/laboratory/M4ReplayThreatVisual.tsx
+1 -1
View File
@@ -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
apps/control-station/src/workspaces/laboratory/m4StaticObstacleBoxes.ts
+56 -17
View File
@@ -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;
});
}
apps/control-station/test/m4ReplayThreat.test.mjs
+74 -3
View File
@@ -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, {
src/k1link/perception/m48s_replay_timeline.py
+120 -8
View File
@@ -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",
tests/test_m48s_replay_timeline.py
+53
View File
@@ -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}]