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

feat(perception): add occupied-only low-step shadow

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-26 12:39:54 +03:00
parent 5983883ffe
commit 343ddeac2d
8 changed files with 1124 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/perception/m48_low_step_occupancy.py
+643
View File
@@ -0,0 +1,643 @@
"""Occupied-only low-step geometry shadow for M4.8R3.
The provider composes the accepted geometry association provider and may only
add class-free current LiDAR observations. It never removes baseline evidence,
publishes free space, changes camera semantics, or performs inference.
"""
from __future__ import annotations
import hashlib
import json
import math
import time
from collections import deque
from dataclasses import dataclass
from pathlib import Path
from threading import Lock
from typing import Final
import numpy as np
import numpy.typing as npt
from .contracts import (
EvidenceBasis,
EvidenceCurrentness,
MetricGeometry,
ObjectProposal2D,
ObstacleObservation,
validate_exclusive_point_ownership,
)
from .geometry import (
GeometryProviderSnapshot,
Ravnoves00GeometryAssociationProvider,
RecordedGeometryStore,
)
from .geometry_math import POINT_OCCUPIED
from .providers import SourcePacket
M48_LOW_STEP_PROFILE_SCHEMA: Final = (
"missioncore.m48-additive-low-step-occupancy-profile/v1"
)
M48_LOW_STEP_PROVIDER_ID: Final = "ravnoves00-additive-low-step-geometry/v1"
IntArray = npt.NDArray[np.int64]
class M48LowStepOccupancyError(RuntimeError):
"""The additive profile, source evidence, or bounded component set is invalid."""
@dataclass(frozen=True, slots=True)
class LowStepComponentProfile:
voxel_size_m: float
neighbor_radius_cells: int
minimum_points: int
minimum_voxels: int
local_radius_m: float
maximum_candidate_points_per_frame: int
maximum_cells_per_component: int
maximum_components_per_frame: int
def __post_init__(self) -> None:
if (
not math.isfinite(self.voxel_size_m)
or not 0.05 <= self.voxel_size_m <= 2.0
or self.neighbor_radius_cells != 1
or not 1 <= self.minimum_points <= 256
or not 1 <= self.minimum_voxels <= 128
or not math.isfinite(self.local_radius_m)
or not 1.0 <= self.local_radius_m <= 100.0
or not 1 <= self.maximum_candidate_points_per_frame <= 4096
or not 1 <= self.maximum_cells_per_component <= 2048
or not 1 <= self.maximum_components_per_frame <= 512
):
raise M48LowStepOccupancyError("low-step component bounds are invalid")
@dataclass(frozen=True, slots=True)
class LowStepSeparationExpectation:
anchor_id: str
sequence: int
expected_minimum_components: int
interpretation: str
@dataclass(frozen=True, slots=True)
class M48LowStepOccupancyProfile:
profile_id: str
provider_id: str
source_id: str
session_id: str
frame_count: int
point_count: int
local_surface_model_id: str
local_surface_sha256: str
base_geometry_profile_id: str
base_geometry_profile_sha256: str
component: LowStepComponentProfile
separation_expectations: tuple[LowStepSeparationExpectation, ...]
profile_sha256: str
@dataclass(frozen=True, slots=True)
class M48LowStepOccupancySnapshot:
base: GeometryProviderSnapshot
input_frames: int
completed_frames: int
failed_frames: int
frames_with_additions: int
candidate_point_count: int
additive_observation_count: int
additive_voxel_count: int
peak_candidate_points_per_frame: int
peak_additive_observations_per_frame: int
peak_voxels_per_component: int
additive_core_duration_ns: int
class M48AdditiveLowStepGeometryProvider:
"""Compose baseline geometry with bounded, spatially separate step components."""
provider_id: str = M48_LOW_STEP_PROVIDER_ID
def __init__(
self,
*,
store: RecordedGeometryStore,
profile: M48LowStepOccupancyProfile,
) -> None:
if profile.provider_id != self.provider_id:
raise M48LowStepOccupancyError("low-step provider identity changed")
geometry = store.profile
if (
geometry.source_id != profile.source_id
or geometry.session_id != profile.session_id
or geometry.frame_count != profile.frame_count
or geometry.point_count != profile.point_count
or geometry.local_surface_model_id != profile.local_surface_model_id
or geometry.local_surface_sha256 != profile.local_surface_sha256
or geometry.profile_id != profile.base_geometry_profile_id
or geometry.profile_sha256 != profile.base_geometry_profile_sha256
):
raise M48LowStepOccupancyError("low-step source binding changed")
self.store = store
self.profile = profile
self.base = Ravnoves00GeometryAssociationProvider(store=store)
self._lock = Lock()
self._input_frames = 0
self._completed_frames = 0
self._failed_frames = 0
self._frames_with_additions = 0
self._candidate_points = 0
self._additive_observation_count = 0
self._additive_voxels = 0
self._peak_candidate_points = 0
self._peak_additive_observations = 0
self._peak_component_voxels = 0
self._additive_core_duration_ns = 0
def associate(
self,
packet: SourcePacket,
proposals: tuple[ObjectProposal2D, ...],
) -> tuple[ObstacleObservation, ...]:
with self._lock:
self._input_frames += 1
baseline = self.base.associate(packet, proposals)
started = time.perf_counter_ns()
try:
additive, candidate_points, voxel_count, peak_component_voxels = (
self._build_additive_observations(packet, baseline)
)
result = (*baseline, *additive)
validate_exclusive_point_ownership(result)
except Exception:
with self._lock:
self._failed_frames += 1
self._additive_core_duration_ns += max(
0, time.perf_counter_ns() - started
)
raise
with self._lock:
self._completed_frames += 1
self._frames_with_additions += bool(additive)
self._candidate_points += candidate_points
self._additive_observation_count += len(additive)
self._additive_voxels += voxel_count
self._peak_candidate_points = max(
self._peak_candidate_points, candidate_points
)
self._peak_additive_observations = max(
self._peak_additive_observations, len(additive)
)
self._peak_component_voxels = max(
self._peak_component_voxels, peak_component_voxels
)
self._additive_core_duration_ns += max(
0, time.perf_counter_ns() - started
)
return tuple(result)
def _build_additive_observations(
self,
packet: SourcePacket,
baseline: tuple[ObstacleObservation, ...],
) -> tuple[tuple[ObstacleObservation, ...], int, int, int]:
frame = self.store.frame(packet)
if frame is None or not frame.surface_valid:
return (), 0, 0, 0
step = self.store.point_step_candidates_for_frame(frame.frame_index)
if step is None or step.shape != (frame.source_point_count,):
raise M48LowStepOccupancyError("low-step point index space changed")
claimed = {
point_id for observation in baseline for point_id in observation.source_point_ids
}
candidate = np.flatnonzero(
(step > 0) & (frame.point_class != POINT_OCCUPIED)
).astype(np.int64)
if claimed and candidate.size:
candidate = candidate[
np.fromiter(
(int(value) not in claimed for value in candidate),
dtype=np.bool_,
count=int(candidate.size),
)
]
if candidate.size:
ranges = np.linalg.norm(
frame.points_map[candidate] - frame.sensor_position_map,
axis=1,
)
candidate = candidate[ranges <= self.profile.component.local_radius_m]
candidate_count = int(candidate.size)
if candidate_count > self.profile.component.maximum_candidate_points_per_frame:
raise M48LowStepOccupancyError(
"low-step candidate point capacity exceeded; dropping is forbidden"
)
components = _voxel_components(
frame.points_map,
candidate,
self.profile.component,
)
qualified = tuple(
item
for item in components
if item[0].size >= self.profile.component.minimum_points
and item[1] >= self.profile.component.minimum_voxels
)
qualified = tuple(
sorted(
qualified,
key=lambda item: (
float(
np.min(
np.linalg.norm(
frame.points_map[item[0]]
- frame.sensor_position_map,
axis=1,
)
)
),
int(item[0][0]),
),
)
)
if len(qualified) > self.profile.component.maximum_components_per_frame:
raise M48LowStepOccupancyError(
"low-step component capacity exceeded; dropping is forbidden"
)
observations: list[ObstacleObservation] = []
voxel_count = 0
peak_voxels = 0
for component_index, (indices, cells) in enumerate(qualified):
if cells > self.profile.component.maximum_cells_per_component:
raise M48LowStepOccupancyError(
"low-step component cell capacity exceeded; dropping is forbidden"
)
point_ids = tuple(sorted(int(value) for value in indices))
points = frame.points_map[indices]
centroid = np.median(points, axis=0)
covariance = points.var(axis=0)
nearest = float(
np.min(np.linalg.norm(points - frame.sensor_position_map, axis=1))
)
observations.append(
ObstacleObservation(
observation_id=(
f"{packet.envelope.frame_id}:low-step:{component_index}"
),
occupancy_key=(
f"{packet.envelope.frame_id}:low-step:{component_index}"
),
source_id=packet.envelope.source_id,
frame_id=packet.envelope.frame_id,
evidence_time_ns=packet.envelope.timestamps.source_ns,
basis=EvidenceBasis.LIDAR,
currentness=EvidenceCurrentness.CURRENT,
occupied_support=True,
source_point_ids=point_ids,
metric_geometry=MetricGeometry(
coordinate_frame=self.store.profile.coordinate_frame,
centroid_xyz_m=(
float(centroid[0]),
float(centroid[1]),
float(centroid[2]),
),
range_m=nearest,
covariance_diagonal_m2=(
float(covariance[0]),
float(covariance[1]),
float(covariance[2]),
),
),
proposal_ids=(),
semantic_hint=None,
reason_codes=(
"additive-low-step-current-component",
"occupied-only-never-free",
),
)
)
voxel_count += cells
peak_voxels = max(peak_voxels, cells)
return tuple(observations), candidate_count, voxel_count, peak_voxels
def snapshot(self) -> M48LowStepOccupancySnapshot:
with self._lock:
return M48LowStepOccupancySnapshot(
base=self.base.snapshot(),
input_frames=self._input_frames,
completed_frames=self._completed_frames,
failed_frames=self._failed_frames,
frames_with_additions=self._frames_with_additions,
candidate_point_count=self._candidate_points,
additive_observation_count=self._additive_observation_count,
additive_voxel_count=self._additive_voxels,
peak_candidate_points_per_frame=self._peak_candidate_points,
peak_additive_observations_per_frame=(
self._peak_additive_observations
),
peak_voxels_per_component=self._peak_component_voxels,
additive_core_duration_ns=self._additive_core_duration_ns,
)
def load_m48_low_step_occupancy_profile(
path: Path,
) -> M48LowStepOccupancyProfile:
resolved = path.resolve(strict=True)
if resolved.is_symlink() or not resolved.is_file():
raise M48LowStepOccupancyError("low-step profile must be a regular file")
raw = resolved.read_bytes()
try:
value = json.loads(raw)
except json.JSONDecodeError as exc:
raise M48LowStepOccupancyError("low-step profile JSON is invalid") from exc
document = _object(value, "low-step profile")
_exact_keys(
document,
{
"schema_version",
"profile_id",
"provider_id",
"base_geometry",
"source",
"componentization",
"separation_expectations",
"acceptance",
"policy",
"authority",
},
"low-step profile",
)
if (
document["schema_version"] != M48_LOW_STEP_PROFILE_SCHEMA
or document["provider_id"] != M48_LOW_STEP_PROVIDER_ID
):
raise M48LowStepOccupancyError("low-step profile identity changed")
base = _object(document["base_geometry"], "base geometry")
source = _object(document["source"], "low-step source")
component = _object(document["componentization"], "componentization")
acceptance = _object(document["acceptance"], "low-step acceptance")
_exact_keys(base, {"profile_id", "sha256"}, "base geometry")
_exact_keys(
source,
{
"source_id",
"session_id",
"frame_count",
"point_count",
"local_surface_model_id",
"local_surface_sha256",
"m48r2_result_id",
"m48r2_cases_sha256",
},
"low-step source",
)
_exact_keys(
component,
{
"voxel_size_m",
"neighbor_radius_cells",
"minimum_points",
"minimum_voxels",
"local_radius_m",
"maximum_candidate_points_per_frame",
"maximum_cells_per_component",
"maximum_components_per_frame",
"exclude_baseline_occupied_points",
"exclude_claimed_source_points",
},
"componentization",
)
_exact_keys(
acceptance,
{
"expected_frames",
"requested_source_rate_hz",
"minimum_effective_world_state_fps",
"maximum_world_state_completion_p95_ms",
"maximum_geometry_stage_p95_ms",
"maximum_geometry_stage_p99_ms",
"maximum_fps_regression_fraction_vs_native_baseline",
"maximum_world_state_p95_delta_ms_vs_native_baseline",
"maximum_additive_component_mean_growth_fraction",
"maximum_additive_cell_mean_growth_fraction",
"maximum_capacity_drop_count",
"minimum_critical_near_recall",
"minimum_canonical_engineering_recall",
"maximum_false_free_count",
},
"low-step acceptance",
)
if (
component["exclude_baseline_occupied_points"] is not True
or component["exclude_claimed_source_points"] is not True
):
raise M48LowStepOccupancyError("low-step point ownership policy changed")
for key in (
"expected_frames",
"maximum_capacity_drop_count",
"maximum_false_free_count",
):
item = acceptance.get(key)
if not isinstance(item, int) or isinstance(item, bool) or item < 0:
raise M48LowStepOccupancyError("low-step acceptance bounds are invalid")
for key in (
"requested_source_rate_hz",
"minimum_effective_world_state_fps",
"maximum_world_state_completion_p95_ms",
"maximum_geometry_stage_p95_ms",
"maximum_geometry_stage_p99_ms",
"maximum_fps_regression_fraction_vs_native_baseline",
"maximum_world_state_p95_delta_ms_vs_native_baseline",
"maximum_additive_component_mean_growth_fraction",
"maximum_additive_cell_mean_growth_fraction",
"minimum_critical_near_recall",
"minimum_canonical_engineering_recall",
):
if _number(acceptance, key) < 0.0:
raise M48LowStepOccupancyError("low-step acceptance bounds are invalid")
if (
not _string(source, "m48r2_result_id").startswith(
"m48-static-occupancy-qualification-"
)
or len(_string(source, "m48r2_result_id"))
!= len("m48-static-occupancy-qualification-") + 64
):
raise M48LowStepOccupancyError("M4.8R2 result binding is invalid")
_digest(source, "m48r2_cases_sha256")
expected_policy = {
"absence_of_points_means_free": False,
"absence_of_camera_detection_means_free": False,
"additive_only": True,
"semantic_class_used": False,
"ray_clearing_used": False,
"planner_authoritative_free_space_claimed": False,
}
expected_authority = {
"mode": "replay-simulated",
"physical_live": False,
"commands_enabled": False,
"actuation_allowed": False,
"navigation_or_safety_accepted": False,
}
if document["policy"] != expected_policy or document["authority"] != expected_authority:
raise M48LowStepOccupancyError("low-step policy or authority changed")
expectations_value = document["separation_expectations"]
if not isinstance(expectations_value, list) or not expectations_value:
raise M48LowStepOccupancyError("low-step separation expectations are missing")
expectations: list[LowStepSeparationExpectation] = []
for value in expectations_value:
item = _object(value, "separation expectation")
_exact_keys(
item,
{"anchor_id", "sequence", "expected_minimum_components", "interpretation"},
"separation expectation",
)
expectations.append(
LowStepSeparationExpectation(
anchor_id=_string(item, "anchor_id"),
sequence=_positive_integer(item, "sequence"),
expected_minimum_components=_positive_integer(
item, "expected_minimum_components"
),
interpretation=_string(item, "interpretation"),
)
)
return M48LowStepOccupancyProfile(
profile_id=_string(document, "profile_id"),
provider_id=_string(document, "provider_id"),
source_id=_string(source, "source_id"),
session_id=_string(source, "session_id"),
frame_count=_positive_integer(source, "frame_count"),
point_count=_positive_integer(source, "point_count"),
local_surface_model_id=_string(source, "local_surface_model_id"),
local_surface_sha256=_digest(source, "local_surface_sha256"),
base_geometry_profile_id=_string(base, "profile_id"),
base_geometry_profile_sha256=_digest(base, "sha256"),
component=LowStepComponentProfile(
voxel_size_m=_number(component, "voxel_size_m"),
neighbor_radius_cells=_positive_integer(
component, "neighbor_radius_cells"
),
minimum_points=_positive_integer(component, "minimum_points"),
minimum_voxels=_positive_integer(component, "minimum_voxels"),
local_radius_m=_number(component, "local_radius_m"),
maximum_candidate_points_per_frame=_positive_integer(
component, "maximum_candidate_points_per_frame"
),
maximum_cells_per_component=_positive_integer(
component, "maximum_cells_per_component"
),
maximum_components_per_frame=_positive_integer(
component, "maximum_components_per_frame"
),
),
separation_expectations=tuple(expectations),
profile_sha256=hashlib.sha256(raw).hexdigest(),
)
def _voxel_components(
points_map: npt.NDArray[np.float64],
source_indices: IntArray,
profile: LowStepComponentProfile,
) -> tuple[tuple[IntArray, int], ...]:
if source_indices.size == 0:
return ()
cells = np.floor(
points_map[source_indices] / profile.voxel_size_m
).astype(np.int64)
cell_points: dict[tuple[int, int, int], list[int]] = {}
for local_index, row in enumerate(cells):
key = (int(row[0]), int(row[1]), int(row[2]))
cell_points.setdefault(key, []).append(int(source_indices[local_index]))
remaining = set(cell_points)
radius = profile.neighbor_radius_cells
neighbors = tuple(
(dx, dy, dz)
for dx in range(-radius, radius + 1)
for dy in range(-radius, radius + 1)
for dz in range(-radius, radius + 1)
if dx or dy or dz
)
components: list[tuple[IntArray, int]] = []
while remaining:
seed = min(remaining)
remaining.remove(seed)
connected = [seed]
queue = deque((seed,))
while queue:
cell = queue.popleft()
for delta in neighbors:
neighbor = (
cell[0] + delta[0],
cell[1] + delta[1],
cell[2] + delta[2],
)
if neighbor not in remaining:
continue
remaining.remove(neighbor)
connected.append(neighbor)
queue.append(neighbor)
indices = np.asarray(
[point for cell in sorted(connected) for point in cell_points[cell]],
dtype=np.int64,
)
components.append((indices, len(connected)))
components.sort(key=lambda item: int(item[0][0]))
return tuple(components)
def _object(value: object, label: str) -> dict[str, object]:
if not isinstance(value, dict) or not all(isinstance(key, str) for key in value):
raise M48LowStepOccupancyError(f"{label} must be an object")
return value
def _exact_keys(value: dict[str, object], expected: set[str], label: str) -> None:
if set(value) != expected:
raise M48LowStepOccupancyError(f"{label} fields changed")
def _string(value: dict[str, object], key: str) -> str:
item = value.get(key)
if not isinstance(item, str) or not item.strip():
raise M48LowStepOccupancyError(f"{key} must be a non-empty string")
return item
def _positive_integer(value: dict[str, object], key: str) -> int:
item = value.get(key)
if not isinstance(item, int) or isinstance(item, bool) or item < 1:
raise M48LowStepOccupancyError(f"{key} must be a positive integer")
return item
def _number(value: dict[str, object], key: str) -> float:
item = value.get(key)
if not isinstance(item, (int, float)) or isinstance(item, bool):
raise M48LowStepOccupancyError(f"{key} must be numeric")
result = float(item)
if not math.isfinite(result):
raise M48LowStepOccupancyError(f"{key} must be finite")
return result
def _digest(value: dict[str, object], key: str) -> str:
item = _string(value, key)
if len(item) != 64 or any(character not in "0123456789abcdef" for character in item):
raise M48LowStepOccupancyError(f"{key} must be a SHA-256 digest")
return item
__all__ = [
"M48AdditiveLowStepGeometryProvider",
"M48LowStepOccupancyError",
"M48LowStepOccupancyProfile",
"M48LowStepOccupancySnapshot",
"M48_LOW_STEP_PROVIDER_ID",
"load_m48_low_step_occupancy_profile",
]
src/k1link/perception/m48s_reference_graph_runtime.py
+22 -2
View File
@@ -26,6 +26,10 @@ from .geometry import (
)
from .graph import DeliveryEvidenceObserver, ReferencePerceptionGraphV2
from .graph_contracts import DeliveredFrame, GraphRunMode
from .m48_low_step_occupancy import (
M48AdditiveLowStepGeometryProvider,
load_m48_low_step_occupancy_profile,
)
from .motion import ClassIndependentMotionEstimator
from .providers import (
DetectorProvider,
@@ -122,6 +126,7 @@ def build_m48s_reference_graph_runtime(
decode_timing_observer: DecodeTimingObserver | None = None,
source_pacing_observer: SourcePacingObserver | None = None,
detector_timing_observer: DetectorTimingObserver | None = None,
additive_low_step_profile: Path | None = None,
maximum_frames: int | None = None,
source_rate_hz: float | None = None,
source_prefetch_capacity_frames: int = 64,
@@ -133,7 +138,9 @@ def build_m48s_reference_graph_runtime(
pinned_files = {
ProviderRole.SOURCE: paths.baseline_profile,
ProviderRole.DETECTOR: detector_profile,
ProviderRole.GEOMETRY: paths.geometry_profile,
ProviderRole.GEOMETRY: (
additive_low_step_profile or paths.geometry_profile
),
ProviderRole.TEMPORAL: paths.temporal_motion_profile,
ProviderRole.MOTION: paths.temporal_motion_profile,
ProviderRole.ROLLING: paths.rolling_map_profile,
@@ -144,6 +151,11 @@ def build_m48s_reference_graph_runtime(
load_m4_baseline(paths.baseline_profile)
geometry_profile = load_geometry_profile(paths.geometry_profile)
low_step_profile = (
None
if additive_low_step_profile is None
else load_m48_low_step_occupancy_profile(additive_low_step_profile)
)
temporal_motion_profile = load_temporal_motion_profile(paths.temporal_motion_profile)
rolling_map_profile = load_rolling_map_profile(paths.rolling_map_profile)
threat_profile = load_replay_threat_profile(paths.threat_profile)
@@ -204,11 +216,19 @@ def build_m48s_reference_graph_runtime(
store,
profile=threat_profile.body_frame,
)
geometry = (
Ravnoves00GeometryAssociationProvider(store=store)
if low_step_profile is None
else M48AdditiveLowStepGeometryProvider(
store=store,
profile=low_step_profile,
)
)
graph = ReferencePerceptionGraphV2(
config=config,
source=source,
detector=detector,
geometry=Ravnoves00GeometryAssociationProvider(store=store),
geometry=geometry,
temporal=BoundedSpatialTemporalProvider(
point_resolver=store,
profile=temporal_motion_profile,