SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(lidar): admit and benchmark GOOSE baseline

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-25 14:14:20 +03:00
parent 881e97312b
commit 951b40c870
20 changed files with 2871 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/compute/lidar_ground.py
+12 -185
View File
@@ -10,15 +10,25 @@ import re
import shutil
import time
from collections.abc import Mapping
from dataclasses import dataclass
from datetime import UTC, datetime
from pathlib import Path
from types import ModuleType
from typing import Any, Final, Protocol
from typing import Any, Final
import numpy as np
import numpy.typing as npt
from k1link.ground_segmentation import (
DEFAULT_GROUND_BENCHMARK_PROFILE,
GroundBenchmarkProfile,
GroundSegmentation,
GroundSegmenter,
LocalPercentileGroundSegmenter,
)
from k1link.ground_segmentation import (
GroundSegmentationError as LidarGroundError,
)
from .lidar_contract import LidarContractError, sensor_frame_xyzi
from .lidar_replay import LidarReplayPackV2
@@ -41,189 +51,6 @@ _ANNOTATION_TEMPLATE_ID = re.compile(r"^ground-annotation-template-[a-f0-9]{64}$
_SHA256 = re.compile(r"^[a-f0-9]{64}$")
_GIT_SHA1 = re.compile(r"^[a-f0-9]{40}$")
BoolArray = npt.NDArray[np.bool_]
FloatArray = npt.NDArray[np.floating[Any]]
class LidarGroundError(ValueError):
"""Ground benchmark evidence violates the diagnostic-only 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 LidarGroundError("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 LidarGroundError("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 LidarGroundError("Ground benchmark cannot apply height without height evidence")
if (
self.patchwork_height_evidence != "missing"
and self.patchwork_sensor_height_proxy_m <= 0
):
raise LidarGroundError(
"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)
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 = np.flatnonzero(inverse == cell_index)
if point_indices.size == 0:
continue
center_xy = np.median(xyz[point_indices, :2], axis=0)
delta_xy = xyz[:, :2] - center_xy
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,
)
class PatchworkPPGroundSegmenter:
"""Runtime-only adapter for the pinned official Patchwork++ Python binding."""