288 lines
11 KiB
Python
288 lines
11 KiB
Python
from __future__ import annotations
|
|
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
|
Kb4ProjectionProfile as HistoricalProjectionProfile,
|
|
)
|
|
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
|
project_map_points_kb4 as historical_project,
|
|
)
|
|
from k1link.perception.contracts import (
|
|
BoundingRegion2D,
|
|
ClockBasis,
|
|
EvidenceBasis,
|
|
EvidenceCurrentness,
|
|
ModalityOutcome,
|
|
ModalityStatus,
|
|
ObjectProposal2D,
|
|
SourceEnvelope,
|
|
TimestampBundle,
|
|
validate_exclusive_point_ownership,
|
|
)
|
|
from k1link.perception.geometry import (
|
|
GeometryFrame,
|
|
GeometryProviderError,
|
|
Ravnoves00GeometryAssociationProvider,
|
|
RecordedGeometryStore,
|
|
load_geometry_profile,
|
|
)
|
|
from k1link.perception.geometry_math import (
|
|
Kb4ProjectionProfile,
|
|
project_map_points_kb4,
|
|
)
|
|
from k1link.perception.providers import SourcePacket
|
|
from k1link.perception.recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference
|
|
|
|
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
|
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-geometry-association-v1.json"
|
|
|
|
|
|
class _Store:
|
|
def __init__(self, frame: GeometryFrame | None) -> None:
|
|
self.profile = load_geometry_profile(PROFILE_PATH)
|
|
self._frame = frame
|
|
|
|
def frame(self, packet: SourcePacket) -> GeometryFrame | None:
|
|
return self._frame
|
|
|
|
|
|
def _status(
|
|
available: bool = True,
|
|
outcome: ModalityOutcome = ModalityOutcome.AVAILABLE,
|
|
) -> ModalityStatus:
|
|
return ModalityStatus(available, outcome, f"test-{outcome.value}")
|
|
|
|
|
|
def _packet(*, available: bool = True) -> SourcePacket:
|
|
status = _status() if available else _status(False, ModalityOutcome.UNAVAILABLE)
|
|
reference = RecordedFrameReference(RECORDED_SOURCE_PACK_ID, 0) if available else None
|
|
return SourcePacket(
|
|
envelope=SourceEnvelope(
|
|
source_id="RAVNOVES00",
|
|
session_id="20260720T065719Z_viewer_live",
|
|
frame_id="frame-000000",
|
|
sequence=0,
|
|
timestamps=TimestampBundle(
|
|
utc_ns=1,
|
|
monotonic_ns=2,
|
|
source_ns=3,
|
|
clock_basis=ClockBasis.RECORDED_HOST,
|
|
),
|
|
source_age_ns=0,
|
|
binding_reason="test-recorded-source",
|
|
calibration_id="camera-1-kb4-test",
|
|
representation_id="registered-map-increment-v1",
|
|
image=_status(),
|
|
registered_point_increment=status,
|
|
pose=status,
|
|
),
|
|
image_payload="image",
|
|
registered_point_increment_payload=reference,
|
|
pose_payload=reference,
|
|
)
|
|
|
|
|
|
def _proposal(
|
|
proposal_id: str,
|
|
region: tuple[float, float, float, float],
|
|
*,
|
|
score: float = 0.8,
|
|
hint: str | None = "person",
|
|
) -> ObjectProposal2D:
|
|
return ObjectProposal2D(
|
|
proposal_id=proposal_id,
|
|
source_id="RAVNOVES00",
|
|
frame_id="frame-000000",
|
|
region=BoundingRegion2D(*region),
|
|
objectness=score,
|
|
provider_id="test-detector/v1",
|
|
model_id="test-model/v1",
|
|
preprocess_id="test-preprocess/v1",
|
|
semantic_hint=hint,
|
|
)
|
|
|
|
|
|
def _point_for_pixel(u: float, *, z: float = 5.0) -> tuple[float, float, float]:
|
|
theta = (u - 50.0) / 100.0
|
|
return (float(np.tan(theta) * z), 0.0, z)
|
|
|
|
|
|
def _frame() -> GeometryFrame:
|
|
semantic = (
|
|
_point_for_pixel(39.5),
|
|
_point_for_pixel(40.5),
|
|
)
|
|
geometry_only = (
|
|
_point_for_pixel(76.0, z=4.0),
|
|
_point_for_pixel(80.0, z=4.0),
|
|
_point_for_pixel(84.0, z=4.0),
|
|
_point_for_pixel(88.0, z=4.0),
|
|
)
|
|
points = np.asarray((*semantic, *geometry_only), dtype=np.float64)
|
|
return GeometryFrame(
|
|
frame_index=0,
|
|
points_map=points,
|
|
point_class=np.full(points.shape[0], 2, dtype=np.uint8),
|
|
sensor_position_map=np.zeros(3, dtype=np.float64),
|
|
sensor_orientation_xyzw=np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float64),
|
|
projection=Kb4ProjectionProfile(
|
|
width=100,
|
|
height=100,
|
|
intrinsic_fx_fy_cx_cy=(100.0, 100.0, 50.0, 50.0),
|
|
distortion_kb4=(0.0, 0.0, 0.0, 0.0),
|
|
t_camera_from_lidar=np.eye(4, dtype=np.float64),
|
|
),
|
|
surface_valid=True,
|
|
)
|
|
|
|
|
|
def test_profile_is_strict_digest_bound_and_store_accepts_exact_evidence() -> None:
|
|
profile = load_geometry_profile(PROFILE_PATH)
|
|
store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT, profile=profile)
|
|
|
|
assert profile.provider_id == "ravnoves00-geometry-association/v1"
|
|
assert len(profile.profile_sha256) == 64
|
|
assert store.profile.source_pack_sha256 == (
|
|
"0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944"
|
|
)
|
|
assert store.profile.local_surface_sha256 == (
|
|
"f57eb2485b6cef47f2a97a2d9ff1aa9fd9265fe1eb69cd5852d12f39e13b8bc6"
|
|
)
|
|
step_candidates = store.point_step_candidates_for_frame(0)
|
|
assert step_candidates is not None
|
|
assert step_candidates.shape == (2389,)
|
|
assert step_candidates.dtype == np.uint8
|
|
assert step_candidates.flags.writeable is False
|
|
with pytest.raises(ValueError):
|
|
step_candidates[0] = 0
|
|
with pytest.raises(GeometryProviderError, match="frame index"):
|
|
store.point_step_candidates_for_frame(True)
|
|
|
|
|
|
def test_provider_arbitrates_points_and_publishes_classless_geometry_only() -> None:
|
|
provider = Ravnoves00GeometryAssociationProvider(store=_Store(_frame())) # type: ignore[arg-type]
|
|
proposals = (
|
|
_proposal("proposal-small", (35.0, 45.0, 45.0, 55.0)),
|
|
_proposal("proposal-large", (30.0, 40.0, 50.0, 60.0), score=0.99),
|
|
)
|
|
|
|
observations = provider.associate(_packet(), proposals)
|
|
|
|
validate_exclusive_point_ownership(observations)
|
|
small = next(item for item in observations if item.proposal_ids == ("proposal-small",))
|
|
large = next(item for item in observations if item.proposal_ids == ("proposal-large",))
|
|
geometry = [item for item in observations if not item.proposal_ids]
|
|
assert small.basis is EvidenceBasis.FUSED
|
|
assert small.metric_geometry is not None
|
|
assert small.source_point_ids == (0, 1)
|
|
assert large.basis is EvidenceBasis.CAMERA
|
|
assert large.metric_geometry is None
|
|
assert "point-ownership-collision-range-withheld" in large.reason_codes
|
|
assert geometry
|
|
assert all(item.basis is EvidenceBasis.LIDAR for item in geometry)
|
|
assert all(item.semantic_hint is None for item in geometry)
|
|
assert all(item.metric_geometry is not None for item in geometry)
|
|
|
|
snapshot = provider.snapshot()
|
|
assert snapshot.proposal_count == 2
|
|
assert snapshot.eligible_proposal_count == 2
|
|
assert snapshot.ranged_proposal_count == 1
|
|
assert snapshot.total_range_coverage == pytest.approx(0.5)
|
|
assert snapshot.eligible_range_coverage == pytest.approx(0.5)
|
|
assert snapshot.overlapping_claims_removed == 2
|
|
|
|
|
|
def test_unavailable_source_never_publishes_metric_or_free_space() -> None:
|
|
provider = Ravnoves00GeometryAssociationProvider(store=_Store(None)) # type: ignore[arg-type]
|
|
|
|
observations = provider.associate(
|
|
_packet(available=False),
|
|
(_proposal("proposal-0", (10.0, 10.0, 20.0, 20.0)),),
|
|
)
|
|
|
|
assert len(observations) == 1
|
|
assert observations[0].basis is EvidenceBasis.CAMERA
|
|
assert observations[0].currentness is EvidenceCurrentness.UNAVAILABLE
|
|
assert observations[0].metric_geometry is None
|
|
assert observations[0].source_point_ids == ()
|
|
assert observations[0].occupied_support is False
|
|
snapshot = provider.snapshot()
|
|
assert snapshot.unavailable_proposal_count == 1
|
|
assert snapshot.eligible_proposal_count == 0
|
|
|
|
|
|
def test_semantic_hint_does_not_change_geometry_or_range() -> None:
|
|
first = Ravnoves00GeometryAssociationProvider(store=_Store(_frame())) # type: ignore[arg-type]
|
|
second = Ravnoves00GeometryAssociationProvider(store=_Store(_frame())) # type: ignore[arg-type]
|
|
region = (35.0, 45.0, 45.0, 55.0)
|
|
|
|
left = first.associate(_packet(), (_proposal("proposal-0", region, hint="person"),))[0]
|
|
right = second.associate(_packet(), (_proposal("proposal-0", region, hint="truck"),))[0]
|
|
|
|
assert left.source_point_ids == right.source_point_ids
|
|
assert left.metric_geometry == right.metric_geometry
|
|
assert left.semantic_hint == "person"
|
|
assert right.semantic_hint == "truck"
|
|
|
|
|
|
def test_product_projection_is_numerically_identical_to_accepted_e29_primitive() -> None:
|
|
store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT)
|
|
source = store._source # noqa: SLF001 - parity test over the sealed artifact
|
|
offsets = source["cloud_offsets"]
|
|
frame_index = 5
|
|
points = np.asarray(
|
|
source["cloud_points_map"][int(offsets[frame_index]) : int(offsets[frame_index + 1])],
|
|
dtype=np.float64,
|
|
)
|
|
position = np.asarray(source["pose_positions_map"][frame_index], dtype=np.float64)
|
|
orientation = np.asarray(
|
|
source["pose_quaternions_map_from_lidar"][frame_index],
|
|
dtype=np.float64,
|
|
)
|
|
product_profile = store._projection # noqa: SLF001 - exact projection identity
|
|
historical_profile = HistoricalProjectionProfile(
|
|
source_id="sensor.camera.right",
|
|
calibration_slot="camera_1",
|
|
width=product_profile.width,
|
|
height=product_profile.height,
|
|
intrinsic_fx_fy_cx_cy=product_profile.intrinsic_fx_fy_cx_cy,
|
|
distortion_kb4=product_profile.distortion_kb4,
|
|
t_camera_from_lidar=product_profile.t_camera_from_lidar,
|
|
)
|
|
|
|
product = project_map_points_kb4(
|
|
points,
|
|
position_map_xyz=position,
|
|
orientation_map_from_lidar_xyzw=orientation,
|
|
profile=product_profile,
|
|
)
|
|
historical = historical_project(
|
|
points,
|
|
position_map_xyz=tuple(float(value) for value in position),
|
|
orientation_map_from_lidar_xyzw=tuple(float(value) for value in orientation),
|
|
profile=historical_profile,
|
|
)
|
|
|
|
np.testing.assert_array_equal(product.source_indices, historical.source_indices)
|
|
np.testing.assert_allclose(product.pixels_xy, historical.pixels_xy, rtol=0.0, atol=1e-12)
|
|
np.testing.assert_allclose(product.depths_m, historical.depths_m, rtol=0.0, atol=1e-12)
|
|
|
|
|
|
def test_store_rejects_a_wrong_source_digest(tmp_path: Path) -> None:
|
|
profile = load_geometry_profile(PROFILE_PATH)
|
|
source = tmp_path / "lidar-pack.npz"
|
|
source.write_bytes(b"not-the-source")
|
|
surface = tmp_path / "local-surface.npz"
|
|
surface.write_bytes(b"not-the-surface")
|
|
|
|
with pytest.raises(GeometryProviderError, match="source pack digest changed"):
|
|
RecordedGeometryStore(
|
|
source_pack_path=source,
|
|
local_surface_path=surface,
|
|
profile=profile,
|
|
)
|