feat(perception): qualify lidar evidence before models
This commit is contained in:
@@ -48,6 +48,26 @@ from .lab_instances import (
|
||||
publish_e26_lab_instance,
|
||||
publish_integrated_lab_instance,
|
||||
)
|
||||
from .lidar_contract import (
|
||||
K1_LAB_LIDAR_PACK_V1_PROFILE,
|
||||
K1_LIVE_LIDAR_PROFILE,
|
||||
LIDAR_EVIDENCE_PROFILE_SCHEMA,
|
||||
LIDAR_READINESS_SCHEMA,
|
||||
LidarContractError,
|
||||
LidarCoordinateSpace,
|
||||
LidarEvidenceProfile,
|
||||
LidarPipelineStage,
|
||||
LidarPointField,
|
||||
LidarPoseStatus,
|
||||
LidarQualityMonitor,
|
||||
LidarReadiness,
|
||||
LidarRepresentation,
|
||||
LidarStageAssessment,
|
||||
LidarTimeBasis,
|
||||
assess_lidar_profile,
|
||||
lidar_readiness_document,
|
||||
sensor_frame_xyzi,
|
||||
)
|
||||
from .live_perception import (
|
||||
LIVE_INGRESS_SCHEMA,
|
||||
LIVE_INGRESS_WIRE_SCHEMA,
|
||||
@@ -118,10 +138,23 @@ __all__ = [
|
||||
"EvaluationFrameRequest",
|
||||
"EvaluationPackFrame",
|
||||
"LatestWinsQueue",
|
||||
"LIDAR_EVIDENCE_PROFILE_SCHEMA",
|
||||
"LIDAR_READINESS_SCHEMA",
|
||||
"LIVE_INGRESS_SCHEMA",
|
||||
"LIVE_INGRESS_WIRE_SCHEMA",
|
||||
"LiveIngressEvent",
|
||||
"LivePerceptionIngress",
|
||||
"LidarContractError",
|
||||
"LidarCoordinateSpace",
|
||||
"LidarEvidenceProfile",
|
||||
"LidarPipelineStage",
|
||||
"LidarPointField",
|
||||
"LidarPoseStatus",
|
||||
"LidarQualityMonitor",
|
||||
"LidarReadiness",
|
||||
"LidarRepresentation",
|
||||
"LidarStageAssessment",
|
||||
"LidarTimeBasis",
|
||||
"IntegratedPerceptionOverlayStore",
|
||||
"PublishedIntegratedLabInstance",
|
||||
"PublishedCameraEgoMotionLabInstance",
|
||||
@@ -133,6 +166,8 @@ __all__ = [
|
||||
"MultiratePerceptionArtifact",
|
||||
"MultiratePerceptionQualificationResult",
|
||||
"QUALIFICATION_POLICY",
|
||||
"K1_LAB_LIDAR_PACK_V1_PROFILE",
|
||||
"K1_LIVE_LIDAR_PROFILE",
|
||||
"QueueSnapshot",
|
||||
"RecordedCalibratedFusion",
|
||||
"RecordedCalibratedFusionStore",
|
||||
@@ -165,6 +200,7 @@ __all__ = [
|
||||
"publish_integrated_lab_instance",
|
||||
"validate_multirate_perception_qualification_result",
|
||||
"prepare_recorded_qualification_slice",
|
||||
"assess_lidar_profile",
|
||||
"DetectionFrame",
|
||||
"ObjectDetection",
|
||||
"RecordedPerceptionOverlayError",
|
||||
@@ -184,4 +220,6 @@ __all__ = [
|
||||
"validate_tracking_qualification_result",
|
||||
"validate_tracked_fusion_qualification_result",
|
||||
"validate_annotation_workspace",
|
||||
"lidar_readiness_document",
|
||||
"sensor_frame_xyzi",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,623 @@
|
||||
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,
|
||||
)
|
||||
Reference in New Issue
Block a user