feat(perception): establish object centric contracts
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,274 @@
|
||||
"""One-way adapters from admitted historical primitives to product contracts.
|
||||
|
||||
The adapters do not mutate TrackGeometry/E34 documents and never upgrade held,
|
||||
persistent or missing evidence into current metric occupancy.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from collections.abc import Mapping
|
||||
|
||||
import numpy as np
|
||||
|
||||
from k1link.compute.temporal_occupied_layer import TemporalFrameProjection
|
||||
from k1link.compute.track_geometry import (
|
||||
TrackGeometryCurrentness,
|
||||
TrackGeometryEvidenceState,
|
||||
TrackGeometryFrame,
|
||||
TrackGeometryMetricBasis,
|
||||
TrackGeometryOwnerKind,
|
||||
)
|
||||
|
||||
from .contracts import (
|
||||
EvidenceBasis,
|
||||
EvidenceCurrentness,
|
||||
GridCell,
|
||||
HistorySample,
|
||||
MetricGeometry,
|
||||
MotionState,
|
||||
ObstacleObservation,
|
||||
TemporalObstacle,
|
||||
TemporalState,
|
||||
validate_exclusive_point_ownership,
|
||||
)
|
||||
|
||||
|
||||
class PerceptionAdapterError(ValueError):
|
||||
"""An admitted historical value cannot be represented without inventing evidence."""
|
||||
|
||||
|
||||
def observations_from_track_geometry(
|
||||
frame: TrackGeometryFrame,
|
||||
*,
|
||||
source_id: str,
|
||||
frame_id: str,
|
||||
evidence_time_ns: int,
|
||||
proposal_ids_by_owner: Mapping[str, tuple[str, ...]] | None = None,
|
||||
) -> tuple[ObstacleObservation, ...]:
|
||||
"""Adapt TrackGeometry v1 while preserving exact point ownership and uncertainty."""
|
||||
|
||||
proposals = proposal_ids_by_owner or {}
|
||||
observations: list[ObstacleObservation] = []
|
||||
for geometry in frame.geometries:
|
||||
source_indices = frame.point_slab.owned_source_indices(geometry.owner_key)
|
||||
source_point_ids = tuple(int(value) for value in source_indices.tolist())
|
||||
metric_geometry: MetricGeometry | None = None
|
||||
currentness = _currentness(geometry.currentness)
|
||||
occupied_support = False
|
||||
if geometry.metric_basis is TrackGeometryMetricBasis.CURRENT_POINTS:
|
||||
if not source_point_ids or geometry.range_m is None:
|
||||
raise PerceptionAdapterError(
|
||||
"current TrackGeometry lost its qualified point support"
|
||||
)
|
||||
owner_index = frame.point_slab.owner_keys.index(geometry.owner_key)
|
||||
points = frame.point_slab.points_xyz_m[
|
||||
frame.point_slab.owner_indices == owner_index
|
||||
].astype(np.float64, copy=False)
|
||||
centroid = points.mean(axis=0)
|
||||
covariance = points.var(axis=0)
|
||||
metric_geometry = MetricGeometry(
|
||||
coordinate_frame=frame.binding.coordinate_frame,
|
||||
centroid_xyz_m=(float(centroid[0]), float(centroid[1]), float(centroid[2])),
|
||||
range_m=float(geometry.range_m),
|
||||
covariance_diagonal_m2=(
|
||||
float(covariance[0]),
|
||||
float(covariance[1]),
|
||||
float(covariance[2]),
|
||||
),
|
||||
)
|
||||
occupied_support = True
|
||||
else:
|
||||
source_point_ids = ()
|
||||
|
||||
basis = _basis(geometry.evidence_state, geometry.owner_kind)
|
||||
if basis in {EvidenceBasis.CAMERA, EvidenceBasis.CONFLICT}:
|
||||
metric_geometry = None
|
||||
source_point_ids = ()
|
||||
occupied_support = False
|
||||
if currentness is not EvidenceCurrentness.CURRENT:
|
||||
metric_geometry = None
|
||||
source_point_ids = ()
|
||||
occupied_support = False
|
||||
|
||||
reason_codes = list(geometry.reason_codes)
|
||||
if geometry.currentness is TrackGeometryCurrentness.PERSISTENT:
|
||||
reason_codes.append("persistent-model-not-product-authority")
|
||||
observation = ObstacleObservation(
|
||||
observation_id=f"{frame_id}:{geometry.owner_key}",
|
||||
occupancy_key=f"{frame_id}:{geometry.owner_key}",
|
||||
source_id=source_id,
|
||||
frame_id=frame_id,
|
||||
evidence_time_ns=evidence_time_ns,
|
||||
basis=basis,
|
||||
currentness=currentness,
|
||||
occupied_support=occupied_support,
|
||||
source_point_ids=source_point_ids,
|
||||
metric_geometry=metric_geometry,
|
||||
proposal_ids=proposals.get(geometry.owner_key, ()),
|
||||
semantic_hint=geometry.semantic_label,
|
||||
reason_codes=tuple(dict.fromkeys(reason_codes)),
|
||||
)
|
||||
if basis is EvidenceBasis.CAMERA and not observation.proposal_ids:
|
||||
raise PerceptionAdapterError(
|
||||
"camera-only geometry requires its source proposal binding"
|
||||
)
|
||||
observations.append(observation)
|
||||
result = tuple(observations)
|
||||
validate_exclusive_point_ownership(result)
|
||||
return result
|
||||
|
||||
|
||||
def temporal_obstacles_from_e34_projection(
|
||||
projection: TemporalFrameProjection,
|
||||
*,
|
||||
coordinate_frame: str,
|
||||
ttl_ns: int,
|
||||
) -> tuple[TemporalObstacle, ...]:
|
||||
"""Adapt current/held/expired E34 components without persistent identity claims."""
|
||||
|
||||
document = projection.document
|
||||
results: list[TemporalObstacle] = []
|
||||
for collection_name, state in (
|
||||
("current", TemporalState.CURRENT),
|
||||
("held", TemporalState.HELD),
|
||||
("expired", TemporalState.EXPIRED),
|
||||
):
|
||||
collection = document.get(collection_name)
|
||||
if not isinstance(collection, list):
|
||||
raise PerceptionAdapterError(f"E34 projection {collection_name} must be an array")
|
||||
for value in collection:
|
||||
component = _object(value, f"E34 {collection_name} component")
|
||||
temporal_id = _integer(component.get("temporal_id"), "E34 temporal id")
|
||||
age_seconds = _number(
|
||||
component.get("last_observed_age_seconds"),
|
||||
"E34 component age",
|
||||
)
|
||||
age_ns = max(0, int(round(age_seconds * 1_000_000_000)))
|
||||
history = _history(component.get("history_tail"))
|
||||
cells = _cells(projection, component) if state is not TemporalState.EXPIRED else ()
|
||||
centroid = (
|
||||
None
|
||||
if not cells
|
||||
else _vector3(component.get("centroid_map_xyz_m"), "E34 centroid")
|
||||
)
|
||||
labels = _semantic_labels(component.get("semantic_provenance"))
|
||||
results.append(
|
||||
TemporalObstacle(
|
||||
component_id=f"e34-component-{temporal_id}",
|
||||
identity_scope="ephemeral",
|
||||
state=state,
|
||||
ttl_ns=ttl_ns,
|
||||
last_hit_ns=max(0, history[-1].evidence_time_ns),
|
||||
age_ns=age_ns,
|
||||
association_basis=_safe_reason(component.get("association_reason")),
|
||||
history=history,
|
||||
cells=cells,
|
||||
coordinate_frame=coordinate_frame if cells else None,
|
||||
last_centroid_xyz_m=centroid,
|
||||
motion=MotionState.UNKNOWN,
|
||||
motion_confidence=0.0,
|
||||
motion_reason="motion-not-estimated",
|
||||
semantic_hint=labels[-1] if labels else None,
|
||||
)
|
||||
)
|
||||
component_ids = [item.component_id for item in results]
|
||||
if len(set(component_ids)) != len(component_ids):
|
||||
raise PerceptionAdapterError("E34 projection contains duplicate temporal ids")
|
||||
return tuple(results)
|
||||
|
||||
|
||||
def _basis(
|
||||
evidence_state: TrackGeometryEvidenceState,
|
||||
owner_kind: TrackGeometryOwnerKind,
|
||||
) -> EvidenceBasis:
|
||||
if evidence_state is TrackGeometryEvidenceState.AGREE:
|
||||
return EvidenceBasis.FUSED
|
||||
if evidence_state is TrackGeometryEvidenceState.CONFLICT:
|
||||
return EvidenceBasis.CONFLICT
|
||||
if owner_kind is TrackGeometryOwnerKind.GEOMETRY_CLUSTER:
|
||||
return EvidenceBasis.LIDAR
|
||||
return EvidenceBasis.CAMERA
|
||||
|
||||
|
||||
def _currentness(value: TrackGeometryCurrentness) -> EvidenceCurrentness:
|
||||
if value is TrackGeometryCurrentness.CURRENT:
|
||||
return EvidenceCurrentness.CURRENT
|
||||
if value is TrackGeometryCurrentness.HELD:
|
||||
return EvidenceCurrentness.HELD
|
||||
return EvidenceCurrentness.STALE
|
||||
|
||||
|
||||
def _history(value: object) -> tuple[HistorySample, ...]:
|
||||
if not isinstance(value, list) or not value:
|
||||
raise PerceptionAdapterError("E34 component history must be nonempty")
|
||||
history: list[HistorySample] = []
|
||||
for item in value[-32:]:
|
||||
document = _object(item, "E34 history sample")
|
||||
frame_index = _integer(document.get("frame_index"), "E34 history frame")
|
||||
session_seconds = _number(document.get("session_seconds"), "E34 history time")
|
||||
history.append(
|
||||
HistorySample(
|
||||
frame_id=f"e34-frame-{frame_index}",
|
||||
evidence_time_ns=max(0, int(round(session_seconds * 1_000_000_000))),
|
||||
centroid_xyz_m=_vector3(
|
||||
document.get("centroid_map_xyz_m"),
|
||||
"E34 history centroid",
|
||||
),
|
||||
)
|
||||
)
|
||||
return tuple(history)
|
||||
|
||||
|
||||
def _cells(
|
||||
projection: TemporalFrameProjection,
|
||||
component: dict[str, object],
|
||||
) -> tuple[GridCell, ...]:
|
||||
start = _integer(component.get("cell_row_start"), "E34 cell start")
|
||||
count = _integer(component.get("cell_row_count"), "E34 cell count")
|
||||
if start < 0 or count < 0 or start + count > int(projection.cell_rows.shape[0]):
|
||||
raise PerceptionAdapterError("E34 component cell range is invalid")
|
||||
rows = projection.cell_rows[start : start + count]
|
||||
return tuple(GridCell(int(row[0]), int(row[1]), int(row[2])) for row in rows)
|
||||
|
||||
|
||||
def _semantic_labels(value: object) -> tuple[str, ...]:
|
||||
document = _object(value, "E34 semantic provenance")
|
||||
labels = document.get("labels")
|
||||
if not isinstance(labels, list):
|
||||
raise PerceptionAdapterError("E34 semantic labels must be an array")
|
||||
return tuple(_safe_reason(label) for label in labels)
|
||||
|
||||
|
||||
def _safe_reason(value: object) -> str:
|
||||
if not isinstance(value, str) or not value:
|
||||
raise PerceptionAdapterError("E34 reason must be a nonempty string")
|
||||
return value
|
||||
|
||||
|
||||
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 PerceptionAdapterError(f"{label} must be an object")
|
||||
return value
|
||||
|
||||
|
||||
def _integer(value: object, label: str) -> int:
|
||||
if not isinstance(value, int) or isinstance(value, bool):
|
||||
raise PerceptionAdapterError(f"{label} must be an integer")
|
||||
return value
|
||||
|
||||
|
||||
def _number(value: object, label: str) -> float:
|
||||
if (
|
||||
not isinstance(value, (int, float))
|
||||
or isinstance(value, bool)
|
||||
or not math.isfinite(float(value))
|
||||
):
|
||||
raise PerceptionAdapterError(f"{label} must be finite")
|
||||
return float(value)
|
||||
|
||||
|
||||
def _vector3(value: object, label: str) -> tuple[float, float, float]:
|
||||
if not isinstance(value, list) or len(value) != 3:
|
||||
raise PerceptionAdapterError(f"{label} must contain three values")
|
||||
return (_number(value[0], label), _number(value[1], label), _number(value[2], label))
|
||||
Reference in New Issue
Block a user