174 lines
6.1 KiB
Python
174 lines
6.1 KiB
Python
from __future__ import annotations
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from k1link.compute.semantic_geometry_fusion import (
|
|
CameraGeometryFusionProfile,
|
|
SemanticGeometryFusionError,
|
|
_geometry_clusters,
|
|
_semantic_support,
|
|
)
|
|
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
|
ProjectedPointCloud,
|
|
)
|
|
|
|
|
|
def _object(*, observed_geometry: bool = False) -> dict[str, object]:
|
|
value: dict[str, object] = {
|
|
"source_track_id": 42,
|
|
"track_id": 4200,
|
|
"label": "car",
|
|
"association_group": "vehicle",
|
|
"score": 0.91,
|
|
"bbox_xyxy": [10.0, 10.0, 90.0, 90.0],
|
|
"cuboid_status": "rejected-fewer-than-8-clustered-points",
|
|
"camera_motion_state": "static",
|
|
"camera_motion_confidence": 0.8,
|
|
"motion_state": "unknown",
|
|
"motion_status": "e26-insufficient-independent-evidence",
|
|
}
|
|
if observed_geometry:
|
|
value.update(
|
|
{
|
|
"observed_cuboid_center_map": [2.0, 0.0, 0.5],
|
|
"observed_cuboid_half_size": [1.0, 0.5, 0.5],
|
|
"observed_cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
|
}
|
|
)
|
|
return value
|
|
|
|
|
|
def _projection(point_count: int) -> ProjectedPointCloud:
|
|
return ProjectedPointCloud(
|
|
pixels_xy=np.column_stack(
|
|
(
|
|
np.linspace(20.0, 80.0, point_count),
|
|
np.linspace(20.0, 80.0, point_count),
|
|
)
|
|
).astype(np.float64),
|
|
depths_m=np.linspace(2.0, 2.4, point_count).astype(np.float64),
|
|
source_indices=np.arange(point_count, dtype=np.int64),
|
|
source_point_count=point_count,
|
|
camera_front_point_count=point_count,
|
|
)
|
|
|
|
|
|
def test_camera_semantic_and_connected_occupied_support_agree() -> None:
|
|
points = np.asarray(
|
|
[
|
|
[2.0, 0.0, 0.5],
|
|
[2.2, 0.0, 0.6],
|
|
[2.1, 0.2, 0.0],
|
|
[2.3, 0.2, -0.1],
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
support = _semantic_support(
|
|
_object(),
|
|
projected=_projection(4),
|
|
frame_points_map=points,
|
|
point_class=np.asarray([2, 2, 1, 3], dtype=np.uint8),
|
|
point_height_m=np.asarray([0.5, 0.6, 0.0, -0.1], dtype=np.float32),
|
|
source_available=True,
|
|
surface_valid=True,
|
|
profile=CameraGeometryFusionProfile(),
|
|
)
|
|
assert support.document["geometry_status"] == "agree"
|
|
assert support.document["range_m"] == pytest.approx(2.0666666667)
|
|
assert support.document["range_estimate_m"] == pytest.approx(2.0666666667)
|
|
assert support.document["range_estimate_available"] is True
|
|
assert support.document["range_support_qualified"] is True
|
|
assert support.document["unknown_is_occupied"] is True
|
|
assert support.document["navigation_or_safety_accepted"] is False
|
|
assert support.occupied_source_indices.tolist() == [0, 1]
|
|
|
|
|
|
def test_camera_only_and_surface_conflict_remain_explicit() -> None:
|
|
points = np.column_stack(
|
|
(
|
|
np.linspace(2.0, 3.0, 6),
|
|
np.zeros(6),
|
|
np.zeros(6),
|
|
)
|
|
).astype(np.float64)
|
|
camera_only = _semantic_support(
|
|
_object(),
|
|
projected=_projection(6),
|
|
frame_points_map=points,
|
|
point_class=np.asarray([2, 1, 1, 1, 1, 1], dtype=np.uint8),
|
|
point_height_m=np.asarray([0.5, 0, 0, 0, 0, 0], dtype=np.float32),
|
|
source_available=True,
|
|
surface_valid=True,
|
|
profile=CameraGeometryFusionProfile(),
|
|
)
|
|
conflict = _semantic_support(
|
|
_object(observed_geometry=True),
|
|
projected=_projection(6),
|
|
frame_points_map=points,
|
|
point_class=np.ones(6, dtype=np.uint8),
|
|
point_height_m=np.zeros(6, dtype=np.float32),
|
|
source_available=True,
|
|
surface_valid=True,
|
|
profile=CameraGeometryFusionProfile(),
|
|
)
|
|
unknown = _semantic_support(
|
|
_object(),
|
|
projected=None,
|
|
frame_points_map=np.empty((0, 3), dtype=np.float64),
|
|
point_class=np.empty(0, dtype=np.uint8),
|
|
point_height_m=np.empty(0, dtype=np.float32),
|
|
source_available=False,
|
|
surface_valid=False,
|
|
profile=CameraGeometryFusionProfile(),
|
|
)
|
|
assert camera_only.document["geometry_status"] == "single-source-camera"
|
|
assert camera_only.document["range_m"] is None
|
|
assert camera_only.document["range_estimate_m"] == pytest.approx(2.0)
|
|
assert camera_only.document["range_estimate_available"] is True
|
|
assert camera_only.document["range_support_qualified"] is False
|
|
assert conflict.document["geometry_status"] == "conflict"
|
|
assert conflict.document["range_m"] is None
|
|
assert conflict.document["range_estimate_available"] is False
|
|
assert conflict.document["range_support_qualified"] is False
|
|
assert unknown.document["geometry_status"] == "unknown"
|
|
assert unknown.document["range_m"] is None
|
|
assert unknown.document["range_estimate_available"] is False
|
|
assert unknown.document["range_support_qualified"] is False
|
|
|
|
|
|
def test_unclaimed_occupied_component_is_a_separate_geometry_layer() -> None:
|
|
points = np.asarray(
|
|
[
|
|
[1.00, 0.00, 0.30],
|
|
[1.20, 0.00, 0.35],
|
|
[1.45, 0.00, 0.40],
|
|
[1.65, 0.00, 0.45],
|
|
[4.00, 0.00, 0.30],
|
|
[4.20, 0.00, 0.35],
|
|
[4.45, 0.00, 0.40],
|
|
[4.65, 0.00, 0.45],
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
clusters = _geometry_clusters(
|
|
points_map=points,
|
|
point_class=np.full(8, 2, dtype=np.uint8),
|
|
point_height_m=np.linspace(0.3, 0.45, 8).astype(np.float32),
|
|
sensor_position_map=np.zeros(3, dtype=np.float64),
|
|
claimed_source_indices={0},
|
|
profile=CameraGeometryFusionProfile(),
|
|
)
|
|
assert len(clusters) == 1
|
|
assert clusters[0]["geometry_status"] == "single-source-geometry"
|
|
assert clusters[0]["semantic_class"] is None
|
|
assert clusters[0]["point_count"] == 4
|
|
assert clusters[0]["unknown_is_occupied"] is True
|
|
|
|
|
|
def test_profile_rejects_free_space_shortcuts() -> None:
|
|
with pytest.raises(SemanticGeometryFusionError):
|
|
CameraGeometryFusionProfile(
|
|
geometry_minimum_cluster_points=0,
|
|
)
|