feat(perception): add occupied-only low-step shadow
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,76 @@
|
||||
{
|
||||
"schema_version": "missioncore.m48-additive-low-step-occupancy-profile/v1",
|
||||
"profile_id": "m48r3-ravnoves00-additive-low-step/v1",
|
||||
"provider_id": "ravnoves00-additive-low-step-geometry/v1",
|
||||
"base_geometry": {
|
||||
"profile_id": "m4-ravnoves00-e29-e32-geometry/v1",
|
||||
"sha256": "cc666c9389a5e221957faddec89584709b66918d14abaf646f1832e001421999"
|
||||
},
|
||||
"source": {
|
||||
"source_id": "RAVNOVES00",
|
||||
"session_id": "20260720T065719Z_viewer_live",
|
||||
"frame_count": 4489,
|
||||
"point_count": 9207270,
|
||||
"local_surface_model_id": "k1-local-surface-23762244c8bdb97de26fb721ac957d7a00bc9a63571ac4cfa4be19c4effc7d55",
|
||||
"local_surface_sha256": "f57eb2485b6cef47f2a97a2d9ff1aa9fd9265fe1eb69cd5852d12f39e13b8bc6",
|
||||
"m48r2_result_id": "m48-static-occupancy-qualification-568024554cff011332ff19ca4739f70555a6232c0607dec2a71be6db408ea69a",
|
||||
"m48r2_cases_sha256": "249f58bf62973bfbcef8cd2f780013830fd4bf61ec342365a541963af96fa2ff"
|
||||
},
|
||||
"componentization": {
|
||||
"voxel_size_m": 0.45,
|
||||
"neighbor_radius_cells": 1,
|
||||
"minimum_points": 2,
|
||||
"minimum_voxels": 1,
|
||||
"local_radius_m": 10.0,
|
||||
"maximum_candidate_points_per_frame": 768,
|
||||
"maximum_cells_per_component": 256,
|
||||
"maximum_components_per_frame": 128,
|
||||
"exclude_baseline_occupied_points": true,
|
||||
"exclude_claimed_source_points": true
|
||||
},
|
||||
"separation_expectations": [
|
||||
{
|
||||
"anchor_id": "anchor-5e6e2a81e0667bdd9faf2a9e",
|
||||
"sequence": 1856,
|
||||
"expected_minimum_components": 2,
|
||||
"interpretation": "two thin posts remain independent occupied components"
|
||||
},
|
||||
{
|
||||
"anchor_id": "anchor-924a4623077fe5df18816b47",
|
||||
"sequence": 1856,
|
||||
"expected_minimum_components": 2,
|
||||
"interpretation": "two concrete hemispheres remain independent occupied components"
|
||||
}
|
||||
],
|
||||
"acceptance": {
|
||||
"expected_frames": 4489,
|
||||
"requested_source_rate_hz": 12.0,
|
||||
"minimum_effective_world_state_fps": 11.5,
|
||||
"maximum_world_state_completion_p95_ms": 60.0,
|
||||
"maximum_geometry_stage_p95_ms": 9.0,
|
||||
"maximum_geometry_stage_p99_ms": 16.0,
|
||||
"maximum_fps_regression_fraction_vs_native_baseline": 0.05,
|
||||
"maximum_world_state_p95_delta_ms_vs_native_baseline": 15.0,
|
||||
"maximum_additive_component_mean_growth_fraction": 1.0,
|
||||
"maximum_additive_cell_mean_growth_fraction": 1.0,
|
||||
"maximum_capacity_drop_count": 0,
|
||||
"minimum_critical_near_recall": 1.0,
|
||||
"minimum_canonical_engineering_recall": 1.0,
|
||||
"maximum_false_free_count": 0
|
||||
},
|
||||
"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
|
||||
},
|
||||
"authority": {
|
||||
"mode": "replay-simulated",
|
||||
"physical_live": false,
|
||||
"commands_enabled": false,
|
||||
"actuation_allowed": false,
|
||||
"navigation_or_safety_accepted": false
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
{
|
||||
"schema_version": "missioncore.reference-perception-graph-config/v2",
|
||||
"graph_id": "reference-perception-graph/v2",
|
||||
"source_profile_id": "m4-ravnoves00-recorded-realtime/v1",
|
||||
"providers": [
|
||||
{
|
||||
"role": "source",
|
||||
"provider_id": "ravnoves00-recorded-source/v1",
|
||||
"version": "1.0.0",
|
||||
"revision": "m4-ravnoves00-recorded-realtime/v1",
|
||||
"sha256": "ea10359339e6cce31b5780a2710299771cab7cc0c1c2a2b56a1621f786b31fa8"
|
||||
},
|
||||
{
|
||||
"role": "detector",
|
||||
"provider_id": "triton-rf-detr-large-coco-native-kb4-risk-fp16-shadow/v0",
|
||||
"version": "0.1.0",
|
||||
"revision": "rf-detr-large-coco-native-kb4-uint8-trt11-fp16-risk-shadow/v0",
|
||||
"sha256": "dbf4da5dbad6c3c22b1280b46ffcad81719bd183c81c263a4859847d829019b6"
|
||||
},
|
||||
{
|
||||
"role": "geometry",
|
||||
"provider_id": "ravnoves00-additive-low-step-geometry/v1",
|
||||
"version": "0.1.0",
|
||||
"revision": "m48r3-ravnoves00-additive-low-step/v1",
|
||||
"sha256": "62dd41400169080e1a38f713b283b67cdf201078c4c7fd807783fe6c1838082c"
|
||||
},
|
||||
{
|
||||
"role": "temporal",
|
||||
"provider_id": "bounded-spatial-temporal-layer/v1",
|
||||
"version": "1.0.0",
|
||||
"revision": "m4-bounded-temporal-motion/v1",
|
||||
"sha256": "7130eaee24a95c7d888bf7598010e03e129e1c3ac5b34bcd8401015ff4244b39"
|
||||
},
|
||||
{
|
||||
"role": "motion",
|
||||
"provider_id": "class-independent-motion-estimator/v1",
|
||||
"version": "1.0.0",
|
||||
"revision": "m4-bounded-temporal-motion/v1",
|
||||
"sha256": "7130eaee24a95c7d888bf7598010e03e129e1c3ac5b34bcd8401015ff4244b39"
|
||||
},
|
||||
{
|
||||
"role": "rolling",
|
||||
"provider_id": "rolling-local-obstacle-map/v1",
|
||||
"version": "1.0.0",
|
||||
"revision": "ravnoves00-rolling-local-obstacle-map/v1",
|
||||
"sha256": "f7e3315eaf6ffaf3aee1e04913933812092cf82bbcc9984c1a6fa2d9250e6784"
|
||||
},
|
||||
{
|
||||
"role": "threat",
|
||||
"provider_id": "dual-evidence-replay-threat/v3",
|
||||
"version": "3.0.0",
|
||||
"revision": "m4-ravnoves00-virtual-corridor/v3",
|
||||
"sha256": "8c3a5aa837da1f028f5998fb504a1381f9b2b68de6420a32160410b6dc0887c7"
|
||||
}
|
||||
],
|
||||
"queues": [
|
||||
{
|
||||
"stage_id": "detector",
|
||||
"capacity": 2,
|
||||
"deadline_ns": 1000000000,
|
||||
"terminal_timeout_ns": 90000000000
|
||||
},
|
||||
{
|
||||
"stage_id": "geometry",
|
||||
"capacity": 2,
|
||||
"deadline_ns": 1500000000,
|
||||
"terminal_timeout_ns": 90000000000
|
||||
},
|
||||
{
|
||||
"stage_id": "temporal",
|
||||
"capacity": 2,
|
||||
"deadline_ns": 1750000000,
|
||||
"terminal_timeout_ns": 90000000000
|
||||
},
|
||||
{
|
||||
"stage_id": "rolling",
|
||||
"capacity": 2,
|
||||
"deadline_ns": 2000000000,
|
||||
"terminal_timeout_ns": 90000000000
|
||||
},
|
||||
{
|
||||
"stage_id": "threat",
|
||||
"capacity": 2,
|
||||
"deadline_ns": 2250000000,
|
||||
"terminal_timeout_ns": 90000000000
|
||||
}
|
||||
],
|
||||
"authority": {
|
||||
"mode": "replay-simulated",
|
||||
"physical_live": false,
|
||||
"commands_enabled": false,
|
||||
"actuation_allowed": false,
|
||||
"navigation_or_safety_accepted": false
|
||||
}
|
||||
}
|
||||
@@ -383,6 +383,7 @@ def main() -> int:
|
||||
):
|
||||
parser.add_argument(f"--{name}", type=Path, required=True)
|
||||
parser.add_argument("--triton-origin", default="http://127.0.0.1:8000")
|
||||
parser.add_argument("--additive-low-step-profile", type=Path)
|
||||
parser.add_argument("--loops", type=int, default=1)
|
||||
parser.add_argument("--maximum-frames", type=int)
|
||||
parser.add_argument("--source-rate-hz", type=float)
|
||||
@@ -477,6 +478,7 @@ def main() -> int:
|
||||
decode_timing_observer=timing_store.observe_decode,
|
||||
source_pacing_observer=timing_store.observe_pacing,
|
||||
detector_timing_observer=timing_store.observe_detector,
|
||||
additive_low_step_profile=arguments.additive_low_step_profile,
|
||||
maximum_frames=arguments.maximum_frames,
|
||||
source_rate_hz=arguments.source_rate_hz,
|
||||
) as runtime:
|
||||
@@ -685,7 +687,11 @@ def main() -> int:
|
||||
"worker_id": "worker-006",
|
||||
"graph_id": "reference-perception-graph/v2",
|
||||
"detector_provider_id": detector_provider_id,
|
||||
"inputs": _input_digests(paths, arguments.detector_profile),
|
||||
"inputs": _input_digests(
|
||||
paths,
|
||||
arguments.detector_profile,
|
||||
arguments.additive_low_step_profile,
|
||||
),
|
||||
"runtime_artifact_sha256": arguments.runtime_artifact_sha256,
|
||||
"runner_sha256": arguments.runner_sha256,
|
||||
},
|
||||
@@ -1105,12 +1111,15 @@ def _gc_telemetry_summary(events: list[dict[str, object]]) -> dict[str, object]:
|
||||
def _input_digests(
|
||||
paths: ReferenceGraphRuntimePaths,
|
||||
detector_profile: Path,
|
||||
additive_low_step_profile: Path | None = None,
|
||||
) -> dict[str, str]:
|
||||
values = {
|
||||
field.name: _sha256(getattr(paths, field.name))
|
||||
for field in fields(ReferenceGraphRuntimePaths)
|
||||
}
|
||||
values["detector_profile"] = _sha256(detector_profile)
|
||||
if additive_low_step_profile is not None:
|
||||
values["additive_low_step_profile"] = _sha256(additive_low_step_profile)
|
||||
return dict(sorted(values.items()))
|
||||
|
||||
|
||||
|
||||
@@ -20,6 +20,7 @@ param(
|
||||
[double]$MinimumEffectiveWorldStateFps = 9.5,
|
||||
[ValidateRange(1.0, 10000.0)]
|
||||
[double]$MaximumWorldStateCompletionP95Ms = 125.0,
|
||||
[switch]$AdditiveLowStep,
|
||||
[string]$OutputRoot = (
|
||||
"D:\NDC_MISSIONCORE\runtime\results\m48n-native-reference-graph-shadow"
|
||||
)
|
||||
@@ -109,6 +110,15 @@ $expectedConfigs = [ordered]@{
|
||||
"8c3a5aa837da1f028f5998fb504a1381f9b2b68de6420a32160410b6dc0887c7"
|
||||
)
|
||||
}
|
||||
if ($AdditiveLowStep) {
|
||||
$expectedConfigs.Remove("m48n-rf-detr-native-reference-graph-shadow-v0.json")
|
||||
$expectedConfigs["m48r3-native-low-step-reference-graph-shadow-v1.json"] = (
|
||||
"d5434825c3a73a9a070aea37927755771e07d38a017111d2014a8e8c88969227"
|
||||
)
|
||||
$expectedConfigs["m48r3-additive-low-step-occupancy-v1.json"] = (
|
||||
"62dd41400169080e1a38f713b283b67cdf201078c4c7fd807783fe6c1838082c"
|
||||
)
|
||||
}
|
||||
foreach ($entry in $expectedConfigs.GetEnumerator()) {
|
||||
$null = Assert-File (Join-Path $release $entry.Key) $entry.Value (
|
||||
"M48N config {0}" -f $entry.Key
|
||||
@@ -203,8 +213,16 @@ if (-not $canonicalTriton.State.Running -or $canonicalTriton.State.Health.Status
|
||||
throw "Canonical Triton must remain healthy during M48N shadow"
|
||||
}
|
||||
$canonicalTritonId = [string]$canonicalTriton.Id
|
||||
$tritonName = "ndc-mission-core-m48n-native-reference-graph-triton"
|
||||
$graphName = "ndc-mission-core-m48n-native-reference-graph"
|
||||
$tritonName = if ($AdditiveLowStep) {
|
||||
"ndc-mission-core-m48r3-low-step-triton"
|
||||
} else {
|
||||
"ndc-mission-core-m48n-native-reference-graph-triton"
|
||||
}
|
||||
$graphName = if ($AdditiveLowStep) {
|
||||
"ndc-mission-core-m48r3-low-step-reference-graph"
|
||||
} else {
|
||||
"ndc-mission-core-m48n-native-reference-graph"
|
||||
}
|
||||
foreach ($name in @($tritonName, $graphName)) {
|
||||
if (& docker ps -a --format "{{.Names}}" --filter "name=^/$name$") {
|
||||
throw "M48N candidate container $name already exists"
|
||||
@@ -263,6 +281,19 @@ try {
|
||||
throw "M48N Triton published a host port"
|
||||
}
|
||||
|
||||
$graphConfigName = if ($AdditiveLowStep) {
|
||||
"m48r3-native-low-step-reference-graph-shadow-v1.json"
|
||||
} else {
|
||||
"m48n-rf-detr-native-reference-graph-shadow-v0.json"
|
||||
}
|
||||
$additiveArguments = if ($AdditiveLowStep) {
|
||||
@(
|
||||
"--additive-low-step-profile",
|
||||
"/release/m48r3-additive-low-step-occupancy-v1.json"
|
||||
)
|
||||
} else {
|
||||
@()
|
||||
}
|
||||
$arguments = @(
|
||||
"run", "--name", $graphName,
|
||||
"--label", "com.nodedc.product=mission-core",
|
||||
@@ -294,7 +325,7 @@ try {
|
||||
"--entrypoint", "python3",
|
||||
$image,
|
||||
"/release/run_m48s_reference_graph_shadow_worker.py",
|
||||
"--graph-config", "/release/m48n-rf-detr-native-reference-graph-shadow-v0.json",
|
||||
"--graph-config", ("/release/{0}" -f $graphConfigName),
|
||||
"--baseline-profile", "/release/m4-recorded-realtime-baseline-v1.json",
|
||||
"--detector-profile", "/release/rf-detr-large-native-kb4-risk-shadow-v0.json",
|
||||
"--geometry-profile", "/release/m4-geometry-association-v1.json",
|
||||
@@ -331,7 +362,7 @@ try {
|
||||
"--output", "/output/result.json",
|
||||
"--progress", "/output/progress.jsonl",
|
||||
"--frame-ledger", "/output/frames.jsonl"
|
||||
)
|
||||
) + $additiveArguments
|
||||
& docker @arguments
|
||||
Assert-LastExitCode "M48N native complete reference graph shadow"
|
||||
foreach ($name in @("result.json", "frames.jsonl", "progress.jsonl")) {
|
||||
|
||||
@@ -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",
|
||||
]
|
||||
@@ -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,
|
||||
|
||||
@@ -0,0 +1,203 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from k1link.perception.contracts import (
|
||||
ClockBasis,
|
||||
ModalityOutcome,
|
||||
ModalityStatus,
|
||||
ObstacleObservation,
|
||||
SourceEnvelope,
|
||||
TimestampBundle,
|
||||
)
|
||||
from k1link.perception.geometry import GeometryFrame, RecordedGeometryStore
|
||||
from k1link.perception.geometry_math import Kb4ProjectionProfile, project_map_points_kb4
|
||||
from k1link.perception.m48_low_step_occupancy import (
|
||||
M48AdditiveLowStepGeometryProvider,
|
||||
load_m48_low_step_occupancy_profile,
|
||||
)
|
||||
from k1link.perception.providers import SourcePacket
|
||||
from k1link.perception.recorded_source import RecordedFrameReference
|
||||
|
||||
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
||||
PROFILE_PATH = (
|
||||
REPOSITORY_ROOT
|
||||
/ "config/perception/m48r3-additive-low-step-occupancy-v1.json"
|
||||
)
|
||||
R2_CASES_PATH = (
|
||||
REPOSITORY_ROOT
|
||||
/ ".runtime/compute-experiments/m48/static-occupancy-qualification-results"
|
||||
/ (
|
||||
"m48-static-occupancy-qualification-"
|
||||
"568024554cff011332ff19ca4739f70555a6232c0607dec2a71be6db408ea69a"
|
||||
)
|
||||
/ "cases.jsonl"
|
||||
)
|
||||
|
||||
|
||||
class _Store:
|
||||
def __init__(self, frame: GeometryFrame, step: np.ndarray) -> None:
|
||||
self.profile = RecordedGeometryStore.from_repository(REPOSITORY_ROOT).profile
|
||||
self._frame = frame
|
||||
self._step = np.asarray(step, dtype=np.uint8)
|
||||
|
||||
def frame(self, _packet: SourcePacket) -> GeometryFrame:
|
||||
return self._frame
|
||||
|
||||
def point_step_candidates_for_frame(self, _frame_index: int) -> np.ndarray:
|
||||
return self._step
|
||||
|
||||
|
||||
def _status() -> ModalityStatus:
|
||||
return ModalityStatus(True, ModalityOutcome.AVAILABLE, "test-available")
|
||||
|
||||
|
||||
def _packet(sequence: int = 0) -> SourcePacket:
|
||||
profile = RecordedGeometryStore.from_repository(REPOSITORY_ROOT).profile
|
||||
reference = RecordedFrameReference(profile.source_pack_id, sequence)
|
||||
return SourcePacket(
|
||||
envelope=SourceEnvelope(
|
||||
source_id=profile.source_id,
|
||||
session_id=profile.session_id,
|
||||
frame_id=f"frame-{sequence:06d}",
|
||||
sequence=sequence,
|
||||
timestamps=TimestampBundle(
|
||||
utc_ns=sequence + 1,
|
||||
monotonic_ns=sequence + 2,
|
||||
source_ns=sequence + 3,
|
||||
clock_basis=ClockBasis.RECORDED_HOST,
|
||||
),
|
||||
source_age_ns=0,
|
||||
binding_reason="test-recorded-source",
|
||||
calibration_id="camera-1-kb4-test",
|
||||
representation_id="registered-map-increment-v1",
|
||||
image=_status(),
|
||||
registered_point_increment=_status(),
|
||||
pose=_status(),
|
||||
),
|
||||
image_payload="image",
|
||||
registered_point_increment_payload=reference,
|
||||
pose_payload=reference,
|
||||
)
|
||||
|
||||
|
||||
def _frame(points: np.ndarray) -> GeometryFrame:
|
||||
return GeometryFrame(
|
||||
frame_index=0,
|
||||
points_map=np.asarray(points, dtype=np.float64),
|
||||
point_class=np.ones(points.shape[0], dtype=np.uint8),
|
||||
sensor_position_map=np.zeros(3, dtype=np.float64),
|
||||
sensor_orientation_xyzw=np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float64),
|
||||
projection=Kb4ProjectionProfile(
|
||||
width=100,
|
||||
height=100,
|
||||
intrinsic_fx_fy_cx_cy=(100.0, 100.0, 50.0, 50.0),
|
||||
distortion_kb4=(0.0, 0.0, 0.0, 0.0),
|
||||
t_camera_from_lidar=np.eye(4, dtype=np.float64),
|
||||
),
|
||||
surface_valid=True,
|
||||
)
|
||||
|
||||
|
||||
def test_wide_operator_region_cannot_bridge_two_spatial_components() -> None:
|
||||
points = np.asarray(
|
||||
(
|
||||
(0.00, 0.0, 5.00),
|
||||
(0.10, 0.0, 5.05),
|
||||
(1.70, 0.0, 5.00),
|
||||
(1.80, 0.0, 5.05),
|
||||
),
|
||||
dtype=np.float64,
|
||||
)
|
||||
store = _Store(_frame(points), np.ones(4, dtype=np.uint8))
|
||||
provider = M48AdditiveLowStepGeometryProvider( # type: ignore[arg-type]
|
||||
store=store,
|
||||
profile=load_m48_low_step_occupancy_profile(PROFILE_PATH),
|
||||
)
|
||||
|
||||
observations = provider.associate(_packet(), ())
|
||||
additive = tuple(
|
||||
item
|
||||
for item in observations
|
||||
if "additive-low-step-current-component" in item.reason_codes
|
||||
)
|
||||
|
||||
assert len(additive) == 2
|
||||
assert {item.source_point_ids for item in additive} == {(0, 1), (2, 3)}
|
||||
assert all(item.semantic_hint is None for item in additive)
|
||||
assert all("occupied-only-never-free" in item.reason_codes for item in additive)
|
||||
snapshot = provider.snapshot()
|
||||
assert snapshot.additive_observation_count == 2
|
||||
assert snapshot.additive_voxel_count == 3
|
||||
assert snapshot.failed_frames == 0
|
||||
|
||||
|
||||
def test_frame_1856_preserves_baseline_posts_and_splits_low_hemisphere_support() -> None:
|
||||
store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT)
|
||||
provider = M48AdditiveLowStepGeometryProvider(
|
||||
store=store,
|
||||
profile=load_m48_low_step_occupancy_profile(PROFILE_PATH),
|
||||
)
|
||||
frame = store.frame_for_index(1856)
|
||||
assert frame is not None
|
||||
observations = provider.associate(_packet(1856), ())
|
||||
projected = project_map_points_kb4(
|
||||
frame.points_map,
|
||||
position_map_xyz=frame.sensor_position_map,
|
||||
orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw,
|
||||
profile=frame.projection,
|
||||
)
|
||||
source_rows = {
|
||||
int(source_index): row
|
||||
for row, source_index in enumerate(projected.source_indices)
|
||||
}
|
||||
cases = [
|
||||
json.loads(line)
|
||||
for line in R2_CASES_PATH.read_text("utf-8").splitlines()
|
||||
if line.strip() and json.loads(line)["sequence"] == 1856
|
||||
]
|
||||
by_anchor = {row["anchor_id"]: row for row in cases}
|
||||
posts = by_anchor["anchor-5e6e2a81e0667bdd9faf2a9e"]
|
||||
hemispheres = by_anchor["anchor-924a4623077fe5df18816b47"]
|
||||
|
||||
assert posts["accepted_graph"]["component_count"] >= 2
|
||||
assert _component_hits(
|
||||
observations,
|
||||
hemispheres["extent_xyxy"],
|
||||
projected.pixels_xy,
|
||||
source_rows,
|
||||
width=frame.projection.width,
|
||||
height=frame.projection.height,
|
||||
) >= 2
|
||||
|
||||
|
||||
def _component_hits(
|
||||
observations: tuple[ObstacleObservation, ...],
|
||||
extent: list[float],
|
||||
pixels: np.ndarray,
|
||||
source_rows: dict[int, int],
|
||||
*,
|
||||
width: int,
|
||||
height: int,
|
||||
) -> int:
|
||||
x1, y1, x2, y2 = (
|
||||
extent[0] * width,
|
||||
extent[1] * height,
|
||||
extent[2] * width,
|
||||
extent[3] * height,
|
||||
)
|
||||
count = 0
|
||||
for value in observations:
|
||||
indices = value.source_point_ids
|
||||
if not indices:
|
||||
continue
|
||||
rows = [source_rows[index] for index in indices if index in source_rows]
|
||||
if any(
|
||||
x1 <= pixels[row, 0] <= x2 and y1 <= pixels[row, 1] <= y2
|
||||
for row in rows
|
||||
):
|
||||
count += 1
|
||||
return count
|
||||
@@ -24,6 +24,10 @@ NATIVE_GRAPH_CONFIG = (
|
||||
REPOSITORY_ROOT
|
||||
/ "config/perception/m48n-rf-detr-native-reference-graph-shadow-v0.json"
|
||||
)
|
||||
LOW_STEP_GRAPH_CONFIG = (
|
||||
REPOSITORY_ROOT
|
||||
/ "config/perception/m48r3-native-low-step-reference-graph-shadow-v1.json"
|
||||
)
|
||||
|
||||
|
||||
def test_m48s_reference_graph_replaces_only_the_detector_pin() -> None:
|
||||
@@ -116,6 +120,42 @@ def test_m48n_native_reference_graph_pins_every_profile_digest() -> None:
|
||||
assert pins[role].sha256 == hashlib.sha256(payload).hexdigest()
|
||||
|
||||
|
||||
def test_m48r3_graph_replaces_only_the_geometry_pin() -> None:
|
||||
candidate = ReferencePerceptionGraphConfigV2.from_dict(
|
||||
json.loads(LOW_STEP_GRAPH_CONFIG.read_text("utf-8"))
|
||||
)
|
||||
native = ReferencePerceptionGraphConfigV2.from_dict(
|
||||
json.loads(NATIVE_GRAPH_CONFIG.read_text("utf-8"))
|
||||
)
|
||||
candidate_pins = {item.role: item for item in candidate.providers}
|
||||
native_pins = {item.role: item for item in native.providers}
|
||||
|
||||
assert candidate.graph_id == native.graph_id == "reference-perception-graph/v2"
|
||||
assert candidate.queues == native.queues
|
||||
assert candidate.authority == native.authority
|
||||
assert candidate_pins[ProviderRole.GEOMETRY].provider_id == (
|
||||
"ravnoves00-additive-low-step-geometry/v1"
|
||||
)
|
||||
assert all(
|
||||
candidate_pins[role] == native_pins[role]
|
||||
for role in ProviderRole
|
||||
if role is not ProviderRole.GEOMETRY
|
||||
)
|
||||
|
||||
|
||||
def test_m48r3_graph_pins_the_additive_profile_digest() -> None:
|
||||
config = ReferencePerceptionGraphConfigV2.from_dict(
|
||||
json.loads(LOW_STEP_GRAPH_CONFIG.read_text("utf-8"))
|
||||
)
|
||||
pin = {item.role: item for item in config.providers}[ProviderRole.GEOMETRY]
|
||||
payload = (
|
||||
REPOSITORY_ROOT
|
||||
/ "config/perception/m48r3-additive-low-step-occupancy-v1.json"
|
||||
).read_bytes()
|
||||
|
||||
assert pin.sha256 == hashlib.sha256(payload).hexdigest()
|
||||
|
||||
|
||||
def test_m48s_advisory_policy_is_bounded_distinct_and_commandless() -> None:
|
||||
matrix = advisory_policy_matrix()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user