638 lines
25 KiB
Python
638 lines
25 KiB
Python
from __future__ import annotations
|
|
|
|
import re
|
|
import threading
|
|
from collections import deque
|
|
from dataclasses import dataclass
|
|
from enum import StrEnum
|
|
from typing import Any, Final
|
|
|
|
import numpy as np
|
|
import numpy.typing as npt
|
|
|
|
from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
|
|
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
|
CalibratedProjectionError,
|
|
map_points_to_lidar,
|
|
)
|
|
|
|
LIDAR_EVIDENCE_PROFILE_SCHEMA: Final = "missioncore.lidar-evidence-profile/v1"
|
|
LIDAR_READINESS_SCHEMA: Final = "missioncore.lidar-readiness/v1"
|
|
_IDENTIFIER = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._:/-]{0,159}$")
|
|
|
|
Float32Array = npt.NDArray[np.float32]
|
|
|
|
|
|
class LidarContractError(ValueError):
|
|
"""A LiDAR evidence profile or conversion violates the admitted contract."""
|
|
|
|
|
|
class LidarRepresentation(StrEnum):
|
|
SENSOR_SCAN = "sensor-scan"
|
|
VENDOR_MAP_INCREMENT = "vendor-map-increment"
|
|
ACCUMULATED_MAP = "accumulated-map"
|
|
|
|
|
|
class LidarCoordinateSpace(StrEnum):
|
|
SENSOR = "sensor"
|
|
MAP = "map"
|
|
|
|
|
|
class LidarTimeBasis(StrEnum):
|
|
SENSOR = "sensor"
|
|
HOST_ARRIVAL = "host-arrival"
|
|
CAMERA_BOUND_HOST_ARRIVAL = "camera-bound-host-arrival"
|
|
|
|
|
|
class LidarPointField(StrEnum):
|
|
XYZ = "xyz"
|
|
INTENSITY = "intensity"
|
|
RING = "ring"
|
|
RELATIVE_TIME = "relative-time"
|
|
|
|
|
|
class LidarPoseStatus(StrEnum):
|
|
NONE = "none"
|
|
BEST_EFFORT = "best-effort"
|
|
SENSOR_SYNCHRONIZED = "sensor-synchronized"
|
|
|
|
|
|
class LidarPipelineStage(StrEnum):
|
|
SCANNER_QUALITY = "scanner-quality"
|
|
GROUND_SEGMENTATION = "ground-segmentation"
|
|
LIDAR_3D_DETECTION = "lidar-3d-detection"
|
|
NVIDIA_NVBLOX = "nvidia-nvblox"
|
|
LIDAR_ODOMETRY = "lidar-odometry"
|
|
LIDAR_INERTIAL_SLAM = "lidar-inertial-slam"
|
|
CAMERA_LIDAR_FUSION = "camera-lidar-fusion"
|
|
|
|
|
|
class LidarReadiness(StrEnum):
|
|
READY = "ready"
|
|
DEGRADED = "degraded"
|
|
BLOCKED = "blocked"
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class LidarEvidenceProfile:
|
|
"""Describe what one LiDAR source actually provides before model selection."""
|
|
|
|
profile_id: str
|
|
representation: LidarRepresentation
|
|
coordinate_space: LidarCoordinateSpace
|
|
coordinate_frame: str
|
|
frame_time_basis: LidarTimeBasis
|
|
point_fields: tuple[LidarPointField, ...]
|
|
pose_status: LidarPoseStatus
|
|
scan_geometry_known: bool
|
|
imu_samples_available: bool
|
|
lidar_imu_extrinsic_available: bool
|
|
camera_extrinsic_available: bool
|
|
commands_enabled: bool = False
|
|
navigation_or_safety_accepted: bool = False
|
|
|
|
def __post_init__(self) -> None:
|
|
_safe_identifier(self.profile_id, "LiDAR profile id")
|
|
_safe_identifier(self.coordinate_frame, "LiDAR coordinate frame")
|
|
if not self.point_fields or LidarPointField.XYZ not in self.point_fields:
|
|
raise LidarContractError("LiDAR evidence must contain xyz")
|
|
if len(self.point_fields) != len(set(self.point_fields)):
|
|
raise LidarContractError("LiDAR point fields must be unique")
|
|
if (
|
|
self.coordinate_space is LidarCoordinateSpace.MAP
|
|
and self.pose_status is LidarPoseStatus.NONE
|
|
):
|
|
raise LidarContractError("map-frame LiDAR evidence requires a sensor pose")
|
|
if self.lidar_imu_extrinsic_available and not self.imu_samples_available:
|
|
raise LidarContractError("LiDAR/IMU extrinsic has no admitted IMU samples")
|
|
if self.commands_enabled or self.navigation_or_safety_accepted:
|
|
raise LidarContractError("v1 LiDAR evidence is diagnostic-only")
|
|
|
|
def to_dict(self) -> dict[str, object]:
|
|
return {
|
|
"schema_version": LIDAR_EVIDENCE_PROFILE_SCHEMA,
|
|
"profile_id": self.profile_id,
|
|
"representation": self.representation.value,
|
|
"coordinates": {
|
|
"space": self.coordinate_space.value,
|
|
"frame": self.coordinate_frame,
|
|
},
|
|
"time": {
|
|
"frame_basis": self.frame_time_basis.value,
|
|
"point_relative_time": LidarPointField.RELATIVE_TIME in self.point_fields,
|
|
},
|
|
"point_fields": [field.value for field in self.point_fields],
|
|
"pose_status": self.pose_status.value,
|
|
"scan_geometry_known": self.scan_geometry_known,
|
|
"imu": {
|
|
"samples_available": self.imu_samples_available,
|
|
"lidar_extrinsic_available": self.lidar_imu_extrinsic_available,
|
|
},
|
|
"camera_extrinsic_available": self.camera_extrinsic_available,
|
|
"authority": {
|
|
"commands_enabled": self.commands_enabled,
|
|
"navigation_or_safety_accepted": self.navigation_or_safety_accepted,
|
|
},
|
|
}
|
|
|
|
@classmethod
|
|
def from_dict(cls, value: object) -> LidarEvidenceProfile:
|
|
document = _object(value, "LiDAR evidence profile")
|
|
_exact_keys(
|
|
document,
|
|
{
|
|
"schema_version",
|
|
"profile_id",
|
|
"representation",
|
|
"coordinates",
|
|
"time",
|
|
"point_fields",
|
|
"pose_status",
|
|
"scan_geometry_known",
|
|
"imu",
|
|
"camera_extrinsic_available",
|
|
"authority",
|
|
},
|
|
"LiDAR evidence profile",
|
|
)
|
|
if document.get("schema_version") != LIDAR_EVIDENCE_PROFILE_SCHEMA:
|
|
raise LidarContractError("LiDAR evidence profile schema is incompatible")
|
|
coordinates = _object(document.get("coordinates"), "LiDAR coordinates")
|
|
time = _object(document.get("time"), "LiDAR time")
|
|
imu = _object(document.get("imu"), "LiDAR IMU evidence")
|
|
authority = _object(document.get("authority"), "LiDAR authority")
|
|
_exact_keys(coordinates, {"space", "frame"}, "LiDAR coordinates")
|
|
_exact_keys(time, {"frame_basis", "point_relative_time"}, "LiDAR time")
|
|
_exact_keys(
|
|
imu,
|
|
{"samples_available", "lidar_extrinsic_available"},
|
|
"LiDAR IMU evidence",
|
|
)
|
|
_exact_keys(
|
|
authority,
|
|
{"commands_enabled", "navigation_or_safety_accepted"},
|
|
"LiDAR authority",
|
|
)
|
|
point_fields = _array(document, "point_fields")
|
|
try:
|
|
parsed_fields = tuple(
|
|
LidarPointField(_string_value(field, "LiDAR point field"))
|
|
for field in point_fields
|
|
)
|
|
profile = cls(
|
|
profile_id=_string(document, "profile_id"),
|
|
representation=LidarRepresentation(
|
|
_string(document, "representation")
|
|
),
|
|
coordinate_space=LidarCoordinateSpace(_string(coordinates, "space")),
|
|
coordinate_frame=_string(coordinates, "frame"),
|
|
frame_time_basis=LidarTimeBasis(_string(time, "frame_basis")),
|
|
point_fields=parsed_fields,
|
|
pose_status=LidarPoseStatus(_string(document, "pose_status")),
|
|
scan_geometry_known=_bool(document, "scan_geometry_known"),
|
|
imu_samples_available=_bool(imu, "samples_available"),
|
|
lidar_imu_extrinsic_available=_bool(imu, "lidar_extrinsic_available"),
|
|
camera_extrinsic_available=_bool(
|
|
document,
|
|
"camera_extrinsic_available",
|
|
),
|
|
commands_enabled=_bool(authority, "commands_enabled"),
|
|
navigation_or_safety_accepted=_bool(
|
|
authority,
|
|
"navigation_or_safety_accepted",
|
|
),
|
|
)
|
|
except ValueError as exc:
|
|
raise LidarContractError("LiDAR evidence profile enum is unknown") from exc
|
|
if (
|
|
time.get("point_relative_time")
|
|
is not (LidarPointField.RELATIVE_TIME in parsed_fields)
|
|
):
|
|
raise LidarContractError("LiDAR point-time declarations disagree")
|
|
return profile
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class LidarStageAssessment:
|
|
stage: LidarPipelineStage
|
|
readiness: LidarReadiness
|
|
reasons: tuple[str, ...]
|
|
|
|
def to_dict(self) -> dict[str, object]:
|
|
return {
|
|
"stage": self.stage.value,
|
|
"readiness": self.readiness.value,
|
|
"reasons": list(self.reasons),
|
|
}
|
|
|
|
|
|
class LidarQualityMonitor:
|
|
"""Bounded scanner telemetry over decoded point frames.
|
|
|
|
The monitor reports observed distributions and field coverage. It does not
|
|
turn those measurements into navigation or safety acceptance.
|
|
"""
|
|
|
|
def __init__(
|
|
self,
|
|
profile: LidarEvidenceProfile,
|
|
*,
|
|
frame_sample_capacity: int = 512,
|
|
point_sample_capacity: int = 32_768,
|
|
points_sampled_per_frame: int = 256,
|
|
) -> None:
|
|
if (
|
|
not 2 <= frame_sample_capacity <= 16_384
|
|
or not 256 <= point_sample_capacity <= 1_048_576
|
|
or not 1 <= points_sampled_per_frame <= 4096
|
|
):
|
|
raise LidarContractError("LiDAR quality monitor bounds are invalid")
|
|
self.profile = profile
|
|
self._frame_points: deque[int] = deque(maxlen=frame_sample_capacity)
|
|
self._frame_intervals_ms: deque[float] = deque(maxlen=frame_sample_capacity)
|
|
self._range_samples_m: deque[float] = deque(maxlen=point_sample_capacity)
|
|
self._intensity_samples: deque[float] = deque(maxlen=point_sample_capacity)
|
|
self._points_sampled_per_frame = points_sampled_per_frame
|
|
self._frames = 0
|
|
self._points = 0
|
|
self._intensity_frames = 0
|
|
self._range_frames = 0
|
|
self._range_unavailable_frames = 0
|
|
self._nonincreasing_frame_times = 0
|
|
self._last_frame_time_ns: int | None = None
|
|
self._lock = threading.Lock()
|
|
|
|
def observe(
|
|
self,
|
|
point_cloud: DecodedPointCloudView,
|
|
*,
|
|
pose: DecodedPoseView | None = None,
|
|
) -> None:
|
|
if point_cloud.frame_id != self.profile.coordinate_frame:
|
|
raise LidarContractError("LiDAR quality frame differs from its evidence profile")
|
|
points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
|
|
sample_indices = _uniform_sample_indices(
|
|
point_cloud.point_count,
|
|
self._points_sampled_per_frame,
|
|
)
|
|
sampled_ranges: npt.NDArray[np.float64] | None = None
|
|
if self.profile.coordinate_space is LidarCoordinateSpace.SENSOR:
|
|
sampled_ranges = np.linalg.norm(points[sample_indices], axis=1)
|
|
elif pose is not None:
|
|
if pose.frame_id != point_cloud.frame_id:
|
|
raise LidarContractError("LiDAR quality pose uses another map frame")
|
|
try:
|
|
points_sensor = map_points_to_lidar(
|
|
points[sample_indices],
|
|
position_map_xyz=pose.position_xyz,
|
|
orientation_map_from_lidar_xyzw=pose.orientation_xyzw,
|
|
)
|
|
except CalibratedProjectionError as exc:
|
|
raise LidarContractError("LiDAR quality pose is invalid") from exc
|
|
sampled_ranges = np.linalg.norm(points_sensor, axis=1)
|
|
|
|
intensity_samples: npt.NDArray[np.float64] | None = None
|
|
if point_cloud.intensities is not None:
|
|
intensities = np.frombuffer(point_cloud.intensities, dtype=np.uint8)
|
|
intensity_samples = intensities[sample_indices].astype(np.float64) / 255.0
|
|
elif LidarPointField.INTENSITY in self.profile.point_fields:
|
|
raise LidarContractError("LiDAR frame dropped profile-required intensity")
|
|
|
|
frame_time_ns = point_cloud.context.captured_at_epoch_ns
|
|
with self._lock:
|
|
if self._last_frame_time_ns is not None:
|
|
delta_ns = frame_time_ns - self._last_frame_time_ns
|
|
if delta_ns <= 0:
|
|
self._nonincreasing_frame_times += 1
|
|
else:
|
|
self._frame_intervals_ms.append(delta_ns / 1_000_000)
|
|
self._last_frame_time_ns = frame_time_ns
|
|
self._frames += 1
|
|
self._points += point_cloud.point_count
|
|
self._frame_points.append(point_cloud.point_count)
|
|
if intensity_samples is not None:
|
|
self._intensity_frames += 1
|
|
self._intensity_samples.extend(float(value) for value in intensity_samples)
|
|
if sampled_ranges is None:
|
|
self._range_unavailable_frames += 1
|
|
else:
|
|
self._range_frames += 1
|
|
self._range_samples_m.extend(float(value) for value in sampled_ranges)
|
|
|
|
def snapshot(self) -> dict[str, object]:
|
|
with self._lock:
|
|
return {
|
|
"schema_version": "missioncore.lidar-quality-report/v1",
|
|
"profile_id": self.profile.profile_id,
|
|
"frames_observed": self._frames,
|
|
"points_observed": self._points,
|
|
"intensity_frames": self._intensity_frames,
|
|
"range_frames": self._range_frames,
|
|
"sensor_range_unavailable_frames": self._range_unavailable_frames,
|
|
"nonincreasing_frame_times": self._nonincreasing_frame_times,
|
|
"sample_bounds": {
|
|
"frame_capacity": self._frame_points.maxlen,
|
|
"point_capacity": self._range_samples_m.maxlen,
|
|
"points_sampled_per_frame": self._points_sampled_per_frame,
|
|
},
|
|
"point_count_per_frame": _distribution(self._frame_points),
|
|
"frame_interval_ms": _distribution(self._frame_intervals_ms),
|
|
"sensor_range_m": _distribution(self._range_samples_m),
|
|
"intensity_0_1": _distribution(self._intensity_samples),
|
|
"authority": {
|
|
"commands_enabled": False,
|
|
"navigation_or_safety_accepted": False,
|
|
},
|
|
}
|
|
|
|
|
|
def assess_lidar_profile(
|
|
profile: LidarEvidenceProfile,
|
|
) -> tuple[LidarStageAssessment, ...]:
|
|
"""Return deterministic readiness without inferring missing sensor evidence."""
|
|
|
|
assessments = [
|
|
_quality_assessment(profile),
|
|
_ground_assessment(profile),
|
|
_detector_assessment(profile),
|
|
_nvblox_assessment(profile),
|
|
_odometry_assessment(profile),
|
|
_lio_assessment(profile),
|
|
_fusion_assessment(profile),
|
|
]
|
|
return tuple(assessments)
|
|
|
|
|
|
def lidar_readiness_document(profile: LidarEvidenceProfile) -> dict[str, object]:
|
|
return {
|
|
"schema_version": LIDAR_READINESS_SCHEMA,
|
|
"profile": profile.to_dict(),
|
|
"stages": [assessment.to_dict() for assessment in assess_lidar_profile(profile)],
|
|
"authority": {
|
|
"commands_enabled": False,
|
|
"navigation_or_safety_accepted": False,
|
|
},
|
|
}
|
|
|
|
|
|
def sensor_frame_xyzi(
|
|
point_cloud: DecodedPointCloudView,
|
|
pose: DecodedPoseView | None = None,
|
|
) -> Float32Array:
|
|
"""Build the finite sensor-frame XYZI tensor expected by LiDAR detectors.
|
|
|
|
K1 `lio_pcl` positions are already in the canonical `map` frame; they are not raw
|
|
sensor-frame sweeps. The matching `T_map_from_lidar` pose is therefore
|
|
required to invert them. Intensity is normalized from the verified uint8
|
|
low byte to the [0, 1] reflectance interval used by the admitted
|
|
PointPillars baseline.
|
|
"""
|
|
|
|
if point_cloud.intensities is None:
|
|
raise LidarContractError("sensor-frame XYZI requires intensity")
|
|
points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
|
|
if point_cloud.frame_id == (pose.child_frame_id if pose is not None else None):
|
|
points_sensor = points
|
|
elif pose is not None and point_cloud.frame_id == pose.frame_id:
|
|
try:
|
|
points_sensor = map_points_to_lidar(
|
|
points,
|
|
position_map_xyz=pose.position_xyz,
|
|
orientation_map_from_lidar_xyzw=pose.orientation_xyzw,
|
|
)
|
|
except CalibratedProjectionError as exc:
|
|
raise LidarContractError("map-frame LiDAR pose is invalid") from exc
|
|
else:
|
|
raise LidarContractError(
|
|
"LiDAR coordinates cannot be bound to the supplied sensor pose"
|
|
)
|
|
intensity = np.frombuffer(point_cloud.intensities, dtype=np.uint8).astype(np.float32)
|
|
xyzi = np.empty((point_cloud.point_count, 4), dtype=np.float32)
|
|
xyzi[:, :3] = points_sensor.astype(np.float32)
|
|
xyzi[:, 3] = intensity / 255.0
|
|
if not np.isfinite(xyzi).all():
|
|
raise LidarContractError("sensor-frame XYZI contains non-finite values")
|
|
return xyzi
|
|
|
|
|
|
def _quality_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
|
reasons: list[str] = []
|
|
if LidarPointField.INTENSITY not in profile.point_fields:
|
|
reasons.append("intensity-unavailable")
|
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
|
reasons.append("sensor-clock-unproven")
|
|
return _assessment(LidarPipelineStage.SCANNER_QUALITY, reasons, blocked=False)
|
|
|
|
|
|
def _ground_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
|
reasons: list[str] = []
|
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
|
reasons.append("vendor-mapped-points-are-not-raw-returns")
|
|
if profile.coordinate_space is LidarCoordinateSpace.MAP:
|
|
reasons.append("sensor-frame-conversion-required")
|
|
return _assessment(LidarPipelineStage.GROUND_SEGMENTATION, reasons, blocked=False)
|
|
|
|
|
|
def _detector_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
|
reasons: list[str] = []
|
|
blocked = False
|
|
if LidarPointField.INTENSITY not in profile.point_fields:
|
|
reasons.append("admitted-pointpillars-baseline-requires-intensity")
|
|
blocked = True
|
|
if (
|
|
profile.coordinate_space is LidarCoordinateSpace.MAP
|
|
and profile.pose_status is LidarPoseStatus.NONE
|
|
):
|
|
reasons.append("sensor-frame-conversion-has-no-pose")
|
|
blocked = True
|
|
elif profile.coordinate_space is LidarCoordinateSpace.MAP:
|
|
reasons.append("sensor-frame-conversion-required")
|
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
|
reasons.append("pretrained-domain-expects-sensor-scan")
|
|
return _assessment(LidarPipelineStage.LIDAR_3D_DETECTION, reasons, blocked)
|
|
|
|
|
|
def _nvblox_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
|
reasons: list[str] = []
|
|
blocked = False
|
|
if not profile.scan_geometry_known:
|
|
reasons.append("lidar-intrinsics-or-scan-geometry-unknown")
|
|
blocked = True
|
|
if profile.pose_status is LidarPoseStatus.NONE:
|
|
reasons.append("pose-unavailable")
|
|
blocked = True
|
|
elif profile.pose_status is not LidarPoseStatus.SENSOR_SYNCHRONIZED:
|
|
reasons.append("pose-is-best-effort")
|
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
|
reasons.append("sensor-clock-unproven")
|
|
return _assessment(LidarPipelineStage.NVIDIA_NVBLOX, reasons, blocked)
|
|
|
|
|
|
def _odometry_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
|
reasons: list[str] = []
|
|
blocked = False
|
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
|
reasons.append("odometry-requires-unregistered-sensor-scans")
|
|
blocked = True
|
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
|
reasons.append("sensor-clock-unproven")
|
|
return _assessment(LidarPipelineStage.LIDAR_ODOMETRY, reasons, blocked)
|
|
|
|
|
|
def _lio_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
|
reasons: list[str] = []
|
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
|
reasons.append("lio-requires-unregistered-sensor-scans")
|
|
if LidarPointField.RELATIVE_TIME not in profile.point_fields:
|
|
reasons.append("per-point-time-unavailable")
|
|
if not profile.imu_samples_available:
|
|
reasons.append("imu-samples-unavailable")
|
|
if not profile.lidar_imu_extrinsic_available:
|
|
reasons.append("lidar-imu-extrinsic-unavailable")
|
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
|
reasons.append("sensor-clock-unproven")
|
|
return _assessment(
|
|
LidarPipelineStage.LIDAR_INERTIAL_SLAM,
|
|
reasons,
|
|
blocked=bool(reasons),
|
|
)
|
|
|
|
|
|
def _fusion_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
|
reasons: list[str] = []
|
|
blocked = False
|
|
if not profile.camera_extrinsic_available:
|
|
reasons.append("camera-extrinsic-unavailable")
|
|
blocked = True
|
|
if profile.pose_status is LidarPoseStatus.NONE:
|
|
reasons.append("pose-unavailable")
|
|
blocked = True
|
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
|
reasons.append("camera-lidar-synchronization-is-best-effort")
|
|
return _assessment(LidarPipelineStage.CAMERA_LIDAR_FUSION, reasons, blocked)
|
|
|
|
|
|
def _assessment(
|
|
stage: LidarPipelineStage,
|
|
reasons: list[str],
|
|
blocked: bool,
|
|
) -> LidarStageAssessment:
|
|
if blocked:
|
|
readiness = LidarReadiness.BLOCKED
|
|
elif reasons:
|
|
readiness = LidarReadiness.DEGRADED
|
|
else:
|
|
readiness = LidarReadiness.READY
|
|
return LidarStageAssessment(stage, readiness, tuple(reasons))
|
|
|
|
|
|
def _safe_identifier(value: str, label: str) -> str:
|
|
if not isinstance(value, str) or _IDENTIFIER.fullmatch(value) is None:
|
|
raise LidarContractError(f"{label} is not a safe identifier")
|
|
return value
|
|
|
|
|
|
def _uniform_sample_indices(point_count: int, maximum: int) -> npt.NDArray[np.int64]:
|
|
if point_count <= maximum:
|
|
return np.arange(point_count, dtype=np.int64)
|
|
return np.linspace(0, point_count - 1, maximum, dtype=np.int64)
|
|
|
|
|
|
def _distribution(values: deque[int] | deque[float]) -> dict[str, float | int | None]:
|
|
if not values:
|
|
return {
|
|
"sample_count": 0,
|
|
"minimum": None,
|
|
"mean": None,
|
|
"p50": None,
|
|
"p95": None,
|
|
"maximum": None,
|
|
}
|
|
array = np.asarray(values, dtype=np.float64)
|
|
return {
|
|
"sample_count": int(array.size),
|
|
"minimum": float(np.min(array)),
|
|
"mean": float(np.mean(array)),
|
|
"p50": float(np.percentile(array, 50)),
|
|
"p95": float(np.percentile(array, 95)),
|
|
"maximum": float(np.max(array)),
|
|
}
|
|
|
|
|
|
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 LidarContractError(f"{label} must be an object")
|
|
return value
|
|
|
|
|
|
def _exact_keys(document: dict[str, Any], expected: set[str], label: str) -> None:
|
|
if set(document) != expected:
|
|
raise LidarContractError(f"{label} fields are incompatible")
|
|
|
|
|
|
def _array(document: dict[str, Any], key: str) -> list[object]:
|
|
value = document.get(key)
|
|
if not isinstance(value, list):
|
|
raise LidarContractError(f"{key} must be an array")
|
|
return value
|
|
|
|
|
|
def _string(document: dict[str, Any], key: str) -> str:
|
|
return _string_value(document.get(key), key)
|
|
|
|
|
|
def _string_value(value: object, label: str) -> str:
|
|
if not isinstance(value, str) or not value:
|
|
raise LidarContractError(f"{label} must be a nonempty string")
|
|
return value
|
|
|
|
|
|
def _bool(document: dict[str, Any], key: str) -> bool:
|
|
value = document.get(key)
|
|
if not isinstance(value, bool):
|
|
raise LidarContractError(f"{key} must be a boolean")
|
|
return value
|
|
|
|
|
|
K1_LIVE_LIDAR_PROFILE: Final = LidarEvidenceProfile(
|
|
profile_id="xgrids-k1-live-lio-pcl/v1",
|
|
representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
|
|
coordinate_space=LidarCoordinateSpace.MAP,
|
|
coordinate_frame="map",
|
|
frame_time_basis=LidarTimeBasis.HOST_ARRIVAL,
|
|
point_fields=(LidarPointField.XYZ, LidarPointField.INTENSITY),
|
|
pose_status=LidarPoseStatus.BEST_EFFORT,
|
|
scan_geometry_known=False,
|
|
imu_samples_available=False,
|
|
lidar_imu_extrinsic_available=False,
|
|
camera_extrinsic_available=True,
|
|
)
|
|
|
|
K1_LAB_LIDAR_PACK_V1_PROFILE: Final = LidarEvidenceProfile(
|
|
profile_id="xgrids-k1-e10-lidar-replay-pack/v1",
|
|
representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
|
|
coordinate_space=LidarCoordinateSpace.MAP,
|
|
coordinate_frame="map",
|
|
frame_time_basis=LidarTimeBasis.CAMERA_BOUND_HOST_ARRIVAL,
|
|
point_fields=(LidarPointField.XYZ,),
|
|
pose_status=LidarPoseStatus.BEST_EFFORT,
|
|
scan_geometry_known=False,
|
|
imu_samples_available=False,
|
|
lidar_imu_extrinsic_available=False,
|
|
camera_extrinsic_available=True,
|
|
)
|
|
|
|
K1_LIDAR_PACK_V2_PROFILE: Final = LidarEvidenceProfile(
|
|
profile_id="xgrids-k1-lidar-replay-pack/v2",
|
|
representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
|
|
coordinate_space=LidarCoordinateSpace.MAP,
|
|
coordinate_frame="map",
|
|
frame_time_basis=LidarTimeBasis.HOST_ARRIVAL,
|
|
point_fields=(LidarPointField.XYZ, LidarPointField.INTENSITY),
|
|
pose_status=LidarPoseStatus.BEST_EFFORT,
|
|
scan_geometry_known=False,
|
|
imu_samples_available=False,
|
|
lidar_imu_extrinsic_available=False,
|
|
camera_extrinsic_available=True,
|
|
)
|