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

feat: qualify bounded K1 surface shadow

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-26 01:06:04 +03:00
parent 4c83e8a4e7
commit e6d5411bdd
8 changed files with 1370 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/compute/__init__.py
+14
View File
@@ -118,6 +118,14 @@ from .lidar_local_surface import (
build_k1_local_surface,
k1_local_surface_catalog_item,
)
from .lidar_local_surface_shadow import (
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
K1_LOCAL_SURFACE_SHADOW_SCHEMA,
K1LocalSurfaceShadowEstimator,
K1LocalSurfaceShadowInput,
K1LocalSurfaceShadowResult,
K1LocalSurfaceShadowRuntime,
)
from .lidar_replay import (
LIDAR_EQUIVALENCE_REPORT_SCHEMA,
LIDAR_QUALITY_REPORT_SCHEMA,
@@ -213,6 +221,8 @@ __all__ = [
"K1_LOCAL_SURFACE_REVIEW_SCHEMA",
"K1_LOCAL_SURFACE_SCHEMA",
"K1_LOCAL_SURFACE_TIMELINE_SCHEMA",
"K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA",
"K1_LOCAL_SURFACE_SHADOW_SCHEMA",
"LIDAR_FIELD_REVIEW_REPORT_SCHEMA",
"LIDAR_FIELD_REVIEW_SCHEMA",
"LIDAR_FIELD_REVIEW_WINDOW_SCHEMA",
@@ -236,6 +246,10 @@ __all__ = [
"LidarGroundBenchmarkV1",
"LidarGroundError",
"K1LocalSurfaceProfile",
"K1LocalSurfaceShadowEstimator",
"K1LocalSurfaceShadowInput",
"K1LocalSurfaceShadowResult",
"K1LocalSurfaceShadowRuntime",
"K1LocalSurfaceV1",
"LidarReplayError",
"LidarReplayPackV2",
src/k1link/compute/lidar_local_surface_shadow.py
+681
View File
@@ -0,0 +1,681 @@
from __future__ import annotations
import math
import threading
import time
from collections import Counter, deque
from dataclasses import asdict, dataclass
from typing import Any, Final, Literal
import numpy as np
import numpy.typing as npt
from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError
from .lidar_local_surface import (
DEFAULT_K1_LOCAL_SURFACE_PROFILE,
POINT_BELOW_SURFACE,
POINT_OCCUPIED,
POINT_SURFACE,
K1LocalSurfaceProfile,
_cloud_cell_observations,
_expire_cache,
_fit_surface,
_height_above_plane,
_local_cache_records,
_point_step_candidates,
_prediction_metrics,
_step_candidate_keys,
_update_cache,
)
from .live_perception import LatestWinsQueue
K1_LOCAL_SURFACE_SHADOW_SCHEMA: Final = "missioncore.k1-local-surface-shadow-runtime/v1"
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-shadow-frame/v1"
ShadowFrameState = Literal[
"valid",
"pose-stale",
"insufficient-surface",
"fit-failed",
]
@dataclass(frozen=True, slots=True)
class K1LocalSurfaceShadowInput:
"""One immutable map-point/pose pair admitted to passive shadow work."""
frame_index: int
source_frame_index: int
session_seconds: float
pose_binding_age_ms: float
points_map: npt.NDArray[np.float64]
position_map: npt.NDArray[np.float64]
published_monotonic_ns: int
def __post_init__(self) -> None:
points = np.array(self.points_map, dtype=np.float64, copy=True)
position = np.array(self.position_map, dtype=np.float64, copy=True)
if (
self.frame_index < 0
or self.source_frame_index < 0
or not math.isfinite(self.session_seconds)
or self.session_seconds < 0
or not math.isfinite(self.pose_binding_age_ms)
or self.pose_binding_age_ms < 0
or self.published_monotonic_ns < 0
or points.ndim != 2
or points.shape[1:] != (3,)
or position.shape != (3,)
or not np.isfinite(points).all()
or not np.isfinite(position).all()
):
raise LidarGroundError("K1 local-surface shadow input is invalid")
points.flags.writeable = False
position.flags.writeable = False
object.__setattr__(self, "points_map", points)
object.__setattr__(self, "position_map", position)
@classmethod
def from_views(
cls,
point_cloud: DecodedPointCloudView,
pose: DecodedPoseView,
) -> K1LocalSurfaceShadowInput:
"""Bind already-normalized views without interpreting vendor payloads."""
if (
point_cloud.frame_id != "map"
or pose.frame_id != "map"
or pose.child_frame_id != "sensor"
):
raise LidarGroundError(
"K1 local-surface shadow requires map points and map-from-sensor pose"
)
point_received_ns = point_cloud.context.received_monotonic_ns
pose_received_ns = pose.context.received_monotonic_ns
if point_received_ns is not None and pose_received_ns is not None:
pose_binding_age_ms = abs(point_received_ns - pose_received_ns) / 1_000_000
else:
pose_binding_age_ms = (
abs(point_cloud.context.captured_at_epoch_ns - pose.context.captured_at_epoch_ns)
/ 1_000_000
)
points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
position = np.asarray(pose.position_xyz, dtype=np.float64)
return cls(
frame_index=point_cloud.context.sequence,
source_frame_index=point_cloud.context.sequence,
session_seconds=point_cloud.context.captured_at_epoch_ns / 1_000_000_000,
pose_binding_age_ms=pose_binding_age_ms,
points_map=points,
position_map=position,
published_monotonic_ns=time.monotonic_ns(),
)
@dataclass(frozen=True, slots=True)
class K1LocalSurfaceShadowResult:
"""One bounded diagnostic result; it never describes free or safe space."""
frame_index: int
source_frame_index: int
session_seconds: float
state: ShadowFrameState
pose_binding_age_ms: float
surface_cell_count: int
surface_inlier_cell_count: int
plane_coefficients_map: tuple[float, float, float, float] | None
sensor_height_m: float | None
slope_deg: float | None
roughness_m: float | None
confidence: float | None
surface_max_age_ms: float | None
prediction_available: bool
prediction_cell_count: int
prediction_residual_p50_m: float | None
prediction_residual_p95_m: float | None
prediction_inlier_fraction: float | None
temporal_compared: bool
temporal_jump: bool
height_delta_m: float | None
slope_delta_deg: float | None
roughness_delta_m: float | None
surface_point_count: int
occupied_point_count: int
below_surface_point_count: int
step_candidate_point_count: int
processing_ms: float
result_age_ms: float
point_class: npt.NDArray[np.uint8]
point_height_m: npt.NDArray[np.float32]
point_step_candidate: npt.NDArray[np.uint8]
@property
def valid(self) -> bool:
return self.state == "valid"
def document(self) -> dict[str, object]:
return {
"schema_version": K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
"frame_index": self.frame_index,
"source_frame_index": self.source_frame_index,
"session_seconds": self.session_seconds,
"state": self.state,
"valid": self.valid,
"surface": {
"pose_binding_age_ms": self.pose_binding_age_ms,
"cell_count": self.surface_cell_count,
"inlier_cell_count": self.surface_inlier_cell_count,
"plane_coefficients_map": (
list(self.plane_coefficients_map)
if self.plane_coefficients_map is not None
else None
),
"sensor_height_m": self.sensor_height_m,
"slope_deg": self.slope_deg,
"roughness_m": self.roughness_m,
"confidence": self.confidence,
"maximum_age_ms": self.surface_max_age_ms,
},
"prediction": {
"available": self.prediction_available,
"cell_count": self.prediction_cell_count,
"residual_p50_m": self.prediction_residual_p50_m,
"residual_p95_m": self.prediction_residual_p95_m,
"inlier_fraction": self.prediction_inlier_fraction,
"current_frame_excluded": True,
},
"temporal": {
"compared": self.temporal_compared,
"jump": self.temporal_jump,
"height_delta_m": self.height_delta_m,
"slope_delta_deg": self.slope_delta_deg,
"roughness_delta_m": self.roughness_delta_m,
},
"counts": {
"surface": self.surface_point_count,
"occupied_observed": self.occupied_point_count,
"below_surface": self.below_surface_point_count,
"step_candidate": self.step_candidate_point_count,
},
"delivery": {
"processing_ms": self.processing_ms,
"result_age_ms": self.result_age_ms,
},
"ground_truth": False,
"occupancy_policy": {
"absence_of_points_means_free": False,
"unknown_is_traversable": False,
},
"authority": {
"commands_enabled": False,
"navigation_or_safety_accepted": False,
},
}
class K1LocalSurfaceShadowEstimator:
"""Streaming-equivalent state for the accepted replay surface profile."""
def __init__(
self,
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE,
) -> None:
self.profile = profile
self._cache: dict[tuple[int, int], tuple[float, float]] = {}
self._previous_surface: tuple[float, float, float, float] | None = None
self._last_frame_index = -1
self._last_session_seconds = -math.inf
def process(
self,
value: K1LocalSurfaceShadowInput,
) -> K1LocalSurfaceShadowResult:
started_ns = time.monotonic_ns()
if (
value.frame_index <= self._last_frame_index
or value.session_seconds < self._last_session_seconds
):
raise LidarGroundError("K1 local-surface shadow input order is not monotonic")
self._last_frame_index = value.frame_index
self._last_session_seconds = value.session_seconds
if value.pose_binding_age_ms > self.profile.maximum_pose_binding_ms:
return self._result(
value,
started_ns=started_ns,
state="pose-stale",
)
cloud = value.points_map
position = value.position_map
local = np.sum((cloud[:, :2] - position[:2]) ** 2, axis=1) <= (
self.profile.local_radius_m**2
)
local_cloud = cloud[local]
_expire_cache(
self._cache,
value.session_seconds,
position,
self.profile,
)
_, prior_cell_points, _ = _local_cache_records(
self._cache,
position,
self.profile,
)
_, current_cell_points = _cloud_cell_observations(
local_cloud,
self.profile,
)
prediction = _prediction_metrics(
prior_cell_points,
current_cell_points,
position,
self.profile,
)
_update_cache(
self._cache,
local_cloud,
value.session_seconds,
self.profile,
)
cell_keys, cell_points, cell_times = _local_cache_records(
self._cache,
position,
self.profile,
)
prediction_available = prediction is not None
prediction_cell_count = prediction.cell_points.shape[0] if prediction is not None else 0
prediction_residual_p50_m = prediction.residual_p50_m if prediction is not None else None
prediction_residual_p95_m = prediction.residual_p95_m if prediction is not None else None
prediction_inlier_fraction = (
prediction.inlier_fraction(self.profile.surface_band_m)
if prediction is not None
else None
)
if cell_points.shape[0] < self.profile.minimum_surface_cells:
return self._result(
value,
started_ns=started_ns,
state="insufficient-surface",
surface_cell_count=cell_points.shape[0],
prediction_available=prediction_available,
prediction_cell_count=prediction_cell_count,
prediction_residual_p50_m=prediction_residual_p50_m,
prediction_residual_p95_m=prediction_residual_p95_m,
prediction_inlier_fraction=prediction_inlier_fraction,
)
fit = _fit_surface(cell_points, position, self.profile)
if fit is None:
return self._result(
value,
started_ns=started_ns,
state="fit-failed",
surface_cell_count=cell_points.shape[0],
prediction_available=prediction_available,
prediction_cell_count=prediction_cell_count,
prediction_residual_p50_m=prediction_residual_p50_m,
prediction_residual_p95_m=prediction_residual_p95_m,
prediction_inlier_fraction=prediction_inlier_fraction,
)
plane, inliers, residuals = fit
slope_deg = math.degrees(
math.atan2(
math.hypot(float(plane[0]), float(plane[1])),
float(plane[2]),
)
)
if not np.isfinite(slope_deg) or slope_deg > self.profile.maximum_slope_deg:
return self._result(
value,
started_ns=started_ns,
state="fit-failed",
surface_cell_count=cell_points.shape[0],
prediction_available=prediction_available,
prediction_cell_count=prediction_cell_count,
prediction_residual_p50_m=prediction_residual_p50_m,
prediction_residual_p95_m=prediction_residual_p95_m,
prediction_inlier_fraction=prediction_inlier_fraction,
)
heights = _height_above_plane(cloud, plane)
point_class = np.zeros(cloud.shape[0], dtype=np.uint8)
point_class[local & (np.abs(heights) <= self.profile.surface_band_m)] = POINT_SURFACE
point_class[
local
& (heights >= self.profile.obstacle_min_height_m)
& (heights <= self.profile.obstacle_max_height_m)
] = POINT_OCCUPIED
point_class[local & (heights < -self.profile.surface_band_m)] = POINT_BELOW_SURFACE
step_keys = _step_candidate_keys(
cell_keys,
cell_points,
plane,
self.profile,
)
point_step_candidate = _point_step_candidates(
cloud,
local,
heights,
step_keys,
self.profile,
)
point_height_m = np.zeros(cloud.shape[0], dtype=np.float32)
point_height_m[local] = heights[local].astype(np.float32)
sensor_height = float(_height_above_plane(position.reshape(1, 3), plane)[0])
roughness = float(np.median(np.abs(residuals[inliers])))
inlier_count = int(np.count_nonzero(inliers))
coverage = min(
1.0,
inlier_count / (self.profile.minimum_surface_cells * 3),
)
roughness_confidence = math.exp(-roughness / max(self.profile.surface_band_m, 1e-6))
pose_confidence = max(
0.0,
1.0 - value.pose_binding_age_ms / self.profile.maximum_pose_binding_ms,
)
confidence = float(
np.clip(
coverage * roughness_confidence * pose_confidence,
0.0,
1.0,
)
)
temporal_compared = False
temporal_jump = False
height_delta_m: float | None = None
slope_delta_deg: float | None = None
roughness_delta_m: float | None = None
if (
self._previous_surface is not None
and value.session_seconds - self._previous_surface[0] <= self.profile.surface_ttl_s
):
temporal_compared = True
height_delta_m = abs(sensor_height - self._previous_surface[1])
slope_delta_deg = abs(slope_deg - self._previous_surface[2])
roughness_delta_m = abs(roughness - self._previous_surface[3])
temporal_jump = (
height_delta_m > self.profile.temporal_height_jump_m
or slope_delta_deg > self.profile.temporal_slope_jump_deg
or roughness_delta_m > self.profile.temporal_roughness_jump_m
)
self._previous_surface = (
value.session_seconds,
sensor_height,
slope_deg,
roughness,
)
point_class.flags.writeable = False
point_height_m.flags.writeable = False
point_step_candidate.flags.writeable = False
return self._result(
value,
started_ns=started_ns,
state="valid",
surface_cell_count=cell_points.shape[0],
surface_inlier_cell_count=inlier_count,
plane_coefficients_map=(
float(plane[0]),
float(plane[1]),
float(plane[2]),
float(plane[3]),
),
sensor_height_m=sensor_height,
slope_deg=slope_deg,
roughness_m=roughness,
confidence=confidence,
surface_max_age_ms=max(
0.0,
(value.session_seconds - float(np.min(cell_times[inliers]))) * 1_000.0,
),
temporal_compared=temporal_compared,
temporal_jump=temporal_jump,
height_delta_m=height_delta_m,
slope_delta_deg=slope_delta_deg,
roughness_delta_m=roughness_delta_m,
surface_point_count=int(np.count_nonzero(point_class == POINT_SURFACE)),
occupied_point_count=int(np.count_nonzero(point_class == POINT_OCCUPIED)),
below_surface_point_count=int(np.count_nonzero(point_class == POINT_BELOW_SURFACE)),
step_candidate_point_count=int(np.count_nonzero(point_step_candidate)),
point_class=point_class,
point_height_m=point_height_m,
point_step_candidate=point_step_candidate,
prediction_available=prediction_available,
prediction_cell_count=prediction_cell_count,
prediction_residual_p50_m=prediction_residual_p50_m,
prediction_residual_p95_m=prediction_residual_p95_m,
prediction_inlier_fraction=prediction_inlier_fraction,
)
def _result(
self,
value: K1LocalSurfaceShadowInput,
*,
started_ns: int,
state: ShadowFrameState,
surface_cell_count: int = 0,
surface_inlier_cell_count: int = 0,
plane_coefficients_map: tuple[float, float, float, float] | None = None,
sensor_height_m: float | None = None,
slope_deg: float | None = None,
roughness_m: float | None = None,
confidence: float | None = None,
surface_max_age_ms: float | None = None,
prediction_available: bool = False,
prediction_cell_count: int = 0,
prediction_residual_p50_m: float | None = None,
prediction_residual_p95_m: float | None = None,
prediction_inlier_fraction: float | None = None,
temporal_compared: bool = False,
temporal_jump: bool = False,
height_delta_m: float | None = None,
slope_delta_deg: float | None = None,
roughness_delta_m: float | None = None,
surface_point_count: int = 0,
occupied_point_count: int = 0,
below_surface_point_count: int = 0,
step_candidate_point_count: int = 0,
point_class: npt.NDArray[np.uint8] | None = None,
point_height_m: npt.NDArray[np.float32] | None = None,
point_step_candidate: npt.NDArray[np.uint8] | None = None,
) -> K1LocalSurfaceShadowResult:
finished_ns = time.monotonic_ns()
if point_class is None:
point_class = np.zeros(value.points_map.shape[0], dtype=np.uint8)
point_class.flags.writeable = False
if point_height_m is None:
point_height_m = np.zeros(value.points_map.shape[0], dtype=np.float32)
point_height_m.flags.writeable = False
if point_step_candidate is None:
point_step_candidate = np.zeros(
value.points_map.shape[0],
dtype=np.uint8,
)
point_step_candidate.flags.writeable = False
return K1LocalSurfaceShadowResult(
frame_index=value.frame_index,
source_frame_index=value.source_frame_index,
session_seconds=value.session_seconds,
state=state,
pose_binding_age_ms=value.pose_binding_age_ms,
surface_cell_count=surface_cell_count,
surface_inlier_cell_count=surface_inlier_cell_count,
plane_coefficients_map=plane_coefficients_map,
sensor_height_m=sensor_height_m,
slope_deg=slope_deg,
roughness_m=roughness_m,
confidence=confidence,
surface_max_age_ms=surface_max_age_ms,
prediction_available=prediction_available,
prediction_cell_count=prediction_cell_count,
prediction_residual_p50_m=prediction_residual_p50_m,
prediction_residual_p95_m=prediction_residual_p95_m,
prediction_inlier_fraction=prediction_inlier_fraction,
temporal_compared=temporal_compared,
temporal_jump=temporal_jump,
height_delta_m=height_delta_m,
slope_delta_deg=slope_delta_deg,
roughness_delta_m=roughness_delta_m,
surface_point_count=surface_point_count,
occupied_point_count=occupied_point_count,
below_surface_point_count=below_surface_point_count,
step_candidate_point_count=step_candidate_point_count,
processing_ms=(finished_ns - started_ns) / 1_000_000,
result_age_ms=max(
0.0,
(finished_ns - value.published_monotonic_ns) / 1_000_000,
),
point_class=point_class,
point_height_m=point_height_m,
point_step_candidate=point_step_candidate,
)
class K1LocalSurfaceShadowRuntime:
"""One bounded latest-wins worker with a bounded diagnostic result ring."""
def __init__(
self,
session_id: str,
*,
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE,
queue_capacity: int = 2,
result_capacity: int = 8,
) -> None:
if (
not session_id
or len(session_id) > 160
or not 1 <= queue_capacity <= 8
or not 1 <= result_capacity <= 256
):
raise LidarGroundError("K1 local-surface shadow runtime bounds are invalid")
self.session_id = session_id
self.profile = profile
self._queue = LatestWinsQueue[K1LocalSurfaceShadowInput](queue_capacity)
self._result_capacity = result_capacity
self._results: deque[K1LocalSurfaceShadowResult] = deque(maxlen=result_capacity)
self._result_dropped = 0
self._processed = 0
self._failed = 0
self._state_counts: Counter[str] = Counter()
self._last_error: str | None = None
self._inflight = False
self._condition = threading.Condition()
self._closed = False
self._estimator = K1LocalSurfaceShadowEstimator(profile)
self._thread = threading.Thread(
target=self._run,
name=f"k1-local-surface-shadow-{session_id}",
daemon=True,
)
self._thread.start()
def publish(self, value: K1LocalSurfaceShadowInput) -> None:
with self._condition:
if self._closed:
raise RuntimeError("K1 local-surface shadow runtime is closed")
self._queue.publish(value)
def publish_views(
self,
point_cloud: DecodedPointCloudView,
pose: DecodedPoseView,
) -> None:
self.publish(K1LocalSurfaceShadowInput.from_views(point_cloud, pose))
def wait_until_idle(self, timeout_seconds: float) -> bool:
if timeout_seconds < 0:
raise ValueError("K1 local-surface shadow wait timeout is invalid")
deadline = time.monotonic() + timeout_seconds
while True:
queue_snapshot = self._queue.snapshot()
with self._condition:
accounted = (
queue_snapshot.consumed + queue_snapshot.dropped_overflow
== queue_snapshot.published
)
if accounted and queue_snapshot.depth == 0 and not self._inflight:
return True
remaining = deadline - time.monotonic()
if remaining <= 0:
return False
self._condition.wait(timeout=remaining)
def close(self, *, timeout_seconds: float = 30.0) -> None:
if timeout_seconds <= 0:
raise ValueError("K1 local-surface shadow close timeout is invalid")
with self._condition:
already_closed = self._closed
self._closed = True
if not already_closed:
self._queue.close()
self._thread.join(timeout=timeout_seconds)
if self._thread.is_alive():
raise RuntimeError("K1 local-surface shadow worker did not stop")
def results(self) -> tuple[K1LocalSurfaceShadowResult, ...]:
with self._condition:
return tuple(self._results)
def snapshot(self) -> dict[str, Any]:
queue_snapshot = self._queue.snapshot()
with self._condition:
latest = self._results[-1] if self._results else None
return {
"schema_version": K1_LOCAL_SURFACE_SHADOW_SCHEMA,
"mode": "live-shadow-diagnostic-only",
"session_id": self.session_id,
"profile": self.profile.to_dict(),
"queue_policy": "bounded-latest-wins",
"queue": asdict(queue_snapshot),
"results": {
"capacity": self._result_capacity,
"depth": len(self._results),
"published": self._processed,
"dropped_ring_overflow": self._result_dropped,
"state_counts": dict(sorted(self._state_counts.items())),
"failed": self._failed,
"latest": latest.document() if latest is not None else None,
},
"last_error": self._last_error,
"closed": self._closed and not self._thread.is_alive(),
"ground_truth": False,
"occupancy_policy": {
"absence_of_points_means_free": False,
"unknown_is_traversable": False,
},
"authority": {
"commands_enabled": False,
"navigation_or_safety_accepted": False,
},
}
def _run(self) -> None:
while True:
value = self._queue.take_next()
if value is None:
with self._condition:
self._condition.notify_all()
return
with self._condition:
self._inflight = True
try:
result = self._estimator.process(value)
except (LidarGroundError, ValueError, np.linalg.LinAlgError) as exc:
with self._condition:
self._failed += 1
self._last_error = type(exc).__name__
else:
with self._condition:
if len(self._results) == self._result_capacity:
self._result_dropped += 1
self._results.append(result)
self._processed += 1
self._state_counts[result.state] += 1
finally:
with self._condition:
self._inflight = False
self._condition.notify_all()