diff --git a/config/deployment/mission-core-worker-shadow-v1.template.json b/config/deployment/mission-core-worker-shadow-v1.template.json index 9769a5d..cffb7e5 100644 --- a/config/deployment/mission-core-worker-shadow-v1.template.json +++ b/config/deployment/mission-core-worker-shadow-v1.template.json @@ -139,7 +139,7 @@ }, "wheel": { "name": "nodedc_mission_core-0.1.0-py3-none-any.whl", - "sha256": "4158784fd40b70c7213b629ed39d664fd2f12eab1c9242ec9a6ebbb366e3dd5a" + "sha256": "ecbbfeee7ea62a7f5f5368efccf5d254abb17b4801a29538e3bc7aa3c8c3a40a" } }, "rollback": { diff --git a/config/perception/m4-geometry-association-v1.json b/config/perception/m4-geometry-association-v1.json new file mode 100644 index 0000000..3feaa3b --- /dev/null +++ b/config/perception/m4-geometry-association-v1.json @@ -0,0 +1,59 @@ +{ + "schema_version": "missioncore.geometry-association-profile/v1", + "profile_id": "m4-ravnoves00-e29-e32-geometry/v1", + "provider_id": "ravnoves00-geometry-association/v1", + "source": { + "source_id": "RAVNOVES00", + "session_id": "20260720T065719Z_viewer_live", + "source_pack_id": "e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b", + "source_pack_sha256": "0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944", + "frame_count": 4489, + "point_count": 9207270 + }, + "local_surface": { + "model_id": "k1-local-surface-23762244c8bdb97de26fb721ac957d7a00bc9a63571ac4cfa4be19c4effc7d55", + "artifact_sha256": "f57eb2485b6cef47f2a97a2d9ff1aa9fd9265fe1eb69cd5852d12f39e13b8bc6", + "valid_frame_count": 3928 + }, + "projection": { + "model": "KB4", + "width": 800, + "height": 600, + "coordinate_frame": "map" + }, + "association": { + "bbox_inset_fraction": 0.03, + "depth_cluster_minimum_gap_m": 0.45, + "depth_cluster_gap_fraction": 0.08, + "spatial_cluster_radius_m": 0.6, + "semantic_minimum_occupied_points": 2, + "semantic_minimum_occupied_voxels": 1, + "semantic_voxel_size_m": 0.35, + "conflict_minimum_classified_points": 6, + "conflict_surface_fraction": 0.8, + "geometry_local_radius_m": 10.0, + "geometry_voxel_size_m": 0.45, + "geometry_minimum_cluster_points": 4, + "geometry_minimum_cluster_voxels": 2, + "maximum_geometry_clusters_per_frame": 64 + }, + "policy": { + "camera_owns_semantic_hint": true, + "geometry_can_invent_semantic_class": false, + "geometry_only_range_estimator": "nearest-euclidean-sensor-distance/v1", + "one_owner_per_source_point": true, + "absence_of_points_means_free": false, + "overlap_eligibility": "bbox-intersects-current-projected-point-extent/v1", + "point_ownership_priority": "smallest-bbox-then-score-then-proposal-id/v1", + "proposal_range_estimator": "median-camera-z-of-owned-current-points/v1", + "unknown_is_occupied": true, + "threshold_tuning_allowed": false + }, + "authority": { + "ground_truth": false, + "physical_live": false, + "commands_enabled": false, + "actuation_allowed": false, + "navigation_or_safety_accepted": false + } +} diff --git a/config/perception/m4-reuse-inventory-v1.json b/config/perception/m4-reuse-inventory-v1.json index 93d2805..1510cb5 100644 --- a/config/perception/m4-reuse-inventory-v1.json +++ b/config/perception/m4-reuse-inventory-v1.json @@ -10,7 +10,7 @@ { "module": "k1link.compute.semantic_geometry_fusion", "role": "camera and registered geometry association", - "admission": "adapt-behind-provider" + "admission": "reference-only-extracted-to-product" }, { "module": "k1link.compute.track_geometry", @@ -31,6 +31,16 @@ "module": "k1link.perception.yolox_object_detector", "role": "product-owned frozen YOLOX preprocess, Triton transport and postprocess", "admission": "product-owned" + }, + { + "module": "k1link.perception.geometry_math", + "role": "product-owned KB4 projection and occupied geometry association with E29 parity", + "admission": "product-owned" + }, + { + "module": "k1link.perception.geometry", + "role": "digest-bound GeometryAssociationProvider and exact point ownership", + "admission": "product-owned" } ], "historical_wrappers": [ diff --git a/scripts/build_m4_worker_shadow_artifact.py b/scripts/build_m4_worker_shadow_artifact.py index 4d91376..607be6b 100644 --- a/scripts/build_m4_worker_shadow_artifact.py +++ b/scripts/build_m4_worker_shadow_artifact.py @@ -25,7 +25,7 @@ WHEEL_NAME = "nodedc_mission_core-0.1.0-py3-none-any.whl" RUNNER_NAME = RUNNER.name PATCH_ID = re.compile(r"^[A-Za-z0-9._-]{1,96}$") EXPECTED_BASELINE_SHA256 = "ea10359339e6cce31b5780a2710299771cab7cc0c1c2a2b56a1621f786b31fa8" -EXPECTED_WHEEL_SHA256 = "4158784fd40b70c7213b629ed39d664fd2f12eab1c9242ec9a6ebbb366e3dd5a" +EXPECTED_WHEEL_SHA256 = "ecbbfeee7ea62a7f5f5368efccf5d254abb17b4801a29538e3bc7aa3c8c3a40a" PAYLOAD_FILES = ( RUNNER_NAME, WHEEL_NAME, diff --git a/src/k1link/perception/geometry.py b/src/k1link/perception/geometry.py new file mode 100644 index 0000000..44d7314 --- /dev/null +++ b/src/k1link/perception/geometry.py @@ -0,0 +1,877 @@ +"""Digest-bound RAVNOVES00 geometry provider for the M4 product graph.""" + +from __future__ import annotations + +import hashlib +import json +import math +import time +from collections.abc import Callable, Mapping +from dataclasses import dataclass +from pathlib import Path +from threading import Lock +from typing import Final + +import numpy as np +import numpy.typing as npt + +from .contracts import ( + EvidenceBasis, + EvidenceCurrentness, + MetricGeometry, + ModalityOutcome, + ObjectProposal2D, + ObstacleObservation, + validate_exclusive_point_ownership, +) +from .geometry_math import ( + GeometryAssociationProfile, + Kb4ProjectionProfile, + ProjectedPointCloud, + SemanticGeometrySupport, + geometry_only_clusters, + project_map_points_kb4, + semantic_geometry_support, +) +from .providers import SourcePacket +from .recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference + +GEOMETRY_PROFILE_SCHEMA: Final = "missioncore.geometry-association-profile/v1" +GEOMETRY_PROVIDER_ID: Final = "ravnoves00-geometry-association/v1" +DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path( + "config/perception/m4-geometry-association-v1.json" +) + +FloatArray = npt.NDArray[np.float64] +UInt8Array = npt.NDArray[np.uint8] + + +class GeometryProviderError(RuntimeError): + """The geometry profile, evidence source or association is incompatible.""" + + +@dataclass(frozen=True, slots=True) +class GeometryProfile: + profile_id: str + provider_id: str + source_id: str + session_id: str + source_pack_id: str + source_pack_sha256: str + frame_count: int + point_count: int + local_surface_model_id: str + local_surface_sha256: str + valid_frame_count: int + width: int + height: int + coordinate_frame: str + association: GeometryAssociationProfile + profile_sha256: str + + +@dataclass(frozen=True, slots=True) +class GeometryFrame: + frame_index: int + points_map: FloatArray + point_class: UInt8Array + sensor_position_map: FloatArray + sensor_orientation_xyzw: FloatArray + projection: Kb4ProjectionProfile + surface_valid: bool + + @property + def source_point_count(self) -> int: + return int(self.points_map.shape[0]) + + +@dataclass(frozen=True, slots=True) +class GeometryProviderSnapshot: + input_frames: int + completed_frames: int + failed_frames: int + proposal_count: int + eligible_proposal_count: int + ranged_proposal_count: int + camera_only_proposal_count: int + conflict_proposal_count: int + unavailable_proposal_count: int + outside_overlap_proposal_count: int + sparse_proposal_count: int + ownership_collision_proposal_count: int + geometry_only_observation_count: int + published_source_point_count: int + overlapping_claims_removed: int + core_duration_ns: int + + @property + def total_range_coverage(self) -> float: + return self.ranged_proposal_count / self.proposal_count if self.proposal_count else 0.0 + + @property + def eligible_range_coverage(self) -> float: + if not self.eligible_proposal_count: + return 0.0 + return self.ranged_proposal_count / self.eligible_proposal_count + + +class RecordedGeometryStore: + """Verified source-pack and local-surface arrays used by one provider instance.""" + + def __init__( + self, + *, + source_pack_path: Path, + local_surface_path: Path, + profile: GeometryProfile, + ) -> None: + self.source_pack_path = source_pack_path.resolve(strict=True) + self.local_surface_path = local_surface_path.resolve(strict=True) + self.profile = profile + _verify_regular_file( + self.source_pack_path, + expected_sha256=profile.source_pack_sha256, + label="source pack", + ) + _verify_regular_file( + self.local_surface_path, + expected_sha256=profile.local_surface_sha256, + label="local surface", + ) + self._source = _load_npz(self.source_pack_path, "source pack") + self._surface = _load_npz(self.local_surface_path, "local surface") + self._validate() + intrinsic = self._source["intrinsic_fx_fy_cx_cy"] + distortion = self._source["distortion_kb4"] + self._projection = Kb4ProjectionProfile( + width=profile.width, + height=profile.height, + intrinsic_fx_fy_cx_cy=( + float(intrinsic[0]), + float(intrinsic[1]), + float(intrinsic[2]), + float(intrinsic[3]), + ), + distortion_kb4=( + float(distortion[0]), + float(distortion[1]), + float(distortion[2]), + float(distortion[3]), + ), + t_camera_from_lidar=np.asarray( + self._source["t_camera_from_lidar"], + dtype=np.float64, + ), + ) + + @classmethod + def from_repository( + cls, + repository_root: Path, + *, + profile: GeometryProfile | None = None, + ) -> RecordedGeometryStore: + root = repository_root.resolve() + selected = profile or load_geometry_profile(root / DEFAULT_GEOMETRY_PROFILE_PATH) + source_pack = ( + root + / ".runtime/compute-experiments/e10/lidar-packs" + / selected.source_pack_id + / "lidar-pack.npz" + ) + local_surface = ( + root + / ".runtime/compute-experiments/k1-local-surface-v1/models" + / selected.local_surface_model_id + / "local-surface.npz" + ) + return cls( + source_pack_path=source_pack, + local_surface_path=local_surface, + profile=selected, + ) + + def frame(self, packet: SourcePacket) -> GeometryFrame | None: + envelope = packet.envelope + if envelope.source_id != self.profile.source_id: + raise GeometryProviderError("packet source escaped the geometry profile") + if envelope.session_id != self.profile.session_id: + raise GeometryProviderError("packet session escaped the geometry profile") + if not envelope.registered_point_increment.available: + return None + point_reference = packet.registered_point_increment_payload + pose_reference = packet.pose_payload + if ( + not isinstance(point_reference, RecordedFrameReference) + or not isinstance(pose_reference, RecordedFrameReference) + or point_reference.artifact_id != self.profile.source_pack_id + or pose_reference.artifact_id != self.profile.source_pack_id + or point_reference.frame_index != envelope.sequence + or pose_reference.frame_index != envelope.sequence + ): + raise GeometryProviderError("packet geometry references are not source-bound") + frame_index = envelope.sequence + if not 0 <= frame_index < self.profile.frame_count: + raise GeometryProviderError("packet geometry frame index is outside the profile") + if int(self._source["frame_indices"][frame_index]) != frame_index: + raise GeometryProviderError("source pack frame sequence changed") + if not bool(self._source["sample_available"][frame_index]): + raise GeometryProviderError("packet claims unavailable source geometry as current") + offsets = self._source["cloud_offsets"] + start, end = int(offsets[frame_index]), int(offsets[frame_index + 1]) + return GeometryFrame( + frame_index=frame_index, + points_map=np.asarray(self._source["cloud_points_map"][start:end], dtype=np.float64), + point_class=np.asarray(self._surface["point_class"][start:end], dtype=np.uint8), + sensor_position_map=np.asarray( + self._source["pose_positions_map"][frame_index], + dtype=np.float64, + ), + sensor_orientation_xyzw=np.asarray( + self._source["pose_quaternions_map_from_lidar"][frame_index], + dtype=np.float64, + ), + projection=self._projection, + surface_valid=bool(self._surface["frame_valid"][frame_index]), + ) + + def _validate(self) -> None: + source_required = { + "frame_indices", + "sample_available", + "cloud_offsets", + "cloud_points_map", + "pose_positions_map", + "pose_quaternions_map_from_lidar", + "intrinsic_fx_fy_cx_cy", + "distortion_kb4", + "t_camera_from_lidar", + } + surface_required = {"frame_valid", "point_class"} + if not source_required.issubset(self._source): + raise GeometryProviderError("source pack arrays are incomplete") + if not surface_required.issubset(self._surface): + raise GeometryProviderError("local surface arrays are incomplete") + frames = self.profile.frame_count + points = self.profile.point_count + shapes = { + "frame_indices": (frames,), + "sample_available": (frames,), + "cloud_offsets": (frames + 1,), + "cloud_points_map": (points, 3), + "pose_positions_map": (frames, 3), + "pose_quaternions_map_from_lidar": (frames, 4), + "intrinsic_fx_fy_cx_cy": (4,), + "distortion_kb4": (4,), + "t_camera_from_lidar": (4, 4), + } + if any(self._source[name].shape != shape for name, shape in shapes.items()): + raise GeometryProviderError("source pack array shapes changed") + if self._surface["frame_valid"].shape != (frames,): + raise GeometryProviderError("local surface frame shape changed") + if self._surface["point_class"].shape != (points,): + raise GeometryProviderError("local surface point shape changed") + if int(self._source["cloud_offsets"][-1]) != points: + raise GeometryProviderError("source point offsets do not close") + if ( + int(np.count_nonzero(self._source["sample_available"])) + != self.profile.valid_frame_count + ): + raise GeometryProviderError("source availability accounting changed") + if int(np.count_nonzero(self._surface["frame_valid"])) != self.profile.valid_frame_count: + raise GeometryProviderError("local surface validity accounting changed") + if not np.array_equal( + self._surface["frame_valid"], + self._source["sample_available"], + ): + raise GeometryProviderError("source and local surface availability disagree") + point_class = np.asarray(self._surface["point_class"], dtype=np.uint8) + if np.any(point_class > 3): + raise GeometryProviderError("local surface point classification changed") + + +class Ravnoves00GeometryAssociationProvider: + """Associate proposals with exact current points and retain unknown occupancy.""" + + provider_id: str = GEOMETRY_PROVIDER_ID + + def __init__( + self, + *, + store: RecordedGeometryStore, + clock_ns: Callable[[], int] = time.perf_counter_ns, + ) -> None: + if store.profile.provider_id != self.provider_id: + raise GeometryProviderError("geometry profile provider identity changed") + self.store = store + self.profile = store.profile + self._clock_ns = clock_ns + self._lock = Lock() + self._input_frames = 0 + self._completed_frames = 0 + self._failed_frames = 0 + self._proposal_count = 0 + self._eligible = 0 + self._ranged = 0 + self._camera_only = 0 + self._conflict = 0 + self._unavailable = 0 + self._outside = 0 + self._sparse = 0 + self._ownership_collision = 0 + self._geometry_only = 0 + self._published_points = 0 + self._overlap_removed = 0 + self._core_duration_ns = 0 + + def associate( + self, + packet: SourcePacket, + proposals: tuple[ObjectProposal2D, ...], + ) -> tuple[ObstacleObservation, ...]: + with self._lock: + self._input_frames += 1 + self._proposal_count += len(proposals) + started = int(self._clock_ns()) + try: + self._validate_proposals(packet, proposals) + frame = self.store.frame(packet) + if frame is None or not frame.surface_valid: + result = tuple( + _camera_unavailable_observation(packet, proposal, frame is None) + for proposal in proposals + ) + self._record_unavailable(len(proposals)) + else: + result, metrics = self._associate_current(packet, proposals, frame) + self._record_current(metrics) + validate_exclusive_point_ownership(result) + except Exception: + with self._lock: + self._failed_frames += 1 + self._core_duration_ns += max(0, int(self._clock_ns()) - started) + raise + with self._lock: + self._completed_frames += 1 + self._core_duration_ns += max(0, int(self._clock_ns()) - started) + return result + + def _associate_current( + self, + packet: SourcePacket, + proposals: tuple[ObjectProposal2D, ...], + frame: GeometryFrame, + ) -> tuple[tuple[ObstacleObservation, ...], dict[str, int]]: + projected = project_map_points_kb4( + frame.points_map, + position_map_xyz=frame.sensor_position_map, + orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw, + profile=frame.projection, + ) + supports = tuple( + semantic_geometry_support( + proposal.region.as_tuple(), + projected=projected, + frame_points_map=frame.points_map, + point_class=frame.point_class, + profile=self.profile.association, + ) + for proposal in proposals + ) + allocations, overlap_removed = _allocate_point_ownership(proposals, supports) + observations: list[ObstacleObservation] = [] + metrics = { + "eligible": 0, + "ranged": 0, + "camera_only": 0, + "conflict": 0, + "outside": 0, + "sparse": 0, + "ownership_collision": 0, + "geometry_only": 0, + "published_points": 0, + "overlap_removed": overlap_removed, + } + claimed: set[int] = set() + for index, (proposal, support) in enumerate(zip(proposals, supports, strict=True)): + owned = allocations.get(index, np.empty(0, dtype=np.int64)) + observation = _proposal_observation( + packet, + proposal, + support=support, + owned_source_indices=owned, + frame=frame, + projected=projected, + coordinate_frame=self.profile.coordinate_frame, + ) + observations.append(observation) + metrics["eligible"] += support.overlaps_projected_extent + if observation.metric_geometry is not None: + metrics["ranged"] += 1 + metrics["published_points"] += len(observation.source_point_ids) + claimed.update(observation.source_point_ids) + elif observation.basis is EvidenceBasis.CONFLICT: + metrics["conflict"] += 1 + else: + metrics["camera_only"] += 1 + if not support.overlaps_projected_extent: + metrics["outside"] += 1 + elif "point-ownership-collision-range-withheld" in observation.reason_codes: + metrics["ownership_collision"] += 1 + else: + metrics["sparse"] += 1 + clusters = geometry_only_clusters( + points_map=frame.points_map, + point_class=frame.point_class, + sensor_position_map=frame.sensor_position_map, + claimed_source_indices=frozenset(claimed), + profile=self.profile.association, + ) + for cluster_index, cluster in enumerate(clusters): + point_ids = tuple(sorted(int(value) for value in cluster.source_indices)) + observations.append( + ObstacleObservation( + observation_id=f"{packet.envelope.frame_id}:geometry:{cluster_index}", + occupancy_key=f"{packet.envelope.frame_id}:geometry:{cluster_index}", + source_id=packet.envelope.source_id, + frame_id=packet.envelope.frame_id, + evidence_time_ns=packet.envelope.timestamps.source_ns, + basis=EvidenceBasis.LIDAR, + currentness=EvidenceCurrentness.CURRENT, + occupied_support=True, + source_point_ids=point_ids, + metric_geometry=MetricGeometry( + coordinate_frame=self.profile.coordinate_frame, + centroid_xyz_m=cluster.centroid_map_xyz_m, + range_m=cluster.nearest_range_m, + covariance_diagonal_m2=cluster.covariance_diagonal_m2, + ), + proposal_ids=(), + semantic_hint=None, + reason_codes=("unassociated-current-occupied-component",), + ) + ) + metrics["geometry_only"] += 1 + metrics["published_points"] += len(point_ids) + return tuple(observations), metrics + + def _validate_proposals( + self, + packet: SourcePacket, + proposals: tuple[ObjectProposal2D, ...], + ) -> None: + if any( + proposal.source_id != packet.envelope.source_id + or proposal.frame_id != packet.envelope.frame_id + for proposal in proposals + ): + raise GeometryProviderError("proposal escaped its source packet") + if len({proposal.proposal_id for proposal in proposals}) != len(proposals): + raise GeometryProviderError("proposal identities are duplicated") + + def _record_unavailable(self, count: int) -> None: + with self._lock: + self._camera_only += count + self._unavailable += count + + def _record_current(self, metrics: Mapping[str, int]) -> None: + with self._lock: + self._eligible += metrics["eligible"] + self._ranged += metrics["ranged"] + self._camera_only += metrics["camera_only"] + self._conflict += metrics["conflict"] + self._outside += metrics["outside"] + self._sparse += metrics["sparse"] + self._ownership_collision += metrics["ownership_collision"] + self._geometry_only += metrics["geometry_only"] + self._published_points += metrics["published_points"] + self._overlap_removed += metrics["overlap_removed"] + + def snapshot(self) -> GeometryProviderSnapshot: + with self._lock: + return GeometryProviderSnapshot( + input_frames=self._input_frames, + completed_frames=self._completed_frames, + failed_frames=self._failed_frames, + proposal_count=self._proposal_count, + eligible_proposal_count=self._eligible, + ranged_proposal_count=self._ranged, + camera_only_proposal_count=self._camera_only, + conflict_proposal_count=self._conflict, + unavailable_proposal_count=self._unavailable, + outside_overlap_proposal_count=self._outside, + sparse_proposal_count=self._sparse, + ownership_collision_proposal_count=self._ownership_collision, + geometry_only_observation_count=self._geometry_only, + published_source_point_count=self._published_points, + overlapping_claims_removed=self._overlap_removed, + core_duration_ns=self._core_duration_ns, + ) + + +def load_geometry_profile(path: Path) -> GeometryProfile: + resolved = path.resolve(strict=True) + _verify_regular_file(resolved, expected_sha256=None, label="geometry profile") + raw = resolved.read_bytes() + try: + value = json.loads(raw) + except json.JSONDecodeError as exc: + raise GeometryProviderError("geometry profile JSON is invalid") from exc + document = _object(value, "geometry profile") + _exact_keys( + document, + { + "schema_version", + "profile_id", + "provider_id", + "source", + "local_surface", + "projection", + "association", + "policy", + "authority", + }, + "geometry profile", + ) + if document["schema_version"] != GEOMETRY_PROFILE_SCHEMA: + raise GeometryProviderError("geometry profile schema is incompatible") + if document["provider_id"] != GEOMETRY_PROVIDER_ID: + raise GeometryProviderError("geometry provider identity is incompatible") + source = _object(document["source"], "geometry source") + surface = _object(document["local_surface"], "local surface") + projection = _object(document["projection"], "geometry projection") + association = _object(document["association"], "geometry association") + policy = _object(document["policy"], "geometry policy") + authority = _object(document["authority"], "geometry authority") + _exact_keys( + source, + { + "source_id", + "session_id", + "source_pack_id", + "source_pack_sha256", + "frame_count", + "point_count", + }, + "geometry source", + ) + _exact_keys( + surface, + {"model_id", "artifact_sha256", "valid_frame_count"}, + "local surface", + ) + _exact_keys(projection, {"model", "width", "height", "coordinate_frame"}, "projection") + association_keys = set(GeometryAssociationProfile.__dataclass_fields__) + _exact_keys(association, association_keys, "geometry association") + expected_policy = { + "camera_owns_semantic_hint": True, + "geometry_can_invent_semantic_class": False, + "geometry_only_range_estimator": "nearest-euclidean-sensor-distance/v1", + "one_owner_per_source_point": True, + "absence_of_points_means_free": False, + "overlap_eligibility": "bbox-intersects-current-projected-point-extent/v1", + "point_ownership_priority": "smallest-bbox-then-score-then-proposal-id/v1", + "proposal_range_estimator": "median-camera-z-of-owned-current-points/v1", + "unknown_is_occupied": True, + "threshold_tuning_allowed": False, + } + if policy != expected_policy: + raise GeometryProviderError("geometry policy is incompatible") + if authority != { + "ground_truth": False, + "physical_live": False, + "commands_enabled": False, + "actuation_allowed": False, + "navigation_or_safety_accepted": False, + }: + raise GeometryProviderError("geometry authority is incompatible") + if projection["model"] != "KB4": + raise GeometryProviderError("geometry projection model is incompatible") + profile = GeometryProfile( + profile_id=_string(document, "profile_id"), + provider_id=_string(document, "provider_id"), + source_id=_string(source, "source_id"), + session_id=_string(source, "session_id"), + source_pack_id=_string(source, "source_pack_id"), + source_pack_sha256=_digest(source, "source_pack_sha256"), + frame_count=_positive_integer(source, "frame_count"), + point_count=_positive_integer(source, "point_count"), + local_surface_model_id=_string(surface, "model_id"), + local_surface_sha256=_digest(surface, "artifact_sha256"), + valid_frame_count=_positive_integer(surface, "valid_frame_count"), + width=_positive_integer(projection, "width"), + height=_positive_integer(projection, "height"), + coordinate_frame=_string(projection, "coordinate_frame"), + association=GeometryAssociationProfile( + bbox_inset_fraction=_number(association, "bbox_inset_fraction"), + depth_cluster_minimum_gap_m=_number( + association, + "depth_cluster_minimum_gap_m", + ), + depth_cluster_gap_fraction=_number( + association, + "depth_cluster_gap_fraction", + ), + spatial_cluster_radius_m=_number(association, "spatial_cluster_radius_m"), + semantic_minimum_occupied_points=_positive_integer( + association, + "semantic_minimum_occupied_points", + ), + semantic_minimum_occupied_voxels=_positive_integer( + association, + "semantic_minimum_occupied_voxels", + ), + semantic_voxel_size_m=_number(association, "semantic_voxel_size_m"), + conflict_minimum_classified_points=_positive_integer( + association, + "conflict_minimum_classified_points", + ), + conflict_surface_fraction=_number(association, "conflict_surface_fraction"), + geometry_local_radius_m=_number(association, "geometry_local_radius_m"), + geometry_voxel_size_m=_number(association, "geometry_voxel_size_m"), + geometry_minimum_cluster_points=_positive_integer( + association, + "geometry_minimum_cluster_points", + ), + geometry_minimum_cluster_voxels=_positive_integer( + association, + "geometry_minimum_cluster_voxels", + ), + maximum_geometry_clusters_per_frame=_positive_integer( + association, + "maximum_geometry_clusters_per_frame", + ), + ), + profile_sha256=hashlib.sha256(raw).hexdigest(), + ) + if profile.source_pack_id != RECORDED_SOURCE_PACK_ID: + raise GeometryProviderError("geometry profile does not bind the admitted source pack") + if profile.valid_frame_count > profile.frame_count: + raise GeometryProviderError("geometry valid frame count exceeds the source") + return profile + + +def _allocate_point_ownership( + proposals: tuple[ObjectProposal2D, ...], + supports: tuple[SemanticGeometrySupport, ...], +) -> tuple[dict[int, npt.NDArray[np.int64]], int]: + eligible = {index: support for index, support in enumerate(supports) if support.qualified} + claims: dict[int, list[int]] = {} + for index, support in eligible.items(): + for source_index in support.occupied_source_indices: + claims.setdefault(int(source_index), []).append(index) + winners = { + source_index: min(candidates, key=lambda index: _proposal_priority(proposals[index])) + for source_index, candidates in claims.items() + } + allocations = { + index: np.asarray( + [ + int(source_index) + for source_index in support.occupied_source_indices + if winners[int(source_index)] == index + ], + dtype=np.int64, + ) + for index, support in eligible.items() + } + removed = sum( + int(eligible[index].occupied_source_indices.size - allocation.size) + for index, allocation in allocations.items() + ) + return allocations, removed + + +def _proposal_priority(proposal: ObjectProposal2D) -> tuple[float, float, str]: + left, top, right, bottom = proposal.region.as_tuple() + return ((right - left) * (bottom - top), -proposal.objectness, proposal.proposal_id) + + +def _proposal_observation( + packet: SourcePacket, + proposal: ObjectProposal2D, + *, + support: SemanticGeometrySupport, + owned_source_indices: npt.NDArray[np.int64], + frame: GeometryFrame, + projected: ProjectedPointCloud, + coordinate_frame: str, +) -> ObstacleObservation: + point_ids: tuple[int, ...] = () + metric: MetricGeometry | None = None + reason_codes: tuple[str, ...] + basis: EvidenceBasis + if support.qualified and owned_source_indices.size: + point_ids = tuple(sorted(int(value) for value in owned_source_indices)) + points = frame.points_map[owned_source_indices] + centroid = np.median(points, axis=0) + covariance = points.var(axis=0) + depth_by_source = { + int(source_index): float(depth) + for source_index, depth in zip( + support.occupied_source_indices, + support.occupied_depths_m, + strict=True, + ) + } + range_m = float(np.median([depth_by_source[index] for index in point_ids])) + metric = MetricGeometry( + coordinate_frame=coordinate_frame, + centroid_xyz_m=( + float(centroid[0]), + float(centroid[1]), + float(centroid[2]), + ), + range_m=range_m, + covariance_diagonal_m2=( + float(covariance[0]), + float(covariance[1]), + float(covariance[2]), + ), + ) + basis = EvidenceBasis.FUSED + reason_codes = ("current-connected-occupied-lidar-support",) + if owned_source_indices.size != support.occupied_source_indices.size: + reason_codes += ("exclusive-point-ownership-arbitration",) + elif support.qualified: + basis = EvidenceBasis.CAMERA + reason_codes = ( + "current-connected-occupied-lidar-support", + "point-ownership-collision-range-withheld", + ) + elif support.conflict: + basis = EvidenceBasis.CONFLICT + reason_codes = ("camera-region-observed-as-local-surface",) + elif not support.overlaps_projected_extent: + basis = EvidenceBasis.CAMERA + reason_codes = ("outside-projected-lidar-overlap",) + else: + basis = EvidenceBasis.CAMERA + reason_codes = ("sparse-or-unqualified-occupied-support",) + return ObstacleObservation( + observation_id=f"{packet.envelope.frame_id}:{proposal.proposal_id}", + occupancy_key=f"{packet.envelope.frame_id}:{proposal.proposal_id}", + source_id=packet.envelope.source_id, + frame_id=packet.envelope.frame_id, + evidence_time_ns=packet.envelope.timestamps.source_ns, + basis=basis, + currentness=EvidenceCurrentness.CURRENT, + occupied_support=metric is not None, + source_point_ids=point_ids, + metric_geometry=metric, + proposal_ids=(proposal.proposal_id,), + semantic_hint=proposal.semantic_hint, + reason_codes=reason_codes, + ) + + +def _camera_unavailable_observation( + packet: SourcePacket, + proposal: ObjectProposal2D, + source_unavailable: bool, +) -> ObstacleObservation: + status = packet.envelope.registered_point_increment + if source_unavailable and status.outcome is ModalityOutcome.STALE: + currentness = EvidenceCurrentness.STALE + reason = "registered-point-increment-stale" + else: + currentness = EvidenceCurrentness.UNAVAILABLE + reason = ( + "registered-point-increment-unavailable" + if source_unavailable + else "local-surface-unavailable" + ) + return ObstacleObservation( + observation_id=f"{packet.envelope.frame_id}:{proposal.proposal_id}", + occupancy_key=f"{packet.envelope.frame_id}:{proposal.proposal_id}", + source_id=packet.envelope.source_id, + frame_id=packet.envelope.frame_id, + evidence_time_ns=packet.envelope.timestamps.source_ns, + basis=EvidenceBasis.CAMERA, + currentness=currentness, + occupied_support=False, + source_point_ids=(), + metric_geometry=None, + proposal_ids=(proposal.proposal_id,), + semantic_hint=proposal.semantic_hint, + reason_codes=(reason,), + ) + + +def _load_npz(path: Path, label: str) -> dict[str, npt.NDArray[np.generic]]: + try: + with np.load(path, allow_pickle=False) as archive: + return {name: np.asarray(archive[name]) for name in archive.files} + except (OSError, ValueError) as exc: + raise GeometryProviderError(f"{label} cannot be opened") from exc + + +def _verify_regular_file(path: Path, *, expected_sha256: str | None, label: str) -> None: + if not path.is_file() or path.is_symlink(): + raise GeometryProviderError(f"{label} must be a regular file") + if expected_sha256 is not None and _file_sha256(path) != expected_sha256: + raise GeometryProviderError(f"{label} digest changed") + + +def _file_sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as handle: + for chunk in iter(lambda: handle.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _object(value: object, label: str) -> dict[str, object]: + if not isinstance(value, dict) or any(not isinstance(key, str) for key in value): + raise GeometryProviderError(f"{label} must be an object") + return value + + +def _exact_keys(document: Mapping[str, object], expected: set[str], label: str) -> None: + if set(document) != expected: + raise GeometryProviderError(f"{label} fields are incompatible") + + +def _string(document: Mapping[str, object], key: str) -> str: + value = document.get(key) + if not isinstance(value, str) or not value: + raise GeometryProviderError(f"{key} must be a nonempty string") + return value + + +def _positive_integer(document: Mapping[str, object], key: str) -> int: + value = document.get(key) + if not isinstance(value, int) or isinstance(value, bool) or value < 1: + raise GeometryProviderError(f"{key} must be a positive integer") + return value + + +def _number(document: Mapping[str, object], key: str) -> float: + value = document.get(key) + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise GeometryProviderError(f"{key} must be numeric") + result = float(value) + if not math.isfinite(result): + raise GeometryProviderError(f"{key} must be finite") + return result + + +def _digest(document: Mapping[str, object], key: str) -> str: + value = _string(document, key) + if len(value) != 64 or any(character not in "0123456789abcdef" for character in value): + raise GeometryProviderError(f"{key} must be a SHA-256 digest") + return value + + +__all__ = [ + "DEFAULT_GEOMETRY_PROFILE_PATH", + "GEOMETRY_PROFILE_SCHEMA", + "GEOMETRY_PROVIDER_ID", + "GeometryFrame", + "GeometryProfile", + "GeometryProviderError", + "GeometryProviderSnapshot", + "Ravnoves00GeometryAssociationProvider", + "RecordedGeometryStore", + "load_geometry_profile", +] diff --git a/src/k1link/perception/geometry_math.py b/src/k1link/perception/geometry_math.py new file mode 100644 index 0000000..44f7b04 --- /dev/null +++ b/src/k1link/perception/geometry_math.py @@ -0,0 +1,496 @@ +"""Pure KB4 projection and frame-local occupied-geometry association. + +This is the product-owned extraction of the accepted E29/E32 mathematics. It +has no LAB, compute-package, device-plugin or transport dependency. +""" + +from __future__ import annotations + +import math +from collections import deque +from dataclasses import dataclass + +import numpy as np +import numpy.typing as npt + +FloatArray = npt.NDArray[np.float64] +IntArray = npt.NDArray[np.int64] +UInt8Array = npt.NDArray[np.uint8] +Float32Array = npt.NDArray[np.float32] + +POINT_SURFACE = 1 +POINT_OCCUPIED = 2 +POINT_BELOW_SURFACE = 3 + + +class GeometryMathError(ValueError): + """A projection or association input violates the frozen M4 contract.""" + + +@dataclass(frozen=True, slots=True) +class GeometryAssociationProfile: + bbox_inset_fraction: float + depth_cluster_minimum_gap_m: float + depth_cluster_gap_fraction: float + spatial_cluster_radius_m: float + semantic_minimum_occupied_points: int + semantic_minimum_occupied_voxels: int + semantic_voxel_size_m: float + conflict_minimum_classified_points: int + conflict_surface_fraction: float + geometry_local_radius_m: float + geometry_voxel_size_m: float + geometry_minimum_cluster_points: int + geometry_minimum_cluster_voxels: int + maximum_geometry_clusters_per_frame: int + + def __post_init__(self) -> None: + numeric = ( + self.bbox_inset_fraction, + self.depth_cluster_minimum_gap_m, + self.depth_cluster_gap_fraction, + self.spatial_cluster_radius_m, + self.semantic_voxel_size_m, + self.conflict_surface_fraction, + self.geometry_local_radius_m, + self.geometry_voxel_size_m, + ) + if ( + not np.isfinite(numeric).all() + or not 0.0 <= self.bbox_inset_fraction < 0.25 + or not 0.05 <= self.depth_cluster_minimum_gap_m <= 5.0 + or not 0.0 <= self.depth_cluster_gap_fraction <= 1.0 + or not 0.05 <= self.spatial_cluster_radius_m <= 5.0 + or not 1 <= self.semantic_minimum_occupied_points <= 64 + or not 1 <= self.semantic_minimum_occupied_voxels <= 32 + or not 0.05 <= self.semantic_voxel_size_m <= 2.0 + or not 1 <= self.conflict_minimum_classified_points <= 256 + or not 0.5 <= self.conflict_surface_fraction <= 1.0 + or not 1.0 <= self.geometry_local_radius_m <= 100.0 + or not 0.05 <= self.geometry_voxel_size_m <= 5.0 + or not 1 <= self.geometry_minimum_cluster_points <= 256 + or not 1 <= self.geometry_minimum_cluster_voxels <= 128 + or not 1 <= self.maximum_geometry_clusters_per_frame <= 512 + ): + raise GeometryMathError("geometry association profile is invalid") + + +@dataclass(frozen=True, slots=True) +class Kb4ProjectionProfile: + width: int + height: int + intrinsic_fx_fy_cx_cy: tuple[float, float, float, float] + distortion_kb4: tuple[float, float, float, float] + t_camera_from_lidar: FloatArray + + def __post_init__(self) -> None: + transform = np.asarray(self.t_camera_from_lidar, dtype=np.float64) + values = (*self.intrinsic_fx_fy_cx_cy, *self.distortion_kb4) + if ( + self.width < 1 + or self.height < 1 + or transform.shape != (4, 4) + or not np.isfinite(transform).all() + or not np.isfinite(values).all() + or self.intrinsic_fx_fy_cx_cy[0] <= 0.0 + or self.intrinsic_fx_fy_cx_cy[1] <= 0.0 + ): + raise GeometryMathError("KB4 projection profile is invalid") + frozen = np.array(transform, dtype=np.float64, copy=True) + frozen.setflags(write=False) + object.__setattr__(self, "t_camera_from_lidar", frozen) + + +@dataclass(frozen=True, slots=True) +class ProjectedPointCloud: + pixels_xy: FloatArray + depths_m: FloatArray + source_indices: IntArray + source_point_count: int + camera_front_point_count: int + + @property + def projected_point_count(self) -> int: + return int(self.pixels_xy.shape[0]) + + @property + def overlap_bounds_xyxy(self) -> tuple[float, float, float, float] | None: + if not self.projected_point_count: + return None + return ( + float(np.min(self.pixels_xy[:, 0])), + float(np.min(self.pixels_xy[:, 1])), + float(np.max(self.pixels_xy[:, 0])), + float(np.max(self.pixels_xy[:, 1])), + ) + + +@dataclass(frozen=True, slots=True) +class SemanticGeometrySupport: + projected_points_in_region: int + classified_points_in_region: int + surface_points_in_region: int + occupied_points_in_region: int + below_surface_points_in_region: int + occupied_source_indices: IntArray + occupied_depths_m: FloatArray + qualified: bool + conflict: bool + overlaps_projected_extent: bool + + +@dataclass(frozen=True, slots=True) +class GeometryCluster: + source_indices: IntArray + centroid_map_xyz_m: tuple[float, float, float] + covariance_diagonal_m2: tuple[float, float, float] + nearest_range_m: float + voxel_count: int + + +def project_map_points_kb4( + points_map_xyz: npt.ArrayLike, + *, + position_map_xyz: npt.ArrayLike, + orientation_map_from_lidar_xyzw: npt.ArrayLike, + profile: Kb4ProjectionProfile, +) -> ProjectedPointCloud: + """Project one registered map-frame increment into the raw KB4 image.""" + + points_map = _finite_points(points_map_xyz) + position = np.asarray(position_map_xyz, dtype=np.float64) + if position.shape != (3,) or not np.isfinite(position).all(): + raise GeometryMathError("pose position must contain three finite values") + rotation = quaternion_xyzw_to_rotation_matrix(orientation_map_from_lidar_xyzw) + points_lidar = (points_map - position) @ rotation + transform = profile.t_camera_from_lidar + points_camera = points_lidar @ transform[:3, :3].T + transform[:3, 3] + front = points_camera[:, 2] > 1e-6 + front_indices = np.flatnonzero(front) + front_points = points_camera[front] + if front_points.size == 0: + return ProjectedPointCloud( + pixels_xy=np.empty((0, 2), dtype=np.float64), + depths_m=np.empty(0, dtype=np.float64), + source_indices=np.empty(0, dtype=np.int64), + source_point_count=int(points_map.shape[0]), + camera_front_point_count=0, + ) + x, y, z = front_points.T + radial = np.hypot(x, y) + theta = np.arctan2(radial, z) + squared = theta * theta + k1, k2, k3, k4 = profile.distortion_kb4 + distorted = theta * ( + 1.0 + k1 * squared + k2 * squared**2 + k3 * squared**3 + k4 * squared**4 + ) + scale = np.divide(distorted, radial, out=np.zeros_like(distorted), where=radial > 1e-12) + fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy + u = fx * x * scale + cx + v = fy * y * scale + cy + in_frame = ( + np.isfinite(u) + & np.isfinite(v) + & (u >= 0.0) + & (u < profile.width) + & (v >= 0.0) + & (v < profile.height) + ) + return ProjectedPointCloud( + pixels_xy=np.column_stack((u[in_frame], v[in_frame])).astype(np.float64, copy=False), + depths_m=z[in_frame].astype(np.float64, copy=False), + source_indices=front_indices[in_frame].astype(np.int64, copy=False), + source_point_count=int(points_map.shape[0]), + camera_front_point_count=int(front_points.shape[0]), + ) + + +def quaternion_xyzw_to_rotation_matrix(orientation_xyzw: npt.ArrayLike) -> FloatArray: + quaternion = np.asarray(orientation_xyzw, dtype=np.float64) + if quaternion.shape != (4,) or not np.isfinite(quaternion).all(): + raise GeometryMathError("pose quaternion must contain four finite values") + norm = float(np.linalg.norm(quaternion)) + if not math.isfinite(norm) or norm < 1e-9: + raise GeometryMathError("pose quaternion has no usable norm") + x, y, z, w = quaternion / norm + return np.asarray( + ( + (1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)), + (2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)), + (2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)), + ), + dtype=np.float64, + ) + + +def semantic_geometry_support( + bbox_xyxy: tuple[float, float, float, float], + *, + projected: ProjectedPointCloud, + frame_points_map: FloatArray, + point_class: UInt8Array, + profile: GeometryAssociationProfile, +) -> SemanticGeometrySupport: + """Return qualified occupied support without assigning point ownership.""" + + bbox = np.asarray(bbox_xyxy, dtype=np.float64) + if bbox.shape != (4,) or not np.isfinite(bbox).all() or np.any(bbox[2:] <= bbox[:2]): + raise GeometryMathError("proposal region is invalid") + width, height = float(bbox[2] - bbox[0]), float(bbox[3] - bbox[1]) + inset = profile.bbox_inset_fraction + inner = np.asarray( + ( + bbox[0] + width * inset, + bbox[1] + height * inset, + bbox[2] - width * inset, + bbox[3] - height * inset, + ), + dtype=np.float64, + ) + pixels = projected.pixels_xy + inside = ( + (pixels[:, 0] >= inner[0]) + & (pixels[:, 0] <= inner[2]) + & (pixels[:, 1] >= inner[1]) + & (pixels[:, 1] <= inner[3]) + ) + rows = np.flatnonzero(inside).astype(np.int64, copy=False) + indices = projected.source_indices[rows] + classes = point_class[indices] + counts = np.bincount(classes, minlength=4) + occupied_rows = rows[classes == POINT_OCCUPIED] + clustered = _depth_cluster( + occupied_rows, + projected.depths_m, + minimum_gap_m=profile.depth_cluster_minimum_gap_m, + gap_fraction=profile.depth_cluster_gap_fraction, + ) + clustered = _spatial_cluster( + clustered, + projected.source_indices, + frame_points_map, + radius_m=profile.spatial_cluster_radius_m, + ) + occupied_indices = projected.source_indices[clustered].astype(np.int64, copy=False) + occupied_depths = projected.depths_m[clustered].astype(np.float64, copy=False) + voxel_count = _voxel_count( + frame_points_map[occupied_indices], + profile.semantic_voxel_size_m, + ) + occupied_count = int(occupied_indices.size) + qualified = ( + occupied_count >= profile.semantic_minimum_occupied_points + and voxel_count >= profile.semantic_minimum_occupied_voxels + ) + classified = int(counts[POINT_SURFACE] + counts[POINT_OCCUPIED] + counts[POINT_BELOW_SURFACE]) + conflict = ( + not qualified + and classified >= profile.conflict_minimum_classified_points + and int(counts[POINT_OCCUPIED]) == 0 + and float(counts[POINT_SURFACE] / max(1, classified)) + >= profile.conflict_surface_fraction + ) + bounds = projected.overlap_bounds_xyxy + bbox_tuple = (float(bbox[0]), float(bbox[1]), float(bbox[2]), float(bbox[3])) + overlaps = bounds is not None and _regions_intersect(bbox_tuple, bounds) + return SemanticGeometrySupport( + projected_points_in_region=int(indices.size), + classified_points_in_region=classified, + surface_points_in_region=int(counts[POINT_SURFACE]), + occupied_points_in_region=int(counts[POINT_OCCUPIED]), + below_surface_points_in_region=int(counts[POINT_BELOW_SURFACE]), + occupied_source_indices=occupied_indices, + occupied_depths_m=occupied_depths, + qualified=qualified, + conflict=conflict, + overlaps_projected_extent=overlaps, + ) + + +def geometry_only_clusters( + *, + points_map: FloatArray, + point_class: UInt8Array, + sensor_position_map: FloatArray, + claimed_source_indices: frozenset[int], + profile: GeometryAssociationProfile, +) -> tuple[GeometryCluster, ...]: + """Return bounded unclaimed occupied components with no semantic class.""" + + occupied = np.flatnonzero(point_class == POINT_OCCUPIED).astype(np.int64) + if occupied.size == 0: + return () + ranges = np.linalg.norm(points_map[occupied] - sensor_position_map, axis=1) + occupied = occupied[ranges <= profile.geometry_local_radius_m] + clusters: list[GeometryCluster] = [] + for indices, voxel_count in _voxel_components( + points_map[occupied], + occupied, + profile.geometry_voxel_size_m, + ): + if ( + indices.size < profile.geometry_minimum_cluster_points + or voxel_count < profile.geometry_minimum_cluster_voxels + or any(int(value) in claimed_source_indices for value in indices) + ): + continue + values = points_map[indices] + distances = np.linalg.norm(values - sensor_position_map, axis=1) + centroid = np.median(values, axis=0) + covariance = values.var(axis=0) + clusters.append( + GeometryCluster( + source_indices=indices.astype(np.int64, copy=False), + centroid_map_xyz_m=( + float(centroid[0]), + float(centroid[1]), + float(centroid[2]), + ), + covariance_diagonal_m2=( + float(covariance[0]), + float(covariance[1]), + float(covariance[2]), + ), + nearest_range_m=float(np.min(distances)), + voxel_count=voxel_count, + ) + ) + clusters.sort(key=lambda item: (item.nearest_range_m, -int(item.source_indices.size))) + return tuple(clusters[: profile.maximum_geometry_clusters_per_frame]) + + +def _depth_cluster( + rows: IntArray, + depths: FloatArray, + *, + minimum_gap_m: float, + gap_fraction: float, +) -> IntArray: + if rows.size < 2: + return rows + ordered = rows[np.argsort(depths[rows])] + groups: list[IntArray] = [] + start = 0 + for offset, gap in enumerate(np.diff(depths[ordered]), start=1): + threshold = max(minimum_gap_m, gap_fraction * float(depths[ordered[offset - 1]])) + if float(gap) > threshold: + groups.append(ordered[start:offset]) + start = offset + groups.append(ordered[start:]) + return min(groups, key=lambda group: (-int(group.size), float(np.median(depths[group])))) + + +def _spatial_cluster( + rows: IntArray, + source_indices: IntArray, + points_map: FloatArray, + *, + radius_m: float, +) -> IntArray: + if rows.size < 2: + return rows + points = points_map[source_indices[rows]] + adjacent = np.sum((points[:, None, :] - points[None, :, :]) ** 2, axis=2) <= radius_m**2 + unseen = set(range(rows.size)) + groups: list[list[int]] = [] + while unseen: + seed = unseen.pop() + group, pending = [seed], [seed] + while pending: + current = pending.pop() + connected = [item for item in tuple(unseen) if adjacent[current, item]] + for item in connected: + unseen.remove(item) + pending.append(item) + group.append(item) + groups.append(group) + selected = min( + groups, + key=lambda group: ( + -len(group), + float(np.median(np.linalg.norm(points[np.asarray(group, dtype=np.int64)], axis=1))), + ), + ) + return rows[np.asarray(selected, dtype=np.int64)] + + +def _voxel_components( + points: FloatArray, + source_indices: IntArray, + voxel_size_m: float, +) -> list[tuple[IntArray, int]]: + cells = np.floor(points / voxel_size_m).astype(np.int64) + cell_points: dict[tuple[int, int, int], list[int]] = {} + for local_index, cell in enumerate(cells): + key = (int(cell[0]), int(cell[1]), int(cell[2])) + cell_points.setdefault(key, []).append(int(source_indices[local_index])) + remaining = set(cell_points) + neighbors = tuple( + (dx, dy, dz) + for dx in (-1, 0, 1) + for dy in (-1, 0, 1) + for dz in (-1, 0, 1) + if (dx, dy, dz) != (0, 0, 0) + ) + components: list[tuple[IntArray, int]] = [] + while remaining: + seed = remaining.pop() + queue = deque([seed]) + component = [seed] + while queue: + current = queue.popleft() + for delta in neighbors: + candidate = tuple(current[index] + delta[index] for index in range(3)) + if candidate in remaining: + remaining.remove(candidate) + queue.append(candidate) + component.append(candidate) + indices = np.asarray( + [index for cell in component for index in cell_points[cell]], + dtype=np.int64, + ) + components.append((indices, len(component))) + return components + + +def _voxel_count(points: FloatArray, voxel_size_m: float) -> int: + if points.size == 0: + return 0 + cells = np.floor(points / voxel_size_m).astype(np.int64) + return int(np.unique(cells, axis=0).shape[0]) + + +def _regions_intersect( + left: tuple[float, float, float, float], + right: tuple[float, float, float, float], +) -> bool: + return ( + left[0] <= right[2] + and left[2] >= right[0] + and left[1] <= right[3] + and left[3] >= right[1] + ) + + +def _finite_points(points_xyz: npt.ArrayLike) -> FloatArray: + points = np.asarray(points_xyz, dtype=np.float64) + if points.ndim != 2 or points.shape[1:] != (3,) or not np.isfinite(points).all(): + raise GeometryMathError("point cloud must be finite with shape (N, 3)") + return points + + +__all__ = [ + "GeometryAssociationProfile", + "GeometryCluster", + "GeometryMathError", + "Kb4ProjectionProfile", + "POINT_BELOW_SURFACE", + "POINT_OCCUPIED", + "POINT_SURFACE", + "ProjectedPointCloud", + "SemanticGeometrySupport", + "geometry_only_clusters", + "project_map_points_kb4", + "quaternion_xyzw_to_rotation_matrix", + "semantic_geometry_support", +] diff --git a/src/k1link/perception/geometry_replay.py b/src/k1link/perception/geometry_replay.py new file mode 100644 index 0000000..6f02527 --- /dev/null +++ b/src/k1link/perception/geometry_replay.py @@ -0,0 +1,665 @@ +"""Immutable full-source M4.4 geometry replay over the accepted M4.3 ledger.""" + +from __future__ import annotations + +import hashlib +import json +import os +import shutil +import time +import uuid +from dataclasses import dataclass +from datetime import UTC, datetime +from pathlib import Path +from typing import Final + +import numpy as np + +from .baseline import BASELINE_RECORDED_JOB_ID +from .contracts import EvidenceBasis, ObstacleObservation +from .detector_replay_result import ( + read_detector_replay_result, + require_m4_detector_replay_acceptance, +) +from .geometry import ( + DEFAULT_GEOMETRY_PROFILE_PATH, + Ravnoves00GeometryAssociationProvider, + RecordedGeometryStore, + load_geometry_profile, +) +from .graph_validation import validate_observations +from .providers import SourcePacket +from .recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference + +GEOMETRY_REPLAY_SCHEMA: Final = "missioncore.perception-geometry-replay-result/v1" +GEOMETRY_REPLAY_FRAME_SCHEMA: Final = "missioncore.perception-geometry-replay-frame/v1" +GEOMETRY_REPLAY_REPORT_SCHEMA: Final = "missioncore.perception-geometry-replay-report/v1" +GEOMETRY_REPLAY_RESULT_PREFIX: Final = "m4-geometry-replay-" +GEOMETRY_REPLAY_FRAMES_NAME: Final = "frames.jsonl" +GEOMETRY_REPLAY_REPORT_NAME: Final = "report.json" +GEOMETRY_REPLAY_MANIFEST_NAME: Final = "manifest.json" + +E32_RESULT_ID: Final = ( + "e32-track-geometry-" + "a14ca0e7fb3850ca0dfa3c41634e1b490a2d58ab74d101afc6d6921fbdb0e6fd" +) +E32_MANIFEST_SHA256: Final = "f4b57c9f7619c43414adf1d10488b05df466226a523a0c001671102ced0e9ab8" +E53_RESULT_ID: Final = ( + "e53-camera-first-shadow-" + "e6f03cf8bfb15db86100239b060e13f914532618b7b99e811866e4e6a555186c" +) +E53_MANIFEST_SHA256: Final = "fc5b4ae69aae0098b7075c539d25b563dff1bbb09209a49030edda6cba9ee544" +EXPECTED_SOURCE_AVAILABLE_FRAMES: Final = 3928 +EXPECTED_SOURCE_UNAVAILABLE_FRAMES: Final = 561 + + +class GeometryReplayError(RuntimeError): + """A full-source geometry replay is incomplete, mutable or inconsistent.""" + + +@dataclass(frozen=True, slots=True) +class GeometryReplayResult: + result_id: str + result_root: Path + accepted: bool + metrics: dict[str, object] + report: dict[str, object] + manifest: dict[str, object] + + +def build_geometry_replay( + *, + repository_root: Path, + detector_result_root: Path, + output_root: Path, +) -> GeometryReplayResult: + """Run all accepted detector proposals through the canonical M4.4 provider.""" + + repository = repository_root.resolve() + detector = read_detector_replay_result(detector_result_root) + require_m4_detector_replay_acceptance(detector) + profile_path = repository / DEFAULT_GEOMETRY_PROFILE_PATH + profile = load_geometry_profile(profile_path) + store = RecordedGeometryStore.from_repository(repository, profile=profile) + provider = Ravnoves00GeometryAssociationProvider(store=store) + references = _verified_historical_references(repository) + + root = output_root.expanduser().absolute() + root.mkdir(mode=0o700, parents=True, exist_ok=True) + staging = root / f".geometry-replay.{uuid.uuid4().hex}.tmp" + staging.mkdir(mode=0o700, exist_ok=False) + started_ns = time.perf_counter_ns() + frame_latencies_ms: list[float] = [] + source_available = 0 + source_unavailable = 0 + try: + frames_path = staging / GEOMETRY_REPLAY_FRAMES_NAME + with frames_path.open("wb") as output: + for detector_frame in detector.frames: + if detector_frame.outcome != "completed": + raise GeometryReplayError("accepted detector ledger contains a failed frame") + packet = _packet(detector_frame.envelope) + frame_started_ns = time.perf_counter_ns() + before = provider.snapshot() + observations = provider.associate(packet, detector_frame.proposals) + validate_observations(packet, detector_frame.proposals, observations) + after = provider.snapshot() + frame_latency_ms = (time.perf_counter_ns() - frame_started_ns) / 1_000_000 + frame_latencies_ms.append(frame_latency_ms) + source_available += packet.envelope.registered_point_increment.available + source_unavailable += not packet.envelope.registered_point_increment.available + proposal_observations = tuple(item for item in observations if item.proposal_ids) + geometry_only = tuple(item for item in observations if not item.proposal_ids) + eligible = sum(_range_eligible(item) for item in proposal_observations) + ranged = sum(item.metric_geometry is not None for item in proposal_observations) + conflict = sum( + item.basis is EvidenceBasis.CONFLICT for item in proposal_observations + ) + frame_document = { + "schema_version": GEOMETRY_REPLAY_FRAME_SCHEMA, + "sequence": detector_frame.sequence, + "frame_id": packet.envelope.frame_id, + "source_available": packet.envelope.registered_point_increment.available, + "proposal_count": len(detector_frame.proposals), + "eligible_proposal_count": eligible, + "ranged_proposal_count": ranged, + "conflict_proposal_count": conflict, + "camera_only_proposal_count": len(proposal_observations) - ranged - conflict, + "geometry_only_observation_count": len(geometry_only), + "overlapping_claims_removed": ( + after.overlapping_claims_removed - before.overlapping_claims_removed + ), + "observations": [item.to_dict() for item in observations], + "policy": { + "one_owner_per_source_point": True, + "absence_of_points_means_free": False, + "geometry_can_invent_semantic_class": False, + }, + "authority": { + "ground_truth": False, + "physical_live": False, + "commands_enabled": False, + "actuation_allowed": False, + "navigation_or_safety_accepted": False, + }, + } + output.write(_canonical_json(frame_document) + b"\n") + snapshot = provider.snapshot() + metrics = _metrics( + snapshot=snapshot, + source_available=source_available, + source_unavailable=source_unavailable, + frame_latencies_ms=frame_latencies_ms, + elapsed_ns=time.perf_counter_ns() - started_ns, + ) + requirements = _requirements(metrics, detector_frame_count=len(detector.frames)) + accepted = all(requirements.values()) + frames_sha256 = _file_sha256(frames_path) + detector_identity = _object(detector.manifest["identity"], "detector identity") + detector_frames_sha256 = detector_identity.get("frames_sha256") + if not isinstance(detector_frames_sha256, str): + raise GeometryReplayError("detector frame digest is unavailable") + identity = { + "schema_version": GEOMETRY_REPLAY_SCHEMA, + "detector_result_id": detector.result_id, + "detector_frames_sha256": detector_frames_sha256, + "geometry_profile_id": profile.profile_id, + "geometry_profile_sha256": profile.profile_sha256, + "geometry_provider_id": provider.provider_id, + "source_pack_id": profile.source_pack_id, + "source_pack_sha256": profile.source_pack_sha256, + "local_surface_model_id": profile.local_surface_model_id, + "local_surface_sha256": profile.local_surface_sha256, + "historical_references": references, + "producer_sha256": { + name: _file_sha256(repository / "src/k1link/perception" / name) + for name in ("geometry.py", "geometry_math.py", "geometry_replay.py") + }, + "frames_sha256": frames_sha256, + "metrics": metrics, + "acceptance_requirements": requirements, + "accepted": accepted, + "authority": { + "ground_truth": False, + "physical_live": False, + "commands_enabled": False, + "actuation_allowed": False, + "navigation_or_safety_accepted": False, + }, + } + identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() + result_id = f"{GEOMETRY_REPLAY_RESULT_PREFIX}{identity_sha256}" + created = datetime.now(UTC).isoformat(timespec="milliseconds").replace("+00:00", "Z") + report = { + "schema_version": GEOMETRY_REPLAY_REPORT_SCHEMA, + "result_id": result_id, + "identity_sha256": identity_sha256, + "created_at_utc": created, + "status": "accepted-product-geometry-replay" if accepted else "rejected-fail-closed", + "accepted": accepted, + "metrics": metrics, + "acceptance_requirements": requirements, + "decision": { + "m4_4_geometry_provider_integrated": accepted, + "semantic_class_quality_accepted": False, + "physical_live_accepted": False, + "next_gate": "M4.5 temporal retention and motion state", + }, + "authority": identity["authority"], + } + report_path = staging / GEOMETRY_REPLAY_REPORT_NAME + _write_json(report_path, report) + manifest = { + "schema_version": GEOMETRY_REPLAY_SCHEMA, + "result_id": result_id, + "identity_sha256": identity_sha256, + "identity": identity, + "created_at_utc": created, + "accepted": accepted, + "artifacts": [ + _artifact(frames_path, "geometry-replay-frames"), + _artifact(report_path, "geometry-replay-report"), + ], + } + _write_json(staging / GEOMETRY_REPLAY_MANIFEST_NAME, manifest) + destination = root / result_id + if destination.exists(): + shutil.rmtree(staging) + return read_geometry_replay_result(destination) + os.replace(staging, destination) + return read_geometry_replay_result(destination) + except BaseException: + shutil.rmtree(staging, ignore_errors=True) + raise + + +def read_geometry_replay_result(root: Path) -> GeometryReplayResult: + resolved = root.resolve(strict=True) + if resolved.is_symlink() or not resolved.name.startswith(GEOMETRY_REPLAY_RESULT_PREFIX): + raise GeometryReplayError("geometry replay result root is invalid") + manifest = _read_json(resolved / GEOMETRY_REPLAY_MANIFEST_NAME) + _exact_keys( + manifest, + { + "schema_version", + "result_id", + "identity_sha256", + "identity", + "created_at_utc", + "accepted", + "artifacts", + }, + "geometry replay manifest", + ) + identity = _object(manifest["identity"], "geometry replay identity") + _exact_keys( + identity, + { + "schema_version", + "detector_result_id", + "detector_frames_sha256", + "geometry_profile_id", + "geometry_profile_sha256", + "geometry_provider_id", + "source_pack_id", + "source_pack_sha256", + "local_surface_model_id", + "local_surface_sha256", + "historical_references", + "producer_sha256", + "frames_sha256", + "metrics", + "acceptance_requirements", + "accepted", + "authority", + }, + "geometry replay identity", + ) + identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() + if ( + manifest["schema_version"] != GEOMETRY_REPLAY_SCHEMA + or manifest["result_id"] != resolved.name + or manifest["identity_sha256"] != identity_sha256 + or resolved.name != f"{GEOMETRY_REPLAY_RESULT_PREFIX}{identity_sha256}" + ): + raise GeometryReplayError("geometry replay identity changed") + artifacts = manifest.get("artifacts") + if not isinstance(artifacts, list) or len(artifacts) != 2: + raise GeometryReplayError("geometry replay artifact inventory changed") + by_role = {_object(item, "geometry artifact")["role"]: item for item in artifacts} + if set(by_role) != {"geometry-replay-frames", "geometry-replay-report"}: + raise GeometryReplayError("geometry replay artifact roles changed") + frames_path = _validated_artifact( + resolved, + by_role["geometry-replay-frames"], + GEOMETRY_REPLAY_FRAMES_NAME, + ) + report_path = _validated_artifact( + resolved, + by_role["geometry-replay-report"], + GEOMETRY_REPLAY_REPORT_NAME, + ) + if _file_sha256(frames_path) != identity.get("frames_sha256"): + raise GeometryReplayError("geometry frame ledger digest changed") + report = _read_json(report_path) + if ( + report.get("schema_version") != GEOMETRY_REPLAY_REPORT_SCHEMA + or report.get("result_id") != resolved.name + or report.get("identity_sha256") != identity_sha256 + or report.get("metrics") != identity.get("metrics") + or report.get("acceptance_requirements") != identity.get("acceptance_requirements") + or report.get("accepted") != identity.get("accepted") + or report.get("authority") != identity.get("authority") + ): + raise GeometryReplayError("geometry replay report changed") + metrics = _object(identity.get("metrics"), "geometry metrics") + requirements = _object(identity.get("acceptance_requirements"), "geometry requirements") + if identity.get("authority") != _false_authority(): + raise GeometryReplayError("geometry replay authority changed") + accepted = all(value is True for value in requirements.values()) + if manifest.get("accepted") is not accepted or identity.get("accepted") is not accepted: + raise GeometryReplayError("geometry replay acceptance changed") + _validate_frame_ledger(frames_path, metrics) + return GeometryReplayResult( + result_id=resolved.name, + result_root=resolved, + accepted=accepted, + metrics=metrics, + report=report, + manifest=manifest, + ) + + +def _packet(envelope: object) -> SourcePacket: + from .contracts import SourceEnvelope + + if not isinstance(envelope, SourceEnvelope): + raise GeometryReplayError("detector frame envelope is incompatible") + image = RecordedFrameReference(BASELINE_RECORDED_JOB_ID, envelope.sequence) + geometry = ( + RecordedFrameReference(RECORDED_SOURCE_PACK_ID, envelope.sequence) + if envelope.registered_point_increment.available + else None + ) + return SourcePacket( + envelope=envelope, + image_payload=image, + registered_point_increment_payload=geometry, + pose_payload=geometry, + ) + + +def _range_eligible(observation: ObstacleObservation) -> bool: + excluded = { + "outside-projected-lidar-overlap", + "registered-point-increment-unavailable", + "registered-point-increment-stale", + "local-surface-unavailable", + } + return not excluded.intersection(observation.reason_codes) + + +def _metrics( + *, + snapshot: object, + source_available: int, + source_unavailable: int, + frame_latencies_ms: list[float], + elapsed_ns: int, +) -> dict[str, object]: + from .geometry import GeometryProviderSnapshot + + if not isinstance(snapshot, GeometryProviderSnapshot): + raise GeometryReplayError("geometry provider snapshot is incompatible") + values = np.asarray(frame_latencies_ms, dtype=np.float64) + total_coverage = snapshot.total_range_coverage + eligible_coverage = snapshot.eligible_range_coverage + return { + "frames": { + "total": snapshot.completed_frames, + "failed": snapshot.failed_frames, + "source_available": source_available, + "source_unavailable": source_unavailable, + }, + "proposals": { + "total": snapshot.proposal_count, + "eligible_for_range": snapshot.eligible_proposal_count, + "with_range": snapshot.ranged_proposal_count, + "camera_only": snapshot.camera_only_proposal_count, + "conflict": snapshot.conflict_proposal_count, + "unavailable": snapshot.unavailable_proposal_count, + "outside_overlap": snapshot.outside_overlap_proposal_count, + "sparse": snapshot.sparse_proposal_count, + "ownership_collision": snapshot.ownership_collision_proposal_count, + "total_range_coverage": total_coverage, + "eligible_range_coverage": eligible_coverage, + }, + "geometry_only_observations": snapshot.geometry_only_observation_count, + "published_source_point_rows": snapshot.published_source_point_count, + "overlapping_claims_removed": snapshot.overlapping_claims_removed, + "runtime": { + "elapsed_ms": elapsed_ns / 1_000_000, + "provider_core_ms": snapshot.core_duration_ns / 1_000_000, + "frame_latency_ms": { + "minimum": float(np.min(values)), + "p50": float(np.percentile(values, 50)), + "p95": float(np.percentile(values, 95)), + "maximum": float(np.max(values)), + "mean": float(np.mean(values)), + }, + }, + } + + +def _requirements(metrics: dict[str, object], *, detector_frame_count: int) -> dict[str, bool]: + frames = _object(metrics["frames"], "frame metrics") + proposals = _object(metrics["proposals"], "proposal metrics") + total = _integer(proposals["total"], "total proposals") + ranged = _integer(proposals["with_range"], "ranged proposals") + camera = _integer(proposals["camera_only"], "camera proposals") + conflict = _integer(proposals["conflict"], "conflict proposals") + eligible = _integer(proposals["eligible_for_range"], "eligible proposals") + return { + "detector_frame_accounting_complete": detector_frame_count == 4489, + "geometry_frame_accounting_complete": ( + frames.get("total") == 4489 and frames.get("failed") == 0 + ), + "source_accounting_reconciles_e32_e53": ( + frames.get("source_available") == EXPECTED_SOURCE_AVAILABLE_FRAMES + and frames.get("source_unavailable") == EXPECTED_SOURCE_UNAVAILABLE_FRAMES + ), + "proposal_accounting_closed": total == ranged + camera + conflict, + "range_denominators_separated": 0 <= ranged <= eligible <= total, + "exclusive_point_ownership_validated_every_frame": True, + "missing_points_never_interpreted_as_free": True, + "range_requires_current_source_points": True, + "geometry_only_semantic_class_absent": True, + "authority_remains_false": True, + } + + +def _verified_historical_references(repository: Path) -> dict[str, object]: + references = { + "e32": ( + repository + / ".runtime/compute-experiments/e32/results" + / E32_RESULT_ID + / "manifest.json", + E32_MANIFEST_SHA256, + ), + "e53": ( + repository + / ".runtime/compute-experiments/e53/results" + / E53_RESULT_ID + / "manifest.json", + E53_MANIFEST_SHA256, + ), + } + result: dict[str, object] = {} + for role, (path, digest) in references.items(): + if not path.is_file() or path.is_symlink() or _file_sha256(path) != digest: + raise GeometryReplayError(f"accepted {role.upper()} reference changed") + result[role] = {"result_id": path.parent.name, "manifest_sha256": digest} + return result + + +def _validate_frame_ledger(path: Path, metrics: dict[str, object]) -> None: + frame_count = 0 + source_available = 0 + proposal_count = 0 + eligible_count = 0 + ranged_count = 0 + conflict_count = 0 + camera_count = 0 + geometry_count = 0 + point_rows = 0 + overlaps_removed = 0 + for line_number, line in enumerate(path.read_text("utf-8").splitlines(), start=1): + try: + value = json.loads(line) + except json.JSONDecodeError as exc: + raise GeometryReplayError(f"geometry frame {line_number} is invalid JSON") from exc + frame = _object(value, "geometry frame") + _exact_keys( + frame, + { + "schema_version", + "sequence", + "frame_id", + "source_available", + "proposal_count", + "eligible_proposal_count", + "ranged_proposal_count", + "conflict_proposal_count", + "camera_only_proposal_count", + "geometry_only_observation_count", + "overlapping_claims_removed", + "observations", + "policy", + "authority", + }, + "geometry frame", + ) + if frame.get("schema_version") != GEOMETRY_REPLAY_FRAME_SCHEMA: + raise GeometryReplayError("geometry frame schema changed") + if frame.get("sequence") != frame_count: + raise GeometryReplayError("geometry frame sequence is incomplete") + if frame.get("policy") != { + "one_owner_per_source_point": True, + "absence_of_points_means_free": False, + "geometry_can_invent_semantic_class": False, + }: + raise GeometryReplayError("geometry frame policy changed") + if frame.get("authority") != _false_authority(): + raise GeometryReplayError("geometry frame authority changed") + observations_value = frame.get("observations") + if not isinstance(observations_value, list): + raise GeometryReplayError("geometry observations are not an array") + observations = tuple(ObstacleObservation.from_dict(item) for item in observations_value) + frame_id = frame.get("frame_id") + proposal_observations = tuple(item for item in observations if item.proposal_ids) + geometry_observations = tuple(item for item in observations if not item.proposal_ids) + if ( + not isinstance(frame_id, str) + or any( + item.frame_id != frame_id or item.source_id != "RAVNOVES00" + for item in observations + ) + or any(len(item.proposal_ids) != 1 for item in proposal_observations) + or len(proposal_observations) + != _integer(frame.get("proposal_count"), "frame proposals") + or len(geometry_observations) + != _integer(frame.get("geometry_only_observation_count"), "geometry-only") + or sum(_range_eligible(item) for item in proposal_observations) + != _integer(frame.get("eligible_proposal_count"), "eligible proposals") + or sum(item.metric_geometry is not None for item in proposal_observations) + != _integer(frame.get("ranged_proposal_count"), "ranged proposals") + or sum(item.basis is EvidenceBasis.CONFLICT for item in proposal_observations) + != _integer(frame.get("conflict_proposal_count"), "conflicts") + ): + raise GeometryReplayError("geometry frame observation accounting changed") + owners: set[int] = set() + for observation in observations: + if owners.intersection(observation.source_point_ids): + raise GeometryReplayError("geometry frame has duplicate point ownership") + owners.update(observation.source_point_ids) + if not observation.proposal_ids and observation.semantic_hint is not None: + raise GeometryReplayError("geometry-only observation invented a semantic class") + if observation.metric_geometry is not None and not observation.source_point_ids: + raise GeometryReplayError("metric range lost its source points") + frame_count += 1 + source_available += frame.get("source_available") is True + proposal_count += _integer(frame.get("proposal_count"), "frame proposals") + eligible_count += _integer(frame.get("eligible_proposal_count"), "eligible proposals") + ranged_count += _integer(frame.get("ranged_proposal_count"), "ranged proposals") + conflict_count += _integer(frame.get("conflict_proposal_count"), "conflicts") + camera_count += _integer(frame.get("camera_only_proposal_count"), "camera-only") + geometry_count += _integer( + frame.get("geometry_only_observation_count"), + "geometry-only", + ) + overlaps_removed += _integer(frame.get("overlapping_claims_removed"), "overlaps") + point_rows += sum(len(item.source_point_ids) for item in observations) + frames = _object(metrics.get("frames"), "frame metrics") + proposals = _object(metrics.get("proposals"), "proposal metrics") + if ( + frame_count != frames.get("total") + or source_available != frames.get("source_available") + or frame_count - source_available != frames.get("source_unavailable") + or proposal_count != proposals.get("total") + or eligible_count != proposals.get("eligible_for_range") + or ranged_count != proposals.get("with_range") + or conflict_count != proposals.get("conflict") + or camera_count != proposals.get("camera_only") + or geometry_count != metrics.get("geometry_only_observations") + or point_rows != metrics.get("published_source_point_rows") + or overlaps_removed != metrics.get("overlapping_claims_removed") + ): + raise GeometryReplayError("geometry frame ledger and metrics disagree") + + +def _validated_artifact(root: Path, value: object, name: str) -> Path: + document = _object(value, "geometry artifact") + _exact_keys(document, {"role", "path", "bytes", "sha256"}, "geometry artifact") + if document.get("path") != name: + raise GeometryReplayError("geometry artifact path changed") + path = root / name + if not path.is_file() or path.is_symlink(): + raise GeometryReplayError("geometry artifact is missing") + if document.get("bytes") != path.stat().st_size or document.get("sha256") != _file_sha256(path): + raise GeometryReplayError("geometry artifact digest changed") + return path + + +def _artifact(path: Path, role: str) -> dict[str, object]: + return { + "role": role, + "path": path.name, + "bytes": path.stat().st_size, + "sha256": _file_sha256(path), + } + + +def _read_json(path: Path) -> dict[str, object]: + if not path.is_file() or path.is_symlink(): + raise GeometryReplayError("geometry JSON artifact is missing") + try: + return _object(json.loads(path.read_text("utf-8")), "geometry JSON artifact") + except json.JSONDecodeError as exc: + raise GeometryReplayError("geometry JSON artifact is invalid") from exc + + +def _write_json(path: Path, value: object) -> None: + path.write_bytes(_canonical_json(value) + b"\n") + + +def _canonical_json(value: object) -> bytes: + return json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode() + + +def _file_sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as handle: + for chunk in iter(lambda: handle.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _object(value: object, label: str) -> dict[str, object]: + if not isinstance(value, dict) or any(not isinstance(key, str) for key in value): + raise GeometryReplayError(f"{label} must be an object") + return value + + +def _exact_keys(document: dict[str, object], keys: set[str], label: str) -> None: + if set(document) != keys: + raise GeometryReplayError(f"{label} fields changed") + + +def _integer(value: object, label: str) -> int: + if not isinstance(value, int) or isinstance(value, bool) or value < 0: + raise GeometryReplayError(f"{label} must be a nonnegative integer") + return value + + +def _false_authority() -> dict[str, bool]: + return { + "ground_truth": False, + "physical_live": False, + "commands_enabled": False, + "actuation_allowed": False, + "navigation_or_safety_accepted": False, + } + + +__all__ = [ + "GEOMETRY_REPLAY_FRAME_SCHEMA", + "GEOMETRY_REPLAY_MANIFEST_NAME", + "GEOMETRY_REPLAY_REPORT_NAME", + "GEOMETRY_REPLAY_RESULT_PREFIX", + "GEOMETRY_REPLAY_SCHEMA", + "GeometryReplayError", + "GeometryReplayResult", + "build_geometry_replay", + "read_geometry_replay_result", +] diff --git a/src/k1link/perception/geometry_replay_cli.py b/src/k1link/perception/geometry_replay_cli.py new file mode 100644 index 0000000..29c4e86 --- /dev/null +++ b/src/k1link/perception/geometry_replay_cli.py @@ -0,0 +1,39 @@ +"""Command-line entrypoint for the local M4.4 full-source geometry replay.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from .geometry_replay import build_geometry_replay + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--repository-root", type=Path, default=Path(__file__).resolve().parents[3]) + parser.add_argument("--detector-result", type=Path, required=True) + parser.add_argument("--output-root", type=Path, required=True) + arguments = parser.parse_args(argv) + result = build_geometry_replay( + repository_root=arguments.repository_root, + detector_result_root=arguments.detector_result, + output_root=arguments.output_root, + ) + print( + json.dumps( + { + "result_id": result.result_id, + "result_root": str(result.result_root), + "accepted": result.accepted, + "metrics": result.metrics, + }, + sort_keys=True, + separators=(",", ":"), + ) + ) + return 0 if result.accepted else 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/test_geometry_association_provider.py b/tests/test_geometry_association_provider.py new file mode 100644 index 0000000..8249e4c --- /dev/null +++ b/tests/test_geometry_association_provider.py @@ -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, + ) diff --git a/tests/test_geometry_replay_result.py b/tests/test_geometry_replay_result.py new file mode 100644 index 0000000..5fe27fb --- /dev/null +++ b/tests/test_geometry_replay_result.py @@ -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" + ), + }, + } diff --git a/tests/test_perception_architecture.py b/tests/test_perception_architecture.py index 6d3657b..a93ffb6 100644 --- a/tests/test_perception_architecture.py +++ b/tests/test_perception_architecture.py @@ -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"):