feat(lab): project system obstacles into fisheye

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
+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)