feat(perception): canonicalize metric geometry
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,877 @@
|
||||
"""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 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_index = envelope.sequence
|
||||
if not 0 <= frame_index < self.profile.frame_count:
|
||||
raise GeometryProviderError("packet geometry frame index 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]):
|
||||
raise GeometryProviderError("packet claims unavailable source geometry as current")
|
||||
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 _validate(self) -> None:
|
||||
source_required = {
|
||||
"frame_indices",
|
||||
"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",
|
||||
}
|
||||
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,),
|
||||
"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),
|
||||
}
|
||||
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")
|
||||
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",
|
||||
"RecordedGeometryStore",
|
||||
"load_geometry_profile",
|
||||
]
|
||||
Reference in New Issue
Block a user