feat: wire physical K1 surface shadow
This commit is contained in:
DCCONSTRUCTIONS
@@ -119,8 +119,12 @@ from .lidar_local_surface import (
|
||||
k1_local_surface_catalog_item,
|
||||
)
|
||||
from .lidar_local_surface_shadow import (
|
||||
K1_LOCAL_SURFACE_BINDER_SCHEMA,
|
||||
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
|
||||
K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
||||
K1LocalSurfaceBoundViews,
|
||||
K1LocalSurfacePoseBinder,
|
||||
K1LocalSurfaceShadowCoordinator,
|
||||
K1LocalSurfaceShadowEstimator,
|
||||
K1LocalSurfaceShadowInput,
|
||||
K1LocalSurfaceShadowResult,
|
||||
@@ -217,6 +221,7 @@ __all__ = [
|
||||
"LIDAR_GROUND_BENCHMARK_SCHEMA",
|
||||
"LIDAR_GROUND_FRAME_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_FRAME_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_BINDER_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_REPORT_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_REVIEW_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_SCHEMA",
|
||||
@@ -246,6 +251,9 @@ __all__ = [
|
||||
"LidarGroundBenchmarkV1",
|
||||
"LidarGroundError",
|
||||
"K1LocalSurfaceProfile",
|
||||
"K1LocalSurfaceBoundViews",
|
||||
"K1LocalSurfacePoseBinder",
|
||||
"K1LocalSurfaceShadowCoordinator",
|
||||
"K1LocalSurfaceShadowEstimator",
|
||||
"K1LocalSurfaceShadowInput",
|
||||
"K1LocalSurfaceShadowResult",
|
||||
|
||||
@@ -0,0 +1,403 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Mapping
|
||||
from dataclasses import dataclass
|
||||
from typing import Final
|
||||
|
||||
import numpy as np
|
||||
import numpy.typing as npt
|
||||
|
||||
from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError
|
||||
|
||||
POINT_UNCLASSIFIED: Final = 0
|
||||
POINT_SURFACE: Final = 1
|
||||
POINT_OCCUPIED: Final = 2
|
||||
POINT_BELOW_SURFACE: Final = 3
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class K1LocalSurfaceProfile:
|
||||
"""Dependency-light parameters shared by replay and live shadow."""
|
||||
|
||||
profile_id: str = "k1-vendor-map-dynamic-local-surface/v1"
|
||||
local_radius_m: float = 10.0
|
||||
cell_size_m: float = 0.45
|
||||
surface_ttl_s: float = 1.25
|
||||
cell_lower_percentile: float = 20.0
|
||||
initial_lower_fraction: float = 0.55
|
||||
minimum_surface_cells: int = 18
|
||||
robust_iterations: int = 5
|
||||
robust_mad_scale: float = 2.8
|
||||
minimum_inlier_band_m: float = 0.10
|
||||
surface_band_m: float = 0.16
|
||||
obstacle_min_height_m: float = 0.20
|
||||
obstacle_max_height_m: float = 3.5
|
||||
maximum_pose_binding_ms: float = 100.0
|
||||
maximum_slope_deg: float = 40.0
|
||||
step_min_height_m: float = 0.07
|
||||
step_max_height_m: float = 0.32
|
||||
step_max_plane_residual_m: float = 0.45
|
||||
temporal_height_jump_m: float = 0.03
|
||||
temporal_slope_jump_deg: float = 0.5
|
||||
temporal_roughness_jump_m: float = 0.015
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
numeric = (
|
||||
self.local_radius_m,
|
||||
self.cell_size_m,
|
||||
self.surface_ttl_s,
|
||||
self.cell_lower_percentile,
|
||||
self.initial_lower_fraction,
|
||||
self.robust_mad_scale,
|
||||
self.minimum_inlier_band_m,
|
||||
self.surface_band_m,
|
||||
self.obstacle_min_height_m,
|
||||
self.obstacle_max_height_m,
|
||||
self.maximum_pose_binding_ms,
|
||||
self.maximum_slope_deg,
|
||||
self.step_min_height_m,
|
||||
self.step_max_height_m,
|
||||
self.step_max_plane_residual_m,
|
||||
self.temporal_height_jump_m,
|
||||
self.temporal_slope_jump_deg,
|
||||
self.temporal_roughness_jump_m,
|
||||
)
|
||||
if (
|
||||
not self.profile_id.strip()
|
||||
or len(self.profile_id) > 160
|
||||
or not np.isfinite(numeric).all()
|
||||
or not 1.0 <= self.local_radius_m <= 100.0
|
||||
or not 0.05 <= self.cell_size_m <= 5.0
|
||||
or not 0.05 <= self.surface_ttl_s <= 30.0
|
||||
or not 0.0 <= self.cell_lower_percentile <= 50.0
|
||||
or not 0.05 <= self.initial_lower_fraction <= 0.95
|
||||
or not 3 <= self.minimum_surface_cells <= 100_000
|
||||
or not 1 <= self.robust_iterations <= 20
|
||||
or not 1.0 <= self.robust_mad_scale <= 10.0
|
||||
or not 0.01 <= self.minimum_inlier_band_m <= 1.0
|
||||
or not 0.01 <= self.surface_band_m <= 1.0
|
||||
or not self.surface_band_m <= self.obstacle_min_height_m
|
||||
or not self.obstacle_min_height_m < self.obstacle_max_height_m <= 20.0
|
||||
or not 1.0 <= self.maximum_pose_binding_ms <= 10_000.0
|
||||
or not 1.0 <= self.maximum_slope_deg < 90.0
|
||||
or not 0.02 <= self.step_min_height_m < self.step_max_height_m
|
||||
or not self.step_max_height_m <= self.step_max_plane_residual_m <= 2.0
|
||||
or not 0.02 <= self.temporal_height_jump_m <= 2.0
|
||||
or not 0.1 <= self.temporal_slope_jump_deg <= 45.0
|
||||
or not 0.005 <= self.temporal_roughness_jump_m <= 1.0
|
||||
):
|
||||
raise LidarGroundError("K1 local-surface profile is invalid")
|
||||
|
||||
def to_dict(self) -> dict[str, object]:
|
||||
return {
|
||||
"schema_version": "missioncore.k1-local-surface-profile/v1",
|
||||
"profile_id": self.profile_id,
|
||||
"input": {
|
||||
"representation": "legacy-e10-vendor-map-with-pose",
|
||||
"gravity_alignment": "vendor-map-z-assumed-diagnostic",
|
||||
"physical_sensor_height_required": False,
|
||||
"hardcoded_height_m": None,
|
||||
},
|
||||
"rolling_surface": {
|
||||
"local_radius_m": self.local_radius_m,
|
||||
"cell_size_m": self.cell_size_m,
|
||||
"surface_ttl_s": self.surface_ttl_s,
|
||||
"cell_lower_percentile": self.cell_lower_percentile,
|
||||
"initial_lower_fraction": self.initial_lower_fraction,
|
||||
"minimum_surface_cells": self.minimum_surface_cells,
|
||||
"robust_iterations": self.robust_iterations,
|
||||
"robust_mad_scale": self.robust_mad_scale,
|
||||
"minimum_inlier_band_m": self.minimum_inlier_band_m,
|
||||
"maximum_slope_deg": self.maximum_slope_deg,
|
||||
"step_min_height_m": self.step_min_height_m,
|
||||
"step_max_height_m": self.step_max_height_m,
|
||||
"step_max_plane_residual_m": self.step_max_plane_residual_m,
|
||||
},
|
||||
"classification": {
|
||||
"surface_band_m": self.surface_band_m,
|
||||
"obstacle_min_height_m": self.obstacle_min_height_m,
|
||||
"obstacle_max_height_m": self.obstacle_max_height_m,
|
||||
"absence_of_points_means_free": False,
|
||||
"unknown_is_traversable": False,
|
||||
},
|
||||
"pose_binding": {
|
||||
"basis": "recorded-nearest-host-monotonic-arrival",
|
||||
"maximum_age_ms": self.maximum_pose_binding_ms,
|
||||
},
|
||||
"temporal_qualification": {
|
||||
"prediction_input": "previous-ttl-window-only",
|
||||
"current_frame_excluded_from_prediction": True,
|
||||
"height_jump_m": self.temporal_height_jump_m,
|
||||
"slope_jump_deg": self.temporal_slope_jump_deg,
|
||||
"roughness_jump_m": self.temporal_roughness_jump_m,
|
||||
},
|
||||
"authority": {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
DEFAULT_K1_LOCAL_SURFACE_PROFILE: Final = K1LocalSurfaceProfile()
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class PredictionEvidence:
|
||||
prior_plane: npt.NDArray[np.float64]
|
||||
cell_points: npt.NDArray[np.float64]
|
||||
signed_residuals: npt.NDArray[np.float64]
|
||||
|
||||
@property
|
||||
def residual_p50_m(self) -> float:
|
||||
return float(np.percentile(np.abs(self.signed_residuals), 50))
|
||||
|
||||
@property
|
||||
def residual_p95_m(self) -> float:
|
||||
return float(np.percentile(np.abs(self.signed_residuals), 95))
|
||||
|
||||
def inlier_fraction(self, surface_band_m: float) -> float:
|
||||
return float(np.mean(np.abs(self.signed_residuals) <= surface_band_m))
|
||||
|
||||
|
||||
def update_cache(
|
||||
cache: dict[tuple[int, int], tuple[float, float]],
|
||||
cloud: npt.NDArray[np.float64],
|
||||
session_seconds: float,
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> None:
|
||||
keys, points = cloud_cell_observations(cloud, profile)
|
||||
for key, point in zip(keys, points, strict=True):
|
||||
cache[(int(key[0]), int(key[1]))] = (float(point[2]), session_seconds)
|
||||
|
||||
|
||||
def cloud_cell_observations(
|
||||
cloud: npt.NDArray[np.float64],
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> tuple[npt.NDArray[np.int64], npt.NDArray[np.float64]]:
|
||||
if cloud.shape[0] == 0:
|
||||
return np.empty((0, 2), dtype=np.int64), np.empty((0, 3), dtype=np.float64)
|
||||
cells = np.floor(cloud[:, :2] / profile.cell_size_m).astype(np.int64)
|
||||
order = np.lexsort((cells[:, 1], cells[:, 0]))
|
||||
sorted_cells = cells[order]
|
||||
sorted_z = cloud[order, 2]
|
||||
changes: npt.NDArray[np.int64] = (
|
||||
np.flatnonzero(np.any(np.diff(sorted_cells, axis=0) != 0, axis=1)) + 1
|
||||
).astype(np.int64, copy=False)
|
||||
starts = np.concatenate((np.asarray([0]), changes))
|
||||
ends = np.concatenate((changes, np.asarray([cloud.shape[0]])))
|
||||
keys = np.empty((starts.shape[0], 2), dtype=np.int64)
|
||||
points = np.empty((starts.shape[0], 3), dtype=np.float64)
|
||||
half_cell = profile.cell_size_m * 0.5
|
||||
for index, (start, end) in enumerate(zip(starts, ends, strict=True)):
|
||||
keys[index] = sorted_cells[start]
|
||||
points[index] = (
|
||||
float(sorted_cells[start, 0]) * profile.cell_size_m + half_cell,
|
||||
float(sorted_cells[start, 1]) * profile.cell_size_m + half_cell,
|
||||
float(np.percentile(sorted_z[start:end], profile.cell_lower_percentile)),
|
||||
)
|
||||
return keys, points
|
||||
|
||||
|
||||
def expire_cache(
|
||||
cache: dict[tuple[int, int], tuple[float, float]],
|
||||
session_seconds: float,
|
||||
position: npt.NDArray[np.float64],
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> None:
|
||||
maximum_radius_sq = (profile.local_radius_m + profile.cell_size_m) ** 2
|
||||
expired = [
|
||||
key
|
||||
for key, (_, observed_seconds) in cache.items()
|
||||
if session_seconds - observed_seconds > profile.surface_ttl_s
|
||||
or ((key[0] + 0.5) * profile.cell_size_m - float(position[0])) ** 2
|
||||
+ ((key[1] + 0.5) * profile.cell_size_m - float(position[1])) ** 2
|
||||
> maximum_radius_sq
|
||||
]
|
||||
for key in expired:
|
||||
del cache[key]
|
||||
|
||||
|
||||
def local_cache_records(
|
||||
cache: Mapping[tuple[int, int], tuple[float, float]],
|
||||
position: npt.NDArray[np.float64],
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> tuple[
|
||||
npt.NDArray[np.int64],
|
||||
npt.NDArray[np.float64],
|
||||
npt.NDArray[np.float64],
|
||||
]:
|
||||
values = [
|
||||
(
|
||||
(key[0] + 0.5) * profile.cell_size_m,
|
||||
(key[1] + 0.5) * profile.cell_size_m,
|
||||
z,
|
||||
observed_seconds,
|
||||
)
|
||||
for key, (z, observed_seconds) in sorted(cache.items())
|
||||
if (
|
||||
((key[0] + 0.5) * profile.cell_size_m - float(position[0])) ** 2
|
||||
+ ((key[1] + 0.5) * profile.cell_size_m - float(position[1])) ** 2
|
||||
<= profile.local_radius_m**2
|
||||
)
|
||||
]
|
||||
if not values:
|
||||
return (
|
||||
np.empty((0, 2), dtype=np.int64),
|
||||
np.empty((0, 3), dtype=np.float64),
|
||||
np.empty(0, dtype=np.float64),
|
||||
)
|
||||
array: npt.NDArray[np.float64] = np.asarray(values, dtype=np.float64)
|
||||
keys = np.floor(array[:, :2] / profile.cell_size_m).astype(np.int64)
|
||||
return keys, array[:, :3], array[:, 3]
|
||||
|
||||
|
||||
def fit_surface(
|
||||
cell_points: npt.NDArray[np.float64],
|
||||
position: npt.NDArray[np.float64],
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> tuple[
|
||||
npt.NDArray[np.float64],
|
||||
npt.NDArray[np.bool_],
|
||||
npt.NDArray[np.float64],
|
||||
] | None:
|
||||
centered_xy = cell_points[:, :2] - position[:2]
|
||||
design = np.column_stack(
|
||||
(centered_xy[:, 0], centered_xy[:, 1], np.ones(cell_points.shape[0]))
|
||||
)
|
||||
cutoff = float(np.quantile(cell_points[:, 2], profile.initial_lower_fraction))
|
||||
inliers = cell_points[:, 2] <= cutoff
|
||||
if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells:
|
||||
return None
|
||||
coefficients: npt.NDArray[np.float64] = np.zeros(3, dtype=np.float64)
|
||||
for _ in range(profile.robust_iterations):
|
||||
try:
|
||||
coefficients, _, rank, _ = np.linalg.lstsq(
|
||||
design[inliers], cell_points[inliers, 2], rcond=None
|
||||
)
|
||||
except np.linalg.LinAlgError:
|
||||
return None
|
||||
if rank < 3 or not np.isfinite(coefficients).all():
|
||||
return None
|
||||
residuals = cell_points[:, 2] - design @ coefficients
|
||||
center = float(np.median(residuals[inliers]))
|
||||
mad = float(np.median(np.abs(residuals[inliers] - center)))
|
||||
band = max(
|
||||
profile.minimum_inlier_band_m,
|
||||
profile.robust_mad_scale * 1.4826 * mad,
|
||||
)
|
||||
updated = np.abs(residuals - center) <= band
|
||||
if int(np.count_nonzero(updated)) < profile.minimum_surface_cells:
|
||||
return None
|
||||
if np.array_equal(updated, inliers):
|
||||
break
|
||||
inliers = updated
|
||||
coefficients, _, rank, _ = np.linalg.lstsq(
|
||||
design[inliers], cell_points[inliers, 2], rcond=None
|
||||
)
|
||||
if rank < 3 or not np.isfinite(coefficients).all():
|
||||
return None
|
||||
a, b, c = (float(value) for value in coefficients)
|
||||
unnormalized: npt.NDArray[np.float64] = np.asarray(
|
||||
[-a, -b, 1.0, a * float(position[0]) + b * float(position[1]) - c],
|
||||
dtype=np.float64,
|
||||
)
|
||||
norm = float(np.linalg.norm(unnormalized[:3]))
|
||||
if norm <= 0 or not np.isfinite(norm):
|
||||
return None
|
||||
plane = unnormalized / norm
|
||||
residuals = height_above_plane(cell_points, plane)
|
||||
center = float(np.median(residuals[inliers]))
|
||||
mad = float(np.median(np.abs(residuals[inliers] - center)))
|
||||
band = max(
|
||||
profile.minimum_inlier_band_m,
|
||||
profile.robust_mad_scale * 1.4826 * mad,
|
||||
)
|
||||
inliers = np.abs(residuals - center) <= band
|
||||
if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells:
|
||||
return None
|
||||
return plane.astype("<f8"), inliers, residuals
|
||||
|
||||
|
||||
def prediction_metrics(
|
||||
prior_cell_points: npt.NDArray[np.float64],
|
||||
current_cell_points: npt.NDArray[np.float64],
|
||||
position: npt.NDArray[np.float64],
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> PredictionEvidence | None:
|
||||
if (
|
||||
prior_cell_points.shape[0] < profile.minimum_surface_cells
|
||||
or current_cell_points.shape[0] < profile.minimum_surface_cells
|
||||
):
|
||||
return None
|
||||
prior_fit = fit_surface(prior_cell_points, position, profile)
|
||||
if prior_fit is None:
|
||||
return None
|
||||
prior_plane, _, _ = prior_fit
|
||||
cutoff = float(np.quantile(current_cell_points[:, 2], profile.initial_lower_fraction))
|
||||
evaluation = current_cell_points[:, 2] <= cutoff
|
||||
if int(np.count_nonzero(evaluation)) < profile.minimum_surface_cells:
|
||||
return None
|
||||
evaluation_points = current_cell_points[evaluation]
|
||||
signed_residuals = height_above_plane(evaluation_points, prior_plane)
|
||||
if signed_residuals.size == 0 or not np.isfinite(signed_residuals).all():
|
||||
return None
|
||||
return PredictionEvidence(
|
||||
prior_plane=prior_plane,
|
||||
cell_points=evaluation_points,
|
||||
signed_residuals=signed_residuals,
|
||||
)
|
||||
|
||||
|
||||
def step_candidate_keys(
|
||||
cell_keys: npt.NDArray[np.int64],
|
||||
cell_points: npt.NDArray[np.float64],
|
||||
plane: npt.NDArray[np.float64],
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> set[tuple[int, int]]:
|
||||
if cell_keys.shape[0] != cell_points.shape[0]:
|
||||
raise LidarGroundError("K1 local-surface cell alignment is invalid")
|
||||
residual = height_above_plane(cell_points, plane)
|
||||
lookup = {
|
||||
(int(key[0]), int(key[1])): float(value)
|
||||
for key, value in zip(cell_keys, residual, strict=True)
|
||||
if abs(float(value)) <= profile.step_max_plane_residual_m
|
||||
}
|
||||
candidates: set[tuple[int, int]] = set()
|
||||
for key, value in lookup.items():
|
||||
for neighbor in ((key[0] + 1, key[1]), (key[0], key[1] + 1)):
|
||||
neighbor_value = lookup.get(neighbor)
|
||||
if neighbor_value is None:
|
||||
continue
|
||||
delta = abs(value - neighbor_value)
|
||||
if profile.step_min_height_m <= delta <= profile.step_max_height_m:
|
||||
candidates.add(key)
|
||||
candidates.add(neighbor)
|
||||
return candidates
|
||||
|
||||
|
||||
def point_step_candidates(
|
||||
cloud: npt.NDArray[np.float64],
|
||||
local: npt.NDArray[np.bool_],
|
||||
heights: npt.NDArray[np.float64],
|
||||
candidate_keys: set[tuple[int, int]],
|
||||
profile: K1LocalSurfaceProfile,
|
||||
) -> npt.NDArray[np.uint8]:
|
||||
result = np.zeros(cloud.shape[0], dtype=np.uint8)
|
||||
if not candidate_keys:
|
||||
return result
|
||||
cells = np.floor(cloud[:, :2] / profile.cell_size_m).astype(np.int64)
|
||||
for index in np.flatnonzero(local):
|
||||
key = (int(cells[index, 0]), int(cells[index, 1]))
|
||||
if (
|
||||
key in candidate_keys
|
||||
and abs(float(heights[index])) <= profile.step_max_plane_residual_m
|
||||
):
|
||||
result[index] = 1
|
||||
return result
|
||||
|
||||
|
||||
def height_above_plane(
|
||||
points: npt.NDArray[np.float64],
|
||||
plane: npt.NDArray[np.float64],
|
||||
) -> npt.NDArray[np.float64]:
|
||||
return points @ plane[:3] + float(plane[3])
|
||||
@@ -13,26 +13,45 @@ 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 (
|
||||
from .lidar_local_surface_geometry 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 .lidar_local_surface_geometry import (
|
||||
cloud_cell_observations as _cloud_cell_observations,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
expire_cache as _expire_cache,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
fit_surface as _fit_surface,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
height_above_plane as _height_above_plane,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
local_cache_records as _local_cache_records,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
point_step_candidates as _point_step_candidates,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
prediction_metrics as _prediction_metrics,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
step_candidate_keys as _step_candidate_keys,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
update_cache as _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"
|
||||
K1_LOCAL_SURFACE_BINDER_SCHEMA: Final = "missioncore.k1-local-surface-pose-binder/v1"
|
||||
|
||||
ShadowFrameState = Literal[
|
||||
"valid",
|
||||
@@ -42,6 +61,208 @@ ShadowFrameState = Literal[
|
||||
]
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class K1LocalSurfaceBoundViews:
|
||||
"""One point frame paired to the nearest admitted host-arrival pose."""
|
||||
|
||||
point_cloud: DecodedPointCloudView
|
||||
pose: DecodedPoseView
|
||||
pose_binding_age_ms: float
|
||||
|
||||
|
||||
class K1LocalSurfacePoseBinder:
|
||||
"""Bounded event-order-independent LiDAR↔pose binding for shadow work."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
maximum_pose_binding_ms: float,
|
||||
point_capacity: int = 2,
|
||||
pose_capacity: int = 16,
|
||||
future_pose_wait_ms: float = 25.0,
|
||||
retention_seconds: float = 3.0,
|
||||
) -> None:
|
||||
if (
|
||||
not math.isfinite(maximum_pose_binding_ms)
|
||||
or not 1 <= maximum_pose_binding_ms <= 10_000
|
||||
or not 1 <= point_capacity <= 8
|
||||
or not 2 <= pose_capacity <= 256
|
||||
or not math.isfinite(future_pose_wait_ms)
|
||||
or not 0 <= future_pose_wait_ms <= maximum_pose_binding_ms
|
||||
or not math.isfinite(retention_seconds)
|
||||
or not 0.1 <= retention_seconds <= 30
|
||||
):
|
||||
raise LidarGroundError("K1 local-surface pose binder bounds are invalid")
|
||||
self._maximum_delta_ns = round(maximum_pose_binding_ms * 1_000_000)
|
||||
self._future_wait_ns = round(future_pose_wait_ms * 1_000_000)
|
||||
self._retention_ns = round(retention_seconds * 1_000_000_000)
|
||||
self._point_capacity = point_capacity
|
||||
self._pose_capacity = pose_capacity
|
||||
self._points: deque[DecodedPointCloudView] = deque()
|
||||
self._poses: deque[DecodedPoseView] = deque()
|
||||
self._lock = threading.Lock()
|
||||
self._latest_time_ns = 0
|
||||
self._last_point_sequence = 0
|
||||
self._last_pose_sequence = 0
|
||||
self._point_published = 0
|
||||
self._pose_published = 0
|
||||
self._point_bound = 0
|
||||
self._point_missed = 0
|
||||
self._point_dropped_overflow = 0
|
||||
self._pose_dropped_overflow = 0
|
||||
self._maximum_point_depth = 0
|
||||
self._maximum_pose_depth = 0
|
||||
self._binding_age_ms: deque[float] = deque(maxlen=512)
|
||||
|
||||
def publish_point_cloud(
|
||||
self,
|
||||
value: DecodedPointCloudView,
|
||||
) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||
if value.frame_id != "map":
|
||||
raise LidarGroundError("K1 local-surface pose binder requires map-frame points")
|
||||
with self._lock:
|
||||
if value.context.sequence <= self._last_point_sequence:
|
||||
raise LidarGroundError("K1 local-surface point sequence is not increasing")
|
||||
self._last_point_sequence = value.context.sequence
|
||||
self._point_published += 1
|
||||
if len(self._points) == self._point_capacity:
|
||||
self._points.popleft()
|
||||
self._point_dropped_overflow += 1
|
||||
self._points.append(value)
|
||||
self._maximum_point_depth = max(
|
||||
self._maximum_point_depth,
|
||||
len(self._points),
|
||||
)
|
||||
self._latest_time_ns = max(self._latest_time_ns, _view_time_ns(value))
|
||||
self._prune_poses_locked()
|
||||
return self._drain_locked(force=False)
|
||||
|
||||
def publish_pose(
|
||||
self,
|
||||
value: DecodedPoseView,
|
||||
) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||
if value.frame_id != "map" or value.child_frame_id != "sensor":
|
||||
raise LidarGroundError("K1 local-surface pose binder requires map-from-sensor pose")
|
||||
with self._lock:
|
||||
if value.context.sequence <= self._last_pose_sequence:
|
||||
raise LidarGroundError("K1 local-surface pose sequence is not increasing")
|
||||
self._last_pose_sequence = value.context.sequence
|
||||
self._pose_published += 1
|
||||
if len(self._poses) == self._pose_capacity:
|
||||
self._poses.popleft()
|
||||
self._pose_dropped_overflow += 1
|
||||
self._poses.append(value)
|
||||
self._maximum_pose_depth = max(
|
||||
self._maximum_pose_depth,
|
||||
len(self._poses),
|
||||
)
|
||||
self._latest_time_ns = max(self._latest_time_ns, _view_time_ns(value))
|
||||
self._prune_poses_locked()
|
||||
return self._drain_locked(force=False)
|
||||
|
||||
def flush(self) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||
with self._lock:
|
||||
return self._drain_locked(force=True)
|
||||
|
||||
def snapshot(self) -> dict[str, object]:
|
||||
with self._lock:
|
||||
ages: npt.NDArray[np.float64] = np.asarray(
|
||||
self._binding_age_ms,
|
||||
dtype=np.float64,
|
||||
)
|
||||
return {
|
||||
"schema_version": K1_LOCAL_SURFACE_BINDER_SCHEMA,
|
||||
"clock_basis": "host-monotonic-arrival",
|
||||
"maximum_pose_binding_ms": self._maximum_delta_ns / 1_000_000,
|
||||
"future_pose_wait_ms": self._future_wait_ns / 1_000_000,
|
||||
"points": {
|
||||
"capacity": self._point_capacity,
|
||||
"depth": len(self._points),
|
||||
"maximum_depth": self._maximum_point_depth,
|
||||
"published": self._point_published,
|
||||
"bound": self._point_bound,
|
||||
"missed": self._point_missed,
|
||||
"dropped_overflow": self._point_dropped_overflow,
|
||||
},
|
||||
"poses": {
|
||||
"capacity": self._pose_capacity,
|
||||
"depth": len(self._poses),
|
||||
"maximum_depth": self._maximum_pose_depth,
|
||||
"published": self._pose_published,
|
||||
"dropped_overflow": self._pose_dropped_overflow,
|
||||
},
|
||||
"binding_age_ms": {
|
||||
"sample_count": int(ages.shape[0]),
|
||||
"p50": float(np.percentile(ages, 50)) if ages.size else None,
|
||||
"p95": float(np.percentile(ages, 95)) if ages.size else None,
|
||||
"maximum": float(np.max(ages)) if ages.size else None,
|
||||
},
|
||||
"authority": {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
|
||||
def _drain_locked(
|
||||
self,
|
||||
*,
|
||||
force: bool,
|
||||
) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||
bound: list[K1LocalSurfaceBoundViews] = []
|
||||
while self._points:
|
||||
point_cloud = self._points[0]
|
||||
point_time_ns = _view_time_ns(point_cloud)
|
||||
pose = min(
|
||||
self._poses,
|
||||
key=lambda candidate: abs(_view_time_ns(candidate) - point_time_ns),
|
||||
default=None,
|
||||
)
|
||||
if pose is None:
|
||||
if force or self._latest_time_ns - point_time_ns >= self._maximum_delta_ns:
|
||||
self._points.popleft()
|
||||
self._point_missed += 1
|
||||
continue
|
||||
break
|
||||
pose_time_ns = _view_time_ns(pose)
|
||||
delta_ns = abs(pose_time_ns - point_time_ns)
|
||||
future_watermark_reached = self._latest_time_ns - point_time_ns >= self._future_wait_ns
|
||||
recent_prior_pose = pose_time_ns <= point_time_ns and delta_ns <= self._future_wait_ns
|
||||
if delta_ns <= self._maximum_delta_ns and (
|
||||
force
|
||||
or pose_time_ns >= point_time_ns
|
||||
or recent_prior_pose
|
||||
or future_watermark_reached
|
||||
):
|
||||
self._points.popleft()
|
||||
age_ms = delta_ns / 1_000_000
|
||||
self._point_bound += 1
|
||||
self._binding_age_ms.append(age_ms)
|
||||
bound.append(
|
||||
K1LocalSurfaceBoundViews(
|
||||
point_cloud=point_cloud,
|
||||
pose=pose,
|
||||
pose_binding_age_ms=age_ms,
|
||||
)
|
||||
)
|
||||
continue
|
||||
if force or self._latest_time_ns - point_time_ns >= self._maximum_delta_ns:
|
||||
self._points.popleft()
|
||||
self._point_missed += 1
|
||||
continue
|
||||
break
|
||||
return tuple(bound)
|
||||
|
||||
def _prune_poses_locked(self) -> None:
|
||||
cutoff = self._latest_time_ns - self._retention_ns
|
||||
while self._poses and _view_time_ns(self._poses[0]) < cutoff:
|
||||
self._poses.popleft()
|
||||
|
||||
|
||||
def _view_time_ns(value: DecodedPointCloudView | DecodedPoseView) -> int:
|
||||
received = value.context.received_monotonic_ns
|
||||
return int(received if received is not None else value.context.captured_at_epoch_ns)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class K1LocalSurfaceShadowInput:
|
||||
"""One immutable map-point/pose pair admitted to passive shadow work."""
|
||||
@@ -111,7 +332,9 @@ class K1LocalSurfaceShadowInput:
|
||||
pose_binding_age_ms=pose_binding_age_ms,
|
||||
points_map=points,
|
||||
position_map=position,
|
||||
published_monotonic_ns=time.monotonic_ns(),
|
||||
published_monotonic_ns=(
|
||||
point_cloud.context.processing_started_monotonic_ns
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
@@ -561,6 +784,8 @@ class K1LocalSurfaceShadowRuntime:
|
||||
self._processed = 0
|
||||
self._failed = 0
|
||||
self._state_counts: Counter[str] = Counter()
|
||||
self._processing_ms: deque[float] = deque(maxlen=512)
|
||||
self._result_age_ms: deque[float] = deque(maxlen=512)
|
||||
self._last_error: str | None = None
|
||||
self._inflight = False
|
||||
self._condition = threading.Condition()
|
||||
@@ -638,6 +863,8 @@ class K1LocalSurfaceShadowRuntime:
|
||||
"dropped_ring_overflow": self._result_dropped,
|
||||
"state_counts": dict(sorted(self._state_counts.items())),
|
||||
"failed": self._failed,
|
||||
"processing_ms": _bounded_distribution(self._processing_ms),
|
||||
"result_age_ms": _bounded_distribution(self._result_age_ms),
|
||||
"latest": latest.document() if latest is not None else None,
|
||||
},
|
||||
"last_error": self._last_error,
|
||||
@@ -675,7 +902,128 @@ class K1LocalSurfaceShadowRuntime:
|
||||
self._results.append(result)
|
||||
self._processed += 1
|
||||
self._state_counts[result.state] += 1
|
||||
self._processing_ms.append(result.processing_ms)
|
||||
self._result_age_ms.append(result.result_age_ms)
|
||||
finally:
|
||||
with self._condition:
|
||||
self._inflight = False
|
||||
self._condition.notify_all()
|
||||
|
||||
|
||||
class K1LocalSurfaceShadowCoordinator:
|
||||
"""Lazy session lifecycle around the bounded pose binder and estimator."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||
point_capacity: int = 2,
|
||||
pose_capacity: int = 16,
|
||||
future_pose_wait_ms: float = 25.0,
|
||||
retention_seconds: float = 3.0,
|
||||
result_capacity: int = 8,
|
||||
) -> None:
|
||||
self.profile = profile
|
||||
self._queue_capacity = point_capacity
|
||||
self._result_capacity = result_capacity
|
||||
self._binder = K1LocalSurfacePoseBinder(
|
||||
maximum_pose_binding_ms=profile.maximum_pose_binding_ms,
|
||||
point_capacity=point_capacity,
|
||||
pose_capacity=pose_capacity,
|
||||
future_pose_wait_ms=future_pose_wait_ms,
|
||||
retention_seconds=retention_seconds,
|
||||
)
|
||||
self._lock = threading.Lock()
|
||||
self._runtime: K1LocalSurfaceShadowRuntime | None = None
|
||||
self._session_id: str | None = None
|
||||
self._closed = False
|
||||
|
||||
def begin_session(self, session_id: str) -> None:
|
||||
with self._lock:
|
||||
if self._closed:
|
||||
raise RuntimeError("K1 local-surface shadow coordinator is closed")
|
||||
if self._runtime is not None:
|
||||
if self._session_id == session_id:
|
||||
return
|
||||
raise RuntimeError(
|
||||
"K1 local-surface shadow coordinator session changed"
|
||||
)
|
||||
self._runtime = K1LocalSurfaceShadowRuntime(
|
||||
session_id,
|
||||
profile=self.profile,
|
||||
queue_capacity=self._queue_capacity,
|
||||
result_capacity=self._result_capacity,
|
||||
)
|
||||
self._session_id = session_id
|
||||
|
||||
def publish_point_cloud(self, value: DecodedPointCloudView) -> int:
|
||||
runtime = self._active_runtime()
|
||||
bindings = self._binder.publish_point_cloud(value)
|
||||
for binding in bindings:
|
||||
runtime.publish_views(binding.point_cloud, binding.pose)
|
||||
return len(bindings)
|
||||
|
||||
def publish_pose(self, value: DecodedPoseView) -> int:
|
||||
runtime = self._active_runtime()
|
||||
bindings = self._binder.publish_pose(value)
|
||||
for binding in bindings:
|
||||
runtime.publish_views(binding.point_cloud, binding.pose)
|
||||
return len(bindings)
|
||||
|
||||
def close(self, *, timeout_seconds: float = 30.0) -> None:
|
||||
with self._lock:
|
||||
if self._closed:
|
||||
return
|
||||
self._closed = True
|
||||
runtime = self._runtime
|
||||
if runtime is None:
|
||||
return
|
||||
for binding in self._binder.flush():
|
||||
runtime.publish_views(binding.point_cloud, binding.pose)
|
||||
runtime.close(timeout_seconds=timeout_seconds)
|
||||
|
||||
def snapshot(self) -> dict[str, object]:
|
||||
with self._lock:
|
||||
runtime = self._runtime
|
||||
session_id = self._session_id
|
||||
closed = self._closed
|
||||
runtime_snapshot = runtime.snapshot() if runtime is not None else None
|
||||
return {
|
||||
"schema_version": K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
||||
"mode": "physical-live-shadow-diagnostic-only",
|
||||
"session_id": session_id,
|
||||
"binder": self._binder.snapshot(),
|
||||
"runtime": runtime_snapshot,
|
||||
"active": runtime is not None and not closed,
|
||||
"closed": closed and (
|
||||
runtime_snapshot is None or bool(runtime_snapshot["closed"])
|
||||
),
|
||||
"ground_truth": False,
|
||||
"authority": {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
|
||||
def _active_runtime(self) -> K1LocalSurfaceShadowRuntime:
|
||||
with self._lock:
|
||||
if self._closed:
|
||||
raise RuntimeError("K1 local-surface shadow coordinator is closed")
|
||||
runtime = self._runtime
|
||||
if runtime is None:
|
||||
raise RuntimeError(
|
||||
"K1 local-surface shadow coordinator session is not active"
|
||||
)
|
||||
return runtime
|
||||
|
||||
|
||||
def _bounded_distribution(
|
||||
values: deque[float],
|
||||
) -> dict[str, float | int | None]:
|
||||
array: npt.NDArray[np.float64] = np.asarray(values, dtype=np.float64)
|
||||
return {
|
||||
"sample_count": int(array.shape[0]),
|
||||
"p50": float(np.percentile(array, 50)) if array.size else None,
|
||||
"p95": float(np.percentile(array, 95)) if array.size else None,
|
||||
"maximum": float(np.max(array)) if array.size else None,
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user