feat(lidar): qualify Patchwork++ on GOOSE
This commit is contained in:
DCCONSTRUCTIONS
@@ -1,18 +1,14 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import importlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import platform
|
||||
import re
|
||||
import shutil
|
||||
import time
|
||||
from collections.abc import Mapping
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from types import ModuleType
|
||||
from typing import Any, Final
|
||||
|
||||
import numpy as np
|
||||
@@ -25,9 +21,21 @@ from k1link.ground_segmentation import (
|
||||
GroundSegmenter,
|
||||
LocalPercentileGroundSegmenter,
|
||||
)
|
||||
from k1link.ground_segmentation import (
|
||||
PATCHWORKPP_SOURCE_COMMIT as PATCHWORKPP_SOURCE_COMMIT,
|
||||
)
|
||||
from k1link.ground_segmentation import (
|
||||
PATCHWORKPP_SOURCE_TAG as PATCHWORKPP_SOURCE_TAG,
|
||||
)
|
||||
from k1link.ground_segmentation import (
|
||||
PATCHWORKPP_SOURCE_URL as PATCHWORKPP_SOURCE_URL,
|
||||
)
|
||||
from k1link.ground_segmentation import (
|
||||
GroundSegmentationError as LidarGroundError,
|
||||
)
|
||||
from k1link.ground_segmentation import (
|
||||
PatchworkPPGroundSegmenter as PatchworkPPGroundSegmenter,
|
||||
)
|
||||
|
||||
from .lidar_contract import LidarContractError, sensor_frame_xyzi
|
||||
from .lidar_replay import LidarReplayPackV2
|
||||
@@ -42,107 +50,9 @@ LIDAR_GROUND_MANIFEST_NAME: Final = "manifest.json"
|
||||
LIDAR_GROUND_ANNOTATION_LABELS_NAME: Final = "labels-template.npz"
|
||||
MAX_GROUND_FRAME_POINTS: Final = 200_000
|
||||
|
||||
PATCHWORKPP_SOURCE_URL: Final = "https://github.com/url-kaist/patchwork-plusplus"
|
||||
PATCHWORKPP_SOURCE_TAG: Final = "v1.4.1"
|
||||
PATCHWORKPP_SOURCE_COMMIT: Final = "3e6903a1d5537a4cc2ace897b0bbb98a92d6014c"
|
||||
|
||||
_BENCHMARK_ID = re.compile(r"^ground-benchmark-[a-f0-9]{64}$")
|
||||
_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}$")
|
||||
|
||||
|
||||
class PatchworkPPGroundSegmenter:
|
||||
"""Runtime-only adapter for the pinned official Patchwork++ Python binding."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
module: ModuleType,
|
||||
profile: GroundBenchmarkProfile,
|
||||
*,
|
||||
source_commit: str = PATCHWORKPP_SOURCE_COMMIT,
|
||||
source_tag: str = PATCHWORKPP_SOURCE_TAG,
|
||||
) -> None:
|
||||
if _GIT_SHA1.fullmatch(source_commit) is None:
|
||||
raise LidarGroundError("Patchwork++ source commit is invalid")
|
||||
if source_tag != PATCHWORKPP_SOURCE_TAG:
|
||||
raise LidarGroundError("Patchwork++ source tag is not admitted")
|
||||
module_path_value = getattr(module, "__file__", None)
|
||||
if not isinstance(module_path_value, str):
|
||||
raise LidarGroundError("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: GroundBenchmarkProfile = 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 LidarGroundError("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 LidarGroundError("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 LidarGroundBenchmarkV1:
|
||||
|
||||
Reference in New Issue
Block a user