fix(lab): show only actionable obstacle boxes
This commit is contained in:
@@ -186,7 +186,7 @@ export interface M4ThreatTimeline {
|
|||||||
cameraPointSampleLimit: number;
|
cameraPointSampleLimit: number;
|
||||||
worldStateDelivery: "source-paced-latest-wins" | null;
|
worldStateDelivery: "source-paced-latest-wins" | null;
|
||||||
occupancyProvenanceDelivery: "baseline-versus-additive-component-diff" | 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;
|
worldStateFrameCount: number;
|
||||||
supersededFrameCount: number;
|
supersededFrameCount: number;
|
||||||
sourceRepresentationId: "registered-map-increment-v1";
|
sourceRepresentationId: "registered-map-increment-v1";
|
||||||
@@ -747,7 +747,7 @@ export async function fetchM4ThreatTimeline(
|
|||||||
? null
|
? null
|
||||||
: exact(
|
: exact(
|
||||||
payload.camera_obstacle_projection_delivery,
|
payload.camera_obstacle_projection_delivery,
|
||||||
"factory-kb4-occupied-voxel-bounds",
|
"factory-kb4-actionable-added-corridor-bounds",
|
||||||
"M4.8R3 camera obstacle projection delivery",
|
"M4.8R3 camera obstacle projection delivery",
|
||||||
),
|
),
|
||||||
worldStateFrameCount: payload.world_state_frame_count === undefined
|
worldStateFrameCount: payload.world_state_frame_count === undefined
|
||||||
|
|||||||
@@ -513,7 +513,7 @@ export function M4ReplayThreatVisual({
|
|||||||
shape="pill"
|
shape="pill"
|
||||||
variant={showStaticObstacles ? "primary" : "secondary"}
|
variant={showStaticObstacles ? "primary" : "secondary"}
|
||||||
aria-pressed={showStaticObstacles}
|
aria-pressed={showStaticObstacles}
|
||||||
title="Автоматические рамки занятых LiDAR-компонентов · без ручной разметки"
|
title="Только LOW-STEP LiDAR-препятствия с решением threat · без ручной разметки"
|
||||||
onClick={() => setShowStaticObstacles((visible) => !visible)}
|
onClick={() => setShowStaticObstacles((visible) => !visible)}
|
||||||
>
|
>
|
||||||
OBSTACLES
|
OBSTACLES
|
||||||
|
|||||||
@@ -2,6 +2,12 @@ import type { RecordedEvidenceBox } from "../../components/laboratory/RecordedEv
|
|||||||
import type { M4ThreatMetricVisual } from "../../core/laboratory/m4ReplayThreat";
|
import type { M4ThreatMetricVisual } from "../../core/laboratory/m4ReplayThreat";
|
||||||
|
|
||||||
const MINIMUM_BOX_SIZE_PX = 12;
|
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 {
|
function shortComponentId(componentId: string): string {
|
||||||
const finalSegment = componentId.split(/[-_]/).pop();
|
const finalSegment = componentId.split(/[-_]/).pop();
|
||||||
@@ -37,26 +43,42 @@ function visibleBox(
|
|||||||
return [left, top, right, bottom];
|
return [left, top, right, bottom];
|
||||||
}
|
}
|
||||||
|
|
||||||
function tone(obstacle: M4ThreatMetricVisual): RecordedEvidenceBox["tone"] {
|
function overlapFractionOfSmaller(
|
||||||
if (obstacle.assessment.decision === "threat") return "danger";
|
left: readonly [number, number, number, number],
|
||||||
if (obstacle.assessment.decision === "not-threat") return "success";
|
right: readonly [number, number, number, number],
|
||||||
return "warning";
|
): 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.
|
* Build actionable camera evidence from the same world-state shown in 3D.
|
||||||
* No image detector or manual review extent participates in these boxes.
|
* Unknown and clear components remain in world-state, but do not compete with
|
||||||
|
* actual corridor threats for the operator's attention.
|
||||||
*/
|
*/
|
||||||
export function buildM4StaticObstacleBoxes(
|
export function buildM4StaticObstacleBoxes(
|
||||||
obstacles: readonly M4ThreatMetricVisual[],
|
obstacles: readonly M4ThreatMetricVisual[],
|
||||||
imageWidth: number,
|
imageWidth: number,
|
||||||
imageHeight: number,
|
imageHeight: number,
|
||||||
): readonly RecordedEvidenceBox[] {
|
): readonly RecordedEvidenceBox[] {
|
||||||
const result: RecordedEvidenceBox[] = [];
|
const candidates: ObstacleBoxCandidate[] = [];
|
||||||
for (const obstacle of obstacles) {
|
for (const obstacle of obstacles) {
|
||||||
if (
|
if (
|
||||||
obstacle.occupancySource === "baseline"
|
obstacle.occupancySource === "baseline"
|
||||||
|| (obstacle.state !== "current" && obstacle.state !== "retained")
|
|| (obstacle.state !== "current" && obstacle.state !== "retained")
|
||||||
|
|| obstacle.assessment.decision !== "threat"
|
||||||
|
|| obstacle.assessment.corridorIntersection !== "intersects"
|
||||||
|| obstacle.cameraProjection === null
|
|| obstacle.cameraProjection === null
|
||||||
) continue;
|
) continue;
|
||||||
const projection = obstacle.cameraProjection;
|
const projection = obstacle.cameraProjection;
|
||||||
@@ -65,16 +87,33 @@ export function buildM4StaticObstacleBoxes(
|
|||||||
const depth = projection.nearestDepthM.toLocaleString("ru-RU", {
|
const depth = projection.nearestDepthM.toLocaleString("ru-RU", {
|
||||||
maximumFractionDigits: 1,
|
maximumFractionDigits: 1,
|
||||||
});
|
});
|
||||||
result.push({
|
candidates.push({
|
||||||
|
state: obstacle.state,
|
||||||
|
box: {
|
||||||
boxXyxy,
|
boxXyxy,
|
||||||
label: `OBS #${shortComponentId(obstacle.componentId)} · ${depth} м`,
|
label: `OBS #${shortComponentId(obstacle.componentId)} · ${depth} м`,
|
||||||
tone: tone(obstacle),
|
tone: "danger",
|
||||||
dashed: false,
|
dashed: false,
|
||||||
|
},
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
return result.sort((left, right) => {
|
const currentBoxes = candidates
|
||||||
const leftArea = (left.boxXyxy[2] - left.boxXyxy[0]) * (left.boxXyxy[3] - left.boxXyxy[1]);
|
.filter((candidate) => candidate.state === "current")
|
||||||
const rightArea = (right.boxXyxy[2] - right.boxXyxy[0]) * (right.boxXyxy[3] - right.boxXyxy[1]);
|
.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;
|
return rightArea - leftArea;
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -370,7 +370,7 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint"
|
|||||||
camera_point_sample_limit: 20000,
|
camera_point_sample_limit: 20000,
|
||||||
world_state_delivery: "source-paced-latest-wins",
|
world_state_delivery: "source-paced-latest-wins",
|
||||||
occupancy_provenance_delivery: "baseline-versus-additive-component-diff",
|
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,
|
world_state_frame_count: 4481,
|
||||||
superseded_frame_count: 8,
|
superseded_frame_count: 8,
|
||||||
local_surface_visualization: {
|
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.supersededFrameCount, 8);
|
||||||
assert.equal(
|
assert.equal(
|
||||||
timeline.cameraObstacleProjectionDelivery,
|
timeline.cameraObstacleProjectionDelivery,
|
||||||
"factory-kb4-occupied-voxel-bounds",
|
"factory-kb4-actionable-added-corridor-bounds",
|
||||||
);
|
);
|
||||||
|
|
||||||
const chunk = await fetchM4ThreatTimelineChunk(replayResultId, 1, 1, {
|
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, new RegExp(`^${endpointRoot}/${replayResultId}/timeline/chunk`));
|
||||||
assert.match(
|
assert.match(
|
||||||
requested,
|
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].worldStateAvailable, false);
|
||||||
assert.equal(chunk.frames[0].terminalOutcome, "superseded");
|
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,
|
||||||
{ ...obstacle, componentId: "baseline", occupancySource: "baseline" },
|
{ ...obstacle, componentId: "baseline", occupancySource: "baseline" },
|
||||||
{ ...obstacle, componentId: "held", state: "held" },
|
{ ...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);
|
], 800, 600);
|
||||||
assert.equal(boxes.length, 1);
|
assert.equal(boxes.length, 1);
|
||||||
assert.deepEqual(boxes[0].boxXyxy, [394, 294, 406, 306]);
|
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);
|
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", () => {
|
test("M4.6 local SLAM surface reprojects registered increments into the active body frame", () => {
|
||||||
const frames = [
|
const frames = [
|
||||||
timelineFrame(0, 10, {
|
timelineFrame(0, 10, {
|
||||||
|
|||||||
@@ -27,6 +27,8 @@ from .spatial_evidence import (
|
|||||||
from .threat import (
|
from .threat import (
|
||||||
DEFAULT_REPLAY_THREAT_PROFILE_PATH,
|
DEFAULT_REPLAY_THREAT_PROFILE_PATH,
|
||||||
RecordedReplayBodyFrameResolver,
|
RecordedReplayBodyFrameResolver,
|
||||||
|
ReplayBodyFrame,
|
||||||
|
ReplayThreatProfile,
|
||||||
load_replay_threat_profile,
|
load_replay_threat_profile,
|
||||||
)
|
)
|
||||||
from .threat_timeline import (
|
from .threat_timeline import (
|
||||||
@@ -49,8 +51,12 @@ EXPECTED_FRAME_COUNT: Final = 4_489
|
|||||||
CAMERA_ACCUMULATION_WINDOW_SECONDS: Final = 2.0
|
CAMERA_ACCUMULATION_WINDOW_SECONDS: Final = 2.0
|
||||||
CAMERA_ACCUMULATION_POINT_LIMIT: Final = 20_000
|
CAMERA_ACCUMULATION_POINT_LIMIT: Final = 20_000
|
||||||
CAMERA_POINT_OVERLAY_SCHEMA: Final = "missioncore.m48s-camera-point-overlay/v1"
|
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":'
|
_SOURCE_ENVELOPE_MARKER: Final = b'"source_envelope":'
|
||||||
_JSON_DECODER: Final = json.JSONDecoder()
|
_JSON_DECODER: Final = json.JSONDecoder()
|
||||||
|
Cell = tuple[int, int, int]
|
||||||
|
|
||||||
|
|
||||||
class M48sReplayTimelineError(RuntimeError):
|
class M48sReplayTimelineError(RuntimeError):
|
||||||
@@ -62,6 +68,12 @@ class _LedgerIndex:
|
|||||||
offsets_by_sequence: dict[int, int]
|
offsets_by_sequence: dict[int, int]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class _FrameDiff:
|
||||||
|
component_provenance: dict[str, str]
|
||||||
|
added_cells: frozenset[Cell]
|
||||||
|
|
||||||
|
|
||||||
class M48sReplayTimeline:
|
class M48sReplayTimeline:
|
||||||
"""Read source-indexed chunks while preserving latest-wins world-state gaps."""
|
"""Read source-indexed chunks while preserving latest-wins world-state gaps."""
|
||||||
|
|
||||||
@@ -188,7 +200,7 @@ class M48sReplayTimeline:
|
|||||||
else None
|
else None
|
||||||
),
|
),
|
||||||
"camera_obstacle_projection_delivery": (
|
"camera_obstacle_projection_delivery": (
|
||||||
"factory-kb4-occupied-voxel-bounds"
|
ACTIONABLE_CAMERA_OBSTACLE_PROJECTION
|
||||||
if self.frame_diff_path is not None
|
if self.frame_diff_path is not None
|
||||||
else None
|
else None
|
||||||
),
|
),
|
||||||
@@ -435,19 +447,31 @@ class M48sReplayTimeline:
|
|||||||
body_frame,
|
body_frame,
|
||||||
occupied_voxel_size_m=self.profile.corridor.occupied_voxel_size_m,
|
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 = (
|
camera_projections = (
|
||||||
{}
|
{}
|
||||||
if frame is None or not provenance
|
if frame is None or not actionable_metric_rows
|
||||||
else project_metric_obstacles_to_camera(
|
else project_metric_obstacles_to_camera(
|
||||||
metric_rows,
|
actionable_metric_rows,
|
||||||
position_map_xyz=frame.sensor_position_map,
|
position_map_xyz=frame.sensor_position_map,
|
||||||
orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw,
|
orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw,
|
||||||
profile=frame.projection,
|
profile=frame.projection,
|
||||||
occupied_voxel_size_m=(
|
occupied_voxel_size_m=(
|
||||||
self.profile.corridor.occupied_voxel_size_m
|
self.profile.corridor.occupied_voxel_size_m
|
||||||
),
|
),
|
||||||
component_ids=set(provenance),
|
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
for visual in metric_visuals:
|
for visual in metric_visuals:
|
||||||
@@ -537,9 +561,9 @@ class M48sReplayTimeline:
|
|||||||
raise M48sReplayTimelineError("M4.8S frame row is invalid")
|
raise M48sReplayTimelineError("M4.8S frame row is invalid")
|
||||||
return value
|
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:
|
if self.frame_diff_path is None:
|
||||||
return {}
|
return None
|
||||||
offset = self.frame_diff_offsets.get(sequence)
|
offset = self.frame_diff_offsets.get(sequence)
|
||||||
if offset is None:
|
if offset is None:
|
||||||
raise M48sReplayTimelineError("M4.8R3 frame diff is incomplete")
|
raise M48sReplayTimelineError("M4.8R3 frame diff is incomplete")
|
||||||
@@ -556,7 +580,88 @@ class M48sReplayTimeline:
|
|||||||
for key, item in provenance.items()
|
for key, item in provenance.items()
|
||||||
):
|
):
|
||||||
raise M48sReplayTimelineError("M4.8R3 component provenance changed")
|
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(
|
def _index_ledger(
|
||||||
@@ -708,7 +813,14 @@ def _text(value: object, label: str) -> str:
|
|||||||
return value
|
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__ = [
|
__all__ = [
|
||||||
|
"ACTIONABLE_CAMERA_OBSTACLE_PROJECTION",
|
||||||
"CAMERA_ACCUMULATION_POINT_LIMIT",
|
"CAMERA_ACCUMULATION_POINT_LIMIT",
|
||||||
"CAMERA_ACCUMULATION_WINDOW_SECONDS",
|
"CAMERA_ACCUMULATION_WINDOW_SECONDS",
|
||||||
"CAMERA_POINT_OVERLAY_SCHEMA",
|
"CAMERA_POINT_OVERLAY_SCHEMA",
|
||||||
|
|||||||
@@ -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}]
|
||||||
Reference in New Issue
Block a user