feat(perception): canonicalize metric geometry
This commit is contained in:
DCCONSTRUCTIONS
@@ -139,7 +139,7 @@
|
||||
},
|
||||
"wheel": {
|
||||
"name": "nodedc_mission_core-0.1.0-py3-none-any.whl",
|
||||
"sha256": "4158784fd40b70c7213b629ed39d664fd2f12eab1c9242ec9a6ebbb366e3dd5a"
|
||||
"sha256": "ecbbfeee7ea62a7f5f5368efccf5d254abb17b4801a29538e3bc7aa3c8c3a40a"
|
||||
}
|
||||
},
|
||||
"rollback": {
|
||||
|
||||
@@ -0,0 +1,59 @@
|
||||
{
|
||||
"schema_version": "missioncore.geometry-association-profile/v1",
|
||||
"profile_id": "m4-ravnoves00-e29-e32-geometry/v1",
|
||||
"provider_id": "ravnoves00-geometry-association/v1",
|
||||
"source": {
|
||||
"source_id": "RAVNOVES00",
|
||||
"session_id": "20260720T065719Z_viewer_live",
|
||||
"source_pack_id": "e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b",
|
||||
"source_pack_sha256": "0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944",
|
||||
"frame_count": 4489,
|
||||
"point_count": 9207270
|
||||
},
|
||||
"local_surface": {
|
||||
"model_id": "k1-local-surface-23762244c8bdb97de26fb721ac957d7a00bc9a63571ac4cfa4be19c4effc7d55",
|
||||
"artifact_sha256": "f57eb2485b6cef47f2a97a2d9ff1aa9fd9265fe1eb69cd5852d12f39e13b8bc6",
|
||||
"valid_frame_count": 3928
|
||||
},
|
||||
"projection": {
|
||||
"model": "KB4",
|
||||
"width": 800,
|
||||
"height": 600,
|
||||
"coordinate_frame": "map"
|
||||
},
|
||||
"association": {
|
||||
"bbox_inset_fraction": 0.03,
|
||||
"depth_cluster_minimum_gap_m": 0.45,
|
||||
"depth_cluster_gap_fraction": 0.08,
|
||||
"spatial_cluster_radius_m": 0.6,
|
||||
"semantic_minimum_occupied_points": 2,
|
||||
"semantic_minimum_occupied_voxels": 1,
|
||||
"semantic_voxel_size_m": 0.35,
|
||||
"conflict_minimum_classified_points": 6,
|
||||
"conflict_surface_fraction": 0.8,
|
||||
"geometry_local_radius_m": 10.0,
|
||||
"geometry_voxel_size_m": 0.45,
|
||||
"geometry_minimum_cluster_points": 4,
|
||||
"geometry_minimum_cluster_voxels": 2,
|
||||
"maximum_geometry_clusters_per_frame": 64
|
||||
},
|
||||
"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
|
||||
},
|
||||
"authority": {
|
||||
"ground_truth": false,
|
||||
"physical_live": false,
|
||||
"commands_enabled": false,
|
||||
"actuation_allowed": false,
|
||||
"navigation_or_safety_accepted": false
|
||||
}
|
||||
}
|
||||
@@ -10,7 +10,7 @@
|
||||
{
|
||||
"module": "k1link.compute.semantic_geometry_fusion",
|
||||
"role": "camera and registered geometry association",
|
||||
"admission": "adapt-behind-provider"
|
||||
"admission": "reference-only-extracted-to-product"
|
||||
},
|
||||
{
|
||||
"module": "k1link.compute.track_geometry",
|
||||
@@ -31,6 +31,16 @@
|
||||
"module": "k1link.perception.yolox_object_detector",
|
||||
"role": "product-owned frozen YOLOX preprocess, Triton transport and postprocess",
|
||||
"admission": "product-owned"
|
||||
},
|
||||
{
|
||||
"module": "k1link.perception.geometry_math",
|
||||
"role": "product-owned KB4 projection and occupied geometry association with E29 parity",
|
||||
"admission": "product-owned"
|
||||
},
|
||||
{
|
||||
"module": "k1link.perception.geometry",
|
||||
"role": "digest-bound GeometryAssociationProvider and exact point ownership",
|
||||
"admission": "product-owned"
|
||||
}
|
||||
],
|
||||
"historical_wrappers": [
|
||||
|
||||
@@ -25,7 +25,7 @@ WHEEL_NAME = "nodedc_mission_core-0.1.0-py3-none-any.whl"
|
||||
RUNNER_NAME = RUNNER.name
|
||||
PATCH_ID = re.compile(r"^[A-Za-z0-9._-]{1,96}$")
|
||||
EXPECTED_BASELINE_SHA256 = "ea10359339e6cce31b5780a2710299771cab7cc0c1c2a2b56a1621f786b31fa8"
|
||||
EXPECTED_WHEEL_SHA256 = "4158784fd40b70c7213b629ed39d664fd2f12eab1c9242ec9a6ebbb366e3dd5a"
|
||||
EXPECTED_WHEEL_SHA256 = "ecbbfeee7ea62a7f5f5368efccf5d254abb17b4801a29538e3bc7aa3c8c3a40a"
|
||||
PAYLOAD_FILES = (
|
||||
RUNNER_NAME,
|
||||
WHEEL_NAME,
|
||||
|
||||
@@ -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",
|
||||
]
|
||||
@@ -0,0 +1,496 @@
|
||||
"""Pure KB4 projection and frame-local occupied-geometry association.
|
||||
|
||||
This is the product-owned extraction of the accepted E29/E32 mathematics. It
|
||||
has no LAB, compute-package, device-plugin or transport dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
import numpy.typing as npt
|
||||
|
||||
FloatArray = npt.NDArray[np.float64]
|
||||
IntArray = npt.NDArray[np.int64]
|
||||
UInt8Array = npt.NDArray[np.uint8]
|
||||
Float32Array = npt.NDArray[np.float32]
|
||||
|
||||
POINT_SURFACE = 1
|
||||
POINT_OCCUPIED = 2
|
||||
POINT_BELOW_SURFACE = 3
|
||||
|
||||
|
||||
class GeometryMathError(ValueError):
|
||||
"""A projection or association input violates the frozen M4 contract."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class GeometryAssociationProfile:
|
||||
bbox_inset_fraction: float
|
||||
depth_cluster_minimum_gap_m: float
|
||||
depth_cluster_gap_fraction: float
|
||||
spatial_cluster_radius_m: float
|
||||
semantic_minimum_occupied_points: int
|
||||
semantic_minimum_occupied_voxels: int
|
||||
semantic_voxel_size_m: float
|
||||
conflict_minimum_classified_points: int
|
||||
conflict_surface_fraction: float
|
||||
geometry_local_radius_m: float
|
||||
geometry_voxel_size_m: float
|
||||
geometry_minimum_cluster_points: int
|
||||
geometry_minimum_cluster_voxels: int
|
||||
maximum_geometry_clusters_per_frame: int
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
numeric = (
|
||||
self.bbox_inset_fraction,
|
||||
self.depth_cluster_minimum_gap_m,
|
||||
self.depth_cluster_gap_fraction,
|
||||
self.spatial_cluster_radius_m,
|
||||
self.semantic_voxel_size_m,
|
||||
self.conflict_surface_fraction,
|
||||
self.geometry_local_radius_m,
|
||||
self.geometry_voxel_size_m,
|
||||
)
|
||||
if (
|
||||
not np.isfinite(numeric).all()
|
||||
or not 0.0 <= self.bbox_inset_fraction < 0.25
|
||||
or not 0.05 <= self.depth_cluster_minimum_gap_m <= 5.0
|
||||
or not 0.0 <= self.depth_cluster_gap_fraction <= 1.0
|
||||
or not 0.05 <= self.spatial_cluster_radius_m <= 5.0
|
||||
or not 1 <= self.semantic_minimum_occupied_points <= 64
|
||||
or not 1 <= self.semantic_minimum_occupied_voxels <= 32
|
||||
or not 0.05 <= self.semantic_voxel_size_m <= 2.0
|
||||
or not 1 <= self.conflict_minimum_classified_points <= 256
|
||||
or not 0.5 <= self.conflict_surface_fraction <= 1.0
|
||||
or not 1.0 <= self.geometry_local_radius_m <= 100.0
|
||||
or not 0.05 <= self.geometry_voxel_size_m <= 5.0
|
||||
or not 1 <= self.geometry_minimum_cluster_points <= 256
|
||||
or not 1 <= self.geometry_minimum_cluster_voxels <= 128
|
||||
or not 1 <= self.maximum_geometry_clusters_per_frame <= 512
|
||||
):
|
||||
raise GeometryMathError("geometry association profile is invalid")
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class Kb4ProjectionProfile:
|
||||
width: int
|
||||
height: int
|
||||
intrinsic_fx_fy_cx_cy: tuple[float, float, float, float]
|
||||
distortion_kb4: tuple[float, float, float, float]
|
||||
t_camera_from_lidar: FloatArray
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
transform = np.asarray(self.t_camera_from_lidar, dtype=np.float64)
|
||||
values = (*self.intrinsic_fx_fy_cx_cy, *self.distortion_kb4)
|
||||
if (
|
||||
self.width < 1
|
||||
or self.height < 1
|
||||
or transform.shape != (4, 4)
|
||||
or not np.isfinite(transform).all()
|
||||
or not np.isfinite(values).all()
|
||||
or self.intrinsic_fx_fy_cx_cy[0] <= 0.0
|
||||
or self.intrinsic_fx_fy_cx_cy[1] <= 0.0
|
||||
):
|
||||
raise GeometryMathError("KB4 projection profile is invalid")
|
||||
frozen = np.array(transform, dtype=np.float64, copy=True)
|
||||
frozen.setflags(write=False)
|
||||
object.__setattr__(self, "t_camera_from_lidar", frozen)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class ProjectedPointCloud:
|
||||
pixels_xy: FloatArray
|
||||
depths_m: FloatArray
|
||||
source_indices: IntArray
|
||||
source_point_count: int
|
||||
camera_front_point_count: int
|
||||
|
||||
@property
|
||||
def projected_point_count(self) -> int:
|
||||
return int(self.pixels_xy.shape[0])
|
||||
|
||||
@property
|
||||
def overlap_bounds_xyxy(self) -> tuple[float, float, float, float] | None:
|
||||
if not self.projected_point_count:
|
||||
return None
|
||||
return (
|
||||
float(np.min(self.pixels_xy[:, 0])),
|
||||
float(np.min(self.pixels_xy[:, 1])),
|
||||
float(np.max(self.pixels_xy[:, 0])),
|
||||
float(np.max(self.pixels_xy[:, 1])),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class SemanticGeometrySupport:
|
||||
projected_points_in_region: int
|
||||
classified_points_in_region: int
|
||||
surface_points_in_region: int
|
||||
occupied_points_in_region: int
|
||||
below_surface_points_in_region: int
|
||||
occupied_source_indices: IntArray
|
||||
occupied_depths_m: FloatArray
|
||||
qualified: bool
|
||||
conflict: bool
|
||||
overlaps_projected_extent: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class GeometryCluster:
|
||||
source_indices: IntArray
|
||||
centroid_map_xyz_m: tuple[float, float, float]
|
||||
covariance_diagonal_m2: tuple[float, float, float]
|
||||
nearest_range_m: float
|
||||
voxel_count: int
|
||||
|
||||
|
||||
def project_map_points_kb4(
|
||||
points_map_xyz: npt.ArrayLike,
|
||||
*,
|
||||
position_map_xyz: npt.ArrayLike,
|
||||
orientation_map_from_lidar_xyzw: npt.ArrayLike,
|
||||
profile: Kb4ProjectionProfile,
|
||||
) -> ProjectedPointCloud:
|
||||
"""Project one registered map-frame increment into the raw KB4 image."""
|
||||
|
||||
points_map = _finite_points(points_map_xyz)
|
||||
position = np.asarray(position_map_xyz, dtype=np.float64)
|
||||
if position.shape != (3,) or not np.isfinite(position).all():
|
||||
raise GeometryMathError("pose position must contain three finite values")
|
||||
rotation = quaternion_xyzw_to_rotation_matrix(orientation_map_from_lidar_xyzw)
|
||||
points_lidar = (points_map - position) @ rotation
|
||||
transform = profile.t_camera_from_lidar
|
||||
points_camera = points_lidar @ transform[:3, :3].T + transform[:3, 3]
|
||||
front = points_camera[:, 2] > 1e-6
|
||||
front_indices = np.flatnonzero(front)
|
||||
front_points = points_camera[front]
|
||||
if front_points.size == 0:
|
||||
return ProjectedPointCloud(
|
||||
pixels_xy=np.empty((0, 2), dtype=np.float64),
|
||||
depths_m=np.empty(0, dtype=np.float64),
|
||||
source_indices=np.empty(0, dtype=np.int64),
|
||||
source_point_count=int(points_map.shape[0]),
|
||||
camera_front_point_count=0,
|
||||
)
|
||||
x, y, z = front_points.T
|
||||
radial = np.hypot(x, y)
|
||||
theta = np.arctan2(radial, z)
|
||||
squared = theta * theta
|
||||
k1, k2, k3, k4 = profile.distortion_kb4
|
||||
distorted = theta * (
|
||||
1.0 + k1 * squared + k2 * squared**2 + k3 * squared**3 + k4 * squared**4
|
||||
)
|
||||
scale = np.divide(distorted, radial, out=np.zeros_like(distorted), where=radial > 1e-12)
|
||||
fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy
|
||||
u = fx * x * scale + cx
|
||||
v = fy * y * scale + cy
|
||||
in_frame = (
|
||||
np.isfinite(u)
|
||||
& np.isfinite(v)
|
||||
& (u >= 0.0)
|
||||
& (u < profile.width)
|
||||
& (v >= 0.0)
|
||||
& (v < profile.height)
|
||||
)
|
||||
return ProjectedPointCloud(
|
||||
pixels_xy=np.column_stack((u[in_frame], v[in_frame])).astype(np.float64, copy=False),
|
||||
depths_m=z[in_frame].astype(np.float64, copy=False),
|
||||
source_indices=front_indices[in_frame].astype(np.int64, copy=False),
|
||||
source_point_count=int(points_map.shape[0]),
|
||||
camera_front_point_count=int(front_points.shape[0]),
|
||||
)
|
||||
|
||||
|
||||
def quaternion_xyzw_to_rotation_matrix(orientation_xyzw: npt.ArrayLike) -> FloatArray:
|
||||
quaternion = np.asarray(orientation_xyzw, dtype=np.float64)
|
||||
if quaternion.shape != (4,) or not np.isfinite(quaternion).all():
|
||||
raise GeometryMathError("pose quaternion must contain four finite values")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if not math.isfinite(norm) or norm < 1e-9:
|
||||
raise GeometryMathError("pose quaternion has no usable norm")
|
||||
x, y, z, w = quaternion / norm
|
||||
return np.asarray(
|
||||
(
|
||||
(1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)),
|
||||
(2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)),
|
||||
(2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)),
|
||||
),
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
|
||||
def semantic_geometry_support(
|
||||
bbox_xyxy: tuple[float, float, float, float],
|
||||
*,
|
||||
projected: ProjectedPointCloud,
|
||||
frame_points_map: FloatArray,
|
||||
point_class: UInt8Array,
|
||||
profile: GeometryAssociationProfile,
|
||||
) -> SemanticGeometrySupport:
|
||||
"""Return qualified occupied support without assigning point ownership."""
|
||||
|
||||
bbox = np.asarray(bbox_xyxy, dtype=np.float64)
|
||||
if bbox.shape != (4,) or not np.isfinite(bbox).all() or np.any(bbox[2:] <= bbox[:2]):
|
||||
raise GeometryMathError("proposal region is invalid")
|
||||
width, height = float(bbox[2] - bbox[0]), float(bbox[3] - bbox[1])
|
||||
inset = profile.bbox_inset_fraction
|
||||
inner = np.asarray(
|
||||
(
|
||||
bbox[0] + width * inset,
|
||||
bbox[1] + height * inset,
|
||||
bbox[2] - width * inset,
|
||||
bbox[3] - height * inset,
|
||||
),
|
||||
dtype=np.float64,
|
||||
)
|
||||
pixels = projected.pixels_xy
|
||||
inside = (
|
||||
(pixels[:, 0] >= inner[0])
|
||||
& (pixels[:, 0] <= inner[2])
|
||||
& (pixels[:, 1] >= inner[1])
|
||||
& (pixels[:, 1] <= inner[3])
|
||||
)
|
||||
rows = np.flatnonzero(inside).astype(np.int64, copy=False)
|
||||
indices = projected.source_indices[rows]
|
||||
classes = point_class[indices]
|
||||
counts = np.bincount(classes, minlength=4)
|
||||
occupied_rows = rows[classes == POINT_OCCUPIED]
|
||||
clustered = _depth_cluster(
|
||||
occupied_rows,
|
||||
projected.depths_m,
|
||||
minimum_gap_m=profile.depth_cluster_minimum_gap_m,
|
||||
gap_fraction=profile.depth_cluster_gap_fraction,
|
||||
)
|
||||
clustered = _spatial_cluster(
|
||||
clustered,
|
||||
projected.source_indices,
|
||||
frame_points_map,
|
||||
radius_m=profile.spatial_cluster_radius_m,
|
||||
)
|
||||
occupied_indices = projected.source_indices[clustered].astype(np.int64, copy=False)
|
||||
occupied_depths = projected.depths_m[clustered].astype(np.float64, copy=False)
|
||||
voxel_count = _voxel_count(
|
||||
frame_points_map[occupied_indices],
|
||||
profile.semantic_voxel_size_m,
|
||||
)
|
||||
occupied_count = int(occupied_indices.size)
|
||||
qualified = (
|
||||
occupied_count >= profile.semantic_minimum_occupied_points
|
||||
and voxel_count >= profile.semantic_minimum_occupied_voxels
|
||||
)
|
||||
classified = int(counts[POINT_SURFACE] + counts[POINT_OCCUPIED] + counts[POINT_BELOW_SURFACE])
|
||||
conflict = (
|
||||
not qualified
|
||||
and classified >= profile.conflict_minimum_classified_points
|
||||
and int(counts[POINT_OCCUPIED]) == 0
|
||||
and float(counts[POINT_SURFACE] / max(1, classified))
|
||||
>= profile.conflict_surface_fraction
|
||||
)
|
||||
bounds = projected.overlap_bounds_xyxy
|
||||
bbox_tuple = (float(bbox[0]), float(bbox[1]), float(bbox[2]), float(bbox[3]))
|
||||
overlaps = bounds is not None and _regions_intersect(bbox_tuple, bounds)
|
||||
return SemanticGeometrySupport(
|
||||
projected_points_in_region=int(indices.size),
|
||||
classified_points_in_region=classified,
|
||||
surface_points_in_region=int(counts[POINT_SURFACE]),
|
||||
occupied_points_in_region=int(counts[POINT_OCCUPIED]),
|
||||
below_surface_points_in_region=int(counts[POINT_BELOW_SURFACE]),
|
||||
occupied_source_indices=occupied_indices,
|
||||
occupied_depths_m=occupied_depths,
|
||||
qualified=qualified,
|
||||
conflict=conflict,
|
||||
overlaps_projected_extent=overlaps,
|
||||
)
|
||||
|
||||
|
||||
def geometry_only_clusters(
|
||||
*,
|
||||
points_map: FloatArray,
|
||||
point_class: UInt8Array,
|
||||
sensor_position_map: FloatArray,
|
||||
claimed_source_indices: frozenset[int],
|
||||
profile: GeometryAssociationProfile,
|
||||
) -> tuple[GeometryCluster, ...]:
|
||||
"""Return bounded unclaimed occupied components with no semantic class."""
|
||||
|
||||
occupied = np.flatnonzero(point_class == POINT_OCCUPIED).astype(np.int64)
|
||||
if occupied.size == 0:
|
||||
return ()
|
||||
ranges = np.linalg.norm(points_map[occupied] - sensor_position_map, axis=1)
|
||||
occupied = occupied[ranges <= profile.geometry_local_radius_m]
|
||||
clusters: list[GeometryCluster] = []
|
||||
for indices, voxel_count in _voxel_components(
|
||||
points_map[occupied],
|
||||
occupied,
|
||||
profile.geometry_voxel_size_m,
|
||||
):
|
||||
if (
|
||||
indices.size < profile.geometry_minimum_cluster_points
|
||||
or voxel_count < profile.geometry_minimum_cluster_voxels
|
||||
or any(int(value) in claimed_source_indices for value in indices)
|
||||
):
|
||||
continue
|
||||
values = points_map[indices]
|
||||
distances = np.linalg.norm(values - sensor_position_map, axis=1)
|
||||
centroid = np.median(values, axis=0)
|
||||
covariance = values.var(axis=0)
|
||||
clusters.append(
|
||||
GeometryCluster(
|
||||
source_indices=indices.astype(np.int64, copy=False),
|
||||
centroid_map_xyz_m=(
|
||||
float(centroid[0]),
|
||||
float(centroid[1]),
|
||||
float(centroid[2]),
|
||||
),
|
||||
covariance_diagonal_m2=(
|
||||
float(covariance[0]),
|
||||
float(covariance[1]),
|
||||
float(covariance[2]),
|
||||
),
|
||||
nearest_range_m=float(np.min(distances)),
|
||||
voxel_count=voxel_count,
|
||||
)
|
||||
)
|
||||
clusters.sort(key=lambda item: (item.nearest_range_m, -int(item.source_indices.size)))
|
||||
return tuple(clusters[: profile.maximum_geometry_clusters_per_frame])
|
||||
|
||||
|
||||
def _depth_cluster(
|
||||
rows: IntArray,
|
||||
depths: FloatArray,
|
||||
*,
|
||||
minimum_gap_m: float,
|
||||
gap_fraction: float,
|
||||
) -> IntArray:
|
||||
if rows.size < 2:
|
||||
return rows
|
||||
ordered = rows[np.argsort(depths[rows])]
|
||||
groups: list[IntArray] = []
|
||||
start = 0
|
||||
for offset, gap in enumerate(np.diff(depths[ordered]), start=1):
|
||||
threshold = max(minimum_gap_m, gap_fraction * float(depths[ordered[offset - 1]]))
|
||||
if float(gap) > threshold:
|
||||
groups.append(ordered[start:offset])
|
||||
start = offset
|
||||
groups.append(ordered[start:])
|
||||
return min(groups, key=lambda group: (-int(group.size), float(np.median(depths[group]))))
|
||||
|
||||
|
||||
def _spatial_cluster(
|
||||
rows: IntArray,
|
||||
source_indices: IntArray,
|
||||
points_map: FloatArray,
|
||||
*,
|
||||
radius_m: float,
|
||||
) -> IntArray:
|
||||
if rows.size < 2:
|
||||
return rows
|
||||
points = points_map[source_indices[rows]]
|
||||
adjacent = np.sum((points[:, None, :] - points[None, :, :]) ** 2, axis=2) <= radius_m**2
|
||||
unseen = set(range(rows.size))
|
||||
groups: list[list[int]] = []
|
||||
while unseen:
|
||||
seed = unseen.pop()
|
||||
group, pending = [seed], [seed]
|
||||
while pending:
|
||||
current = pending.pop()
|
||||
connected = [item for item in tuple(unseen) if adjacent[current, item]]
|
||||
for item in connected:
|
||||
unseen.remove(item)
|
||||
pending.append(item)
|
||||
group.append(item)
|
||||
groups.append(group)
|
||||
selected = min(
|
||||
groups,
|
||||
key=lambda group: (
|
||||
-len(group),
|
||||
float(np.median(np.linalg.norm(points[np.asarray(group, dtype=np.int64)], axis=1))),
|
||||
),
|
||||
)
|
||||
return rows[np.asarray(selected, dtype=np.int64)]
|
||||
|
||||
|
||||
def _voxel_components(
|
||||
points: FloatArray,
|
||||
source_indices: IntArray,
|
||||
voxel_size_m: float,
|
||||
) -> list[tuple[IntArray, int]]:
|
||||
cells = np.floor(points / voxel_size_m).astype(np.int64)
|
||||
cell_points: dict[tuple[int, int, int], list[int]] = {}
|
||||
for local_index, cell in enumerate(cells):
|
||||
key = (int(cell[0]), int(cell[1]), int(cell[2]))
|
||||
cell_points.setdefault(key, []).append(int(source_indices[local_index]))
|
||||
remaining = set(cell_points)
|
||||
neighbors = tuple(
|
||||
(dx, dy, dz)
|
||||
for dx in (-1, 0, 1)
|
||||
for dy in (-1, 0, 1)
|
||||
for dz in (-1, 0, 1)
|
||||
if (dx, dy, dz) != (0, 0, 0)
|
||||
)
|
||||
components: list[tuple[IntArray, int]] = []
|
||||
while remaining:
|
||||
seed = remaining.pop()
|
||||
queue = deque([seed])
|
||||
component = [seed]
|
||||
while queue:
|
||||
current = queue.popleft()
|
||||
for delta in neighbors:
|
||||
candidate = tuple(current[index] + delta[index] for index in range(3))
|
||||
if candidate in remaining:
|
||||
remaining.remove(candidate)
|
||||
queue.append(candidate)
|
||||
component.append(candidate)
|
||||
indices = np.asarray(
|
||||
[index for cell in component for index in cell_points[cell]],
|
||||
dtype=np.int64,
|
||||
)
|
||||
components.append((indices, len(component)))
|
||||
return components
|
||||
|
||||
|
||||
def _voxel_count(points: FloatArray, voxel_size_m: float) -> int:
|
||||
if points.size == 0:
|
||||
return 0
|
||||
cells = np.floor(points / voxel_size_m).astype(np.int64)
|
||||
return int(np.unique(cells, axis=0).shape[0])
|
||||
|
||||
|
||||
def _regions_intersect(
|
||||
left: tuple[float, float, float, float],
|
||||
right: tuple[float, float, float, float],
|
||||
) -> bool:
|
||||
return (
|
||||
left[0] <= right[2]
|
||||
and left[2] >= right[0]
|
||||
and left[1] <= right[3]
|
||||
and left[3] >= right[1]
|
||||
)
|
||||
|
||||
|
||||
def _finite_points(points_xyz: npt.ArrayLike) -> FloatArray:
|
||||
points = np.asarray(points_xyz, dtype=np.float64)
|
||||
if points.ndim != 2 or points.shape[1:] != (3,) or not np.isfinite(points).all():
|
||||
raise GeometryMathError("point cloud must be finite with shape (N, 3)")
|
||||
return points
|
||||
|
||||
|
||||
__all__ = [
|
||||
"GeometryAssociationProfile",
|
||||
"GeometryCluster",
|
||||
"GeometryMathError",
|
||||
"Kb4ProjectionProfile",
|
||||
"POINT_BELOW_SURFACE",
|
||||
"POINT_OCCUPIED",
|
||||
"POINT_SURFACE",
|
||||
"ProjectedPointCloud",
|
||||
"SemanticGeometrySupport",
|
||||
"geometry_only_clusters",
|
||||
"project_map_points_kb4",
|
||||
"quaternion_xyzw_to_rotation_matrix",
|
||||
"semantic_geometry_support",
|
||||
]
|
||||
@@ -0,0 +1,665 @@
|
||||
"""Immutable full-source M4.4 geometry replay over the accepted M4.3 ledger."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import os
|
||||
import shutil
|
||||
import time
|
||||
import uuid
|
||||
from dataclasses import dataclass
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from typing import Final
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .baseline import BASELINE_RECORDED_JOB_ID
|
||||
from .contracts import EvidenceBasis, ObstacleObservation
|
||||
from .detector_replay_result import (
|
||||
read_detector_replay_result,
|
||||
require_m4_detector_replay_acceptance,
|
||||
)
|
||||
from .geometry import (
|
||||
DEFAULT_GEOMETRY_PROFILE_PATH,
|
||||
Ravnoves00GeometryAssociationProvider,
|
||||
RecordedGeometryStore,
|
||||
load_geometry_profile,
|
||||
)
|
||||
from .graph_validation import validate_observations
|
||||
from .providers import SourcePacket
|
||||
from .recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference
|
||||
|
||||
GEOMETRY_REPLAY_SCHEMA: Final = "missioncore.perception-geometry-replay-result/v1"
|
||||
GEOMETRY_REPLAY_FRAME_SCHEMA: Final = "missioncore.perception-geometry-replay-frame/v1"
|
||||
GEOMETRY_REPLAY_REPORT_SCHEMA: Final = "missioncore.perception-geometry-replay-report/v1"
|
||||
GEOMETRY_REPLAY_RESULT_PREFIX: Final = "m4-geometry-replay-"
|
||||
GEOMETRY_REPLAY_FRAMES_NAME: Final = "frames.jsonl"
|
||||
GEOMETRY_REPLAY_REPORT_NAME: Final = "report.json"
|
||||
GEOMETRY_REPLAY_MANIFEST_NAME: Final = "manifest.json"
|
||||
|
||||
E32_RESULT_ID: Final = (
|
||||
"e32-track-geometry-"
|
||||
"a14ca0e7fb3850ca0dfa3c41634e1b490a2d58ab74d101afc6d6921fbdb0e6fd"
|
||||
)
|
||||
E32_MANIFEST_SHA256: Final = "f4b57c9f7619c43414adf1d10488b05df466226a523a0c001671102ced0e9ab8"
|
||||
E53_RESULT_ID: Final = (
|
||||
"e53-camera-first-shadow-"
|
||||
"e6f03cf8bfb15db86100239b060e13f914532618b7b99e811866e4e6a555186c"
|
||||
)
|
||||
E53_MANIFEST_SHA256: Final = "fc5b4ae69aae0098b7075c539d25b563dff1bbb09209a49030edda6cba9ee544"
|
||||
EXPECTED_SOURCE_AVAILABLE_FRAMES: Final = 3928
|
||||
EXPECTED_SOURCE_UNAVAILABLE_FRAMES: Final = 561
|
||||
|
||||
|
||||
class GeometryReplayError(RuntimeError):
|
||||
"""A full-source geometry replay is incomplete, mutable or inconsistent."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class GeometryReplayResult:
|
||||
result_id: str
|
||||
result_root: Path
|
||||
accepted: bool
|
||||
metrics: dict[str, object]
|
||||
report: dict[str, object]
|
||||
manifest: dict[str, object]
|
||||
|
||||
|
||||
def build_geometry_replay(
|
||||
*,
|
||||
repository_root: Path,
|
||||
detector_result_root: Path,
|
||||
output_root: Path,
|
||||
) -> GeometryReplayResult:
|
||||
"""Run all accepted detector proposals through the canonical M4.4 provider."""
|
||||
|
||||
repository = repository_root.resolve()
|
||||
detector = read_detector_replay_result(detector_result_root)
|
||||
require_m4_detector_replay_acceptance(detector)
|
||||
profile_path = repository / DEFAULT_GEOMETRY_PROFILE_PATH
|
||||
profile = load_geometry_profile(profile_path)
|
||||
store = RecordedGeometryStore.from_repository(repository, profile=profile)
|
||||
provider = Ravnoves00GeometryAssociationProvider(store=store)
|
||||
references = _verified_historical_references(repository)
|
||||
|
||||
root = output_root.expanduser().absolute()
|
||||
root.mkdir(mode=0o700, parents=True, exist_ok=True)
|
||||
staging = root / f".geometry-replay.{uuid.uuid4().hex}.tmp"
|
||||
staging.mkdir(mode=0o700, exist_ok=False)
|
||||
started_ns = time.perf_counter_ns()
|
||||
frame_latencies_ms: list[float] = []
|
||||
source_available = 0
|
||||
source_unavailable = 0
|
||||
try:
|
||||
frames_path = staging / GEOMETRY_REPLAY_FRAMES_NAME
|
||||
with frames_path.open("wb") as output:
|
||||
for detector_frame in detector.frames:
|
||||
if detector_frame.outcome != "completed":
|
||||
raise GeometryReplayError("accepted detector ledger contains a failed frame")
|
||||
packet = _packet(detector_frame.envelope)
|
||||
frame_started_ns = time.perf_counter_ns()
|
||||
before = provider.snapshot()
|
||||
observations = provider.associate(packet, detector_frame.proposals)
|
||||
validate_observations(packet, detector_frame.proposals, observations)
|
||||
after = provider.snapshot()
|
||||
frame_latency_ms = (time.perf_counter_ns() - frame_started_ns) / 1_000_000
|
||||
frame_latencies_ms.append(frame_latency_ms)
|
||||
source_available += packet.envelope.registered_point_increment.available
|
||||
source_unavailable += not packet.envelope.registered_point_increment.available
|
||||
proposal_observations = tuple(item for item in observations if item.proposal_ids)
|
||||
geometry_only = tuple(item for item in observations if not item.proposal_ids)
|
||||
eligible = sum(_range_eligible(item) for item in proposal_observations)
|
||||
ranged = sum(item.metric_geometry is not None for item in proposal_observations)
|
||||
conflict = sum(
|
||||
item.basis is EvidenceBasis.CONFLICT for item in proposal_observations
|
||||
)
|
||||
frame_document = {
|
||||
"schema_version": GEOMETRY_REPLAY_FRAME_SCHEMA,
|
||||
"sequence": detector_frame.sequence,
|
||||
"frame_id": packet.envelope.frame_id,
|
||||
"source_available": packet.envelope.registered_point_increment.available,
|
||||
"proposal_count": len(detector_frame.proposals),
|
||||
"eligible_proposal_count": eligible,
|
||||
"ranged_proposal_count": ranged,
|
||||
"conflict_proposal_count": conflict,
|
||||
"camera_only_proposal_count": len(proposal_observations) - ranged - conflict,
|
||||
"geometry_only_observation_count": len(geometry_only),
|
||||
"overlapping_claims_removed": (
|
||||
after.overlapping_claims_removed - before.overlapping_claims_removed
|
||||
),
|
||||
"observations": [item.to_dict() for item in observations],
|
||||
"policy": {
|
||||
"one_owner_per_source_point": True,
|
||||
"absence_of_points_means_free": False,
|
||||
"geometry_can_invent_semantic_class": False,
|
||||
},
|
||||
"authority": {
|
||||
"ground_truth": False,
|
||||
"physical_live": False,
|
||||
"commands_enabled": False,
|
||||
"actuation_allowed": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
output.write(_canonical_json(frame_document) + b"\n")
|
||||
snapshot = provider.snapshot()
|
||||
metrics = _metrics(
|
||||
snapshot=snapshot,
|
||||
source_available=source_available,
|
||||
source_unavailable=source_unavailable,
|
||||
frame_latencies_ms=frame_latencies_ms,
|
||||
elapsed_ns=time.perf_counter_ns() - started_ns,
|
||||
)
|
||||
requirements = _requirements(metrics, detector_frame_count=len(detector.frames))
|
||||
accepted = all(requirements.values())
|
||||
frames_sha256 = _file_sha256(frames_path)
|
||||
detector_identity = _object(detector.manifest["identity"], "detector identity")
|
||||
detector_frames_sha256 = detector_identity.get("frames_sha256")
|
||||
if not isinstance(detector_frames_sha256, str):
|
||||
raise GeometryReplayError("detector frame digest is unavailable")
|
||||
identity = {
|
||||
"schema_version": GEOMETRY_REPLAY_SCHEMA,
|
||||
"detector_result_id": detector.result_id,
|
||||
"detector_frames_sha256": detector_frames_sha256,
|
||||
"geometry_profile_id": profile.profile_id,
|
||||
"geometry_profile_sha256": profile.profile_sha256,
|
||||
"geometry_provider_id": provider.provider_id,
|
||||
"source_pack_id": profile.source_pack_id,
|
||||
"source_pack_sha256": profile.source_pack_sha256,
|
||||
"local_surface_model_id": profile.local_surface_model_id,
|
||||
"local_surface_sha256": profile.local_surface_sha256,
|
||||
"historical_references": references,
|
||||
"producer_sha256": {
|
||||
name: _file_sha256(repository / "src/k1link/perception" / name)
|
||||
for name in ("geometry.py", "geometry_math.py", "geometry_replay.py")
|
||||
},
|
||||
"frames_sha256": frames_sha256,
|
||||
"metrics": metrics,
|
||||
"acceptance_requirements": requirements,
|
||||
"accepted": accepted,
|
||||
"authority": {
|
||||
"ground_truth": False,
|
||||
"physical_live": False,
|
||||
"commands_enabled": False,
|
||||
"actuation_allowed": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
|
||||
result_id = f"{GEOMETRY_REPLAY_RESULT_PREFIX}{identity_sha256}"
|
||||
created = datetime.now(UTC).isoformat(timespec="milliseconds").replace("+00:00", "Z")
|
||||
report = {
|
||||
"schema_version": GEOMETRY_REPLAY_REPORT_SCHEMA,
|
||||
"result_id": result_id,
|
||||
"identity_sha256": identity_sha256,
|
||||
"created_at_utc": created,
|
||||
"status": "accepted-product-geometry-replay" if accepted else "rejected-fail-closed",
|
||||
"accepted": accepted,
|
||||
"metrics": metrics,
|
||||
"acceptance_requirements": requirements,
|
||||
"decision": {
|
||||
"m4_4_geometry_provider_integrated": accepted,
|
||||
"semantic_class_quality_accepted": False,
|
||||
"physical_live_accepted": False,
|
||||
"next_gate": "M4.5 temporal retention and motion state",
|
||||
},
|
||||
"authority": identity["authority"],
|
||||
}
|
||||
report_path = staging / GEOMETRY_REPLAY_REPORT_NAME
|
||||
_write_json(report_path, report)
|
||||
manifest = {
|
||||
"schema_version": GEOMETRY_REPLAY_SCHEMA,
|
||||
"result_id": result_id,
|
||||
"identity_sha256": identity_sha256,
|
||||
"identity": identity,
|
||||
"created_at_utc": created,
|
||||
"accepted": accepted,
|
||||
"artifacts": [
|
||||
_artifact(frames_path, "geometry-replay-frames"),
|
||||
_artifact(report_path, "geometry-replay-report"),
|
||||
],
|
||||
}
|
||||
_write_json(staging / GEOMETRY_REPLAY_MANIFEST_NAME, manifest)
|
||||
destination = root / result_id
|
||||
if destination.exists():
|
||||
shutil.rmtree(staging)
|
||||
return read_geometry_replay_result(destination)
|
||||
os.replace(staging, destination)
|
||||
return read_geometry_replay_result(destination)
|
||||
except BaseException:
|
||||
shutil.rmtree(staging, ignore_errors=True)
|
||||
raise
|
||||
|
||||
|
||||
def read_geometry_replay_result(root: Path) -> GeometryReplayResult:
|
||||
resolved = root.resolve(strict=True)
|
||||
if resolved.is_symlink() or not resolved.name.startswith(GEOMETRY_REPLAY_RESULT_PREFIX):
|
||||
raise GeometryReplayError("geometry replay result root is invalid")
|
||||
manifest = _read_json(resolved / GEOMETRY_REPLAY_MANIFEST_NAME)
|
||||
_exact_keys(
|
||||
manifest,
|
||||
{
|
||||
"schema_version",
|
||||
"result_id",
|
||||
"identity_sha256",
|
||||
"identity",
|
||||
"created_at_utc",
|
||||
"accepted",
|
||||
"artifacts",
|
||||
},
|
||||
"geometry replay manifest",
|
||||
)
|
||||
identity = _object(manifest["identity"], "geometry replay identity")
|
||||
_exact_keys(
|
||||
identity,
|
||||
{
|
||||
"schema_version",
|
||||
"detector_result_id",
|
||||
"detector_frames_sha256",
|
||||
"geometry_profile_id",
|
||||
"geometry_profile_sha256",
|
||||
"geometry_provider_id",
|
||||
"source_pack_id",
|
||||
"source_pack_sha256",
|
||||
"local_surface_model_id",
|
||||
"local_surface_sha256",
|
||||
"historical_references",
|
||||
"producer_sha256",
|
||||
"frames_sha256",
|
||||
"metrics",
|
||||
"acceptance_requirements",
|
||||
"accepted",
|
||||
"authority",
|
||||
},
|
||||
"geometry replay identity",
|
||||
)
|
||||
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
|
||||
if (
|
||||
manifest["schema_version"] != GEOMETRY_REPLAY_SCHEMA
|
||||
or manifest["result_id"] != resolved.name
|
||||
or manifest["identity_sha256"] != identity_sha256
|
||||
or resolved.name != f"{GEOMETRY_REPLAY_RESULT_PREFIX}{identity_sha256}"
|
||||
):
|
||||
raise GeometryReplayError("geometry replay identity changed")
|
||||
artifacts = manifest.get("artifacts")
|
||||
if not isinstance(artifacts, list) or len(artifacts) != 2:
|
||||
raise GeometryReplayError("geometry replay artifact inventory changed")
|
||||
by_role = {_object(item, "geometry artifact")["role"]: item for item in artifacts}
|
||||
if set(by_role) != {"geometry-replay-frames", "geometry-replay-report"}:
|
||||
raise GeometryReplayError("geometry replay artifact roles changed")
|
||||
frames_path = _validated_artifact(
|
||||
resolved,
|
||||
by_role["geometry-replay-frames"],
|
||||
GEOMETRY_REPLAY_FRAMES_NAME,
|
||||
)
|
||||
report_path = _validated_artifact(
|
||||
resolved,
|
||||
by_role["geometry-replay-report"],
|
||||
GEOMETRY_REPLAY_REPORT_NAME,
|
||||
)
|
||||
if _file_sha256(frames_path) != identity.get("frames_sha256"):
|
||||
raise GeometryReplayError("geometry frame ledger digest changed")
|
||||
report = _read_json(report_path)
|
||||
if (
|
||||
report.get("schema_version") != GEOMETRY_REPLAY_REPORT_SCHEMA
|
||||
or report.get("result_id") != resolved.name
|
||||
or report.get("identity_sha256") != identity_sha256
|
||||
or report.get("metrics") != identity.get("metrics")
|
||||
or report.get("acceptance_requirements") != identity.get("acceptance_requirements")
|
||||
or report.get("accepted") != identity.get("accepted")
|
||||
or report.get("authority") != identity.get("authority")
|
||||
):
|
||||
raise GeometryReplayError("geometry replay report changed")
|
||||
metrics = _object(identity.get("metrics"), "geometry metrics")
|
||||
requirements = _object(identity.get("acceptance_requirements"), "geometry requirements")
|
||||
if identity.get("authority") != _false_authority():
|
||||
raise GeometryReplayError("geometry replay authority changed")
|
||||
accepted = all(value is True for value in requirements.values())
|
||||
if manifest.get("accepted") is not accepted or identity.get("accepted") is not accepted:
|
||||
raise GeometryReplayError("geometry replay acceptance changed")
|
||||
_validate_frame_ledger(frames_path, metrics)
|
||||
return GeometryReplayResult(
|
||||
result_id=resolved.name,
|
||||
result_root=resolved,
|
||||
accepted=accepted,
|
||||
metrics=metrics,
|
||||
report=report,
|
||||
manifest=manifest,
|
||||
)
|
||||
|
||||
|
||||
def _packet(envelope: object) -> SourcePacket:
|
||||
from .contracts import SourceEnvelope
|
||||
|
||||
if not isinstance(envelope, SourceEnvelope):
|
||||
raise GeometryReplayError("detector frame envelope is incompatible")
|
||||
image = RecordedFrameReference(BASELINE_RECORDED_JOB_ID, envelope.sequence)
|
||||
geometry = (
|
||||
RecordedFrameReference(RECORDED_SOURCE_PACK_ID, envelope.sequence)
|
||||
if envelope.registered_point_increment.available
|
||||
else None
|
||||
)
|
||||
return SourcePacket(
|
||||
envelope=envelope,
|
||||
image_payload=image,
|
||||
registered_point_increment_payload=geometry,
|
||||
pose_payload=geometry,
|
||||
)
|
||||
|
||||
|
||||
def _range_eligible(observation: ObstacleObservation) -> bool:
|
||||
excluded = {
|
||||
"outside-projected-lidar-overlap",
|
||||
"registered-point-increment-unavailable",
|
||||
"registered-point-increment-stale",
|
||||
"local-surface-unavailable",
|
||||
}
|
||||
return not excluded.intersection(observation.reason_codes)
|
||||
|
||||
|
||||
def _metrics(
|
||||
*,
|
||||
snapshot: object,
|
||||
source_available: int,
|
||||
source_unavailable: int,
|
||||
frame_latencies_ms: list[float],
|
||||
elapsed_ns: int,
|
||||
) -> dict[str, object]:
|
||||
from .geometry import GeometryProviderSnapshot
|
||||
|
||||
if not isinstance(snapshot, GeometryProviderSnapshot):
|
||||
raise GeometryReplayError("geometry provider snapshot is incompatible")
|
||||
values = np.asarray(frame_latencies_ms, dtype=np.float64)
|
||||
total_coverage = snapshot.total_range_coverage
|
||||
eligible_coverage = snapshot.eligible_range_coverage
|
||||
return {
|
||||
"frames": {
|
||||
"total": snapshot.completed_frames,
|
||||
"failed": snapshot.failed_frames,
|
||||
"source_available": source_available,
|
||||
"source_unavailable": source_unavailable,
|
||||
},
|
||||
"proposals": {
|
||||
"total": snapshot.proposal_count,
|
||||
"eligible_for_range": snapshot.eligible_proposal_count,
|
||||
"with_range": snapshot.ranged_proposal_count,
|
||||
"camera_only": snapshot.camera_only_proposal_count,
|
||||
"conflict": snapshot.conflict_proposal_count,
|
||||
"unavailable": snapshot.unavailable_proposal_count,
|
||||
"outside_overlap": snapshot.outside_overlap_proposal_count,
|
||||
"sparse": snapshot.sparse_proposal_count,
|
||||
"ownership_collision": snapshot.ownership_collision_proposal_count,
|
||||
"total_range_coverage": total_coverage,
|
||||
"eligible_range_coverage": eligible_coverage,
|
||||
},
|
||||
"geometry_only_observations": snapshot.geometry_only_observation_count,
|
||||
"published_source_point_rows": snapshot.published_source_point_count,
|
||||
"overlapping_claims_removed": snapshot.overlapping_claims_removed,
|
||||
"runtime": {
|
||||
"elapsed_ms": elapsed_ns / 1_000_000,
|
||||
"provider_core_ms": snapshot.core_duration_ns / 1_000_000,
|
||||
"frame_latency_ms": {
|
||||
"minimum": float(np.min(values)),
|
||||
"p50": float(np.percentile(values, 50)),
|
||||
"p95": float(np.percentile(values, 95)),
|
||||
"maximum": float(np.max(values)),
|
||||
"mean": float(np.mean(values)),
|
||||
},
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _requirements(metrics: dict[str, object], *, detector_frame_count: int) -> dict[str, bool]:
|
||||
frames = _object(metrics["frames"], "frame metrics")
|
||||
proposals = _object(metrics["proposals"], "proposal metrics")
|
||||
total = _integer(proposals["total"], "total proposals")
|
||||
ranged = _integer(proposals["with_range"], "ranged proposals")
|
||||
camera = _integer(proposals["camera_only"], "camera proposals")
|
||||
conflict = _integer(proposals["conflict"], "conflict proposals")
|
||||
eligible = _integer(proposals["eligible_for_range"], "eligible proposals")
|
||||
return {
|
||||
"detector_frame_accounting_complete": detector_frame_count == 4489,
|
||||
"geometry_frame_accounting_complete": (
|
||||
frames.get("total") == 4489 and frames.get("failed") == 0
|
||||
),
|
||||
"source_accounting_reconciles_e32_e53": (
|
||||
frames.get("source_available") == EXPECTED_SOURCE_AVAILABLE_FRAMES
|
||||
and frames.get("source_unavailable") == EXPECTED_SOURCE_UNAVAILABLE_FRAMES
|
||||
),
|
||||
"proposal_accounting_closed": total == ranged + camera + conflict,
|
||||
"range_denominators_separated": 0 <= ranged <= eligible <= total,
|
||||
"exclusive_point_ownership_validated_every_frame": True,
|
||||
"missing_points_never_interpreted_as_free": True,
|
||||
"range_requires_current_source_points": True,
|
||||
"geometry_only_semantic_class_absent": True,
|
||||
"authority_remains_false": True,
|
||||
}
|
||||
|
||||
|
||||
def _verified_historical_references(repository: Path) -> dict[str, object]:
|
||||
references = {
|
||||
"e32": (
|
||||
repository
|
||||
/ ".runtime/compute-experiments/e32/results"
|
||||
/ E32_RESULT_ID
|
||||
/ "manifest.json",
|
||||
E32_MANIFEST_SHA256,
|
||||
),
|
||||
"e53": (
|
||||
repository
|
||||
/ ".runtime/compute-experiments/e53/results"
|
||||
/ E53_RESULT_ID
|
||||
/ "manifest.json",
|
||||
E53_MANIFEST_SHA256,
|
||||
),
|
||||
}
|
||||
result: dict[str, object] = {}
|
||||
for role, (path, digest) in references.items():
|
||||
if not path.is_file() or path.is_symlink() or _file_sha256(path) != digest:
|
||||
raise GeometryReplayError(f"accepted {role.upper()} reference changed")
|
||||
result[role] = {"result_id": path.parent.name, "manifest_sha256": digest}
|
||||
return result
|
||||
|
||||
|
||||
def _validate_frame_ledger(path: Path, metrics: dict[str, object]) -> None:
|
||||
frame_count = 0
|
||||
source_available = 0
|
||||
proposal_count = 0
|
||||
eligible_count = 0
|
||||
ranged_count = 0
|
||||
conflict_count = 0
|
||||
camera_count = 0
|
||||
geometry_count = 0
|
||||
point_rows = 0
|
||||
overlaps_removed = 0
|
||||
for line_number, line in enumerate(path.read_text("utf-8").splitlines(), start=1):
|
||||
try:
|
||||
value = json.loads(line)
|
||||
except json.JSONDecodeError as exc:
|
||||
raise GeometryReplayError(f"geometry frame {line_number} is invalid JSON") from exc
|
||||
frame = _object(value, "geometry frame")
|
||||
_exact_keys(
|
||||
frame,
|
||||
{
|
||||
"schema_version",
|
||||
"sequence",
|
||||
"frame_id",
|
||||
"source_available",
|
||||
"proposal_count",
|
||||
"eligible_proposal_count",
|
||||
"ranged_proposal_count",
|
||||
"conflict_proposal_count",
|
||||
"camera_only_proposal_count",
|
||||
"geometry_only_observation_count",
|
||||
"overlapping_claims_removed",
|
||||
"observations",
|
||||
"policy",
|
||||
"authority",
|
||||
},
|
||||
"geometry frame",
|
||||
)
|
||||
if frame.get("schema_version") != GEOMETRY_REPLAY_FRAME_SCHEMA:
|
||||
raise GeometryReplayError("geometry frame schema changed")
|
||||
if frame.get("sequence") != frame_count:
|
||||
raise GeometryReplayError("geometry frame sequence is incomplete")
|
||||
if frame.get("policy") != {
|
||||
"one_owner_per_source_point": True,
|
||||
"absence_of_points_means_free": False,
|
||||
"geometry_can_invent_semantic_class": False,
|
||||
}:
|
||||
raise GeometryReplayError("geometry frame policy changed")
|
||||
if frame.get("authority") != _false_authority():
|
||||
raise GeometryReplayError("geometry frame authority changed")
|
||||
observations_value = frame.get("observations")
|
||||
if not isinstance(observations_value, list):
|
||||
raise GeometryReplayError("geometry observations are not an array")
|
||||
observations = tuple(ObstacleObservation.from_dict(item) for item in observations_value)
|
||||
frame_id = frame.get("frame_id")
|
||||
proposal_observations = tuple(item for item in observations if item.proposal_ids)
|
||||
geometry_observations = tuple(item for item in observations if not item.proposal_ids)
|
||||
if (
|
||||
not isinstance(frame_id, str)
|
||||
or any(
|
||||
item.frame_id != frame_id or item.source_id != "RAVNOVES00"
|
||||
for item in observations
|
||||
)
|
||||
or any(len(item.proposal_ids) != 1 for item in proposal_observations)
|
||||
or len(proposal_observations)
|
||||
!= _integer(frame.get("proposal_count"), "frame proposals")
|
||||
or len(geometry_observations)
|
||||
!= _integer(frame.get("geometry_only_observation_count"), "geometry-only")
|
||||
or sum(_range_eligible(item) for item in proposal_observations)
|
||||
!= _integer(frame.get("eligible_proposal_count"), "eligible proposals")
|
||||
or sum(item.metric_geometry is not None for item in proposal_observations)
|
||||
!= _integer(frame.get("ranged_proposal_count"), "ranged proposals")
|
||||
or sum(item.basis is EvidenceBasis.CONFLICT for item in proposal_observations)
|
||||
!= _integer(frame.get("conflict_proposal_count"), "conflicts")
|
||||
):
|
||||
raise GeometryReplayError("geometry frame observation accounting changed")
|
||||
owners: set[int] = set()
|
||||
for observation in observations:
|
||||
if owners.intersection(observation.source_point_ids):
|
||||
raise GeometryReplayError("geometry frame has duplicate point ownership")
|
||||
owners.update(observation.source_point_ids)
|
||||
if not observation.proposal_ids and observation.semantic_hint is not None:
|
||||
raise GeometryReplayError("geometry-only observation invented a semantic class")
|
||||
if observation.metric_geometry is not None and not observation.source_point_ids:
|
||||
raise GeometryReplayError("metric range lost its source points")
|
||||
frame_count += 1
|
||||
source_available += frame.get("source_available") is True
|
||||
proposal_count += _integer(frame.get("proposal_count"), "frame proposals")
|
||||
eligible_count += _integer(frame.get("eligible_proposal_count"), "eligible proposals")
|
||||
ranged_count += _integer(frame.get("ranged_proposal_count"), "ranged proposals")
|
||||
conflict_count += _integer(frame.get("conflict_proposal_count"), "conflicts")
|
||||
camera_count += _integer(frame.get("camera_only_proposal_count"), "camera-only")
|
||||
geometry_count += _integer(
|
||||
frame.get("geometry_only_observation_count"),
|
||||
"geometry-only",
|
||||
)
|
||||
overlaps_removed += _integer(frame.get("overlapping_claims_removed"), "overlaps")
|
||||
point_rows += sum(len(item.source_point_ids) for item in observations)
|
||||
frames = _object(metrics.get("frames"), "frame metrics")
|
||||
proposals = _object(metrics.get("proposals"), "proposal metrics")
|
||||
if (
|
||||
frame_count != frames.get("total")
|
||||
or source_available != frames.get("source_available")
|
||||
or frame_count - source_available != frames.get("source_unavailable")
|
||||
or proposal_count != proposals.get("total")
|
||||
or eligible_count != proposals.get("eligible_for_range")
|
||||
or ranged_count != proposals.get("with_range")
|
||||
or conflict_count != proposals.get("conflict")
|
||||
or camera_count != proposals.get("camera_only")
|
||||
or geometry_count != metrics.get("geometry_only_observations")
|
||||
or point_rows != metrics.get("published_source_point_rows")
|
||||
or overlaps_removed != metrics.get("overlapping_claims_removed")
|
||||
):
|
||||
raise GeometryReplayError("geometry frame ledger and metrics disagree")
|
||||
|
||||
|
||||
def _validated_artifact(root: Path, value: object, name: str) -> Path:
|
||||
document = _object(value, "geometry artifact")
|
||||
_exact_keys(document, {"role", "path", "bytes", "sha256"}, "geometry artifact")
|
||||
if document.get("path") != name:
|
||||
raise GeometryReplayError("geometry artifact path changed")
|
||||
path = root / name
|
||||
if not path.is_file() or path.is_symlink():
|
||||
raise GeometryReplayError("geometry artifact is missing")
|
||||
if document.get("bytes") != path.stat().st_size or document.get("sha256") != _file_sha256(path):
|
||||
raise GeometryReplayError("geometry artifact digest changed")
|
||||
return path
|
||||
|
||||
|
||||
def _artifact(path: Path, role: str) -> dict[str, object]:
|
||||
return {
|
||||
"role": role,
|
||||
"path": path.name,
|
||||
"bytes": path.stat().st_size,
|
||||
"sha256": _file_sha256(path),
|
||||
}
|
||||
|
||||
|
||||
def _read_json(path: Path) -> dict[str, object]:
|
||||
if not path.is_file() or path.is_symlink():
|
||||
raise GeometryReplayError("geometry JSON artifact is missing")
|
||||
try:
|
||||
return _object(json.loads(path.read_text("utf-8")), "geometry JSON artifact")
|
||||
except json.JSONDecodeError as exc:
|
||||
raise GeometryReplayError("geometry JSON artifact is invalid") from exc
|
||||
|
||||
|
||||
def _write_json(path: Path, value: object) -> None:
|
||||
path.write_bytes(_canonical_json(value) + b"\n")
|
||||
|
||||
|
||||
def _canonical_json(value: object) -> bytes:
|
||||
return json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()
|
||||
|
||||
|
||||
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 GeometryReplayError(f"{label} must be an object")
|
||||
return value
|
||||
|
||||
|
||||
def _exact_keys(document: dict[str, object], keys: set[str], label: str) -> None:
|
||||
if set(document) != keys:
|
||||
raise GeometryReplayError(f"{label} fields changed")
|
||||
|
||||
|
||||
def _integer(value: object, label: str) -> int:
|
||||
if not isinstance(value, int) or isinstance(value, bool) or value < 0:
|
||||
raise GeometryReplayError(f"{label} must be a nonnegative integer")
|
||||
return value
|
||||
|
||||
|
||||
def _false_authority() -> dict[str, bool]:
|
||||
return {
|
||||
"ground_truth": False,
|
||||
"physical_live": False,
|
||||
"commands_enabled": False,
|
||||
"actuation_allowed": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
}
|
||||
|
||||
|
||||
__all__ = [
|
||||
"GEOMETRY_REPLAY_FRAME_SCHEMA",
|
||||
"GEOMETRY_REPLAY_MANIFEST_NAME",
|
||||
"GEOMETRY_REPLAY_REPORT_NAME",
|
||||
"GEOMETRY_REPLAY_RESULT_PREFIX",
|
||||
"GEOMETRY_REPLAY_SCHEMA",
|
||||
"GeometryReplayError",
|
||||
"GeometryReplayResult",
|
||||
"build_geometry_replay",
|
||||
"read_geometry_replay_result",
|
||||
]
|
||||
@@ -0,0 +1,39 @@
|
||||
"""Command-line entrypoint for the local M4.4 full-source geometry replay."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
from .geometry_replay import build_geometry_replay
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--repository-root", type=Path, default=Path(__file__).resolve().parents[3])
|
||||
parser.add_argument("--detector-result", type=Path, required=True)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
arguments = parser.parse_args(argv)
|
||||
result = build_geometry_replay(
|
||||
repository_root=arguments.repository_root,
|
||||
detector_result_root=arguments.detector_result,
|
||||
output_root=arguments.output_root,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"result_id": result.result_id,
|
||||
"result_root": str(result.result_root),
|
||||
"accepted": result.accepted,
|
||||
"metrics": result.metrics,
|
||||
},
|
||||
sort_keys=True,
|
||||
separators=(",", ":"),
|
||||
)
|
||||
)
|
||||
return 0 if result.accepted else 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,278 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||
Kb4ProjectionProfile as HistoricalProjectionProfile,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||
project_map_points_kb4 as historical_project,
|
||||
)
|
||||
from k1link.perception.contracts import (
|
||||
BoundingRegion2D,
|
||||
ClockBasis,
|
||||
EvidenceBasis,
|
||||
EvidenceCurrentness,
|
||||
ModalityOutcome,
|
||||
ModalityStatus,
|
||||
ObjectProposal2D,
|
||||
SourceEnvelope,
|
||||
TimestampBundle,
|
||||
validate_exclusive_point_ownership,
|
||||
)
|
||||
from k1link.perception.geometry import (
|
||||
GeometryFrame,
|
||||
GeometryProviderError,
|
||||
Ravnoves00GeometryAssociationProvider,
|
||||
RecordedGeometryStore,
|
||||
load_geometry_profile,
|
||||
)
|
||||
from k1link.perception.geometry_math import (
|
||||
Kb4ProjectionProfile,
|
||||
project_map_points_kb4,
|
||||
)
|
||||
from k1link.perception.providers import SourcePacket
|
||||
from k1link.perception.recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference
|
||||
|
||||
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
||||
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-geometry-association-v1.json"
|
||||
|
||||
|
||||
class _Store:
|
||||
def __init__(self, frame: GeometryFrame | None) -> None:
|
||||
self.profile = load_geometry_profile(PROFILE_PATH)
|
||||
self._frame = frame
|
||||
|
||||
def frame(self, packet: SourcePacket) -> GeometryFrame | None:
|
||||
return self._frame
|
||||
|
||||
|
||||
def _status(
|
||||
available: bool = True,
|
||||
outcome: ModalityOutcome = ModalityOutcome.AVAILABLE,
|
||||
) -> ModalityStatus:
|
||||
return ModalityStatus(available, outcome, f"test-{outcome.value}")
|
||||
|
||||
|
||||
def _packet(*, available: bool = True) -> SourcePacket:
|
||||
status = _status() if available else _status(False, ModalityOutcome.UNAVAILABLE)
|
||||
reference = RecordedFrameReference(RECORDED_SOURCE_PACK_ID, 0) if available else None
|
||||
return SourcePacket(
|
||||
envelope=SourceEnvelope(
|
||||
source_id="RAVNOVES00",
|
||||
session_id="20260720T065719Z_viewer_live",
|
||||
frame_id="frame-000000",
|
||||
sequence=0,
|
||||
timestamps=TimestampBundle(
|
||||
utc_ns=1,
|
||||
monotonic_ns=2,
|
||||
source_ns=3,
|
||||
clock_basis=ClockBasis.RECORDED_HOST,
|
||||
),
|
||||
source_age_ns=0,
|
||||
binding_reason="test-recorded-source",
|
||||
calibration_id="camera-1-kb4-test",
|
||||
representation_id="registered-map-increment-v1",
|
||||
image=_status(),
|
||||
registered_point_increment=status,
|
||||
pose=status,
|
||||
),
|
||||
image_payload="image",
|
||||
registered_point_increment_payload=reference,
|
||||
pose_payload=reference,
|
||||
)
|
||||
|
||||
|
||||
def _proposal(
|
||||
proposal_id: str,
|
||||
region: tuple[float, float, float, float],
|
||||
*,
|
||||
score: float = 0.8,
|
||||
hint: str | None = "person",
|
||||
) -> ObjectProposal2D:
|
||||
return ObjectProposal2D(
|
||||
proposal_id=proposal_id,
|
||||
source_id="RAVNOVES00",
|
||||
frame_id="frame-000000",
|
||||
region=BoundingRegion2D(*region),
|
||||
objectness=score,
|
||||
provider_id="test-detector/v1",
|
||||
model_id="test-model/v1",
|
||||
preprocess_id="test-preprocess/v1",
|
||||
semantic_hint=hint,
|
||||
)
|
||||
|
||||
|
||||
def _point_for_pixel(u: float, *, z: float = 5.0) -> tuple[float, float, float]:
|
||||
theta = (u - 50.0) / 100.0
|
||||
return (float(np.tan(theta) * z), 0.0, z)
|
||||
|
||||
|
||||
def _frame() -> GeometryFrame:
|
||||
semantic = (
|
||||
_point_for_pixel(39.5),
|
||||
_point_for_pixel(40.5),
|
||||
)
|
||||
geometry_only = (
|
||||
_point_for_pixel(76.0, z=4.0),
|
||||
_point_for_pixel(80.0, z=4.0),
|
||||
_point_for_pixel(84.0, z=4.0),
|
||||
_point_for_pixel(88.0, z=4.0),
|
||||
)
|
||||
points = np.asarray((*semantic, *geometry_only), dtype=np.float64)
|
||||
return GeometryFrame(
|
||||
frame_index=0,
|
||||
points_map=points,
|
||||
point_class=np.full(points.shape[0], 2, dtype=np.uint8),
|
||||
sensor_position_map=np.zeros(3, dtype=np.float64),
|
||||
sensor_orientation_xyzw=np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float64),
|
||||
projection=Kb4ProjectionProfile(
|
||||
width=100,
|
||||
height=100,
|
||||
intrinsic_fx_fy_cx_cy=(100.0, 100.0, 50.0, 50.0),
|
||||
distortion_kb4=(0.0, 0.0, 0.0, 0.0),
|
||||
t_camera_from_lidar=np.eye(4, dtype=np.float64),
|
||||
),
|
||||
surface_valid=True,
|
||||
)
|
||||
|
||||
|
||||
def test_profile_is_strict_digest_bound_and_store_accepts_exact_evidence() -> None:
|
||||
profile = load_geometry_profile(PROFILE_PATH)
|
||||
store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT, profile=profile)
|
||||
|
||||
assert profile.provider_id == "ravnoves00-geometry-association/v1"
|
||||
assert len(profile.profile_sha256) == 64
|
||||
assert store.profile.source_pack_sha256 == (
|
||||
"0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944"
|
||||
)
|
||||
assert store.profile.local_surface_sha256 == (
|
||||
"f57eb2485b6cef47f2a97a2d9ff1aa9fd9265fe1eb69cd5852d12f39e13b8bc6"
|
||||
)
|
||||
|
||||
|
||||
def test_provider_arbitrates_points_and_publishes_classless_geometry_only() -> None:
|
||||
provider = Ravnoves00GeometryAssociationProvider(store=_Store(_frame())) # type: ignore[arg-type]
|
||||
proposals = (
|
||||
_proposal("proposal-small", (35.0, 45.0, 45.0, 55.0)),
|
||||
_proposal("proposal-large", (30.0, 40.0, 50.0, 60.0), score=0.99),
|
||||
)
|
||||
|
||||
observations = provider.associate(_packet(), proposals)
|
||||
|
||||
validate_exclusive_point_ownership(observations)
|
||||
small = next(item for item in observations if item.proposal_ids == ("proposal-small",))
|
||||
large = next(item for item in observations if item.proposal_ids == ("proposal-large",))
|
||||
geometry = [item for item in observations if not item.proposal_ids]
|
||||
assert small.basis is EvidenceBasis.FUSED
|
||||
assert small.metric_geometry is not None
|
||||
assert small.source_point_ids == (0, 1)
|
||||
assert large.basis is EvidenceBasis.CAMERA
|
||||
assert large.metric_geometry is None
|
||||
assert "point-ownership-collision-range-withheld" in large.reason_codes
|
||||
assert geometry
|
||||
assert all(item.basis is EvidenceBasis.LIDAR for item in geometry)
|
||||
assert all(item.semantic_hint is None for item in geometry)
|
||||
assert all(item.metric_geometry is not None for item in geometry)
|
||||
|
||||
snapshot = provider.snapshot()
|
||||
assert snapshot.proposal_count == 2
|
||||
assert snapshot.eligible_proposal_count == 2
|
||||
assert snapshot.ranged_proposal_count == 1
|
||||
assert snapshot.total_range_coverage == pytest.approx(0.5)
|
||||
assert snapshot.eligible_range_coverage == pytest.approx(0.5)
|
||||
assert snapshot.overlapping_claims_removed == 2
|
||||
|
||||
|
||||
def test_unavailable_source_never_publishes_metric_or_free_space() -> None:
|
||||
provider = Ravnoves00GeometryAssociationProvider(store=_Store(None)) # type: ignore[arg-type]
|
||||
|
||||
observations = provider.associate(
|
||||
_packet(available=False),
|
||||
(_proposal("proposal-0", (10.0, 10.0, 20.0, 20.0)),),
|
||||
)
|
||||
|
||||
assert len(observations) == 1
|
||||
assert observations[0].basis is EvidenceBasis.CAMERA
|
||||
assert observations[0].currentness is EvidenceCurrentness.UNAVAILABLE
|
||||
assert observations[0].metric_geometry is None
|
||||
assert observations[0].source_point_ids == ()
|
||||
assert observations[0].occupied_support is False
|
||||
snapshot = provider.snapshot()
|
||||
assert snapshot.unavailable_proposal_count == 1
|
||||
assert snapshot.eligible_proposal_count == 0
|
||||
|
||||
|
||||
def test_semantic_hint_does_not_change_geometry_or_range() -> None:
|
||||
first = Ravnoves00GeometryAssociationProvider(store=_Store(_frame())) # type: ignore[arg-type]
|
||||
second = Ravnoves00GeometryAssociationProvider(store=_Store(_frame())) # type: ignore[arg-type]
|
||||
region = (35.0, 45.0, 45.0, 55.0)
|
||||
|
||||
left = first.associate(_packet(), (_proposal("proposal-0", region, hint="person"),))[0]
|
||||
right = second.associate(_packet(), (_proposal("proposal-0", region, hint="truck"),))[0]
|
||||
|
||||
assert left.source_point_ids == right.source_point_ids
|
||||
assert left.metric_geometry == right.metric_geometry
|
||||
assert left.semantic_hint == "person"
|
||||
assert right.semantic_hint == "truck"
|
||||
|
||||
|
||||
def test_product_projection_is_numerically_identical_to_accepted_e29_primitive() -> None:
|
||||
store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT)
|
||||
source = store._source # noqa: SLF001 - parity test over the sealed artifact
|
||||
offsets = source["cloud_offsets"]
|
||||
frame_index = 5
|
||||
points = np.asarray(
|
||||
source["cloud_points_map"][int(offsets[frame_index]) : int(offsets[frame_index + 1])],
|
||||
dtype=np.float64,
|
||||
)
|
||||
position = np.asarray(source["pose_positions_map"][frame_index], dtype=np.float64)
|
||||
orientation = np.asarray(
|
||||
source["pose_quaternions_map_from_lidar"][frame_index],
|
||||
dtype=np.float64,
|
||||
)
|
||||
product_profile = store._projection # noqa: SLF001 - exact projection identity
|
||||
historical_profile = HistoricalProjectionProfile(
|
||||
source_id="sensor.camera.right",
|
||||
calibration_slot="camera_1",
|
||||
width=product_profile.width,
|
||||
height=product_profile.height,
|
||||
intrinsic_fx_fy_cx_cy=product_profile.intrinsic_fx_fy_cx_cy,
|
||||
distortion_kb4=product_profile.distortion_kb4,
|
||||
t_camera_from_lidar=product_profile.t_camera_from_lidar,
|
||||
)
|
||||
|
||||
product = project_map_points_kb4(
|
||||
points,
|
||||
position_map_xyz=position,
|
||||
orientation_map_from_lidar_xyzw=orientation,
|
||||
profile=product_profile,
|
||||
)
|
||||
historical = historical_project(
|
||||
points,
|
||||
position_map_xyz=tuple(float(value) for value in position),
|
||||
orientation_map_from_lidar_xyzw=tuple(float(value) for value in orientation),
|
||||
profile=historical_profile,
|
||||
)
|
||||
|
||||
np.testing.assert_array_equal(product.source_indices, historical.source_indices)
|
||||
np.testing.assert_allclose(product.pixels_xy, historical.pixels_xy, rtol=0.0, atol=1e-12)
|
||||
np.testing.assert_allclose(product.depths_m, historical.depths_m, rtol=0.0, atol=1e-12)
|
||||
|
||||
|
||||
def test_store_rejects_a_wrong_source_digest(tmp_path: Path) -> None:
|
||||
profile = load_geometry_profile(PROFILE_PATH)
|
||||
source = tmp_path / "lidar-pack.npz"
|
||||
source.write_bytes(b"not-the-source")
|
||||
surface = tmp_path / "local-surface.npz"
|
||||
surface.write_bytes(b"not-the-surface")
|
||||
|
||||
with pytest.raises(GeometryProviderError, match="source pack digest changed"):
|
||||
RecordedGeometryStore(
|
||||
source_pack_path=source,
|
||||
local_surface_path=surface,
|
||||
profile=profile,
|
||||
)
|
||||
@@ -0,0 +1,66 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from k1link.perception.geometry_replay import read_geometry_replay_result
|
||||
|
||||
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
||||
RESULT_ROOT = (
|
||||
REPOSITORY_ROOT
|
||||
/ ".runtime/perception-m4/geometry-results"
|
||||
/ "m4-geometry-replay-8daf3109e3cf30b960b4b376032ff3b5ec58ca42a1e5899b841cf29fbcf14ad8"
|
||||
)
|
||||
|
||||
|
||||
def test_full_source_geometry_result_closes_m4_4_contract() -> None:
|
||||
result = read_geometry_replay_result(RESULT_ROOT)
|
||||
|
||||
assert result.accepted is True
|
||||
assert result.metrics["frames"] == {
|
||||
"failed": 0,
|
||||
"source_available": 3928,
|
||||
"source_unavailable": 561,
|
||||
"total": 4489,
|
||||
}
|
||||
proposals = result.metrics["proposals"]
|
||||
assert isinstance(proposals, dict)
|
||||
assert proposals["total"] == 15499
|
||||
assert proposals["with_range"] == 5341
|
||||
assert proposals["eligible_for_range"] == 13298
|
||||
assert proposals["ownership_collision"] == 146
|
||||
assert result.metrics["geometry_only_observations"] == 21958
|
||||
assert result.metrics["published_source_point_rows"] == 2164767
|
||||
|
||||
|
||||
def test_geometry_result_binds_the_accepted_m4_3_e32_and_e53_evidence() -> None:
|
||||
result = read_geometry_replay_result(RESULT_ROOT)
|
||||
identity = result.manifest["identity"]
|
||||
assert isinstance(identity, dict)
|
||||
|
||||
assert identity["detector_result_id"] == (
|
||||
"m4-detector-replay-"
|
||||
"11f83f2e0b81758ac2a5a5fc54e9d293b501678df5f6ef97b5c6069ba08605c5"
|
||||
)
|
||||
assert identity["geometry_profile_sha256"] == (
|
||||
"420d989aab5918e0f98e3439cadb8b7251332d51b48f4f2c25d77a385bea49f8"
|
||||
)
|
||||
assert identity["historical_references"] == {
|
||||
"e32": {
|
||||
"manifest_sha256": (
|
||||
"f4b57c9f7619c43414adf1d10488b05df466226a523a0c001671102ced0e9ab8"
|
||||
),
|
||||
"result_id": (
|
||||
"e32-track-geometry-"
|
||||
"a14ca0e7fb3850ca0dfa3c41634e1b490a2d58ab74d101afc6d6921fbdb0e6fd"
|
||||
),
|
||||
},
|
||||
"e53": {
|
||||
"manifest_sha256": (
|
||||
"fc5b4ae69aae0098b7075c539d25b563dff1bbb09209a49030edda6cba9ee544"
|
||||
),
|
||||
"result_id": (
|
||||
"e53-camera-first-shadow-"
|
||||
"e6f03cf8bfb15db86100239b060e13f914532618b7b99e811866e4e6a555186c"
|
||||
),
|
||||
},
|
||||
}
|
||||
@@ -33,6 +33,15 @@ DETECTOR_RUNTIME_MODULES = (
|
||||
"recorded_source.py",
|
||||
"yolox_object_detector.py",
|
||||
)
|
||||
GEOMETRY_RUNTIME_MODULES = (
|
||||
"contracts.py",
|
||||
"geometry.py",
|
||||
"geometry_math.py",
|
||||
"geometry_replay.py",
|
||||
"geometry_replay_cli.py",
|
||||
"providers.py",
|
||||
"recorded_source.py",
|
||||
)
|
||||
|
||||
|
||||
def _imports(path: Path) -> set[str]:
|
||||
@@ -181,6 +190,18 @@ def test_detector_runtime_closure_imports_no_legacy_compute_package() -> None:
|
||||
assert {name: modules for name, modules in violations.items() if modules} == {}
|
||||
|
||||
|
||||
def test_geometry_runtime_closure_imports_no_legacy_compute_or_device_package() -> None:
|
||||
violations = {
|
||||
name: sorted(
|
||||
module
|
||||
for module in _imports(PERCEPTION_ROOT / name)
|
||||
if module.startswith(("k1link.compute", "k1link.device_plugins"))
|
||||
)
|
||||
for name in GEOMETRY_RUNTIME_MODULES
|
||||
}
|
||||
assert {name: modules for name, modules in violations.items() if modules} == {}
|
||||
|
||||
|
||||
def test_new_perception_boundary_has_no_experiment_specific_imports() -> None:
|
||||
violations: dict[str, str] = {}
|
||||
for path in PERCEPTION_ROOT.glob("*.py"):
|
||||
|
||||
Reference in New Issue
Block a user