50 lines
1.6 KiB
Python
50 lines
1.6 KiB
Python
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)
|