239 lines
9.8 KiB
Python
239 lines
9.8 KiB
Python
"""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()
|