"""Bounded spatial temporal occupancy for the Mission Core product graph.""" from __future__ import annotations import hashlib import json import math from collections import deque from dataclasses import dataclass, field from pathlib import Path from typing import Final, Protocol import numpy as np import numpy.typing as npt from .contracts import ( EvidenceCurrentness, GridCell, HistorySample, MotionState, ObstacleObservation, TemporalObstacle, TemporalState, ) from .providers import SourcePacket TEMPORAL_MOTION_PROFILE_SCHEMA: Final = "missioncore.temporal-motion-profile/v1" TEMPORAL_PROVIDER_ID: Final = "bounded-spatial-temporal-layer/v1" MOTION_PROVIDER_ID: Final = "class-independent-motion-estimator/v1" DEFAULT_TEMPORAL_MOTION_PROFILE_PATH: Final = Path( "config/perception/m4-temporal-motion-v1.json" ) FloatArray = npt.NDArray[np.float64] class TemporalProviderError(RuntimeError): """Temporal input, configuration or bounded state is incompatible.""" class CurrentPointResolver(Protocol): """Resolve the current frame's source-local point index space.""" def current_points(self, packet: SourcePacket) -> FloatArray | None: ... @dataclass(frozen=True, slots=True) class TemporalLayerProfile: coordinate_frame: str voxel_size_m: float occupied_ttl_seconds: float maximum_active_components: int maximum_cells_per_component: int maximum_history_samples: int association_maximum_gap_seconds: float association_maximum_centroid_distance_m: float association_minimum_voxel_overlap_fraction: float association_neighbor_radius_cells: int jump_maximum_adjacent_gap_seconds: float jump_minimum_matched_components: int jump_minimum_median_displacement_m: float jump_minimum_p25_displacement_m: float def __post_init__(self) -> None: positive = ( self.voxel_size_m, self.occupied_ttl_seconds, self.association_maximum_gap_seconds, self.association_maximum_centroid_distance_m, self.association_minimum_voxel_overlap_fraction, self.jump_maximum_adjacent_gap_seconds, self.jump_minimum_median_displacement_m, self.jump_minimum_p25_displacement_m, ) if any(not _positive_finite(value) for value in positive): raise TemporalProviderError("temporal numeric bound is invalid") if self.association_minimum_voxel_overlap_fraction > 1.0: raise TemporalProviderError("temporal overlap fraction is invalid") positive_integers = ( self.maximum_active_components, self.maximum_cells_per_component, self.maximum_history_samples, self.association_neighbor_radius_cells, self.jump_minimum_matched_components, ) if any(not _positive_integer(value) for value in positive_integers): raise TemporalProviderError("temporal integer bound is invalid") if self.maximum_history_samples > 32: raise TemporalProviderError("temporal history exceeds the product contract") @property def ttl_ns(self) -> int: return round(self.occupied_ttl_seconds * 1_000_000_000) @property def association_maximum_gap_ns(self) -> int: return round(self.association_maximum_gap_seconds * 1_000_000_000) @dataclass(frozen=True, slots=True) class MotionEstimatorProfile: minimum_observations: int minimum_span_seconds: float moving_minimum_displacement_m: float moving_minimum_speed_mps: float stationary_maximum_displacement_m: float stationary_maximum_speed_mps: float maximum_speed_mps: float full_confidence_observations: int full_confidence_span_seconds: float minimum_confidence: float def __post_init__(self) -> None: if any( not _positive_integer(value) for value in (self.minimum_observations, self.full_confidence_observations) ): raise TemporalProviderError("motion observation bound is invalid") numeric = ( self.minimum_span_seconds, self.moving_minimum_displacement_m, self.moving_minimum_speed_mps, self.stationary_maximum_displacement_m, self.stationary_maximum_speed_mps, self.maximum_speed_mps, self.full_confidence_span_seconds, self.minimum_confidence, ) if any(not _positive_finite(value) for value in numeric): raise TemporalProviderError("motion numeric bound is invalid") if not 0.0 < self.minimum_confidence <= 1.0: raise TemporalProviderError("motion confidence threshold is invalid") if ( self.stationary_maximum_displacement_m >= self.moving_minimum_displacement_m or self.stationary_maximum_speed_mps >= self.moving_minimum_speed_mps or self.moving_minimum_speed_mps >= self.maximum_speed_mps ): raise TemporalProviderError("motion thresholds have no conservative deadband") @dataclass(frozen=True, slots=True) class TemporalMotionProfile: profile_id: str source_id: str session_id: str geometry_result_id: str geometry_frames_sha256: str temporal: TemporalLayerProfile motion: MotionEstimatorProfile profile_sha256: str @dataclass(frozen=True, slots=True) class TemporalProviderSnapshot: input_frames: int completed_frames: int failed_frames: int input_observations: int current_occupied_observations: int nonmetric_uncertainty_observations: int created_components: int spatial_reassociations: int detector_identity_changes_reassociated: int current_publications: int held_publications: int expired_publications: int map_frame_jump_candidates: int peak_active_components: int peak_cells_per_component: int maximum_history_samples: int maximum_held_age_ns: int maximum_expiry_materialization_delay_ns: int past_ttl_occupied_publications: int @dataclass(frozen=True, slots=True) class _SpatialObservation: occupancy_key: str frame_id: str evidence_time_ns: int centroid_xyz_m: tuple[float, float, float] cells: frozenset[GridCell] semantic_hint: str | None @dataclass(slots=True) class _Component: component_id: str last_occupancy_key: str last_hit_ns: int cells: frozenset[GridCell] centroid_xyz_m: tuple[float, float, float] semantic_hint: str | None history: deque[HistorySample] = field(default_factory=deque) class BoundedSpatialTemporalProvider: """Publish hit-backed current, short-held unknown and cell-free expiry.""" provider_id: str = TEMPORAL_PROVIDER_ID def __init__( self, *, point_resolver: CurrentPointResolver, profile: TemporalMotionProfile, ) -> None: self.point_resolver = point_resolver self.profile = profile self.config = profile.temporal self._components: dict[str, _Component] = {} self._next_component = 1 self._previous_sequence: int | None = None self._previous_time_ns: int | None = None self._previous_observations: tuple[_SpatialObservation, ...] = () self._input_frames = 0 self._completed_frames = 0 self._failed_frames = 0 self._input_observations = 0 self._current_observations = 0 self._uncertainty_observations = 0 self._created_components = 0 self._spatial_reassociations = 0 self._identity_changes_reassociated = 0 self._current_publications = 0 self._held_publications = 0 self._expired_publications = 0 self._map_frame_jumps = 0 self._peak_active_components = 0 self._peak_cells = 0 self._maximum_history = 0 self._maximum_held_age_ns = 0 self._maximum_expiry_materialization_delay_ns = 0 def update( self, packet: SourcePacket, observations: tuple[ObstacleObservation, ...], ) -> tuple[TemporalObstacle, ...]: self._input_frames += 1 self._input_observations += len(observations) try: self._validate_packet(packet) spatial, uncertainty_count = self._spatial_observations(packet, observations) self._current_observations += len(spatial) self._uncertainty_observations += uncertainty_count now_ns = packet.envelope.timestamps.source_ns expired = self._expire(now_ns) jump = self._map_frame_jump(now_ns, spatial) if jump: self._map_frame_jumps += 1 assignments = {} if jump else self._assign(now_ns, spatial) matched: set[str] = set() current: list[TemporalObstacle] = [] for observation_index, observation in enumerate(spatial): component_id = assignments.get(observation_index) if component_id is None: component = self._create(observation) basis = "map-frame-discontinuity" if jump else "new-spatial-hit" else: component = self._components[component_id] if component.last_occupancy_key != observation.occupancy_key: self._identity_changes_reassociated += 1 self._observe(component, observation) self._spatial_reassociations += 1 basis = "spatial-reassociation" matched.add(component.component_id) current.append( self._contract( component, state=TemporalState.CURRENT, now_ns=now_ns, association_basis=basis, ) ) held: list[TemporalObstacle] = [] for component_id, component in sorted(self._components.items()): if component_id in matched: continue age_ns = now_ns - component.last_hit_ns if not 0 < age_ns <= self.config.ttl_ns: raise TemporalProviderError("temporal component escaped its TTL") self._maximum_held_age_ns = max(self._maximum_held_age_ns, age_ns) held.append( self._contract( component, state=TemporalState.HELD, now_ns=now_ns, association_basis="ttl-hold", ) ) if len(self._components) > self.config.maximum_active_components: raise TemporalProviderError("active temporal component bound exceeded") self._peak_active_components = max( self._peak_active_components, len(self._components), ) self._current_publications += len(current) self._held_publications += len(held) self._expired_publications += len(expired) self._previous_sequence = packet.envelope.sequence self._previous_time_ns = now_ns self._previous_observations = spatial self._completed_frames += 1 return tuple( sorted( (*current, *held, *expired), key=lambda item: (item.state.value, item.component_id), ) ) except Exception: self._failed_frames += 1 raise def _validate_packet(self, packet: SourcePacket) -> None: envelope = packet.envelope if ( envelope.source_id != self.profile.source_id or envelope.session_id != self.profile.session_id ): raise TemporalProviderError("packet escaped the temporal source profile") now_ns = envelope.timestamps.source_ns if self._previous_sequence is not None and ( envelope.sequence <= self._previous_sequence or self._previous_time_ns is None or now_ns <= self._previous_time_ns ): raise TemporalProviderError("temporal packet order is not monotonic") def _spatial_observations( self, packet: SourcePacket, observations: tuple[ObstacleObservation, ...], ) -> tuple[tuple[_SpatialObservation, ...], int]: envelope = packet.envelope if any( item.source_id != envelope.source_id or item.frame_id != envelope.frame_id or item.evidence_time_ns != envelope.timestamps.source_ns for item in observations ): raise TemporalProviderError("observation escaped its source packet") qualified = tuple( item for item in observations if item.currentness is EvidenceCurrentness.CURRENT and item.occupied_support ) uncertainty_count = len(observations) - len(qualified) if not qualified: return (), uncertainty_count frame_points = self.point_resolver.current_points(packet) if frame_points is None: raise TemporalProviderError("current occupied evidence has no point index space") points = np.asarray(frame_points, dtype=np.float64) if points.ndim != 2 or points.shape[1] != 3 or not np.isfinite(points).all(): raise TemporalProviderError("resolved current point frame is invalid") result: list[_SpatialObservation] = [] for observation in sorted(qualified, key=lambda item: item.observation_id): metric = observation.metric_geometry if metric is None or metric.coordinate_frame != self.config.coordinate_frame: raise TemporalProviderError("occupied observation has incompatible metric geometry") indices = np.asarray(observation.source_point_ids, dtype=np.int64) if ( indices.size == 0 or int(indices[0]) < 0 or int(indices[-1]) >= points.shape[0] ): raise TemporalProviderError("occupied observation point indices are invalid") owned = points[indices] cell_rows = np.unique( np.floor(owned / self.config.voxel_size_m).astype(np.int64), axis=0, ) if cell_rows.shape[0] > self.config.maximum_cells_per_component: raise TemporalProviderError("temporal component cell bound exceeded") self._peak_cells = max(self._peak_cells, int(cell_rows.shape[0])) centroid = np.median(owned, axis=0) result.append( _SpatialObservation( occupancy_key=observation.occupancy_key, frame_id=observation.frame_id, evidence_time_ns=observation.evidence_time_ns, centroid_xyz_m=( float(centroid[0]), float(centroid[1]), float(centroid[2]), ), cells=frozenset( GridCell(int(row[0]), int(row[1]), int(row[2])) for row in cell_rows ), semantic_hint=observation.semantic_hint, ) ) return tuple(result), uncertainty_count def _expire(self, now_ns: int) -> tuple[TemporalObstacle, ...]: expired: list[TemporalObstacle] = [] for component_id, component in tuple(sorted(self._components.items())): age_ns = now_ns - component.last_hit_ns if age_ns <= self.config.ttl_ns: continue materialization_delay = age_ns - self.config.ttl_ns self._maximum_expiry_materialization_delay_ns = max( self._maximum_expiry_materialization_delay_ns, materialization_delay, ) expired.append( self._contract( component, state=TemporalState.EXPIRED, now_ns=now_ns, association_basis="ttl-expired", ) ) del self._components[component_id] return tuple(expired) def _assign( self, now_ns: int, observations: tuple[_SpatialObservation, ...], ) -> dict[int, str]: candidates: list[tuple[float, str, int]] = [] for observation_index, observation in enumerate(observations): for component_id, component in self._components.items(): age_ns = now_ns - component.last_hit_ns if not 0 < age_ns <= self.config.association_maximum_gap_ns: continue distance = math.dist(observation.centroid_xyz_m, component.centroid_xyz_m) if distance > self.config.association_maximum_centroid_distance_m: continue if distance <= self.config.voxel_size_m or _overlap_at_least( observation.cells, component.cells, minimum_fraction=( self.config.association_minimum_voxel_overlap_fraction ), neighbor_radius=self.config.association_neighbor_radius_cells, ): candidates.append((distance, component_id, observation_index)) assignments: dict[int, str] = {} used_components: set[str] = set() for _, component_id, observation_index in sorted(candidates): if component_id in used_components or observation_index in assignments: continue used_components.add(component_id) assignments[observation_index] = component_id return assignments def _create(self, observation: _SpatialObservation) -> _Component: component = _Component( component_id=f"temporal-{self._next_component:08d}", last_occupancy_key=observation.occupancy_key, last_hit_ns=observation.evidence_time_ns, cells=observation.cells, centroid_xyz_m=observation.centroid_xyz_m, semantic_hint=observation.semantic_hint, history=deque(maxlen=self.config.maximum_history_samples), ) component.history.append(_history(observation)) self._components[component.component_id] = component self._next_component += 1 self._created_components += 1 self._maximum_history = max(self._maximum_history, len(component.history)) return component def _observe(self, component: _Component, observation: _SpatialObservation) -> None: component.last_occupancy_key = observation.occupancy_key component.last_hit_ns = observation.evidence_time_ns component.cells = observation.cells component.centroid_xyz_m = observation.centroid_xyz_m component.semantic_hint = observation.semantic_hint component.history.append(_history(observation)) self._maximum_history = max(self._maximum_history, len(component.history)) def _contract( self, component: _Component, *, state: TemporalState, now_ns: int, association_basis: str, ) -> TemporalObstacle: active = state is not TemporalState.EXPIRED return TemporalObstacle( component_id=component.component_id, identity_scope="ephemeral", state=state, ttl_ns=self.config.ttl_ns, last_hit_ns=component.last_hit_ns, age_ns=now_ns - component.last_hit_ns, association_basis=association_basis, history=tuple(component.history), cells=tuple(sorted(component.cells, key=lambda cell: (cell.x, cell.y, cell.z))) if active else (), coordinate_frame=self.config.coordinate_frame if active else None, last_centroid_xyz_m=component.centroid_xyz_m if active else None, motion=MotionState.UNKNOWN, motion_confidence=0.0, motion_reason="motion-not-estimated", semantic_hint=component.semantic_hint, ) def _map_frame_jump( self, now_ns: int, current: tuple[_SpatialObservation, ...], ) -> bool: if ( self._previous_time_ns is None or now_ns - self._previous_time_ns > round(self.config.jump_maximum_adjacent_gap_seconds * 1_000_000_000) or len(self._previous_observations) < self.config.jump_minimum_matched_components or len(current) < self.config.jump_minimum_matched_components ): return False displacements = _rank_aligned_displacements( self._previous_observations, current, residual_gate=self.config.voxel_size_m, minimum_matches=self.config.jump_minimum_matched_components, ) if len(displacements) < self.config.jump_minimum_matched_components: return False distances = np.linalg.norm(displacements, axis=1) return bool( np.median(distances) >= self.config.jump_minimum_median_displacement_m and np.percentile(distances, 25) >= self.config.jump_minimum_p25_displacement_m ) def snapshot(self) -> TemporalProviderSnapshot: return TemporalProviderSnapshot( input_frames=self._input_frames, completed_frames=self._completed_frames, failed_frames=self._failed_frames, input_observations=self._input_observations, current_occupied_observations=self._current_observations, nonmetric_uncertainty_observations=self._uncertainty_observations, created_components=self._created_components, spatial_reassociations=self._spatial_reassociations, detector_identity_changes_reassociated=self._identity_changes_reassociated, current_publications=self._current_publications, held_publications=self._held_publications, expired_publications=self._expired_publications, map_frame_jump_candidates=self._map_frame_jumps, peak_active_components=self._peak_active_components, peak_cells_per_component=self._peak_cells, maximum_history_samples=self._maximum_history, maximum_held_age_ns=self._maximum_held_age_ns, maximum_expiry_materialization_delay_ns=( self._maximum_expiry_materialization_delay_ns ), past_ttl_occupied_publications=0, ) def load_temporal_motion_profile(path: Path) -> TemporalMotionProfile: resolved = path.resolve(strict=True) if resolved.is_symlink() or not resolved.is_file(): raise TemporalProviderError("temporal motion profile is not a regular file") raw = resolved.read_bytes() try: document = _object(json.loads(raw), "temporal motion profile") except json.JSONDecodeError as exc: raise TemporalProviderError("temporal motion profile JSON is invalid") from exc _exact_keys( document, { "schema_version", "profile_id", "temporal_provider_id", "motion_provider_id", "source", "temporal", "motion", "policy", "authority", }, "temporal motion profile", ) if ( document["schema_version"] != TEMPORAL_MOTION_PROFILE_SCHEMA or document["temporal_provider_id"] != TEMPORAL_PROVIDER_ID or document["motion_provider_id"] != MOTION_PROVIDER_ID ): raise TemporalProviderError("temporal motion profile identity is incompatible") source = _object(document["source"], "temporal source") temporal = _object(document["temporal"], "temporal bounds") motion = _object(document["motion"], "motion bounds") _exact_keys( source, { "source_id", "session_id", "geometry_result_id", "geometry_frames_sha256", }, "temporal source", ) _exact_keys( temporal, set(TemporalLayerProfile.__dataclass_fields__), "temporal bounds", ) _exact_keys( motion, set(MotionEstimatorProfile.__dataclass_fields__), "motion bounds", ) if document["policy"] != { "component_identity_scope": "ephemeral", "association_uses_detector_id": False, "association_uses_semantic_class": False, "motion_uses_semantic_class": False, "camera_only_creates_occupied_component": False, "absence_of_points_means_free": False, "held_state": "unknown", "expired_cells_published": False, "long_term_identity_available": False, "history_is_bounded": True, }: raise TemporalProviderError("temporal motion policy is incompatible") if document["authority"] != { "ground_truth": False, "physical_live": False, "commands_enabled": False, "actuation_allowed": False, "navigation_or_safety_accepted": False, }: raise TemporalProviderError("temporal motion authority is incompatible") return TemporalMotionProfile( profile_id=_string(document, "profile_id"), source_id=_string(source, "source_id"), session_id=_string(source, "session_id"), geometry_result_id=_string(source, "geometry_result_id"), geometry_frames_sha256=_digest(source, "geometry_frames_sha256"), temporal=TemporalLayerProfile(**temporal), # type: ignore[arg-type] motion=MotionEstimatorProfile(**motion), # type: ignore[arg-type] profile_sha256=hashlib.sha256(raw).hexdigest(), ) def _history(observation: _SpatialObservation) -> HistorySample: return HistorySample( frame_id=observation.frame_id, evidence_time_ns=observation.evidence_time_ns, centroid_xyz_m=observation.centroid_xyz_m, ) def _overlap_at_least( left: frozenset[GridCell], right: frozenset[GridCell], *, minimum_fraction: float, neighbor_radius: int, ) -> bool: smaller, larger = (left, right) if len(left) <= len(right) else (right, left) required = max(1, math.ceil(len(smaller) * minimum_fraction)) matched = 0 for cell in smaller: if any( GridCell(cell.x + dx, cell.y + dy, cell.z + dz) in larger for dx in range(-neighbor_radius, neighbor_radius + 1) for dy in range(-neighbor_radius, neighbor_radius + 1) for dz in range(-neighbor_radius, neighbor_radius + 1) ): matched += 1 if matched >= required: return True return False def _rank_aligned_displacements( previous: tuple[_SpatialObservation, ...], current: tuple[_SpatialObservation, ...], *, residual_gate: float, minimum_matches: int, ) -> FloatArray: before = sorted(previous, key=lambda item: item.centroid_xyz_m) after = sorted(current, key=lambda item: item.centroid_xyz_m) best = np.empty((0, 3), dtype=np.float64) for before_start in range(max(1, len(before) - minimum_matches + 1)): for after_start in range(max(1, len(after) - minimum_matches + 1)): count = min(len(before) - before_start, len(after) - after_start) if count < minimum_matches: continue left = np.asarray( [item.centroid_xyz_m for item in before[before_start : before_start + count]], dtype=np.float64, ) right = np.asarray( [item.centroid_xyz_m for item in after[after_start : after_start + count]], dtype=np.float64, ) vectors = right - left median = np.median(vectors, axis=0) coherent = vectors[np.linalg.norm(vectors - median, axis=1) <= residual_gate] if coherent.shape[0] > best.shape[0]: best = coherent return best def _positive_finite(value: object) -> bool: return ( isinstance(value, (int, float)) and not isinstance(value, bool) and math.isfinite(float(value)) and float(value) > 0.0 ) def _positive_integer(value: object) -> bool: return isinstance(value, int) and not isinstance(value, bool) and value > 0 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 TemporalProviderError(f"{label} must be an object") return value def _exact_keys(document: dict[str, object], expected: set[str], label: str) -> None: if set(document) != expected: raise TemporalProviderError(f"{label} fields are incompatible") def _string(document: dict[str, object], key: str) -> str: value = document.get(key) if not isinstance(value, str) or not value: raise TemporalProviderError(f"{key} must be a nonempty string") return value def _digest(document: dict[str, object], key: str) -> str: value = _string(document, key) if len(value) != 64 or any(character not in "0123456789abcdef" for character in value): raise TemporalProviderError(f"{key} must be a SHA-256 digest") return value __all__ = [ "DEFAULT_TEMPORAL_MOTION_PROFILE_PATH", "MOTION_PROVIDER_ID", "TEMPORAL_MOTION_PROFILE_SCHEMA", "TEMPORAL_PROVIDER_ID", "BoundedSpatialTemporalProvider", "CurrentPointResolver", "MotionEstimatorProfile", "TemporalLayerProfile", "TemporalMotionProfile", "TemporalProviderError", "TemporalProviderSnapshot", "load_temporal_motion_profile", ]