50 lines
1.5 KiB
Python
50 lines
1.5 KiB
Python
from __future__ import annotations
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from k1link.compute.l31_pointpillars_ravnoves import (
|
|
L31PointPillarsRavnovesError,
|
|
nearest_pose_indices,
|
|
select_visual_frame_indices,
|
|
sensor_frame_xyzi,
|
|
)
|
|
|
|
|
|
def test_nearest_pose_binding_prefers_earlier_pose_on_tie() -> None:
|
|
indices, age_ms = nearest_pose_indices(
|
|
np.asarray([10, 20, 31], dtype=np.int64) * 1_000_000,
|
|
np.asarray([5, 15, 30], dtype=np.int64) * 1_000_000,
|
|
)
|
|
assert indices.tolist() == [0, 1, 2]
|
|
assert age_ms.tolist() == [5.0, 5.0, 1.0]
|
|
|
|
|
|
def test_sensor_frame_xyzi_inverts_map_pose() -> None:
|
|
points = np.asarray([[11.0, 20.0, 30.0], [10.0, 22.0, 30.0]])
|
|
intensity = np.asarray([255, 0], dtype=np.uint8)
|
|
result = sensor_frame_xyzi(
|
|
points,
|
|
intensity,
|
|
position_map_xyz=np.asarray([10.0, 20.0, 30.0]),
|
|
orientation_map_from_lidar_xyzw=np.asarray([0.0, 0.0, 0.0, 1.0]),
|
|
)
|
|
np.testing.assert_allclose(
|
|
result,
|
|
np.asarray([[1.0, 0.0, 0.0, 1.0], [0.0, 2.0, 0.0, 0.0]]),
|
|
)
|
|
|
|
|
|
def test_visual_selection_prefers_vehicles_with_route_coverage() -> None:
|
|
selected = select_visual_frame_indices(
|
|
[0, 3, 1, 0, 0, 2],
|
|
[5, 3, 9, 0, 4, 2],
|
|
maximum_frames=3,
|
|
)
|
|
assert selected == (1, 2, 5)
|
|
|
|
|
|
def test_visual_selection_rejects_impossible_class_count() -> None:
|
|
with pytest.raises(L31PointPillarsRavnovesError):
|
|
select_visual_frame_indices([2], [1])
|