feat(perception): canonicalize metric geometry
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,278 @@
|
||||
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"
|
||||
)
|
||||
|
||||
|
||||
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,
|
||||
)
|
||||
@@ -0,0 +1,66 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from k1link.perception.geometry_replay import read_geometry_replay_result
|
||||
|
||||
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
||||
RESULT_ROOT = (
|
||||
REPOSITORY_ROOT
|
||||
/ ".runtime/perception-m4/geometry-results"
|
||||
/ "m4-geometry-replay-8daf3109e3cf30b960b4b376032ff3b5ec58ca42a1e5899b841cf29fbcf14ad8"
|
||||
)
|
||||
|
||||
|
||||
def test_full_source_geometry_result_closes_m4_4_contract() -> None:
|
||||
result = read_geometry_replay_result(RESULT_ROOT)
|
||||
|
||||
assert result.accepted is True
|
||||
assert result.metrics["frames"] == {
|
||||
"failed": 0,
|
||||
"source_available": 3928,
|
||||
"source_unavailable": 561,
|
||||
"total": 4489,
|
||||
}
|
||||
proposals = result.metrics["proposals"]
|
||||
assert isinstance(proposals, dict)
|
||||
assert proposals["total"] == 15499
|
||||
assert proposals["with_range"] == 5341
|
||||
assert proposals["eligible_for_range"] == 13298
|
||||
assert proposals["ownership_collision"] == 146
|
||||
assert result.metrics["geometry_only_observations"] == 21958
|
||||
assert result.metrics["published_source_point_rows"] == 2164767
|
||||
|
||||
|
||||
def test_geometry_result_binds_the_accepted_m4_3_e32_and_e53_evidence() -> None:
|
||||
result = read_geometry_replay_result(RESULT_ROOT)
|
||||
identity = result.manifest["identity"]
|
||||
assert isinstance(identity, dict)
|
||||
|
||||
assert identity["detector_result_id"] == (
|
||||
"m4-detector-replay-"
|
||||
"11f83f2e0b81758ac2a5a5fc54e9d293b501678df5f6ef97b5c6069ba08605c5"
|
||||
)
|
||||
assert identity["geometry_profile_sha256"] == (
|
||||
"420d989aab5918e0f98e3439cadb8b7251332d51b48f4f2c25d77a385bea49f8"
|
||||
)
|
||||
assert identity["historical_references"] == {
|
||||
"e32": {
|
||||
"manifest_sha256": (
|
||||
"f4b57c9f7619c43414adf1d10488b05df466226a523a0c001671102ced0e9ab8"
|
||||
),
|
||||
"result_id": (
|
||||
"e32-track-geometry-"
|
||||
"a14ca0e7fb3850ca0dfa3c41634e1b490a2d58ab74d101afc6d6921fbdb0e6fd"
|
||||
),
|
||||
},
|
||||
"e53": {
|
||||
"manifest_sha256": (
|
||||
"fc5b4ae69aae0098b7075c539d25b563dff1bbb09209a49030edda6cba9ee544"
|
||||
),
|
||||
"result_id": (
|
||||
"e53-camera-first-shadow-"
|
||||
"e6f03cf8bfb15db86100239b060e13f914532618b7b99e811866e4e6a555186c"
|
||||
),
|
||||
},
|
||||
}
|
||||
@@ -33,6 +33,15 @@ DETECTOR_RUNTIME_MODULES = (
|
||||
"recorded_source.py",
|
||||
"yolox_object_detector.py",
|
||||
)
|
||||
GEOMETRY_RUNTIME_MODULES = (
|
||||
"contracts.py",
|
||||
"geometry.py",
|
||||
"geometry_math.py",
|
||||
"geometry_replay.py",
|
||||
"geometry_replay_cli.py",
|
||||
"providers.py",
|
||||
"recorded_source.py",
|
||||
)
|
||||
|
||||
|
||||
def _imports(path: Path) -> set[str]:
|
||||
@@ -181,6 +190,18 @@ def test_detector_runtime_closure_imports_no_legacy_compute_package() -> None:
|
||||
assert {name: modules for name, modules in violations.items() if modules} == {}
|
||||
|
||||
|
||||
def test_geometry_runtime_closure_imports_no_legacy_compute_or_device_package() -> None:
|
||||
violations = {
|
||||
name: sorted(
|
||||
module
|
||||
for module in _imports(PERCEPTION_ROOT / name)
|
||||
if module.startswith(("k1link.compute", "k1link.device_plugins"))
|
||||
)
|
||||
for name in GEOMETRY_RUNTIME_MODULES
|
||||
}
|
||||
assert {name: modules for name, modules in violations.items() if modules} == {}
|
||||
|
||||
|
||||
def test_new_perception_boundary_has_no_experiment_specific_imports() -> None:
|
||||
violations: dict[str, str] = {}
|
||||
for path in PERCEPTION_ROOT.glob("*.py"):
|
||||
|
||||
Reference in New Issue
Block a user