275 lines
11 KiB
Python
275 lines
11 KiB
Python
"""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))
|