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, )