feat(lidar): qualify Patchwork++ on GOOSE

src/k1link/ground_segmentation.py

@@ -3,17 +3,23 @@
 from __future__ import annotations
 
 import hashlib
+import importlib
 import math
+import platform
 import time
 from collections.abc import Mapping
 from dataclasses import dataclass
 from pathlib import Path
+from types import ModuleType
 from typing import Final, Protocol
 
 import numpy as np
 import numpy.typing as npt
 
 BoolArray = npt.NDArray[np.bool_]
+PATCHWORKPP_SOURCE_URL: Final = "https://github.com/url-kaist/patchwork-plusplus"
+PATCHWORKPP_SOURCE_TAG: Final = "v1.4.1"
+PATCHWORKPP_SOURCE_COMMIT: Final = "3e6903a1d5537a4cc2ace897b0bbb98a92d6014c"
 
 
 class GroundSegmentationError(ValueError):
@@ -142,6 +148,114 @@ class GroundSegmenter(Protocol):
     def segment(self, xyzi: npt.NDArray[np.float32]) -> GroundSegmentation: ...
 
 
+class PatchworkGroundProfile(Protocol):
+    @property
+    def patchwork_sensor_height_proxy_m(self) -> float: ...
+
+    @property
+    def patchwork_minimum_range_m(self) -> float: ...
+
+    @property
+    def patchwork_maximum_range_m(self) -> float: ...
+
+
+class PatchworkPPGroundSegmenter:
+    """Runtime-only adapter for the pinned official Patchwork++ Python binding."""
+
+    def __init__(
+        self,
+        module: ModuleType,
+        profile: PatchworkGroundProfile,
+        *,
+        source_commit: str = PATCHWORKPP_SOURCE_COMMIT,
+        source_tag: str = PATCHWORKPP_SOURCE_TAG,
+    ) -> None:
+        if len(source_commit) != 40 or any(
+            character not in "0123456789abcdef" for character in source_commit
+        ):
+            raise GroundSegmentationError("Patchwork++ source commit is invalid")
+        if source_tag != PATCHWORKPP_SOURCE_TAG:
+            raise GroundSegmentationError("Patchwork++ source tag is not admitted")
+        module_path_value = getattr(module, "__file__", None)
+        if not isinstance(module_path_value, str):
+            raise GroundSegmentationError("Patchwork++ module has no verifiable binary")
+        module_path = Path(module_path_value).resolve(strict=True)
+        params = module.Parameters()
+        params.sensor_height = profile.patchwork_sensor_height_proxy_m
+        params.min_range = profile.patchwork_minimum_range_m
+        params.max_range = profile.patchwork_maximum_range_m
+        params.enable_RNR = True
+        params.enable_RVPF = True
+        params.enable_TGR = True
+        params.verbose = False
+        self._estimator = module.patchworkpp(params)
+        self._identity = {
+            "provider_id": "patchworkpp/v1.4.1",
+            "source_url": PATCHWORKPP_SOURCE_URL,
+            "source_tag": source_tag,
+            "source_commit": source_commit,
+            "binding_version": str(getattr(module, "__version__", "unknown")),
+            "binary_sha256": _sha256(module_path),
+            "platform": platform.system().lower(),
+            "machine": platform.machine().lower(),
+            "ground_truth": False,
+        }
+
+    @classmethod
+    def load(
+        cls,
+        profile: PatchworkGroundProfile = DEFAULT_GROUND_BENCHMARK_PROFILE,
+        *,
+        module_name: str = "pypatchworkpp",
+        source_commit: str = PATCHWORKPP_SOURCE_COMMIT,
+        source_tag: str = PATCHWORKPP_SOURCE_TAG,
+    ) -> PatchworkPPGroundSegmenter:
+        try:
+            module = importlib.import_module(module_name)
+        except ImportError as exc:
+            raise GroundSegmentationError(
+                "Pinned Patchwork++ Python binding is unavailable"
+            ) from exc
+        return cls(
+            module,
+            profile,
+            source_commit=source_commit,
+            source_tag=source_tag,
+        )
+
+    @property
+    def identity(self) -> Mapping[str, object]:
+        return self._identity
+
+    def segment(self, xyzi: npt.NDArray[np.float32]) -> GroundSegmentation:
+        points = np.ascontiguousarray(_xyzi(xyzi), dtype=np.float32)
+        started = time.perf_counter_ns()
+        self._estimator.estimateGround(points)
+        latency_ms = (time.perf_counter_ns() - started) / 1_000_000
+        ground_indices = np.asarray(
+            self._estimator.getGroundIndices(),
+            dtype=np.int64,
+        ).reshape((-1,))
+        nonground_indices = np.asarray(
+            self._estimator.getNongroundIndices(),
+            dtype=np.int64,
+        ).reshape((-1,))
+        _indices(ground_indices, points.shape[0], "Patchwork++ ground")
+        _indices(nonground_indices, points.shape[0], "Patchwork++ non-ground")
+        if np.intersect1d(ground_indices, nonground_indices).size:
+            raise GroundSegmentationError("Patchwork++ assigned one point twice")
+        ground = np.zeros(points.shape[0], dtype=np.bool_)
+        assigned = np.zeros(points.shape[0], dtype=np.bool_)
+        ground[ground_indices] = True
+        assigned[ground_indices] = True
+        assigned[nonground_indices] = True
+        return GroundSegmentation(
+            ground_mask=ground,
+            assigned_mask=assigned,
+            latency_ms=latency_ms,
+        )
+
+
 class LocalPercentileGroundSegmenter:
     """Full-frame diagnostic extension of the existing E19 local ground heuristic."""
 
@@ -167,8 +281,7 @@ class LocalPercentileGroundSegmenter:
         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)
+            (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_)
@@ -189,9 +302,7 @@ class LocalPercentileGroundSegmenter:
                     if neighbor_cell_index is None:
                         continue
                     neighbor_slices.append(
-                        order[
-                            offsets[neighbor_cell_index] : offsets[neighbor_cell_index + 1]
-                        ]
+                        order[offsets[neighbor_cell_index] : offsets[neighbor_cell_index + 1]]
                     )
             local_indices = np.concatenate(neighbor_slices)
             local_xyz = xyz[local_indices]
@@ -236,3 +347,10 @@ def _sha256(path: Path) -> str:
         for chunk in iter(lambda: source.read(1024**2), b""):
             digest.update(chunk)
     return digest.hexdigest()
+
+
+def _indices(value: npt.NDArray[np.int64], count: int, label: str) -> None:
+    if value.ndim != 1 or np.any(value < 0) or np.any(value >= count):
+        raise GroundSegmentationError(f"{label} indices are invalid")
+    if np.unique(value).size != value.size:
+        raise GroundSegmentationError(f"{label} indices are duplicated")