feat(lidar): admit and benchmark GOOSE baseline
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,238 @@
|
||||
"""Provider-neutral, dependency-light LiDAR ground segmentation primitives."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import math
|
||||
import time
|
||||
from collections.abc import Mapping
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Final, Protocol
|
||||
|
||||
import numpy as np
|
||||
import numpy.typing as npt
|
||||
|
||||
BoolArray = npt.NDArray[np.bool_]
|
||||
|
||||
|
||||
class GroundSegmentationError(ValueError):
|
||||
"""A ground provider or profile violates the shared segmentation contract."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class GroundBenchmarkProfile:
|
||||
profile_id: str = "k1-vendor-map-ground-ab/v1"
|
||||
pose_binding_threshold_ms: float = 100.0
|
||||
current_cell_size_m: float = 0.5
|
||||
current_local_radius_m: float = 2.5
|
||||
current_lower_percentile: float = 8.0
|
||||
current_maximum_below_ground_m: float = 0.25
|
||||
current_maximum_above_ground_m: float = 0.12
|
||||
current_minimum_local_points: int = 8
|
||||
patchwork_sensor_height_proxy_m: float = 0.0
|
||||
patchwork_map_vertical_origin_offset_m: float = 0.0
|
||||
patchwork_height_evidence: str = "missing"
|
||||
patchwork_minimum_range_m: float = 0.1
|
||||
patchwork_maximum_range_m: float = 20.0
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
finite_values = (
|
||||
self.pose_binding_threshold_ms,
|
||||
self.current_cell_size_m,
|
||||
self.current_local_radius_m,
|
||||
self.current_lower_percentile,
|
||||
self.current_maximum_below_ground_m,
|
||||
self.current_maximum_above_ground_m,
|
||||
self.patchwork_sensor_height_proxy_m,
|
||||
self.patchwork_map_vertical_origin_offset_m,
|
||||
self.patchwork_minimum_range_m,
|
||||
self.patchwork_maximum_range_m,
|
||||
)
|
||||
if not all(math.isfinite(value) for value in finite_values):
|
||||
raise GroundSegmentationError("Ground benchmark profile must be finite")
|
||||
if (
|
||||
not 0 < self.pose_binding_threshold_ms <= 10_000
|
||||
or not 0 < self.current_cell_size_m <= 100
|
||||
or not 0 < self.current_local_radius_m <= 1_000
|
||||
or not 0 <= self.current_lower_percentile <= 100
|
||||
or not 0 <= self.current_maximum_below_ground_m <= 100
|
||||
or not 0 <= self.current_maximum_above_ground_m <= 100
|
||||
or not 1 <= self.current_minimum_local_points <= 1_000_000
|
||||
or not 0 <= self.patchwork_sensor_height_proxy_m <= 10
|
||||
or not -10 <= self.patchwork_map_vertical_origin_offset_m <= 10
|
||||
or not 0 <= self.patchwork_minimum_range_m < self.patchwork_maximum_range_m <= 1_000
|
||||
or self.patchwork_height_evidence
|
||||
not in {"missing", "operator-estimated", "runtime-calibrated"}
|
||||
):
|
||||
raise GroundSegmentationError("Ground benchmark profile is invalid")
|
||||
if self.patchwork_height_evidence == "missing" and (
|
||||
self.patchwork_sensor_height_proxy_m != 0
|
||||
or self.patchwork_map_vertical_origin_offset_m != 0
|
||||
):
|
||||
raise GroundSegmentationError(
|
||||
"Ground benchmark cannot apply height without height evidence"
|
||||
)
|
||||
if (
|
||||
self.patchwork_height_evidence != "missing"
|
||||
and self.patchwork_sensor_height_proxy_m <= 0
|
||||
):
|
||||
raise GroundSegmentationError(
|
||||
"Ground benchmark height evidence requires a positive sensor height"
|
||||
)
|
||||
|
||||
def to_dict(self) -> dict[str, object]:
|
||||
return {
|
||||
"schema_version": "missioncore.lidar-ground-benchmark-profile/v1",
|
||||
"profile_id": self.profile_id,
|
||||
"pose_binding": {
|
||||
"basis": "nearest-recorded-host-monotonic-arrival",
|
||||
"threshold_ms": self.pose_binding_threshold_ms,
|
||||
},
|
||||
"current_baseline": {
|
||||
"provider_id": "missioncore-local-percentile-ground/v1",
|
||||
"derived_from": "local-ground-relative-object-support-v1",
|
||||
"cell_size_m": self.current_cell_size_m,
|
||||
"local_radius_m": self.current_local_radius_m,
|
||||
"lower_percentile": self.current_lower_percentile,
|
||||
"maximum_below_ground_m": self.current_maximum_below_ground_m,
|
||||
"maximum_above_ground_m": self.current_maximum_above_ground_m,
|
||||
"minimum_local_points": self.current_minimum_local_points,
|
||||
},
|
||||
"candidate": {
|
||||
"provider_id": "patchworkpp/v1.4.1",
|
||||
"sensor_height_proxy_m": self.patchwork_sensor_height_proxy_m,
|
||||
"map_vertical_origin_offset_m": self.patchwork_map_vertical_origin_offset_m,
|
||||
"height_evidence": self.patchwork_height_evidence,
|
||||
"minimum_range_m": self.patchwork_minimum_range_m,
|
||||
"maximum_range_m": self.patchwork_maximum_range_m,
|
||||
"enable_rnr": True,
|
||||
"enable_rvpf": True,
|
||||
"enable_tgr": True,
|
||||
},
|
||||
"input_normalization": {
|
||||
"current": "vendor-map-xyz",
|
||||
"candidate": "best-effort-map-to-lidar-pose-inversion",
|
||||
"physical_sensor_height_known": (
|
||||
self.patchwork_height_evidence == "runtime-calibrated"
|
||||
),
|
||||
"sensor_scan_geometry_known": False,
|
||||
},
|
||||
"authority": {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
DEFAULT_GROUND_BENCHMARK_PROFILE: Final = GroundBenchmarkProfile()
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class GroundSegmentation:
|
||||
ground_mask: BoolArray
|
||||
assigned_mask: BoolArray
|
||||
latency_ms: float
|
||||
|
||||
|
||||
class GroundSegmenter(Protocol):
|
||||
@property
|
||||
def identity(self) -> Mapping[str, object]: ...
|
||||
|
||||
def segment(self, xyzi: npt.NDArray[np.float32]) -> GroundSegmentation: ...
|
||||
|
||||
|
||||
class LocalPercentileGroundSegmenter:
|
||||
"""Full-frame diagnostic extension of the existing E19 local ground heuristic."""
|
||||
|
||||
def __init__(self, profile: GroundBenchmarkProfile) -> None:
|
||||
self.profile = profile
|
||||
|
||||
@property
|
||||
def identity(self) -> Mapping[str, object]:
|
||||
return {
|
||||
"provider_id": "missioncore-local-percentile-ground/v1",
|
||||
"implementation_sha256": _sha256(Path(__file__).resolve(strict=True)),
|
||||
"ground_truth": False,
|
||||
}
|
||||
|
||||
def segment(self, xyzi: npt.NDArray[np.float32]) -> GroundSegmentation:
|
||||
points = _xyzi(xyzi)
|
||||
started = time.perf_counter_ns()
|
||||
xyz = points[:, :3].astype(np.float64, copy=False)
|
||||
cell_size = self.profile.current_cell_size_m
|
||||
cell_keys = np.floor(xyz[:, :2] / cell_size).astype(np.int64)
|
||||
unique_cells, inverse = np.unique(cell_keys, axis=0, return_inverse=True)
|
||||
order = np.argsort(inverse, kind="stable")
|
||||
counts = np.bincount(inverse, minlength=unique_cells.shape[0])
|
||||
offsets = np.concatenate(([0], np.cumsum(counts)))
|
||||
cell_lookup = {
|
||||
(int(cell[0]), int(cell[1])): cell_index
|
||||
for cell_index, cell in enumerate(unique_cells)
|
||||
}
|
||||
neighbor_span = math.ceil(self.profile.current_local_radius_m / cell_size) + 1
|
||||
ground = np.zeros(points.shape[0], dtype=np.bool_)
|
||||
global_ground_z = float(np.percentile(xyz[:, 2], self.profile.current_lower_percentile))
|
||||
radius_squared = self.profile.current_local_radius_m**2
|
||||
for cell_index in range(unique_cells.shape[0]):
|
||||
point_indices = order[offsets[cell_index] : offsets[cell_index + 1]]
|
||||
if point_indices.size == 0:
|
||||
continue
|
||||
center_xy = np.median(xyz[point_indices, :2], axis=0)
|
||||
cell_x, cell_y = unique_cells[cell_index]
|
||||
neighbor_slices: list[npt.NDArray[np.int64]] = []
|
||||
for delta_x in range(-neighbor_span, neighbor_span + 1):
|
||||
for delta_y in range(-neighbor_span, neighbor_span + 1):
|
||||
neighbor_cell_index = cell_lookup.get(
|
||||
(int(cell_x + delta_x), int(cell_y + delta_y))
|
||||
)
|
||||
if neighbor_cell_index is None:
|
||||
continue
|
||||
neighbor_slices.append(
|
||||
order[
|
||||
offsets[neighbor_cell_index] : offsets[neighbor_cell_index + 1]
|
||||
]
|
||||
)
|
||||
local_indices = np.concatenate(neighbor_slices)
|
||||
local_xyz = xyz[local_indices]
|
||||
delta_xy = local_xyz[:, :2] - center_xy
|
||||
local = local_xyz[
|
||||
np.einsum("ij,ij->i", delta_xy, delta_xy) <= radius_squared,
|
||||
2,
|
||||
]
|
||||
ground_z = (
|
||||
float(np.percentile(local, self.profile.current_lower_percentile))
|
||||
if local.size >= self.profile.current_minimum_local_points
|
||||
else global_ground_z
|
||||
)
|
||||
z = xyz[point_indices, 2]
|
||||
ground[point_indices] = (
|
||||
z >= ground_z - self.profile.current_maximum_below_ground_m
|
||||
) & (z <= ground_z + self.profile.current_maximum_above_ground_m)
|
||||
latency_ms = (time.perf_counter_ns() - started) / 1_000_000
|
||||
return GroundSegmentation(
|
||||
ground_mask=ground,
|
||||
assigned_mask=np.ones(points.shape[0], dtype=np.bool_),
|
||||
latency_ms=latency_ms,
|
||||
)
|
||||
|
||||
|
||||
def _xyzi(value: npt.NDArray[np.float32]) -> npt.NDArray[np.float32]:
|
||||
points = np.asarray(value)
|
||||
if (
|
||||
points.dtype != np.dtype(np.float32)
|
||||
or points.ndim != 2
|
||||
or points.shape[1] != 4
|
||||
or points.shape[0] == 0
|
||||
or not np.isfinite(points).all()
|
||||
):
|
||||
raise GroundSegmentationError("Ground input must be a non-empty finite float32 XYZI matrix")
|
||||
return points
|
||||
|
||||
|
||||
def _sha256(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as source:
|
||||
for chunk in iter(lambda: source.read(1024**2), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
Reference in New Issue
Block a user