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

feat(lab): project system obstacles into fisheye

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-26 15:31:06 +03:00
parent 30227b661f
commit 5a9738d6aa
8 changed files with 424 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/control-station/src/core/laboratory/m4ReplayThreat.ts
+51
View File
@@ -70,6 +70,14 @@ export interface M4ThreatAssessment {
reasonCodes: readonly string[];
}
export interface M4ThreatCameraObstacleProjection {
bboxXyxy: readonly [number, number, number, number];
nearestDepthM: number;
projectedCellCount: number;
projection: "factory-kb4-occupied-voxel-bounds";
authority: "visual-derived";
}
export interface M4ThreatMetricVisual {
componentId: string;
state: "current" | "retained" | "held" | "expired";
@@ -78,6 +86,7 @@ export interface M4ThreatMetricVisual {
cellCentersBodyXyzM: readonly M4Point3[];
assessment: M4ThreatAssessment;
occupancySource: M4OccupancySource;
cameraProjection: M4ThreatCameraObstacleProjection | null;
}
export interface M4ThreatCameraProposal {
@@ -177,6 +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;
worldStateFrameCount: number;
supersededFrameCount: number;
sourceRepresentationId: "registered-map-increment-v1";
@@ -323,6 +333,32 @@ function parseCameraProposal(value: unknown): M4ThreatCameraProposal {
};
}
function parseCameraObstacleProjection(value: unknown): M4ThreatCameraObstacleProjection {
const item = object(value, "M4.8R3 camera obstacle projection");
const bbox = vector(item.bbox_xyxy, 4, "M4.8R3 camera obstacle bbox");
if (bbox[2]! < bbox[0]! || bbox[3]! < bbox[1]!) {
throw new M4ThreatContractError("M4.8R3 camera obstacle bbox: нарушена геометрия.");
}
return {
bboxXyxy: [bbox[0]!, bbox[1]!, bbox[2]!, bbox[3]!],
nearestDepthM: number(item.nearest_depth_m, "M4.8R3 camera obstacle depth"),
projectedCellCount: integer(
item.projected_cell_count,
"M4.8R3 camera obstacle cells",
),
projection: exact(
item.projection,
"factory-kb4-occupied-voxel-bounds",
"M4.8R3 camera obstacle projection",
),
authority: exact(
item.authority,
"visual-derived",
"M4.8R3 camera obstacle authority",
),
};
}
function parseMetricVisual(value: unknown): M4ThreatMetricVisual {
const item = object(value, "M4.6 metric visual");
const state = text(item.state, "M4.6 temporal state");
@@ -347,6 +383,9 @@ function parseMetricVisual(value: unknown): M4ThreatMetricVisual {
),
assessment: parseAssessment(item.assessment),
occupancySource,
cameraProjection: item.camera_projection === undefined
? null
: parseCameraObstacleProjection(item.camera_projection),
};
}
@@ -704,6 +743,13 @@ export async function fetchM4ThreatTimeline(
"baseline-versus-additive-component-diff",
"M4.8R3 occupancy provenance",
),
cameraObstacleProjectionDelivery: payload.camera_obstacle_projection_delivery == null
? null
: exact(
payload.camera_obstacle_projection_delivery,
"factory-kb4-occupied-voxel-bounds",
"M4.8R3 camera obstacle projection delivery",
),
worldStateFrameCount: payload.world_state_frame_count === undefined
? frameCount
: integer(payload.world_state_frame_count, "M4.6 world-state frames"),
@@ -756,16 +802,21 @@ export async function fetchM4ThreatTimelineChunk(
fetcher = fetch,
signal,
endpointRoot = M4_THREAT_TIMELINE_ENDPOINT_ROOT,
cameraObstacleProjectionDelivery = null,
}: {
fetcher?: LaboratoryFetch;
signal?: AbortSignal;
endpointRoot?: string;
cameraObstacleProjectionDelivery?: M4ThreatTimeline["cameraObstacleProjectionDelivery"];
} = {},
): Promise<M4ThreatTimelineChunk> {
const params = new URLSearchParams({
start: String(startSequence),
count: String(frameCount),
});
if (cameraObstacleProjectionDelivery !== null) {
params.set("obstacle_projection", cameraObstacleProjectionDelivery);
}
const response = await fetcher(
`${endpointRoot}/${result}/timeline/chunk?${params}`,
{ headers: { Accept: "application/json" }, signal },
apps/control-station/src/workspaces/laboratory/M4ReplayThreatVisual.tsx
+38 -4
View File
@@ -47,6 +47,7 @@ import {
useM4ThreatTimelineMetadata,
} from "./useM4ThreatTimeline";
import { useE47SemanticTimelineFrame } from "./useE47SemanticTimeline";
import { buildM4StaticObstacleBoxes } from "./m4StaticObstacleBoxes";
type M4ThreatMediaMode = "video" | "camera";
type M4ThreatMediaSelection = M4ThreatMediaMode | "none";
@@ -132,6 +133,7 @@ export function M4ReplayThreatVisual({
const [showMediaSemantic, setShowMediaSemantic] = useState(true);
const [showSpatialSemantic, setShowSpatialSemantic] = useState(true);
const [showMediaPoints, setShowMediaPoints] = useState(false);
const [showStaticObstacles, setShowStaticObstacles] = useState(true);
const [splitPrimarySize, setSplitPrimarySize] = useState(50);
const [splitOrientation, setSplitOrientation] = useState<SplitPaneOrientation>(() => (
typeof window !== "undefined" && window.matchMedia("(max-width: 900px)").matches
@@ -287,9 +289,26 @@ export function M4ReplayThreatVisual({
};
});
}, [frame, metadata.timeline, reviewAnchors, showReviewAnchorBoxes]);
const staticObstacleBoxes = useMemo<readonly RecordedEvidenceBox[]>(() => {
const timeline = metadata.timeline;
if (
!frame
|| !timeline?.cameraObstacleProjectionDelivery
|| !showStaticObstacles
) return [];
return buildM4StaticObstacleBoxes(
frame.metricObstacles,
timeline.imageWidth,
timeline.imageHeight,
);
}, [frame, metadata.timeline, showStaticObstacles]);
const activeBoxes = useMemo(
() => [...boxes(frame?.cameraProposals ?? []), ...reviewAnchorBoxes],
[frame, reviewAnchorBoxes],
() => [
...boxes(frame?.cameraProposals ?? []),
...staticObstacleBoxes,
...reviewAnchorBoxes,
],
[frame, reviewAnchorBoxes, staticObstacleBoxes],
);
const semanticClasses = useMemo<readonly RecordedEvidenceSemanticClass[]>(
() => semantic?.taxonomy.map((item) => ({
@@ -359,7 +378,8 @@ export function M4ReplayThreatVisual({
(item) => item.state === "retained",
) ?? [];
const lowStepObstacles = spatialFrame?.metricObstacles.filter(
(item) => item.occupancySource !== "baseline",
(item) => item.occupancySource !== "baseline"
&& (item.state === "current" || item.state === "retained"),
) ?? [];
const nearest = spatialFrame?.metricObstacles
.map((item) => item.assessment.closestApproachM)
@@ -456,7 +476,9 @@ export function M4ReplayThreatVisual({
</div>
);
const mediaLayerControls = semantic || metadata.timeline?.cameraPointDelivery ? (
const mediaLayerControls = semantic
|| metadata.timeline?.cameraPointDelivery
|| metadata.timeline?.cameraObstacleProjectionDelivery ? (
<div
className="m4-replay-threat-visual__pane-layer-controls"
role="group"
@@ -485,6 +507,18 @@ export function M4ReplayThreatVisual({
POINTS
</Button>
) : null}
{metadata.timeline?.cameraObstacleProjectionDelivery ? (
<Button
size="compact"
shape="pill"
variant={showStaticObstacles ? "primary" : "secondary"}
aria-pressed={showStaticObstacles}
title="Автоматические рамки занятых LiDAR-компонентов · без ручной разметки"
onClick={() => setShowStaticObstacles((visible) => !visible)}
>
OBSTACLES
</Button>
) : null}
</div>
) : null;
apps/control-station/src/workspaces/laboratory/m4StaticObstacleBoxes.ts
+80
View File
@@ -0,0 +1,80 @@
import type { RecordedEvidenceBox } from "../../components/laboratory/RecordedEvidenceVideoScene";
import type { M4ThreatMetricVisual } from "../../core/laboratory/m4ReplayThreat";
const MINIMUM_BOX_SIZE_PX = 12;
function shortComponentId(componentId: string): string {
const finalSegment = componentId.split(/[-_]/).pop();
return finalSegment && /^\d+$/.test(finalSegment)
? finalSegment
: componentId.slice(-6).toUpperCase();
}
function clamp(value: number, minimum: number, maximum: number): number {
return Math.min(maximum, Math.max(minimum, value));
}
function visibleBox(
box: readonly [number, number, number, number],
imageWidth: number,
imageHeight: number,
): readonly [number, number, number, number] | null {
const [sourceLeft, sourceTop, sourceRight, sourceBottom] = box;
if (
sourceRight < 0
|| sourceBottom < 0
|| sourceLeft >= imageWidth
|| sourceTop >= imageHeight
) return null;
const centerX = (sourceLeft + sourceRight) / 2;
const centerY = (sourceTop + sourceBottom) / 2;
const halfWidth = Math.max((sourceRight - sourceLeft) / 2, MINIMUM_BOX_SIZE_PX / 2);
const halfHeight = Math.max((sourceBottom - sourceTop) / 2, MINIMUM_BOX_SIZE_PX / 2);
const left = clamp(centerX - halfWidth, 0, imageWidth - 1);
const top = clamp(centerY - halfHeight, 0, imageHeight - 1);
const right = clamp(centerX + halfWidth, left + 1, imageWidth);
const bottom = clamp(centerY + halfHeight, top + 1, imageHeight);
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";
}
/**
* Build camera evidence from the same world-state components shown in 3D.
* No image detector or manual review extent participates in these boxes.
*/
export function buildM4StaticObstacleBoxes(
obstacles: readonly M4ThreatMetricVisual[],
imageWidth: number,
imageHeight: number,
): readonly RecordedEvidenceBox[] {
const result: RecordedEvidenceBox[] = [];
for (const obstacle of obstacles) {
if (
obstacle.occupancySource === "baseline"
|| (obstacle.state !== "current" && obstacle.state !== "retained")
|| obstacle.cameraProjection === null
) continue;
const projection = obstacle.cameraProjection;
const boxXyxy = visibleBox(projection.bboxXyxy, imageWidth, imageHeight);
if (!boxXyxy) continue;
const depth = projection.nearestDepthM.toLocaleString("ru-RU", {
maximumFractionDigits: 1,
});
result.push({
boxXyxy,
label: `OBS #${shortComponentId(obstacle.componentId)} · ${depth} м`,
tone: tone(obstacle),
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;
});
}
apps/control-station/src/workspaces/laboratory/useM4ThreatTimeline.ts
+1
View File
@@ -130,6 +130,7 @@ export function useM4ThreatTimelineFrame({
void fetchM4ThreatTimelineChunk(resultId, start, chunkSize, {
signal: controller.signal,
endpointRoot,
cameraObstacleProjectionDelivery: timeline.cameraObstacleProjectionDelivery,
})
.then((chunk) => {
if (controller.signal.aborted) return;
apps/control-station/test/m4ReplayThreat.test.mjs
+83
View File
@@ -17,6 +17,7 @@ let synchronizeRecordedEvidencePlayback;
let m4ThreatChunkWindowStarts;
let cancelM4ThreatChunkRequestsOutsideWindow;
let buildM4LocalSurface;
let buildM4StaticObstacleBoxes;
const resultId = `m4-threat-replay-${"a".repeat(64)}`;
@@ -50,6 +51,9 @@ before(async () => {
({ buildM4LocalSurface } = await server.ssrLoadModule(
"/src/core/laboratory/m4LocalSurface.ts",
));
({ buildM4StaticObstacleBoxes } = await server.ssrLoadModule(
"/src/workspaces/laboratory/m4StaticObstacleBoxes.ts",
));
});
after(async () => {
@@ -365,6 +369,8 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint"
camera_point_window_seconds: 2,
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",
world_state_frame_count: 4481,
superseded_frame_count: 8,
local_surface_visualization: {
@@ -392,9 +398,14 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint"
assert.equal(timeline.cameraPointWindowSeconds, 2);
assert.equal(timeline.worldStateFrameCount, 4481);
assert.equal(timeline.supersededFrameCount, 8);
assert.equal(
timeline.cameraObstacleProjectionDelivery,
"factory-kb4-occupied-voxel-bounds",
);
const chunk = await fetchM4ThreatTimelineChunk(replayResultId, 1, 1, {
endpointRoot,
cameraObstacleProjectionDelivery: timeline.cameraObstacleProjectionDelivery,
fetcher: async (input) => {
requested = String(input);
return new Response(JSON.stringify({
@@ -411,6 +422,30 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint"
camera_projected_point_count: 1,
camera_projected_sample_count: 1,
camera_projection: "factory-kb4-exact",
metric_obstacles: [{
component_id: "temporal-1855-7",
state: "current",
motion: "stationary",
centroid_body_xyz_m: [2.4, 0.1, 0.3],
cell_centers_body_xyz_m: [[2.4, 0.1, 0.3]],
occupancy_source: "additive-low-step",
camera_projection: {
bbox_xyxy: [390.5, 280.25, 408.75, 318.5],
nearest_depth_m: 2.3,
projected_cell_count: 1,
projection: "factory-kb4-occupied-voxel-bounds",
authority: "visual-derived",
},
assessment: {
component_id: "temporal-1855-7",
decision: "threat",
corridor_intersection: "intersects",
relative_speed_mps: null,
closest_approach_m: 0.2,
ttc_seconds: null,
reason_codes: ["current-corridor-intersection"],
},
}],
camera_url: `${endpointRoot}/${replayResultId}/timeline/frames/1/camera`,
})],
authority: "replay-simulated",
@@ -418,10 +453,18 @@ 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/,
);
assert.equal(chunk.frames[0].worldStateAvailable, false);
assert.equal(chunk.frames[0].terminalOutcome, "superseded");
assert.deepEqual(chunk.frames[0].cameraProjectedPointsXyd[0], [100.5, 200.25, 3.75]);
assert.equal(chunk.frames[0].cameraProjection, "factory-kb4-exact");
assert.deepEqual(
chunk.frames[0].metricObstacles[0].cameraProjection.bboxXyxy,
[390.5, 280.25, 408.75, 318.5],
);
const overlay = await fetchM4ThreatCameraPointOverlay(replayResultId, 1, {
endpointRoot,
@@ -453,6 +496,43 @@ test("M4.8S timeline binds factory-KB4 camera points through its exact endpoint"
assert.equal(overlay.projection, "factory-kb4-causal-registered-accumulation");
});
test("M4.8R3 turns active low-step components into native fisheye obstacle boxes", () => {
const obstacle = {
componentId: "temporal-1855-7",
state: "current",
motion: "stationary",
centroidBodyXyzM: [2.4, 0.1, 0.3],
cellCentersBodyXyzM: [[2.4, 0.1, 0.3]],
occupancySource: "additive-low-step",
cameraProjection: {
bboxXyxy: [398, 298, 402, 302],
nearestDepthM: 2.3,
projectedCellCount: 1,
projection: "factory-kb4-occupied-voxel-bounds",
authority: "visual-derived",
},
assessment: {
componentId: "temporal-1855-7",
decision: "threat",
corridorIntersection: "intersects",
relativeSpeedMps: null,
closestApproachM: 0.2,
ttcSeconds: null,
reasonCodes: ["current-corridor-intersection"],
},
};
const boxes = buildM4StaticObstacleBoxes([
obstacle,
{ ...obstacle, componentId: "baseline", occupancySource: "baseline" },
{ ...obstacle, componentId: "held", state: "held" },
], 800, 600);
assert.equal(boxes.length, 1);
assert.deepEqual(boxes[0].boxXyxy, [394, 294, 406, 306]);
assert.equal(boxes[0].label, "OBS #7 · 2,3 м");
assert.equal(boxes[0].tone, "danger");
assert.equal(boxes[0].dashed, false);
});
test("M4.6 local SLAM surface reprojects registered increments into the active body frame", () => {
const frames = [
timelineFrame(0, 10, {
@@ -603,6 +683,9 @@ test("M4.6 viewer keeps media and spatial panes on one playback clock", async ()
assert.match(visual, /label: "3D"/);
assert.match(visual, /label: "PLAN"/);
assert.match(visual, />\s*POINTS\s*</);
assert.match(visual, />\s*OBSTACLES\s*</);
assert.match(visual, /useState\(true\);[\s\S]*setShowStaticObstacles/);
assert.match(visual, /buildM4StaticObstacleBoxes/);
assert.match(visual, /pointCloudOverlay=/);
assert.match(visual, /mediaMode/);
assert.match(visual, /spatialMode/);
src/k1link/perception/m48s_replay_timeline.py
+30 -6
View File
@@ -19,7 +19,11 @@ import numpy as np
from .geometry import RecordedGeometryStore
from .geometry_math import project_map_points_kb4
from .recorded_source import RECORDED_REPRESENTATION_ID
from .spatial_evidence import project_metric_obstacles_to_body, sample_points_in_body_frame
from .spatial_evidence import (
project_metric_obstacles_to_body,
project_metric_obstacles_to_camera,
sample_points_in_body_frame,
)
from .threat import (
DEFAULT_REPLAY_THREAT_PROFILE_PATH,
RecordedReplayBodyFrameResolver,
@@ -183,6 +187,11 @@ class M48sReplayTimeline:
if self.frame_diff_path is not None
else None
),
"camera_obstacle_projection_delivery": (
"factory-kb4-occupied-voxel-bounds"
if self.frame_diff_path is not None
else None
),
"world_state_frame_count": len(self.index.offsets_by_sequence),
"superseded_frame_count": sum(
value == "superseded" for value in self.outcomes.values()
@@ -383,6 +392,7 @@ class M48sReplayTimeline:
)
metric_visuals: list[dict[str, object]] = []
metric_rows: list[dict[str, object]] = []
camera_proposals: list[dict[str, object]] = []
assessments: list[dict[str, object]] = []
if row is not None:
@@ -401,7 +411,6 @@ class M48sReplayTimeline:
_text(item.get("component_id"), "assessment component"): item
for item in assessments
}
metric_rows: list[dict[str, object]] = []
for obstacle in (
*_objects(obstacle_map.get("occupied"), "occupied obstacles"),
*_objects(obstacle_map.get("unknown"), "unknown obstacles"),
@@ -427,11 +436,26 @@ class M48sReplayTimeline:
occupied_voxel_size_m=self.profile.corridor.occupied_voxel_size_m,
)
provenance = self._component_provenance(sequence)
for visual in metric_visuals:
visual["occupancy_source"] = provenance.get(
str(visual["component_id"]),
"baseline",
camera_projections = (
{}
if frame is None or not provenance
else project_metric_obstacles_to_camera(
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:
component_id = str(visual["component_id"])
visual["occupancy_source"] = provenance.get(component_id, "baseline")
projection = camera_projections.get(component_id)
if projection is not None:
visual["camera_projection"] = projection
associated = set(_strings(row.get("associated_proposal_ids"), "associated ids"))
for proposal in _objects(row.get("detector_proposals"), "detector proposals"):
proposal_id = _text(proposal.get("proposal_id"), "proposal id")
src/k1link/perception/spatial_evidence.py
+92
View File
@@ -8,6 +8,7 @@ from collections.abc import Mapping, Sequence
import numpy as np
import numpy.typing as npt
from .geometry_math import Kb4ProjectionProfile, project_map_points_kb4
from .threat import ReplayBodyFrame
FloatArray = npt.NDArray[np.float64]
@@ -86,6 +87,96 @@ def project_metric_obstacles_to_body(
return visuals
def project_metric_obstacles_to_camera(
metric_rows: Sequence[Mapping[str, object]],
*,
position_map_xyz: npt.ArrayLike,
orientation_map_from_lidar_xyzw: npt.ArrayLike,
profile: Kb4ProjectionProfile,
occupied_voxel_size_m: float,
component_ids: set[str] | None = None,
) -> dict[str, dict[str, object]]:
"""Project ledger-owned occupied voxel bounds into the native KB4 frame.
This is a visualization projection of already accepted world-state
components. It neither reclusters geometry nor changes threat authority.
"""
if not math.isfinite(occupied_voxel_size_m) or occupied_voxel_size_m <= 0:
raise SpatialEvidenceProjectionError("occupied voxel size is invalid")
points: list[tuple[float, float, float]] = []
point_owners: list[tuple[str, int]] = []
for row in metric_rows:
component_id = row.get("component_id")
cells = row.get("cells")
if (
not isinstance(component_id, str)
or not component_id
or (component_ids is not None and component_id not in component_ids)
or not isinstance(cells, list)
):
continue
for cell_index, raw_cell in enumerate(cells):
if not isinstance(raw_cell, dict):
raise SpatialEvidenceProjectionError("occupied cell is invalid")
indices = (
_signed_integer(raw_cell.get("x"), "cell x"),
_signed_integer(raw_cell.get("y"), "cell y"),
_signed_integer(raw_cell.get("z"), "cell z"),
)
bounds = tuple(
(index * occupied_voxel_size_m, (index + 1) * occupied_voxel_size_m)
for index in indices
)
for x in bounds[0]:
for y in bounds[1]:
for z in bounds[2]:
points.append((x, y, z))
point_owners.append((component_id, cell_index))
if not points:
return {}
projected = project_map_points_kb4(
points,
position_map_xyz=position_map_xyz,
orientation_map_from_lidar_xyzw=orientation_map_from_lidar_xyzw,
profile=profile,
)
grouped_pixels: dict[str, list[npt.NDArray[np.float64]]] = {}
grouped_depths: dict[str, list[float]] = {}
grouped_cells: dict[str, set[int]] = {}
for pixel, depth, source_index in zip(
projected.pixels_xy,
projected.depths_m,
projected.source_indices,
strict=True,
):
component_id, cell_index = point_owners[int(source_index)]
grouped_pixels.setdefault(component_id, []).append(pixel)
grouped_depths.setdefault(component_id, []).append(float(depth))
grouped_cells.setdefault(component_id, set()).add(cell_index)
result: dict[str, dict[str, object]] = {}
for component_id, pixel_values in grouped_pixels.items():
pixels = np.asarray(pixel_values, dtype=np.float64)
depths = np.asarray(grouped_depths[component_id], dtype=np.float64)
if pixels.size == 0 or depths.size == 0:
continue
minimum = np.min(pixels, axis=0)
maximum = np.max(pixels, axis=0)
result[component_id] = {
"bbox_xyxy": [
round(float(minimum[0]), 3),
round(float(minimum[1]), 3),
round(float(maximum[0]), 3),
round(float(maximum[1]), 3),
],
"nearest_depth_m": round(float(np.min(depths)), 6),
"projected_cell_count": len(grouped_cells[component_id]),
"projection": "factory-kb4-occupied-voxel-bounds",
"authority": "visual-derived",
}
return result
def _finite_vector3(value: Sequence[object], label: str) -> tuple[float, float, float]:
if len(value) != 3:
raise SpatialEvidenceProjectionError(f"{label} is invalid")
@@ -113,6 +204,7 @@ def _signed_integer(value: object, label: str) -> int:
__all__ = [
"SpatialEvidenceProjectionError",
"project_metric_obstacles_to_camera",
"project_metric_obstacles_to_body",
"sample_points_in_body_frame",
]
tests/test_spatial_evidence.py
+49
View File
@@ -0,0 +1,49 @@
from __future__ import annotations
import numpy as np
import pytest
from k1link.perception.geometry_math import Kb4ProjectionProfile
from k1link.perception.spatial_evidence import project_metric_obstacles_to_camera
def _identity_profile() -> Kb4ProjectionProfile:
transform = np.eye(4, dtype=np.float64)
transform.setflags(write=False)
return Kb4ProjectionProfile(
width=800,
height=600,
intrinsic_fx_fy_cx_cy=(100.0, 100.0, 400.0, 300.0),
distortion_kb4=(0.0, 0.0, 0.0, 0.0),
t_camera_from_lidar=transform,
)
def test_metric_obstacle_projection_uses_native_kb4_voxel_bounds() -> None:
projections = project_metric_obstacles_to_camera(
[
{
"component_id": "static-a",
"cells": [{"x": -1, "y": -1, "z": 2}],
},
{
"component_id": "baseline-b",
"cells": [{"x": 1, "y": 1, "z": 2}],
},
],
position_map_xyz=(0.0, 0.0, 0.0),
orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
profile=_identity_profile(),
occupied_voxel_size_m=1.0,
component_ids={"static-a"},
)
assert set(projections) == {"static-a"}
projection = projections["static-a"]
assert projection["projection"] == "factory-kb4-occupied-voxel-bounds"
assert projection["authority"] == "visual-derived"
assert projection["projected_cell_count"] == 1
assert projection["nearest_depth_m"] == pytest.approx(2.0)
left, top, right, bottom = projection["bbox_xyxy"]
assert left < right == pytest.approx(400.0)
assert top < bottom == pytest.approx(300.0)