762 lines
29 KiB
Python
762 lines
29 KiB
Python
"""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",
|
|
]
|