feat(perception): canonicalize temporal motion
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,761 @@
|
||||
"""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",
|
||||
]
|
||||
Reference in New Issue
Block a user