1133 lines
46 KiB
Python
1133 lines
46 KiB
Python
"""Digest-bound RAVNOVES00 geometry provider for the M4 product graph."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import math
|
|
import time
|
|
from collections.abc import Callable, Mapping
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
from threading import Lock
|
|
from typing import Final
|
|
|
|
import numpy as np
|
|
import numpy.typing as npt
|
|
|
|
from .contracts import (
|
|
EvidenceBasis,
|
|
EvidenceCurrentness,
|
|
MetricGeometry,
|
|
ModalityOutcome,
|
|
ObjectProposal2D,
|
|
ObstacleObservation,
|
|
validate_exclusive_point_ownership,
|
|
)
|
|
from .geometry_math import (
|
|
GeometryAssociationProfile,
|
|
Kb4ProjectionProfile,
|
|
ProjectedPointCloud,
|
|
SemanticGeometrySupport,
|
|
geometry_only_clusters,
|
|
project_map_points_kb4,
|
|
semantic_geometry_support,
|
|
)
|
|
from .providers import SourcePacket
|
|
from .recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference
|
|
|
|
GEOMETRY_PROFILE_SCHEMA: Final = "missioncore.geometry-association-profile/v1"
|
|
GEOMETRY_PROVIDER_ID: Final = "ravnoves00-geometry-association/v1"
|
|
DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path("config/perception/m4-geometry-association-v1.json")
|
|
|
|
FloatArray = npt.NDArray[np.float64]
|
|
UInt8Array = npt.NDArray[np.uint8]
|
|
|
|
|
|
class GeometryProviderError(RuntimeError):
|
|
"""The geometry profile, evidence source or association is incompatible."""
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class GeometryProfile:
|
|
profile_id: str
|
|
provider_id: str
|
|
source_id: str
|
|
session_id: str
|
|
source_pack_id: str
|
|
source_pack_sha256: str
|
|
frame_count: int
|
|
point_count: int
|
|
local_surface_model_id: str
|
|
local_surface_sha256: str
|
|
valid_frame_count: int
|
|
width: int
|
|
height: int
|
|
coordinate_frame: str
|
|
association: GeometryAssociationProfile
|
|
profile_sha256: str
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class GeometryFrame:
|
|
frame_index: int
|
|
points_map: FloatArray
|
|
point_class: UInt8Array
|
|
sensor_position_map: FloatArray
|
|
sensor_orientation_xyzw: FloatArray
|
|
projection: Kb4ProjectionProfile
|
|
surface_valid: bool
|
|
|
|
@property
|
|
def source_point_count(self) -> int:
|
|
return int(self.points_map.shape[0])
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class RecordedFrameTemporalBinding:
|
|
"""Digest-bound recorded timing evidence for one camera-indexed increment.
|
|
|
|
The shared session time binds the camera ordinal to the E10 pack entry. The
|
|
LiDAR and pose deltas retain their admitted E6 meaning: nearest host-arrival
|
|
best effort, not hardware synchronization.
|
|
"""
|
|
|
|
frame_index: int
|
|
source_time_ns: int
|
|
source_available: bool
|
|
lidar_camera_delta_ms: float | None
|
|
pose_point_delta_ms: float | None
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class ReplayBodyFrameInputs:
|
|
"""Verified inputs required to derive one replay-only virtual body frame."""
|
|
|
|
sensor_position_map: FloatArray
|
|
sensor_orientation_map_from_lidar_xyzw: FloatArray
|
|
ground_plane_coefficients_map: FloatArray
|
|
sensor_height_m: float
|
|
surface_slope_deg: float
|
|
trajectory_start_position_map: FloatArray
|
|
trajectory_end_position_map: FloatArray
|
|
t_camera_from_lidar: FloatArray
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class GeometryProviderSnapshot:
|
|
input_frames: int
|
|
completed_frames: int
|
|
failed_frames: int
|
|
proposal_count: int
|
|
eligible_proposal_count: int
|
|
ranged_proposal_count: int
|
|
camera_only_proposal_count: int
|
|
conflict_proposal_count: int
|
|
unavailable_proposal_count: int
|
|
outside_overlap_proposal_count: int
|
|
sparse_proposal_count: int
|
|
ownership_collision_proposal_count: int
|
|
geometry_only_observation_count: int
|
|
published_source_point_count: int
|
|
overlapping_claims_removed: int
|
|
core_duration_ns: int
|
|
|
|
@property
|
|
def total_range_coverage(self) -> float:
|
|
return self.ranged_proposal_count / self.proposal_count if self.proposal_count else 0.0
|
|
|
|
@property
|
|
def eligible_range_coverage(self) -> float:
|
|
if not self.eligible_proposal_count:
|
|
return 0.0
|
|
return self.ranged_proposal_count / self.eligible_proposal_count
|
|
|
|
|
|
class RecordedGeometryStore:
|
|
"""Verified source-pack and local-surface arrays used by one provider instance."""
|
|
|
|
def __init__(
|
|
self,
|
|
*,
|
|
source_pack_path: Path,
|
|
local_surface_path: Path,
|
|
profile: GeometryProfile,
|
|
) -> None:
|
|
self.source_pack_path = source_pack_path.resolve(strict=True)
|
|
self.local_surface_path = local_surface_path.resolve(strict=True)
|
|
self.profile = profile
|
|
_verify_regular_file(
|
|
self.source_pack_path,
|
|
expected_sha256=profile.source_pack_sha256,
|
|
label="source pack",
|
|
)
|
|
_verify_regular_file(
|
|
self.local_surface_path,
|
|
expected_sha256=profile.local_surface_sha256,
|
|
label="local surface",
|
|
)
|
|
self._source = _load_npz(self.source_pack_path, "source pack")
|
|
self._surface = _load_npz(self.local_surface_path, "local surface")
|
|
self._validate()
|
|
intrinsic = self._source["intrinsic_fx_fy_cx_cy"]
|
|
distortion = self._source["distortion_kb4"]
|
|
self._projection = Kb4ProjectionProfile(
|
|
width=profile.width,
|
|
height=profile.height,
|
|
intrinsic_fx_fy_cx_cy=(
|
|
float(intrinsic[0]),
|
|
float(intrinsic[1]),
|
|
float(intrinsic[2]),
|
|
float(intrinsic[3]),
|
|
),
|
|
distortion_kb4=(
|
|
float(distortion[0]),
|
|
float(distortion[1]),
|
|
float(distortion[2]),
|
|
float(distortion[3]),
|
|
),
|
|
t_camera_from_lidar=np.asarray(
|
|
self._source["t_camera_from_lidar"],
|
|
dtype=np.float64,
|
|
),
|
|
)
|
|
|
|
@classmethod
|
|
def from_repository(
|
|
cls,
|
|
repository_root: Path,
|
|
*,
|
|
profile: GeometryProfile | None = None,
|
|
) -> RecordedGeometryStore:
|
|
root = repository_root.resolve()
|
|
selected = profile or load_geometry_profile(root / DEFAULT_GEOMETRY_PROFILE_PATH)
|
|
source_pack = (
|
|
root
|
|
/ ".runtime/compute-experiments/e10/lidar-packs"
|
|
/ selected.source_pack_id
|
|
/ "lidar-pack.npz"
|
|
)
|
|
local_surface = (
|
|
root
|
|
/ ".runtime/compute-experiments/k1-local-surface-v1/models"
|
|
/ selected.local_surface_model_id
|
|
/ "local-surface.npz"
|
|
)
|
|
return cls(
|
|
source_pack_path=source_pack,
|
|
local_surface_path=local_surface,
|
|
profile=selected,
|
|
)
|
|
|
|
def frame(self, packet: SourcePacket) -> GeometryFrame | None:
|
|
envelope = packet.envelope
|
|
if envelope.source_id != self.profile.source_id:
|
|
raise GeometryProviderError("packet source escaped the geometry profile")
|
|
if envelope.session_id != self.profile.session_id:
|
|
raise GeometryProviderError("packet session escaped the geometry profile")
|
|
if not envelope.registered_point_increment.available:
|
|
return None
|
|
point_reference = packet.registered_point_increment_payload
|
|
pose_reference = packet.pose_payload
|
|
if (
|
|
not isinstance(point_reference, RecordedFrameReference)
|
|
or not isinstance(pose_reference, RecordedFrameReference)
|
|
or point_reference.artifact_id != self.profile.source_pack_id
|
|
or pose_reference.artifact_id != self.profile.source_pack_id
|
|
or point_reference.frame_index != envelope.sequence
|
|
or pose_reference.frame_index != envelope.sequence
|
|
):
|
|
raise GeometryProviderError("packet geometry references are not source-bound")
|
|
frame = self.frame_for_index(envelope.sequence)
|
|
if frame is None:
|
|
raise GeometryProviderError("packet claims unavailable source geometry as current")
|
|
return frame
|
|
|
|
def frame_for_index(self, frame_index: int) -> GeometryFrame | None:
|
|
"""Expose one verified source increment with its pose and KB4 calibration.
|
|
|
|
This read-only seam is intentionally narrower than the source archive. It
|
|
exists for deterministic replay diagnostics which must project the exact
|
|
frame-local point index space without manufacturing a ``SourcePacket``.
|
|
An unavailable recorded increment remains ``None``; surface validity is
|
|
retained on the returned frame rather than silently filtering its points.
|
|
"""
|
|
|
|
if not isinstance(frame_index, int) or isinstance(frame_index, bool):
|
|
raise GeometryProviderError("replay geometry frame index is invalid")
|
|
if not 0 <= frame_index < self.profile.frame_count:
|
|
raise GeometryProviderError("replay geometry frame is outside the profile")
|
|
if int(self._source["frame_indices"][frame_index]) != frame_index:
|
|
raise GeometryProviderError("source pack frame sequence changed")
|
|
if not bool(self._source["sample_available"][frame_index]):
|
|
return None
|
|
offsets = self._source["cloud_offsets"]
|
|
start, end = int(offsets[frame_index]), int(offsets[frame_index + 1])
|
|
return GeometryFrame(
|
|
frame_index=frame_index,
|
|
points_map=np.asarray(self._source["cloud_points_map"][start:end], dtype=np.float64),
|
|
point_class=np.asarray(self._surface["point_class"][start:end], dtype=np.uint8),
|
|
sensor_position_map=np.asarray(
|
|
self._source["pose_positions_map"][frame_index],
|
|
dtype=np.float64,
|
|
),
|
|
sensor_orientation_xyzw=np.asarray(
|
|
self._source["pose_quaternions_map_from_lidar"][frame_index],
|
|
dtype=np.float64,
|
|
),
|
|
projection=self._projection,
|
|
surface_valid=bool(self._surface["frame_valid"][frame_index]),
|
|
)
|
|
|
|
def temporal_binding_for_index(self, frame_index: int) -> RecordedFrameTemporalBinding:
|
|
"""Return the sealed ordinal/session binding and admitted best-effort deltas."""
|
|
|
|
if not isinstance(frame_index, int) or isinstance(frame_index, bool):
|
|
raise GeometryProviderError("replay temporal frame index is invalid")
|
|
if not 0 <= frame_index < self.profile.frame_count:
|
|
raise GeometryProviderError("replay temporal frame is outside the profile")
|
|
if (
|
|
int(self._source["frame_indices"][frame_index]) != frame_index
|
|
or int(self._source["source_frame_indices"][frame_index]) != frame_index
|
|
):
|
|
raise GeometryProviderError("source pack temporal sequence changed")
|
|
session_seconds = float(self._source["session_seconds"][frame_index])
|
|
if not math.isfinite(session_seconds) or session_seconds < 0.0:
|
|
raise GeometryProviderError("source pack session time is invalid")
|
|
source_available = bool(self._source["sample_available"][frame_index])
|
|
lidar_delta = float(self._source["lidar_camera_delta_ms"][frame_index])
|
|
pose_delta = float(self._source["pose_point_delta_ms"][frame_index])
|
|
if source_available:
|
|
if not math.isfinite(lidar_delta) or not math.isfinite(pose_delta):
|
|
raise GeometryProviderError("available source temporal deltas are invalid")
|
|
lidar_value: float | None = lidar_delta
|
|
pose_value: float | None = pose_delta
|
|
else:
|
|
if not math.isnan(lidar_delta) or not math.isnan(pose_delta):
|
|
raise GeometryProviderError("unavailable source carries temporal deltas")
|
|
lidar_value = None
|
|
pose_value = None
|
|
return RecordedFrameTemporalBinding(
|
|
frame_index=frame_index,
|
|
source_time_ns=round(session_seconds * 1_000_000_000),
|
|
source_available=source_available,
|
|
lidar_camera_delta_ms=lidar_value,
|
|
pose_point_delta_ms=pose_value,
|
|
)
|
|
|
|
def current_points(self, packet: SourcePacket) -> FloatArray | None:
|
|
"""Expose the verified frame-local point index space to temporal occupancy."""
|
|
|
|
frame = self.frame(packet)
|
|
if frame is None or not frame.surface_valid:
|
|
return None
|
|
points = np.asarray(frame.points_map, dtype=np.float64)
|
|
points.setflags(write=False)
|
|
return points
|
|
|
|
def current_points_for_frame(self, frame_index: int) -> FloatArray | None:
|
|
"""Expose one verified increment to a read-only recorded-evidence projector."""
|
|
|
|
if not isinstance(frame_index, int) or isinstance(frame_index, bool):
|
|
raise GeometryProviderError("replay evidence frame index is invalid")
|
|
if not 0 <= frame_index < self.profile.frame_count:
|
|
raise GeometryProviderError("replay evidence frame is outside the source profile")
|
|
if int(self._source["frame_indices"][frame_index]) != frame_index:
|
|
raise GeometryProviderError("source pack frame sequence changed")
|
|
if not bool(self._source["sample_available"][frame_index]) or not bool(
|
|
self._surface["frame_valid"][frame_index]
|
|
):
|
|
return None
|
|
offsets = self._source["cloud_offsets"]
|
|
start, end = int(offsets[frame_index]), int(offsets[frame_index + 1])
|
|
points = np.asarray(self._source["cloud_points_map"][start:end], dtype=np.float64)
|
|
points.setflags(write=False)
|
|
return points
|
|
|
|
@property
|
|
def maximum_current_point_count(self) -> int:
|
|
"""Return the immutable source-pack upper bound for one recorded increment."""
|
|
|
|
counts = np.diff(np.asarray(self._source["cloud_offsets"], dtype=np.int64))
|
|
return int(counts.max(initial=0))
|
|
|
|
def pose_values_for_frame(
|
|
self,
|
|
frame_id: str,
|
|
) -> tuple[tuple[float, float, float], tuple[float, float, float, float]] | None:
|
|
"""Return one verified replay pose without exposing the source archive."""
|
|
|
|
prefix = "frame-"
|
|
if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit():
|
|
raise GeometryProviderError("replay pose frame identity is invalid")
|
|
frame_index = int(frame_id[len(prefix) :])
|
|
if not 0 <= frame_index < self.profile.frame_count:
|
|
raise GeometryProviderError("replay pose frame is outside the source profile")
|
|
if not bool(self._source["sample_available"][frame_index]):
|
|
return None
|
|
position = np.asarray(
|
|
self._source["pose_positions_map"][frame_index],
|
|
dtype=np.float64,
|
|
)
|
|
orientation = np.asarray(
|
|
self._source["pose_quaternions_map_from_lidar"][frame_index],
|
|
dtype=np.float64,
|
|
)
|
|
if not np.isfinite(position).all() or not np.isfinite(orientation).all():
|
|
raise GeometryProviderError("available replay pose is not finite")
|
|
return (
|
|
(float(position[0]), float(position[1]), float(position[2])),
|
|
(
|
|
float(orientation[0]),
|
|
float(orientation[1]),
|
|
float(orientation[2]),
|
|
float(orientation[3]),
|
|
),
|
|
)
|
|
|
|
def replay_body_frame_inputs(
|
|
self,
|
|
frame_id: str,
|
|
*,
|
|
trajectory_half_window_frames: int,
|
|
) -> ReplayBodyFrameInputs | None:
|
|
"""Return source-bound pose, surface and route evidence without inventing axes."""
|
|
|
|
if trajectory_half_window_frames < 1:
|
|
raise GeometryProviderError("trajectory half-window must be positive")
|
|
prefix = "frame-"
|
|
if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit():
|
|
raise GeometryProviderError("replay body frame identity is invalid")
|
|
frame_index = int(frame_id[len(prefix) :])
|
|
if not 0 <= frame_index < self.profile.frame_count:
|
|
raise GeometryProviderError("replay body frame is outside the source profile")
|
|
if not bool(self._source["sample_available"][frame_index]) or not bool(
|
|
self._surface["frame_valid"][frame_index]
|
|
):
|
|
return None
|
|
required = {
|
|
"plane_coefficients_map",
|
|
"sensor_height_m",
|
|
"slope_deg",
|
|
}
|
|
if not required.issubset(self._surface):
|
|
raise GeometryProviderError("local surface lacks replay body-frame evidence")
|
|
first = max(0, frame_index - trajectory_half_window_frames)
|
|
last = min(self.profile.frame_count, frame_index + trajectory_half_window_frames + 1)
|
|
available = np.flatnonzero(self._source["sample_available"][first:last]) + first
|
|
if available.size == 0:
|
|
return None
|
|
values = ReplayBodyFrameInputs(
|
|
sensor_position_map=np.asarray(
|
|
self._source["pose_positions_map"][frame_index], dtype=np.float64
|
|
),
|
|
sensor_orientation_map_from_lidar_xyzw=np.asarray(
|
|
self._source["pose_quaternions_map_from_lidar"][frame_index],
|
|
dtype=np.float64,
|
|
),
|
|
ground_plane_coefficients_map=np.asarray(
|
|
self._surface["plane_coefficients_map"][frame_index],
|
|
dtype=np.float64,
|
|
),
|
|
sensor_height_m=float(self._surface["sensor_height_m"][frame_index]),
|
|
surface_slope_deg=float(self._surface["slope_deg"][frame_index]),
|
|
trajectory_start_position_map=np.asarray(
|
|
self._source["pose_positions_map"][int(available[0])], dtype=np.float64
|
|
),
|
|
trajectory_end_position_map=np.asarray(
|
|
self._source["pose_positions_map"][int(available[-1])], dtype=np.float64
|
|
),
|
|
t_camera_from_lidar=np.asarray(self._source["t_camera_from_lidar"], dtype=np.float64),
|
|
)
|
|
if not all(
|
|
np.isfinite(value).all()
|
|
for value in (
|
|
values.sensor_position_map,
|
|
values.sensor_orientation_map_from_lidar_xyzw,
|
|
values.ground_plane_coefficients_map,
|
|
values.trajectory_start_position_map,
|
|
values.trajectory_end_position_map,
|
|
values.t_camera_from_lidar,
|
|
)
|
|
) or not math.isfinite(values.sensor_height_m + values.surface_slope_deg):
|
|
raise GeometryProviderError("replay body-frame evidence is not finite")
|
|
return values
|
|
|
|
def available_frame_indices(self) -> tuple[int, ...]:
|
|
"""Expose the immutable availability partition for deterministic sampling."""
|
|
|
|
return tuple(int(index) for index in np.flatnonzero(self._source["sample_available"]))
|
|
|
|
def _validate(self) -> None:
|
|
source_required = {
|
|
"frame_indices",
|
|
"source_frame_indices",
|
|
"session_seconds",
|
|
"sample_available",
|
|
"cloud_offsets",
|
|
"cloud_points_map",
|
|
"pose_positions_map",
|
|
"pose_quaternions_map_from_lidar",
|
|
"intrinsic_fx_fy_cx_cy",
|
|
"distortion_kb4",
|
|
"t_camera_from_lidar",
|
|
"lidar_camera_delta_ms",
|
|
"pose_point_delta_ms",
|
|
}
|
|
surface_required = {"frame_valid", "point_class"}
|
|
if not source_required.issubset(self._source):
|
|
raise GeometryProviderError("source pack arrays are incomplete")
|
|
if not surface_required.issubset(self._surface):
|
|
raise GeometryProviderError("local surface arrays are incomplete")
|
|
frames = self.profile.frame_count
|
|
points = self.profile.point_count
|
|
shapes = {
|
|
"frame_indices": (frames,),
|
|
"source_frame_indices": (frames,),
|
|
"session_seconds": (frames,),
|
|
"sample_available": (frames,),
|
|
"cloud_offsets": (frames + 1,),
|
|
"cloud_points_map": (points, 3),
|
|
"pose_positions_map": (frames, 3),
|
|
"pose_quaternions_map_from_lidar": (frames, 4),
|
|
"intrinsic_fx_fy_cx_cy": (4,),
|
|
"distortion_kb4": (4,),
|
|
"t_camera_from_lidar": (4, 4),
|
|
"lidar_camera_delta_ms": (frames,),
|
|
"pose_point_delta_ms": (frames,),
|
|
}
|
|
if any(self._source[name].shape != shape for name, shape in shapes.items()):
|
|
raise GeometryProviderError("source pack array shapes changed")
|
|
if self._surface["frame_valid"].shape != (frames,):
|
|
raise GeometryProviderError("local surface frame shape changed")
|
|
if self._surface["point_class"].shape != (points,):
|
|
raise GeometryProviderError("local surface point shape changed")
|
|
if int(self._source["cloud_offsets"][-1]) != points:
|
|
raise GeometryProviderError("source point offsets do not close")
|
|
expected_indices = np.arange(frames, dtype=np.int64)
|
|
session_seconds = np.asarray(self._source["session_seconds"], dtype=np.float64)
|
|
if (
|
|
not np.array_equal(self._source["frame_indices"], expected_indices)
|
|
or not np.array_equal(self._source["source_frame_indices"], expected_indices)
|
|
or not np.isfinite(session_seconds).all()
|
|
or np.any(session_seconds < 0.0)
|
|
or np.any(np.diff(session_seconds) <= 0.0)
|
|
):
|
|
raise GeometryProviderError("source temporal index changed")
|
|
available = np.asarray(self._source["sample_available"], dtype=np.bool_)
|
|
lidar_deltas = np.asarray(self._source["lidar_camera_delta_ms"], dtype=np.float64)
|
|
pose_deltas = np.asarray(self._source["pose_point_delta_ms"], dtype=np.float64)
|
|
if (
|
|
not np.isfinite(lidar_deltas[available]).all()
|
|
or not np.isfinite(pose_deltas[available]).all()
|
|
or not np.isnan(lidar_deltas[~available]).all()
|
|
or not np.isnan(pose_deltas[~available]).all()
|
|
):
|
|
raise GeometryProviderError("source temporal delta availability changed")
|
|
if (
|
|
int(np.count_nonzero(self._source["sample_available"]))
|
|
!= self.profile.valid_frame_count
|
|
):
|
|
raise GeometryProviderError("source availability accounting changed")
|
|
if int(np.count_nonzero(self._surface["frame_valid"])) != self.profile.valid_frame_count:
|
|
raise GeometryProviderError("local surface validity accounting changed")
|
|
if not np.array_equal(
|
|
self._surface["frame_valid"],
|
|
self._source["sample_available"],
|
|
):
|
|
raise GeometryProviderError("source and local surface availability disagree")
|
|
point_class = np.asarray(self._surface["point_class"], dtype=np.uint8)
|
|
if np.any(point_class > 3):
|
|
raise GeometryProviderError("local surface point classification changed")
|
|
|
|
|
|
class Ravnoves00GeometryAssociationProvider:
|
|
"""Associate proposals with exact current points and retain unknown occupancy."""
|
|
|
|
provider_id: str = GEOMETRY_PROVIDER_ID
|
|
|
|
def __init__(
|
|
self,
|
|
*,
|
|
store: RecordedGeometryStore,
|
|
clock_ns: Callable[[], int] = time.perf_counter_ns,
|
|
) -> None:
|
|
if store.profile.provider_id != self.provider_id:
|
|
raise GeometryProviderError("geometry profile provider identity changed")
|
|
self.store = store
|
|
self.profile = store.profile
|
|
self._clock_ns = clock_ns
|
|
self._lock = Lock()
|
|
self._input_frames = 0
|
|
self._completed_frames = 0
|
|
self._failed_frames = 0
|
|
self._proposal_count = 0
|
|
self._eligible = 0
|
|
self._ranged = 0
|
|
self._camera_only = 0
|
|
self._conflict = 0
|
|
self._unavailable = 0
|
|
self._outside = 0
|
|
self._sparse = 0
|
|
self._ownership_collision = 0
|
|
self._geometry_only = 0
|
|
self._published_points = 0
|
|
self._overlap_removed = 0
|
|
self._core_duration_ns = 0
|
|
|
|
def associate(
|
|
self,
|
|
packet: SourcePacket,
|
|
proposals: tuple[ObjectProposal2D, ...],
|
|
) -> tuple[ObstacleObservation, ...]:
|
|
with self._lock:
|
|
self._input_frames += 1
|
|
self._proposal_count += len(proposals)
|
|
started = int(self._clock_ns())
|
|
try:
|
|
self._validate_proposals(packet, proposals)
|
|
frame = self.store.frame(packet)
|
|
if frame is None or not frame.surface_valid:
|
|
result = tuple(
|
|
_camera_unavailable_observation(packet, proposal, frame is None)
|
|
for proposal in proposals
|
|
)
|
|
self._record_unavailable(len(proposals))
|
|
else:
|
|
result, metrics = self._associate_current(packet, proposals, frame)
|
|
self._record_current(metrics)
|
|
validate_exclusive_point_ownership(result)
|
|
except Exception:
|
|
with self._lock:
|
|
self._failed_frames += 1
|
|
self._core_duration_ns += max(0, int(self._clock_ns()) - started)
|
|
raise
|
|
with self._lock:
|
|
self._completed_frames += 1
|
|
self._core_duration_ns += max(0, int(self._clock_ns()) - started)
|
|
return result
|
|
|
|
def _associate_current(
|
|
self,
|
|
packet: SourcePacket,
|
|
proposals: tuple[ObjectProposal2D, ...],
|
|
frame: GeometryFrame,
|
|
) -> tuple[tuple[ObstacleObservation, ...], dict[str, int]]:
|
|
projected = project_map_points_kb4(
|
|
frame.points_map,
|
|
position_map_xyz=frame.sensor_position_map,
|
|
orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw,
|
|
profile=frame.projection,
|
|
)
|
|
supports = tuple(
|
|
semantic_geometry_support(
|
|
proposal.region.as_tuple(),
|
|
projected=projected,
|
|
frame_points_map=frame.points_map,
|
|
point_class=frame.point_class,
|
|
profile=self.profile.association,
|
|
)
|
|
for proposal in proposals
|
|
)
|
|
allocations, overlap_removed = _allocate_point_ownership(proposals, supports)
|
|
observations: list[ObstacleObservation] = []
|
|
metrics = {
|
|
"eligible": 0,
|
|
"ranged": 0,
|
|
"camera_only": 0,
|
|
"conflict": 0,
|
|
"outside": 0,
|
|
"sparse": 0,
|
|
"ownership_collision": 0,
|
|
"geometry_only": 0,
|
|
"published_points": 0,
|
|
"overlap_removed": overlap_removed,
|
|
}
|
|
claimed: set[int] = set()
|
|
for index, (proposal, support) in enumerate(zip(proposals, supports, strict=True)):
|
|
owned = allocations.get(index, np.empty(0, dtype=np.int64))
|
|
observation = _proposal_observation(
|
|
packet,
|
|
proposal,
|
|
support=support,
|
|
owned_source_indices=owned,
|
|
frame=frame,
|
|
projected=projected,
|
|
coordinate_frame=self.profile.coordinate_frame,
|
|
)
|
|
observations.append(observation)
|
|
metrics["eligible"] += support.overlaps_projected_extent
|
|
if observation.metric_geometry is not None:
|
|
metrics["ranged"] += 1
|
|
metrics["published_points"] += len(observation.source_point_ids)
|
|
claimed.update(observation.source_point_ids)
|
|
elif observation.basis is EvidenceBasis.CONFLICT:
|
|
metrics["conflict"] += 1
|
|
else:
|
|
metrics["camera_only"] += 1
|
|
if not support.overlaps_projected_extent:
|
|
metrics["outside"] += 1
|
|
elif "point-ownership-collision-range-withheld" in observation.reason_codes:
|
|
metrics["ownership_collision"] += 1
|
|
else:
|
|
metrics["sparse"] += 1
|
|
clusters = geometry_only_clusters(
|
|
points_map=frame.points_map,
|
|
point_class=frame.point_class,
|
|
sensor_position_map=frame.sensor_position_map,
|
|
claimed_source_indices=frozenset(claimed),
|
|
profile=self.profile.association,
|
|
)
|
|
for cluster_index, cluster in enumerate(clusters):
|
|
point_ids = tuple(sorted(int(value) for value in cluster.source_indices))
|
|
observations.append(
|
|
ObstacleObservation(
|
|
observation_id=f"{packet.envelope.frame_id}:geometry:{cluster_index}",
|
|
occupancy_key=f"{packet.envelope.frame_id}:geometry:{cluster_index}",
|
|
source_id=packet.envelope.source_id,
|
|
frame_id=packet.envelope.frame_id,
|
|
evidence_time_ns=packet.envelope.timestamps.source_ns,
|
|
basis=EvidenceBasis.LIDAR,
|
|
currentness=EvidenceCurrentness.CURRENT,
|
|
occupied_support=True,
|
|
source_point_ids=point_ids,
|
|
metric_geometry=MetricGeometry(
|
|
coordinate_frame=self.profile.coordinate_frame,
|
|
centroid_xyz_m=cluster.centroid_map_xyz_m,
|
|
range_m=cluster.nearest_range_m,
|
|
covariance_diagonal_m2=cluster.covariance_diagonal_m2,
|
|
),
|
|
proposal_ids=(),
|
|
semantic_hint=None,
|
|
reason_codes=("unassociated-current-occupied-component",),
|
|
)
|
|
)
|
|
metrics["geometry_only"] += 1
|
|
metrics["published_points"] += len(point_ids)
|
|
return tuple(observations), metrics
|
|
|
|
def _validate_proposals(
|
|
self,
|
|
packet: SourcePacket,
|
|
proposals: tuple[ObjectProposal2D, ...],
|
|
) -> None:
|
|
if any(
|
|
proposal.source_id != packet.envelope.source_id
|
|
or proposal.frame_id != packet.envelope.frame_id
|
|
for proposal in proposals
|
|
):
|
|
raise GeometryProviderError("proposal escaped its source packet")
|
|
if len({proposal.proposal_id for proposal in proposals}) != len(proposals):
|
|
raise GeometryProviderError("proposal identities are duplicated")
|
|
|
|
def _record_unavailable(self, count: int) -> None:
|
|
with self._lock:
|
|
self._camera_only += count
|
|
self._unavailable += count
|
|
|
|
def _record_current(self, metrics: Mapping[str, int]) -> None:
|
|
with self._lock:
|
|
self._eligible += metrics["eligible"]
|
|
self._ranged += metrics["ranged"]
|
|
self._camera_only += metrics["camera_only"]
|
|
self._conflict += metrics["conflict"]
|
|
self._outside += metrics["outside"]
|
|
self._sparse += metrics["sparse"]
|
|
self._ownership_collision += metrics["ownership_collision"]
|
|
self._geometry_only += metrics["geometry_only"]
|
|
self._published_points += metrics["published_points"]
|
|
self._overlap_removed += metrics["overlap_removed"]
|
|
|
|
def snapshot(self) -> GeometryProviderSnapshot:
|
|
with self._lock:
|
|
return GeometryProviderSnapshot(
|
|
input_frames=self._input_frames,
|
|
completed_frames=self._completed_frames,
|
|
failed_frames=self._failed_frames,
|
|
proposal_count=self._proposal_count,
|
|
eligible_proposal_count=self._eligible,
|
|
ranged_proposal_count=self._ranged,
|
|
camera_only_proposal_count=self._camera_only,
|
|
conflict_proposal_count=self._conflict,
|
|
unavailable_proposal_count=self._unavailable,
|
|
outside_overlap_proposal_count=self._outside,
|
|
sparse_proposal_count=self._sparse,
|
|
ownership_collision_proposal_count=self._ownership_collision,
|
|
geometry_only_observation_count=self._geometry_only,
|
|
published_source_point_count=self._published_points,
|
|
overlapping_claims_removed=self._overlap_removed,
|
|
core_duration_ns=self._core_duration_ns,
|
|
)
|
|
|
|
|
|
def load_geometry_profile(path: Path) -> GeometryProfile:
|
|
resolved = path.resolve(strict=True)
|
|
_verify_regular_file(resolved, expected_sha256=None, label="geometry profile")
|
|
raw = resolved.read_bytes()
|
|
try:
|
|
value = json.loads(raw)
|
|
except json.JSONDecodeError as exc:
|
|
raise GeometryProviderError("geometry profile JSON is invalid") from exc
|
|
document = _object(value, "geometry profile")
|
|
_exact_keys(
|
|
document,
|
|
{
|
|
"schema_version",
|
|
"profile_id",
|
|
"provider_id",
|
|
"source",
|
|
"local_surface",
|
|
"projection",
|
|
"association",
|
|
"policy",
|
|
"authority",
|
|
},
|
|
"geometry profile",
|
|
)
|
|
if document["schema_version"] != GEOMETRY_PROFILE_SCHEMA:
|
|
raise GeometryProviderError("geometry profile schema is incompatible")
|
|
if document["provider_id"] != GEOMETRY_PROVIDER_ID:
|
|
raise GeometryProviderError("geometry provider identity is incompatible")
|
|
source = _object(document["source"], "geometry source")
|
|
surface = _object(document["local_surface"], "local surface")
|
|
projection = _object(document["projection"], "geometry projection")
|
|
association = _object(document["association"], "geometry association")
|
|
policy = _object(document["policy"], "geometry policy")
|
|
authority = _object(document["authority"], "geometry authority")
|
|
_exact_keys(
|
|
source,
|
|
{
|
|
"source_id",
|
|
"session_id",
|
|
"source_pack_id",
|
|
"source_pack_sha256",
|
|
"frame_count",
|
|
"point_count",
|
|
},
|
|
"geometry source",
|
|
)
|
|
_exact_keys(
|
|
surface,
|
|
{"model_id", "artifact_sha256", "valid_frame_count"},
|
|
"local surface",
|
|
)
|
|
_exact_keys(projection, {"model", "width", "height", "coordinate_frame"}, "projection")
|
|
association_keys = set(GeometryAssociationProfile.__dataclass_fields__)
|
|
_exact_keys(association, association_keys, "geometry association")
|
|
expected_policy = {
|
|
"camera_owns_semantic_hint": True,
|
|
"geometry_can_invent_semantic_class": False,
|
|
"geometry_only_range_estimator": "nearest-euclidean-sensor-distance/v1",
|
|
"one_owner_per_source_point": True,
|
|
"absence_of_points_means_free": False,
|
|
"overlap_eligibility": "bbox-intersects-current-projected-point-extent/v1",
|
|
"point_ownership_priority": "smallest-bbox-then-score-then-proposal-id/v1",
|
|
"proposal_range_estimator": "median-camera-z-of-owned-current-points/v1",
|
|
"unknown_is_occupied": True,
|
|
"threshold_tuning_allowed": False,
|
|
}
|
|
if policy != expected_policy:
|
|
raise GeometryProviderError("geometry policy is incompatible")
|
|
if authority != {
|
|
"ground_truth": False,
|
|
"physical_live": False,
|
|
"commands_enabled": False,
|
|
"actuation_allowed": False,
|
|
"navigation_or_safety_accepted": False,
|
|
}:
|
|
raise GeometryProviderError("geometry authority is incompatible")
|
|
if projection["model"] != "KB4":
|
|
raise GeometryProviderError("geometry projection model is incompatible")
|
|
profile = GeometryProfile(
|
|
profile_id=_string(document, "profile_id"),
|
|
provider_id=_string(document, "provider_id"),
|
|
source_id=_string(source, "source_id"),
|
|
session_id=_string(source, "session_id"),
|
|
source_pack_id=_string(source, "source_pack_id"),
|
|
source_pack_sha256=_digest(source, "source_pack_sha256"),
|
|
frame_count=_positive_integer(source, "frame_count"),
|
|
point_count=_positive_integer(source, "point_count"),
|
|
local_surface_model_id=_string(surface, "model_id"),
|
|
local_surface_sha256=_digest(surface, "artifact_sha256"),
|
|
valid_frame_count=_positive_integer(surface, "valid_frame_count"),
|
|
width=_positive_integer(projection, "width"),
|
|
height=_positive_integer(projection, "height"),
|
|
coordinate_frame=_string(projection, "coordinate_frame"),
|
|
association=GeometryAssociationProfile(
|
|
bbox_inset_fraction=_number(association, "bbox_inset_fraction"),
|
|
depth_cluster_minimum_gap_m=_number(
|
|
association,
|
|
"depth_cluster_minimum_gap_m",
|
|
),
|
|
depth_cluster_gap_fraction=_number(
|
|
association,
|
|
"depth_cluster_gap_fraction",
|
|
),
|
|
spatial_cluster_radius_m=_number(association, "spatial_cluster_radius_m"),
|
|
semantic_minimum_occupied_points=_positive_integer(
|
|
association,
|
|
"semantic_minimum_occupied_points",
|
|
),
|
|
semantic_minimum_occupied_voxels=_positive_integer(
|
|
association,
|
|
"semantic_minimum_occupied_voxels",
|
|
),
|
|
semantic_voxel_size_m=_number(association, "semantic_voxel_size_m"),
|
|
conflict_minimum_classified_points=_positive_integer(
|
|
association,
|
|
"conflict_minimum_classified_points",
|
|
),
|
|
conflict_surface_fraction=_number(association, "conflict_surface_fraction"),
|
|
geometry_local_radius_m=_number(association, "geometry_local_radius_m"),
|
|
geometry_voxel_size_m=_number(association, "geometry_voxel_size_m"),
|
|
geometry_minimum_cluster_points=_positive_integer(
|
|
association,
|
|
"geometry_minimum_cluster_points",
|
|
),
|
|
geometry_minimum_cluster_voxels=_positive_integer(
|
|
association,
|
|
"geometry_minimum_cluster_voxels",
|
|
),
|
|
maximum_geometry_clusters_per_frame=_positive_integer(
|
|
association,
|
|
"maximum_geometry_clusters_per_frame",
|
|
),
|
|
),
|
|
profile_sha256=hashlib.sha256(raw).hexdigest(),
|
|
)
|
|
if profile.source_pack_id != RECORDED_SOURCE_PACK_ID:
|
|
raise GeometryProviderError("geometry profile does not bind the admitted source pack")
|
|
if profile.valid_frame_count > profile.frame_count:
|
|
raise GeometryProviderError("geometry valid frame count exceeds the source")
|
|
return profile
|
|
|
|
|
|
def _allocate_point_ownership(
|
|
proposals: tuple[ObjectProposal2D, ...],
|
|
supports: tuple[SemanticGeometrySupport, ...],
|
|
) -> tuple[dict[int, npt.NDArray[np.int64]], int]:
|
|
eligible = {index: support for index, support in enumerate(supports) if support.qualified}
|
|
claims: dict[int, list[int]] = {}
|
|
for index, support in eligible.items():
|
|
for source_index in support.occupied_source_indices:
|
|
claims.setdefault(int(source_index), []).append(index)
|
|
winners = {
|
|
source_index: min(candidates, key=lambda index: _proposal_priority(proposals[index]))
|
|
for source_index, candidates in claims.items()
|
|
}
|
|
allocations = {
|
|
index: np.asarray(
|
|
[
|
|
int(source_index)
|
|
for source_index in support.occupied_source_indices
|
|
if winners[int(source_index)] == index
|
|
],
|
|
dtype=np.int64,
|
|
)
|
|
for index, support in eligible.items()
|
|
}
|
|
removed = sum(
|
|
int(eligible[index].occupied_source_indices.size - allocation.size)
|
|
for index, allocation in allocations.items()
|
|
)
|
|
return allocations, removed
|
|
|
|
|
|
def _proposal_priority(proposal: ObjectProposal2D) -> tuple[float, float, str]:
|
|
left, top, right, bottom = proposal.region.as_tuple()
|
|
return ((right - left) * (bottom - top), -proposal.objectness, proposal.proposal_id)
|
|
|
|
|
|
def _proposal_observation(
|
|
packet: SourcePacket,
|
|
proposal: ObjectProposal2D,
|
|
*,
|
|
support: SemanticGeometrySupport,
|
|
owned_source_indices: npt.NDArray[np.int64],
|
|
frame: GeometryFrame,
|
|
projected: ProjectedPointCloud,
|
|
coordinate_frame: str,
|
|
) -> ObstacleObservation:
|
|
point_ids: tuple[int, ...] = ()
|
|
metric: MetricGeometry | None = None
|
|
reason_codes: tuple[str, ...]
|
|
basis: EvidenceBasis
|
|
if support.qualified and owned_source_indices.size:
|
|
point_ids = tuple(sorted(int(value) for value in owned_source_indices))
|
|
points = frame.points_map[owned_source_indices]
|
|
centroid = np.median(points, axis=0)
|
|
covariance = points.var(axis=0)
|
|
depth_by_source = {
|
|
int(source_index): float(depth)
|
|
for source_index, depth in zip(
|
|
support.occupied_source_indices,
|
|
support.occupied_depths_m,
|
|
strict=True,
|
|
)
|
|
}
|
|
range_m = float(np.median([depth_by_source[index] for index in point_ids]))
|
|
metric = MetricGeometry(
|
|
coordinate_frame=coordinate_frame,
|
|
centroid_xyz_m=(
|
|
float(centroid[0]),
|
|
float(centroid[1]),
|
|
float(centroid[2]),
|
|
),
|
|
range_m=range_m,
|
|
covariance_diagonal_m2=(
|
|
float(covariance[0]),
|
|
float(covariance[1]),
|
|
float(covariance[2]),
|
|
),
|
|
)
|
|
basis = EvidenceBasis.FUSED
|
|
reason_codes = ("current-connected-occupied-lidar-support",)
|
|
if owned_source_indices.size != support.occupied_source_indices.size:
|
|
reason_codes += ("exclusive-point-ownership-arbitration",)
|
|
elif support.qualified:
|
|
basis = EvidenceBasis.CAMERA
|
|
reason_codes = (
|
|
"current-connected-occupied-lidar-support",
|
|
"point-ownership-collision-range-withheld",
|
|
)
|
|
elif support.conflict:
|
|
basis = EvidenceBasis.CONFLICT
|
|
reason_codes = ("camera-region-observed-as-local-surface",)
|
|
elif not support.overlaps_projected_extent:
|
|
basis = EvidenceBasis.CAMERA
|
|
reason_codes = ("outside-projected-lidar-overlap",)
|
|
else:
|
|
basis = EvidenceBasis.CAMERA
|
|
reason_codes = ("sparse-or-unqualified-occupied-support",)
|
|
return ObstacleObservation(
|
|
observation_id=f"{packet.envelope.frame_id}:{proposal.proposal_id}",
|
|
occupancy_key=f"{packet.envelope.frame_id}:{proposal.proposal_id}",
|
|
source_id=packet.envelope.source_id,
|
|
frame_id=packet.envelope.frame_id,
|
|
evidence_time_ns=packet.envelope.timestamps.source_ns,
|
|
basis=basis,
|
|
currentness=EvidenceCurrentness.CURRENT,
|
|
occupied_support=metric is not None,
|
|
source_point_ids=point_ids,
|
|
metric_geometry=metric,
|
|
proposal_ids=(proposal.proposal_id,),
|
|
semantic_hint=proposal.semantic_hint,
|
|
reason_codes=reason_codes,
|
|
)
|
|
|
|
|
|
def _camera_unavailable_observation(
|
|
packet: SourcePacket,
|
|
proposal: ObjectProposal2D,
|
|
source_unavailable: bool,
|
|
) -> ObstacleObservation:
|
|
status = packet.envelope.registered_point_increment
|
|
if source_unavailable and status.outcome is ModalityOutcome.STALE:
|
|
currentness = EvidenceCurrentness.STALE
|
|
reason = "registered-point-increment-stale"
|
|
else:
|
|
currentness = EvidenceCurrentness.UNAVAILABLE
|
|
reason = (
|
|
"registered-point-increment-unavailable"
|
|
if source_unavailable
|
|
else "local-surface-unavailable"
|
|
)
|
|
return ObstacleObservation(
|
|
observation_id=f"{packet.envelope.frame_id}:{proposal.proposal_id}",
|
|
occupancy_key=f"{packet.envelope.frame_id}:{proposal.proposal_id}",
|
|
source_id=packet.envelope.source_id,
|
|
frame_id=packet.envelope.frame_id,
|
|
evidence_time_ns=packet.envelope.timestamps.source_ns,
|
|
basis=EvidenceBasis.CAMERA,
|
|
currentness=currentness,
|
|
occupied_support=False,
|
|
source_point_ids=(),
|
|
metric_geometry=None,
|
|
proposal_ids=(proposal.proposal_id,),
|
|
semantic_hint=proposal.semantic_hint,
|
|
reason_codes=(reason,),
|
|
)
|
|
|
|
|
|
def _load_npz(
|
|
path: Path,
|
|
label: str,
|
|
) -> dict[str, npt.NDArray[np.generic]]:
|
|
try:
|
|
with np.load(path, allow_pickle=False) as archive:
|
|
return {name: np.asarray(archive[name]) for name in archive.files}
|
|
except (OSError, ValueError) as exc:
|
|
raise GeometryProviderError(f"{label} cannot be opened") from exc
|
|
|
|
|
|
def _verify_regular_file(path: Path, *, expected_sha256: str | None, label: str) -> None:
|
|
if not path.is_file() or path.is_symlink():
|
|
raise GeometryProviderError(f"{label} must be a regular file")
|
|
if expected_sha256 is not None and _file_sha256(path) != expected_sha256:
|
|
raise GeometryProviderError(f"{label} digest changed")
|
|
|
|
|
|
def _file_sha256(path: Path) -> str:
|
|
digest = hashlib.sha256()
|
|
with path.open("rb") as handle:
|
|
for chunk in iter(lambda: handle.read(1024 * 1024), b""):
|
|
digest.update(chunk)
|
|
return digest.hexdigest()
|
|
|
|
|
|
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 GeometryProviderError(f"{label} must be an object")
|
|
return value
|
|
|
|
|
|
def _exact_keys(document: Mapping[str, object], expected: set[str], label: str) -> None:
|
|
if set(document) != expected:
|
|
raise GeometryProviderError(f"{label} fields are incompatible")
|
|
|
|
|
|
def _string(document: Mapping[str, object], key: str) -> str:
|
|
value = document.get(key)
|
|
if not isinstance(value, str) or not value:
|
|
raise GeometryProviderError(f"{key} must be a nonempty string")
|
|
return value
|
|
|
|
|
|
def _positive_integer(document: Mapping[str, object], key: str) -> int:
|
|
value = document.get(key)
|
|
if not isinstance(value, int) or isinstance(value, bool) or value < 1:
|
|
raise GeometryProviderError(f"{key} must be a positive integer")
|
|
return value
|
|
|
|
|
|
def _number(document: Mapping[str, object], key: str) -> float:
|
|
value = document.get(key)
|
|
if not isinstance(value, (int, float)) or isinstance(value, bool):
|
|
raise GeometryProviderError(f"{key} must be numeric")
|
|
result = float(value)
|
|
if not math.isfinite(result):
|
|
raise GeometryProviderError(f"{key} must be finite")
|
|
return result
|
|
|
|
|
|
def _digest(document: Mapping[str, object], key: str) -> str:
|
|
value = _string(document, key)
|
|
if len(value) != 64 or any(character not in "0123456789abcdef" for character in value):
|
|
raise GeometryProviderError(f"{key} must be a SHA-256 digest")
|
|
return value
|
|
|
|
|
|
__all__ = [
|
|
"DEFAULT_GEOMETRY_PROFILE_PATH",
|
|
"GEOMETRY_PROFILE_SCHEMA",
|
|
"GEOMETRY_PROVIDER_ID",
|
|
"GeometryFrame",
|
|
"GeometryProfile",
|
|
"GeometryProviderError",
|
|
"GeometryProviderSnapshot",
|
|
"Ravnoves00GeometryAssociationProvider",
|
|
"RecordedFrameTemporalBinding",
|
|
"RecordedGeometryStore",
|
|
"load_geometry_profile",
|
|
]
|