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

feat(lidar): add point-aligned ground review

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-25 09:44:38 +03:00
parent 75a3e669d9
commit 2dfb34ef21
15 changed files with 1548 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/compute/__init__.py
+4
View File
@@ -74,6 +74,7 @@ from .lidar_ground import (
LIDAR_GROUND_ANNOTATION_TEMPLATE_SCHEMA,
LIDAR_GROUND_BENCHMARK_REPORT_SCHEMA,
LIDAR_GROUND_BENCHMARK_SCHEMA,
LIDAR_GROUND_FRAME_SCHEMA,
PATCHWORKPP_SOURCE_COMMIT,
PATCHWORKPP_SOURCE_TAG,
PATCHWORKPP_SOURCE_URL,
@@ -86,6 +87,7 @@ from .lidar_ground import (
build_lidar_ground_annotation_template,
build_lidar_ground_benchmark,
lidar_ground_benchmark_catalog_item,
lidar_ground_frame_detail,
score_ground_labels,
)
from .lidar_replay import (
@@ -176,6 +178,7 @@ __all__ = [
"LIDAR_GROUND_ANNOTATION_TEMPLATE_SCHEMA",
"LIDAR_GROUND_BENCHMARK_REPORT_SCHEMA",
"LIDAR_GROUND_BENCHMARK_SCHEMA",
"LIDAR_GROUND_FRAME_SCHEMA",
"LIDAR_EVIDENCE_PROFILE_SCHEMA",
"LIDAR_EQUIVALENCE_REPORT_SCHEMA",
"LIDAR_QUALITY_REPORT_SCHEMA",
@@ -258,6 +261,7 @@ __all__ = [
"build_lidar_replay_pack_v2",
"build_lidar_ground_annotation_template",
"build_lidar_ground_benchmark",
"lidar_ground_frame_detail",
"DetectionFrame",
"ObjectDetection",
"RecordedPerceptionOverlayError",
src/k1link/compute/lidar_ground.py
+190 -92
View File
@@ -23,25 +23,21 @@ from .lidar_contract import LidarContractError, sensor_frame_xyzi
from .lidar_replay import LidarReplayPackV2
LIDAR_GROUND_BENCHMARK_SCHEMA: Final = "missioncore.lidar-ground-benchmark/v1"
LIDAR_GROUND_BENCHMARK_REPORT_SCHEMA: Final = (
"missioncore.lidar-ground-benchmark-report/v1"
)
LIDAR_GROUND_ANNOTATION_TEMPLATE_SCHEMA: Final = (
"missioncore.lidar-ground-annotation-template/v1"
)
LIDAR_GROUND_BENCHMARK_REPORT_SCHEMA: Final = "missioncore.lidar-ground-benchmark-report/v1"
LIDAR_GROUND_ANNOTATION_TEMPLATE_SCHEMA: Final = "missioncore.lidar-ground-annotation-template/v1"
LIDAR_GROUND_FRAME_SCHEMA: Final = "missioncore.lidar-ground-frame/v1"
LIDAR_GROUND_RESULTS_NAME: Final = "ground-results.npz"
LIDAR_GROUND_REPORT_NAME: Final = "ground-report.json"
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}$"
)
_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}$")
@@ -64,9 +60,54 @@ class GroundBenchmarkProfile:
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",
@@ -88,6 +129,8 @@ class GroundBenchmarkProfile:
"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,
@@ -97,7 +140,9 @@ class GroundBenchmarkProfile:
"input_normalization": {
"current": "vendor-map-xyz",
"candidate": "best-effort-map-to-lidar-pose-inversion",
"physical_sensor_height_known": False,
"physical_sensor_height_known": (
self.patchwork_height_evidence == "runtime-calibrated"
),
"sensor_scan_geometry_known": False,
},
"authority": {
@@ -146,9 +191,7 @@ class LocalPercentileGroundSegmenter:
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)
)
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)
@@ -172,9 +215,8 @@ class LocalPercentileGroundSegmenter:
)
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)
)
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,
@@ -235,9 +277,7 @@ class PatchworkPPGroundSegmenter:
try:
module = importlib.import_module(module_name)
except ImportError as exc:
raise LidarGroundError(
"Pinned Patchwork++ Python binding is unavailable"
) from exc
raise LidarGroundError("Pinned Patchwork++ Python binding is unavailable") from exc
return cls(
module,
profile,
@@ -295,8 +335,7 @@ class LidarGroundBenchmarkV1:
self.manifest.get("schema_version") != LIDAR_GROUND_BENCHMARK_SCHEMA
or self.identity.get("schema_version") != LIDAR_GROUND_BENCHMARK_SCHEMA
or not isinstance(identity_sha256, str)
or hashlib.sha256(_canonical_json(self.identity)).hexdigest()
!= identity_sha256
or hashlib.sha256(_canonical_json(self.identity)).hexdigest() != identity_sha256
or self.root.name != f"ground-benchmark-{identity_sha256}"
or self.manifest.get("benchmark_id") != self.root.name
):
@@ -313,19 +352,15 @@ class LidarGroundBenchmarkV1:
labels = _object(self.report.get("labels"), "ground labels")
decision = _object(self.report.get("decision"), "ground decision")
if (
_ground_logical_sha256(self.arrays)
!= self.identity.get("logical_results_sha256")
or self.report.get("schema_version")
!= LIDAR_GROUND_BENCHMARK_REPORT_SCHEMA
_ground_logical_sha256(self.arrays) != self.identity.get("logical_results_sha256")
or self.report.get("schema_version") != LIDAR_GROUND_BENCHMARK_REPORT_SCHEMA
or self.report.get("benchmark_id") != self.root.name
or self.report.get("replay_pack_id")
!= self.identity.get("replay_pack_id")
or self.report.get("replay_pack_id") != self.identity.get("replay_pack_id")
or self.report.get("status") != "diagnostic-only"
or input_domain.get("accepted") is not False
or labels.get("status") != "missing-independent-review"
or labels.get("metrics_available") is not False
or decision.get("status")
!= "do-not-promote-on-current-vendor-map"
or decision.get("status") != "do-not-promote-on-current-vendor-map"
or decision.get("production_promotion") is not False
):
raise LidarGroundError("Ground benchmark report is incompatible")
@@ -370,14 +405,9 @@ def build_lidar_ground_benchmark(
)
for frame_index in range(replay.point_frame_count):
point = replay.point_frame(frame_index)
nearest_pose_index = int(
np.argmin(np.abs(pose_times - point.received_monotonic_ns))
)
nearest_pose_index = int(np.argmin(np.abs(pose_times - point.received_monotonic_ns)))
pose = replay.pose_frame(nearest_pose_index)
delta_ms = (
abs(pose.received_monotonic_ns - point.received_monotonic_ns)
/ 1_000_000
)
delta_ms = abs(pose.received_monotonic_ns - point.received_monotonic_ns) / 1_000_000
if delta_ms > profile.pose_binding_threshold_ms:
raise LidarGroundError("Ground A/B point frame has no admitted pose")
try:
@@ -387,6 +417,9 @@ def build_lidar_ground_benchmark(
)
except LidarContractError as exc:
raise LidarGroundError("Ground A/B sensor conversion failed") from exc
if profile.patchwork_map_vertical_origin_offset_m:
candidate_input = candidate_input.copy()
candidate_input[:, 2] -= profile.patchwork_map_vertical_origin_offset_m
current_input = np.empty((point.xyz_map.shape[0], 4), dtype=np.float32)
current_input[:, :3] = point.xyz_map.astype(np.float32)
current_input[:, 3] = point.intensity.astype(np.float32) / 255.0
@@ -404,26 +437,14 @@ def build_lidar_ground_benchmark(
pose_delta_ms.append(delta_ms)
current_fraction.append(float(np.mean(current_result.ground_mask)))
candidate_fraction.append(float(np.mean(candidate_result.ground_mask)))
candidate_assigned_fraction.append(
float(np.mean(candidate_result.assigned_mask))
)
candidate_assigned_fraction.append(float(np.mean(candidate_result.assigned_mask)))
intersection = int(
np.count_nonzero(
current_result.ground_mask & candidate_result.ground_mask
)
)
union = int(
np.count_nonzero(
current_result.ground_mask | candidate_result.ground_mask
)
np.count_nonzero(current_result.ground_mask & candidate_result.ground_mask)
)
union = int(np.count_nonzero(current_result.ground_mask | candidate_result.ground_mask))
inter_provider_iou.append(float(intersection / union) if union else 1.0)
disagreement_fraction.append(
float(
np.mean(
current_result.ground_mask != candidate_result.ground_mask
)
)
float(np.mean(current_result.ground_mask != candidate_result.ground_mask))
)
arrays: dict[str, npt.NDArray[Any]] = {
@@ -477,11 +498,19 @@ def build_lidar_ground_benchmark(
"input_domain": {
"accepted": False,
"representation": replay.profile.representation.value,
"physical_sensor_height_known": False,
"physical_sensor_height_known": (
profile.patchwork_height_evidence == "runtime-calibrated"
),
"sensor_scan_geometry_known": replay.profile.scan_geometry_known,
"normalization": {
"sensor_height_m": profile.patchwork_sensor_height_proxy_m,
"map_vertical_origin_offset_m": (profile.patchwork_map_vertical_origin_offset_m),
"height_evidence": profile.patchwork_height_evidence,
},
"reason": (
"Patchwork++ expects a sensor-centric scan and physical sensor "
"height; K1 lio_pcl is a vendor-mapped increment."
"Patchwork++ expects a sensor-centric scan; K1 lio_pcl is an "
"externally downsampled LIO/map product. Height correction does "
"not admit the external feed as a raw MID-360 scan."
),
},
"labels": {
@@ -498,9 +527,7 @@ def build_lidar_ground_benchmark(
"current": {
"provider": dict(current.identity),
"ground_fraction": _distribution(current_fraction),
"assigned_fraction": _distribution(
[1.0] * replay.point_frame_count
),
"assigned_fraction": _distribution([1.0] * replay.point_frame_count),
"latency_ms": _distribution(current_latency),
},
"candidate": {
@@ -510,12 +537,8 @@ def build_lidar_ground_benchmark(
"latency_ms": _distribution(candidate_latency),
},
"comparison": {
"algorithm_to_algorithm_ground_iou": _distribution(
inter_provider_iou
),
"ground_disagreement_fraction": _distribution(
disagreement_fraction
),
"algorithm_to_algorithm_ground_iou": _distribution(inter_provider_iou),
"ground_disagreement_fraction": _distribution(disagreement_fraction),
"is_accuracy_metric": False,
},
"decision": {
@@ -596,10 +619,7 @@ def build_lidar_ground_annotation_template(
)
source_offsets = np.asarray(replay.arrays["point_offsets"], dtype=np.int64)
selected_counts = np.asarray(
[
int(source_offsets[index + 1] - source_offsets[index])
for index in selected
],
[int(source_offsets[index + 1] - source_offsets[index]) for index in selected],
dtype="<i8",
)
selected_offsets = np.concatenate(
@@ -739,6 +759,101 @@ def lidar_ground_benchmark_catalog_item(
}
def lidar_ground_frame_detail(
benchmark: LidarGroundBenchmarkV1,
replay: LidarReplayPackV2,
frame_index: int,
) -> dict[str, object]:
"""Return one bounded, point-aligned frame for browser diagnostic review."""
if (
benchmark.identity.get("replay_pack_id") != replay.pack_id
or benchmark.identity.get("replay_logical_content_sha256")
!= replay.identity.get("logical_content_sha256")
or benchmark.identity.get("point_frames") != replay.point_frame_count
or benchmark.identity.get("points") != replay.point_count
):
raise LidarGroundError("Ground benchmark is not bound to this replay pack")
if not 0 <= frame_index < replay.point_frame_count:
raise IndexError(frame_index)
benchmark_offsets = np.asarray(
benchmark.arrays["point_offsets"],
dtype=np.int64,
)
replay_offsets = np.asarray(replay.arrays["point_offsets"], dtype=np.int64)
if not np.array_equal(benchmark_offsets, replay_offsets):
raise LidarGroundError("Ground benchmark point offsets changed")
start = int(benchmark_offsets[frame_index])
end = int(benchmark_offsets[frame_index + 1])
point_count = end - start
if not 0 < point_count <= MAX_GROUND_FRAME_POINTS:
raise LidarGroundError("Ground frame point count exceeds viewer limit")
frame = replay.point_frame(frame_index)
capture_sequence = int(benchmark.arrays["point_capture_sequence"][frame_index])
if capture_sequence != frame.capture_sequence:
raise LidarGroundError("Ground frame capture sequence changed")
xyz = np.asarray(frame.xyz_map, dtype=np.float64)
intensity = np.asarray(frame.intensity, dtype=np.uint8)
current_ground = np.asarray(
benchmark.arrays["current_ground"][start:end],
dtype=np.uint8,
)
current_assigned = np.asarray(
benchmark.arrays["current_assigned"][start:end],
dtype=np.uint8,
)
candidate_ground = np.asarray(
benchmark.arrays["candidate_ground"][start:end],
dtype=np.uint8,
)
candidate_assigned = np.asarray(
benchmark.arrays["candidate_assigned"][start:end],
dtype=np.uint8,
)
disagreement = (current_ground != candidate_ground).astype(np.uint8)
if (
xyz.shape != (point_count, 3)
or intensity.shape != (point_count,)
or not np.isfinite(xyz).all()
):
raise LidarGroundError("Ground frame replay content is incompatible")
return {
"schema_version": LIDAR_GROUND_FRAME_SCHEMA,
"benchmark_id": benchmark.benchmark_id,
"replay_pack_id": replay.pack_id,
"session_id": replay.identity["session_id"],
"frame_index": frame_index,
"frame_count": replay.point_frame_count,
"capture_sequence": capture_sequence,
"point_count": point_count,
"coordinate_frame": "map",
"distance_unit": "m",
"points_xyz_m": xyz.tolist(),
"intensity_0_255": intensity.astype(np.int64).tolist(),
"masks": {
"current_ground": current_ground.astype(np.int64).tolist(),
"current_assigned": current_assigned.astype(np.int64).tolist(),
"candidate_ground": candidate_ground.astype(np.int64).tolist(),
"candidate_assigned": candidate_assigned.astype(np.int64).tolist(),
"disagreement": disagreement.astype(np.int64).tolist(),
},
"counts": {
"current_ground": int(np.count_nonzero(current_ground)),
"candidate_ground": int(np.count_nonzero(candidate_ground)),
"disagreement": int(np.count_nonzero(disagreement)),
},
"access": "read-only",
"ground_truth": False,
"authority": {
"commands_enabled": False,
"navigation_or_safety_accepted": False,
},
}
def _validate_annotation_template(root: Path) -> None:
resolved = root.resolve(strict=True)
if (
@@ -752,8 +867,7 @@ def _validate_annotation_template(root: Path) -> None:
identity_sha256 = manifest.get("identity_sha256")
if (
manifest.get("schema_version") != LIDAR_GROUND_ANNOTATION_TEMPLATE_SCHEMA
or identity.get("schema_version")
!= LIDAR_GROUND_ANNOTATION_TEMPLATE_SCHEMA
or identity.get("schema_version") != LIDAR_GROUND_ANNOTATION_TEMPLATE_SCHEMA
or not isinstance(identity_sha256, str)
or hashlib.sha256(_canonical_json(identity)).hexdigest() != identity_sha256
or resolved.name != f"ground-annotation-template-{identity_sha256}"
@@ -774,8 +888,7 @@ def _validate_annotation_template(root: Path) -> None:
set(arrays.files) != required
or arrays["labels"].dtype != np.dtype("u1")
or np.any(arrays["labels"] != 0)
or _ground_logical_sha256(arrays)
!= identity.get("logical_content_sha256")
or _ground_logical_sha256(arrays) != identity.get("logical_content_sha256")
):
raise LidarGroundError("Ground annotation template content is invalid")
finally:
@@ -836,13 +949,8 @@ def _xyzi(value: npt.NDArray[np.float32]) -> npt.NDArray[np.float32]:
def _indices(value: npt.NDArray[np.int64], count: int, label: str) -> None:
if (
value.size
and (
np.any(value < 0)
or np.any(value >= count)
or np.unique(value).shape[0] != value.shape[0]
)
if value.size and (
np.any(value < 0) or np.any(value >= count) or np.unique(value).shape[0] != value.shape[0]
):
raise LidarGroundError(f"{label} indices are invalid")
@@ -889,11 +997,7 @@ def _ratio(numerator: int, denominator: int) -> float | None:
def _nonnegative_int(value: object, label: str) -> int:
if (
not isinstance(value, int)
or isinstance(value, bool)
or value < 0
):
if not isinstance(value, int) or isinstance(value, bool) or value < 0:
raise LidarGroundError(f"{label} must be a non-negative integer")
return value
@@ -983,9 +1087,7 @@ def _validate_artifacts(
def _object(value: object, label: str) -> dict[str, Any]:
if not isinstance(value, dict) or not all(
isinstance(key, str) for key in value
):
if not isinstance(value, dict) or not all(isinstance(key, str) for key in value):
raise LidarGroundError(f"{label} must be an object")
return value
@@ -1021,8 +1123,4 @@ def _sha256(path: Path) -> str:
def _utc_now() -> str:
return (
datetime.now(UTC)
.isoformat(timespec="milliseconds")
.replace("+00:00", "Z")
)
return datetime.now(UTC).isoformat(timespec="milliseconds").replace("+00:00", "Z")