feat(lab): project system obstacles into fisheye
This commit is contained in:
@@ -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)
|
||||
Reference in New Issue
Block a user