"""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 RecordedFrameTemporalBinding: """Digest-bound recorded timing evidence for one camera-indexed increment. The shared session time binds the camera ordinal to the E10 pack entry. The LiDAR and pose deltas retain their admitted E6 meaning: nearest host-arrival best effort, not hardware synchronization. """ frame_index: int source_time_ns: int source_available: bool lidar_camera_delta_ms: float | None pose_point_delta_ms: float | None @dataclass(frozen=True, slots=True) class ReplayBodyFrameInputs: """Verified inputs required to derive one replay-only virtual body frame.""" sensor_position_map: FloatArray sensor_orientation_map_from_lidar_xyzw: FloatArray ground_plane_coefficients_map: FloatArray sensor_height_m: float surface_slope_deg: float trajectory_start_position_map: FloatArray trajectory_end_position_map: FloatArray t_camera_from_lidar: FloatArray @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 = self.frame_for_index(envelope.sequence) if frame is None: raise GeometryProviderError("packet claims unavailable source geometry as current") return frame def frame_for_index(self, frame_index: int) -> GeometryFrame | None: """Expose one verified source increment with its pose and KB4 calibration. This read-only seam is intentionally narrower than the source archive. It exists for deterministic replay diagnostics which must project the exact frame-local point index space without manufacturing a ``SourcePacket``. An unavailable recorded increment remains ``None``; surface validity is retained on the returned frame rather than silently filtering its points. """ if not isinstance(frame_index, int) or isinstance(frame_index, bool): raise GeometryProviderError("replay geometry frame index is invalid") if not 0 <= frame_index < self.profile.frame_count: raise GeometryProviderError("replay geometry frame 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]): return None 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 temporal_binding_for_index(self, frame_index: int) -> RecordedFrameTemporalBinding: """Return the sealed ordinal/session binding and admitted best-effort deltas.""" if not isinstance(frame_index, int) or isinstance(frame_index, bool): raise GeometryProviderError("replay temporal frame index is invalid") if not 0 <= frame_index < self.profile.frame_count: raise GeometryProviderError("replay temporal frame is outside the profile") if ( int(self._source["frame_indices"][frame_index]) != frame_index or int(self._source["source_frame_indices"][frame_index]) != frame_index ): raise GeometryProviderError("source pack temporal sequence changed") session_seconds = float(self._source["session_seconds"][frame_index]) if not math.isfinite(session_seconds) or session_seconds < 0.0: raise GeometryProviderError("source pack session time is invalid") source_available = bool(self._source["sample_available"][frame_index]) lidar_delta = float(self._source["lidar_camera_delta_ms"][frame_index]) pose_delta = float(self._source["pose_point_delta_ms"][frame_index]) if source_available: if not math.isfinite(lidar_delta) or not math.isfinite(pose_delta): raise GeometryProviderError("available source temporal deltas are invalid") lidar_value: float | None = lidar_delta pose_value: float | None = pose_delta else: if not math.isnan(lidar_delta) or not math.isnan(pose_delta): raise GeometryProviderError("unavailable source carries temporal deltas") lidar_value = None pose_value = None return RecordedFrameTemporalBinding( frame_index=frame_index, source_time_ns=round(session_seconds * 1_000_000_000), source_available=source_available, lidar_camera_delta_ms=lidar_value, pose_point_delta_ms=pose_value, ) def current_points(self, packet: SourcePacket) -> FloatArray | None: """Expose the verified frame-local point index space to temporal occupancy.""" frame = self.frame(packet) if frame is None or not frame.surface_valid: return None points = np.asarray(frame.points_map, dtype=np.float64) points.setflags(write=False) return points def current_points_for_frame(self, frame_index: int) -> FloatArray | None: """Expose one verified increment to a read-only recorded-evidence projector.""" if not isinstance(frame_index, int) or isinstance(frame_index, bool): raise GeometryProviderError("replay evidence frame index is invalid") if not 0 <= frame_index < self.profile.frame_count: raise GeometryProviderError("replay evidence frame is outside the source 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]) or not bool( self._surface["frame_valid"][frame_index] ): return None offsets = self._source["cloud_offsets"] start, end = int(offsets[frame_index]), int(offsets[frame_index + 1]) points = np.asarray(self._source["cloud_points_map"][start:end], dtype=np.float64) points.setflags(write=False) return points def playback_points_map(self) -> FloatArray: """Expose the sealed contiguous map-point track for binary LAB playback. The returned array is the exact source-pack point index space. It is read-only and deliberately excludes any UI projection or resampling so the browser can retain it once and derive the current increment by the verified offsets below. """ points = np.asarray(self._source["cloud_points_map"], dtype=np.dtype(" tuple[int, ...]: """Return immutable offsets into :meth:`playback_points_map`.""" offsets = np.asarray(self._source["cloud_offsets"], dtype=np.int64) return tuple(int(value) for value in offsets) def point_step_candidates_for_frame(self, frame_index: int) -> UInt8Array | None: """Expose the sealed low-step diagnostic in the source point index space. The array is evidence only: a non-zero value may add conservative occupied/unknown support, but it never clears a cell or claims free space. Unavailable and surface-invalid frames remain unavailable. """ frame = self.frame_for_index(frame_index) if frame is None or not frame.surface_valid: return None offsets = self._source["cloud_offsets"] start, end = int(offsets[frame_index]), int(offsets[frame_index + 1]) values = np.asarray( self._surface["point_step_candidate"][start:end], dtype=np.uint8, ) if values.shape != (frame.source_point_count,): raise GeometryProviderError("local-surface step evidence changed") values.setflags(write=False) return values @property def maximum_current_point_count(self) -> int: """Return the immutable source-pack upper bound for one recorded increment.""" counts = np.diff(np.asarray(self._source["cloud_offsets"], dtype=np.int64)) return int(counts.max(initial=0)) def pose_values_for_frame( self, frame_id: str, ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]] | None: """Return one verified replay pose without exposing the source archive.""" prefix = "frame-" if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit(): raise GeometryProviderError("replay pose frame identity is invalid") frame_index = int(frame_id[len(prefix) :]) if not 0 <= frame_index < self.profile.frame_count: raise GeometryProviderError("replay pose frame is outside the source profile") if not bool(self._source["sample_available"][frame_index]): return None position = np.asarray( self._source["pose_positions_map"][frame_index], dtype=np.float64, ) orientation = np.asarray( self._source["pose_quaternions_map_from_lidar"][frame_index], dtype=np.float64, ) if not np.isfinite(position).all() or not np.isfinite(orientation).all(): raise GeometryProviderError("available replay pose is not finite") return ( (float(position[0]), float(position[1]), float(position[2])), ( float(orientation[0]), float(orientation[1]), float(orientation[2]), float(orientation[3]), ), ) def replay_body_frame_inputs( self, frame_id: str, *, trajectory_half_window_frames: int, ) -> ReplayBodyFrameInputs | None: """Return source-bound pose, surface and route evidence without inventing axes.""" if trajectory_half_window_frames < 1: raise GeometryProviderError("trajectory half-window must be positive") prefix = "frame-" if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit(): raise GeometryProviderError("replay body frame identity is invalid") frame_index = int(frame_id[len(prefix) :]) if not 0 <= frame_index < self.profile.frame_count: raise GeometryProviderError("replay body frame is outside the source profile") if not bool(self._source["sample_available"][frame_index]) or not bool( self._surface["frame_valid"][frame_index] ): return None required = { "plane_coefficients_map", "sensor_height_m", "slope_deg", } if not required.issubset(self._surface): raise GeometryProviderError("local surface lacks replay body-frame evidence") first = max(0, frame_index - trajectory_half_window_frames) last = min(self.profile.frame_count, frame_index + trajectory_half_window_frames + 1) available = np.flatnonzero(self._source["sample_available"][first:last]) + first if available.size == 0: return None values = ReplayBodyFrameInputs( sensor_position_map=np.asarray( self._source["pose_positions_map"][frame_index], dtype=np.float64 ), sensor_orientation_map_from_lidar_xyzw=np.asarray( self._source["pose_quaternions_map_from_lidar"][frame_index], dtype=np.float64, ), ground_plane_coefficients_map=np.asarray( self._surface["plane_coefficients_map"][frame_index], dtype=np.float64, ), sensor_height_m=float(self._surface["sensor_height_m"][frame_index]), surface_slope_deg=float(self._surface["slope_deg"][frame_index]), trajectory_start_position_map=np.asarray( self._source["pose_positions_map"][int(available[0])], dtype=np.float64 ), trajectory_end_position_map=np.asarray( self._source["pose_positions_map"][int(available[-1])], dtype=np.float64 ), t_camera_from_lidar=np.asarray(self._source["t_camera_from_lidar"], dtype=np.float64), ) if not all( np.isfinite(value).all() for value in ( values.sensor_position_map, values.sensor_orientation_map_from_lidar_xyzw, values.ground_plane_coefficients_map, values.trajectory_start_position_map, values.trajectory_end_position_map, values.t_camera_from_lidar, ) ) or not math.isfinite(values.sensor_height_m + values.surface_slope_deg): raise GeometryProviderError("replay body-frame evidence is not finite") return values def available_frame_indices(self) -> tuple[int, ...]: """Expose the immutable availability partition for deterministic sampling.""" return tuple(int(index) for index in np.flatnonzero(self._source["sample_available"])) def _validate(self) -> None: source_required = { "frame_indices", "source_frame_indices", "session_seconds", "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", "lidar_camera_delta_ms", "pose_point_delta_ms", } 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,), "source_frame_indices": (frames,), "session_seconds": (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), "lidar_camera_delta_ms": (frames,), "pose_point_delta_ms": (frames,), } 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") expected_indices = np.arange(frames, dtype=np.int64) session_seconds = np.asarray(self._source["session_seconds"], dtype=np.float64) if ( not np.array_equal(self._source["frame_indices"], expected_indices) or not np.array_equal(self._source["source_frame_indices"], expected_indices) or not np.isfinite(session_seconds).all() or np.any(session_seconds < 0.0) or np.any(np.diff(session_seconds) <= 0.0) ): raise GeometryProviderError("source temporal index changed") available = np.asarray(self._source["sample_available"], dtype=np.bool_) lidar_deltas = np.asarray(self._source["lidar_camera_delta_ms"], dtype=np.float64) pose_deltas = np.asarray(self._source["pose_point_delta_ms"], dtype=np.float64) if ( not np.isfinite(lidar_deltas[available]).all() or not np.isfinite(pose_deltas[available]).all() or not np.isnan(lidar_deltas[~available]).all() or not np.isnan(pose_deltas[~available]).all() ): raise GeometryProviderError("source temporal delta availability changed") 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", "RecordedFrameTemporalBinding", "RecordedGeometryStore", "load_geometry_profile", ]