feat(perception): complete E32 full replay
This commit is contained in:
DCCONSTRUCTIONS
@@ -17,6 +17,16 @@ from .e31_source_qualification import (
|
||||
E31SourceQualificationProfile,
|
||||
build_e31_source_qualification,
|
||||
)
|
||||
from .e32_track_geometry_replay import (
|
||||
E32_TRACK_GEOMETRY_RECORD_SCHEMA,
|
||||
E32_TRACK_GEOMETRY_REPLAY_SCHEMA,
|
||||
E32_TRACK_GEOMETRY_REPORT_SCHEMA,
|
||||
E32TrackGeometryReplay,
|
||||
E32TrackGeometryReplayError,
|
||||
build_e32_track_geometry_replay,
|
||||
e32_track_geometry_frame,
|
||||
read_e32_track_geometry_replay,
|
||||
)
|
||||
from .evaluation_pack import (
|
||||
ANNOTATION_CONTRACT_SCHEMA,
|
||||
EVALUATION_PACK_SCHEMA,
|
||||
@@ -354,6 +364,14 @@ __all__ = [
|
||||
"assess_lidar_profile",
|
||||
"build_lidar_replay_pack_v2",
|
||||
"build_e31_source_qualification",
|
||||
"build_e32_track_geometry_replay",
|
||||
"read_e32_track_geometry_replay",
|
||||
"e32_track_geometry_frame",
|
||||
"E32_TRACK_GEOMETRY_REPLAY_SCHEMA",
|
||||
"E32_TRACK_GEOMETRY_REPORT_SCHEMA",
|
||||
"E32_TRACK_GEOMETRY_RECORD_SCHEMA",
|
||||
"E32TrackGeometryReplay",
|
||||
"E32TrackGeometryReplayError",
|
||||
"build_lidar_ground_annotation_template",
|
||||
"build_lidar_ground_benchmark",
|
||||
"build_k1_local_surface",
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,347 @@
|
||||
"""Compact, strict storage adapter for E32 TrackGeometry frames."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from collections.abc import Mapping
|
||||
from pathlib import Path
|
||||
from typing import Any, Final, cast
|
||||
|
||||
import numpy as np
|
||||
import numpy.typing as npt
|
||||
|
||||
from .track_geometry import (
|
||||
POINT_SLAB_SCHEMA,
|
||||
TRACK_GEOMETRY_FRAME_SCHEMA,
|
||||
PointSlab,
|
||||
TrackGeometry,
|
||||
TrackGeometryFrame,
|
||||
TrackGeometrySourceBinding,
|
||||
)
|
||||
|
||||
E32_TRACK_GEOMETRY_RECORD_SCHEMA: Final = "missioncore.e32-track-geometry-record/v1"
|
||||
E32_POINT_SLAB_REFERENCE_SCHEMA: Final = "missioncore.e32-point-slab-reference/v1"
|
||||
|
||||
E32_FRAMES_NAME: Final = "track-geometry-frames.jsonl"
|
||||
E32_FRAME_OFFSETS_NAME: Final = "frame-point-offsets.npy"
|
||||
E32_SOURCE_INDICES_NAME: Final = "point-source-indices.npy"
|
||||
E32_POINTS_NAME: Final = "point-coordinates-map-f32.npy"
|
||||
E32_OWNER_INDICES_NAME: Final = "point-owner-indices.npy"
|
||||
|
||||
Int64Array = npt.NDArray[np.int64]
|
||||
UInt32Array = npt.NDArray[np.uint32]
|
||||
|
||||
|
||||
class E32TrackGeometryStorageError(ValueError):
|
||||
"""Compact E32 storage no longer satisfies the TrackGeometry contract."""
|
||||
|
||||
|
||||
def write_point_storage(
|
||||
*,
|
||||
staging: Path,
|
||||
frame_offsets: Int64Array,
|
||||
source_indices: Int64Array,
|
||||
points: npt.NDArray[np.float32],
|
||||
owner_indices: UInt32Array,
|
||||
) -> None:
|
||||
"""Write deterministic non-pickle arrays for the compact frame stream."""
|
||||
|
||||
_save_npy(staging / E32_FRAME_OFFSETS_NAME, frame_offsets)
|
||||
_save_npy(staging / E32_SOURCE_INDICES_NAME, source_indices)
|
||||
_save_npy(staging / E32_POINTS_NAME, points)
|
||||
_save_npy(staging / E32_OWNER_INDICES_NAME, owner_indices)
|
||||
|
||||
|
||||
def validate_storage(
|
||||
*,
|
||||
artifacts: Mapping[str, Path],
|
||||
identity: Mapping[str, object],
|
||||
) -> None:
|
||||
"""Reconstruct and validate every persisted TrackGeometry frame."""
|
||||
|
||||
frame_count = _positive_int(identity.get("frame_count"), "E32 frame count")
|
||||
frame_offsets, source_indices, points, owner_indices = load_point_storage(
|
||||
artifacts,
|
||||
frame_count=frame_count,
|
||||
)
|
||||
binding = TrackGeometrySourceBinding.from_dict(
|
||||
identity.get("track_geometry_binding")
|
||||
)
|
||||
observed_frames = 0
|
||||
with artifacts["track-geometry-frames"].open("r", encoding="utf-8") as stream:
|
||||
for expected_frame_index, line in enumerate(stream):
|
||||
if expected_frame_index >= frame_count:
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 frame stream has extra rows"
|
||||
)
|
||||
value = record(line, expected_frame_index=expected_frame_index)
|
||||
frame_from_record(
|
||||
record_value=value,
|
||||
binding=binding,
|
||||
frame_offsets=frame_offsets,
|
||||
source_indices=source_indices,
|
||||
points=points,
|
||||
owner_indices=owner_indices,
|
||||
)
|
||||
observed_frames += 1
|
||||
if observed_frames != frame_count:
|
||||
raise E32TrackGeometryStorageError("E32 frame stream is incomplete")
|
||||
|
||||
|
||||
def load_point_storage(
|
||||
artifacts: Mapping[str, Path],
|
||||
*,
|
||||
frame_count: int,
|
||||
) -> tuple[
|
||||
Int64Array,
|
||||
Int64Array,
|
||||
npt.NDArray[np.float32],
|
||||
UInt32Array,
|
||||
]:
|
||||
"""Open the four digest-verified E32 arrays as read-only memory maps."""
|
||||
|
||||
try:
|
||||
frame_offsets = np.load(
|
||||
artifacts["frame-point-offsets"],
|
||||
allow_pickle=False,
|
||||
mmap_mode="r",
|
||||
)
|
||||
source_indices = np.load(
|
||||
artifacts["point-source-indices"],
|
||||
allow_pickle=False,
|
||||
mmap_mode="r",
|
||||
)
|
||||
points = np.load(
|
||||
artifacts["point-coordinates-map-f32"],
|
||||
allow_pickle=False,
|
||||
mmap_mode="r",
|
||||
)
|
||||
owner_indices = np.load(
|
||||
artifacts["point-owner-indices"],
|
||||
allow_pickle=False,
|
||||
mmap_mode="r",
|
||||
)
|
||||
except (KeyError, OSError, ValueError) as exc:
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 point storage is unreadable"
|
||||
) from exc
|
||||
if (
|
||||
frame_offsets.dtype != np.dtype("<i8")
|
||||
or frame_offsets.shape != (frame_count + 1,)
|
||||
or source_indices.dtype != np.dtype("<i8")
|
||||
or source_indices.ndim != 1
|
||||
or points.dtype != np.dtype("<f4")
|
||||
or points.shape != (source_indices.size, 3)
|
||||
or owner_indices.dtype != np.dtype("<u4")
|
||||
or owner_indices.shape != (source_indices.size,)
|
||||
or frame_offsets[0] != 0
|
||||
or frame_offsets[-1] != source_indices.size
|
||||
or np.any(np.diff(frame_offsets) < 0)
|
||||
or not np.isfinite(points).all()
|
||||
):
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 point storage contract changed"
|
||||
)
|
||||
return (
|
||||
cast(Int64Array, frame_offsets),
|
||||
cast(Int64Array, source_indices),
|
||||
cast(npt.NDArray[np.float32], points),
|
||||
cast(UInt32Array, owner_indices),
|
||||
)
|
||||
|
||||
|
||||
def frame_from_record(
|
||||
*,
|
||||
record_value: Mapping[str, object],
|
||||
binding: TrackGeometrySourceBinding,
|
||||
frame_offsets: Int64Array,
|
||||
source_indices: Int64Array,
|
||||
points: npt.NDArray[np.float32],
|
||||
owner_indices: UInt32Array,
|
||||
) -> TrackGeometryFrame:
|
||||
"""Reconstruct one TrackGeometryFrame from its JSON row and slab slices."""
|
||||
|
||||
expected_keys = {
|
||||
"schema_version",
|
||||
"track_geometry_frame_schema",
|
||||
"frame_index",
|
||||
"source_frame_index",
|
||||
"session_seconds",
|
||||
"source_available",
|
||||
"point_slab",
|
||||
"geometries",
|
||||
"policy",
|
||||
"authority",
|
||||
}
|
||||
if (
|
||||
set(record_value) != expected_keys
|
||||
or record_value.get("schema_version") != E32_TRACK_GEOMETRY_RECORD_SCHEMA
|
||||
or record_value.get("track_geometry_frame_schema")
|
||||
!= TRACK_GEOMETRY_FRAME_SCHEMA
|
||||
or record_value.get("authority") != _authority()
|
||||
or record_value.get("policy")
|
||||
!= {
|
||||
"camera_owns_semantics": True,
|
||||
"one_owner_per_source_point": True,
|
||||
"current_held_persistent_are_separate": True,
|
||||
"absence_of_points_means_free": False,
|
||||
"unknown_remains_unknown": True,
|
||||
}
|
||||
):
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 frame record contract changed"
|
||||
)
|
||||
frame_index = _nonnegative_int(record_value.get("frame_index"), "frame index")
|
||||
if frame_index + 1 >= frame_offsets.size:
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 frame point offset is missing"
|
||||
)
|
||||
row_start = int(frame_offsets[frame_index])
|
||||
row_end = int(frame_offsets[frame_index + 1])
|
||||
slab_reference = _object(
|
||||
record_value.get("point_slab"),
|
||||
"E32 PointSlab reference",
|
||||
)
|
||||
if (
|
||||
set(slab_reference)
|
||||
!= {
|
||||
"schema_version",
|
||||
"contract_schema",
|
||||
"source_point_count",
|
||||
"coordinate_frame",
|
||||
"owner_keys",
|
||||
"row_count",
|
||||
}
|
||||
or slab_reference.get("schema_version")
|
||||
!= E32_POINT_SLAB_REFERENCE_SCHEMA
|
||||
or slab_reference.get("contract_schema") != POINT_SLAB_SCHEMA
|
||||
or slab_reference.get("row_count") != row_end - row_start
|
||||
):
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 PointSlab reference changed"
|
||||
)
|
||||
owner_key_values = slab_reference.get("owner_keys")
|
||||
geometry_values = record_value.get("geometries")
|
||||
if not isinstance(owner_key_values, list) or not isinstance(
|
||||
geometry_values,
|
||||
list,
|
||||
):
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 frame owner or geometry table changed"
|
||||
)
|
||||
slab = PointSlab(
|
||||
frame_index=frame_index,
|
||||
source_frame_index=_nonnegative_int(
|
||||
record_value.get("source_frame_index"),
|
||||
"source frame index",
|
||||
),
|
||||
source_point_count=_nonnegative_int(
|
||||
slab_reference.get("source_point_count"),
|
||||
"source point count",
|
||||
),
|
||||
coordinate_frame=_string(
|
||||
slab_reference.get("coordinate_frame"),
|
||||
"point coordinate frame",
|
||||
),
|
||||
owner_keys=tuple(
|
||||
_string(value, "point owner key") for value in owner_key_values
|
||||
),
|
||||
source_indices=np.asarray(source_indices[row_start:row_end], dtype="<i8"),
|
||||
points_xyz_m=np.asarray(points[row_start:row_end], dtype="<f4"),
|
||||
owner_indices=np.asarray(owner_indices[row_start:row_end], dtype="<u4"),
|
||||
)
|
||||
return TrackGeometryFrame(
|
||||
binding=binding,
|
||||
frame_index=frame_index,
|
||||
source_frame_index=slab.source_frame_index,
|
||||
session_seconds=_nonnegative_float(
|
||||
record_value.get("session_seconds"),
|
||||
"session time",
|
||||
),
|
||||
source_available=_boolean(
|
||||
record_value.get("source_available"),
|
||||
"source availability",
|
||||
),
|
||||
point_slab=slab,
|
||||
geometries=tuple(
|
||||
TrackGeometry.from_dict(value) for value in geometry_values
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def record(line: str, *, expected_frame_index: int) -> dict[str, Any]:
|
||||
"""Parse one ordered compact frame record."""
|
||||
|
||||
try:
|
||||
value = json.loads(line)
|
||||
except json.JSONDecodeError as exc:
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 frame record JSON is invalid"
|
||||
) from exc
|
||||
result = _object(value, "E32 frame record")
|
||||
if result.get("frame_index") != expected_frame_index:
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 frame record order changed"
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def _save_npy(path: Path, value: npt.NDArray[Any]) -> None:
|
||||
if path.exists():
|
||||
raise E32TrackGeometryStorageError(
|
||||
"E32 point artifact already exists"
|
||||
)
|
||||
with path.open("xb") as stream:
|
||||
np.save(stream, value, allow_pickle=False)
|
||||
|
||||
|
||||
def _nonnegative_float(value: object, label: str) -> float:
|
||||
if (
|
||||
not isinstance(value, (int, float))
|
||||
or isinstance(value, bool)
|
||||
or not math.isfinite(float(value))
|
||||
or float(value) < 0.0
|
||||
):
|
||||
raise E32TrackGeometryStorageError(f"{label} is invalid")
|
||||
return float(value)
|
||||
|
||||
|
||||
def _nonnegative_int(value: object, label: str) -> int:
|
||||
if not isinstance(value, int) or isinstance(value, bool) or value < 0:
|
||||
raise E32TrackGeometryStorageError(f"{label} is invalid")
|
||||
return value
|
||||
|
||||
|
||||
def _positive_int(value: object, label: str) -> int:
|
||||
result = _nonnegative_int(value, label)
|
||||
if result == 0:
|
||||
raise E32TrackGeometryStorageError(f"{label} is invalid")
|
||||
return result
|
||||
|
||||
|
||||
def _boolean(value: object, label: str) -> bool:
|
||||
if not isinstance(value, bool):
|
||||
raise E32TrackGeometryStorageError(f"{label} is invalid")
|
||||
return value
|
||||
|
||||
|
||||
def _string(value: object, label: str) -> str:
|
||||
if not isinstance(value, str) or not value or len(value) > 256:
|
||||
raise E32TrackGeometryStorageError(f"{label} is invalid")
|
||||
return value
|
||||
|
||||
|
||||
def _object(value: object, label: str) -> dict[str, Any]:
|
||||
if not isinstance(value, dict) or any(
|
||||
not isinstance(key, str) for key in value
|
||||
):
|
||||
raise E32TrackGeometryStorageError(f"{label} must be an object")
|
||||
return value
|
||||
|
||||
|
||||
def _authority() -> dict[str, bool]:
|
||||
return {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
}
|
||||
Reference in New Issue
Block a user