feat(perception): add occupied-only low-step shadow
This commit is contained in:
@@ -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(f"--{name}", type=Path, required=True)
|
||||||
parser.add_argument("--triton-origin", default="http://127.0.0.1:8000")
|
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("--loops", type=int, default=1)
|
||||||
parser.add_argument("--maximum-frames", type=int)
|
parser.add_argument("--maximum-frames", type=int)
|
||||||
parser.add_argument("--source-rate-hz", type=float)
|
parser.add_argument("--source-rate-hz", type=float)
|
||||||
@@ -477,6 +478,7 @@ def main() -> int:
|
|||||||
decode_timing_observer=timing_store.observe_decode,
|
decode_timing_observer=timing_store.observe_decode,
|
||||||
source_pacing_observer=timing_store.observe_pacing,
|
source_pacing_observer=timing_store.observe_pacing,
|
||||||
detector_timing_observer=timing_store.observe_detector,
|
detector_timing_observer=timing_store.observe_detector,
|
||||||
|
additive_low_step_profile=arguments.additive_low_step_profile,
|
||||||
maximum_frames=arguments.maximum_frames,
|
maximum_frames=arguments.maximum_frames,
|
||||||
source_rate_hz=arguments.source_rate_hz,
|
source_rate_hz=arguments.source_rate_hz,
|
||||||
) as runtime:
|
) as runtime:
|
||||||
@@ -685,7 +687,11 @@ def main() -> int:
|
|||||||
"worker_id": "worker-006",
|
"worker_id": "worker-006",
|
||||||
"graph_id": "reference-perception-graph/v2",
|
"graph_id": "reference-perception-graph/v2",
|
||||||
"detector_provider_id": detector_provider_id,
|
"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,
|
"runtime_artifact_sha256": arguments.runtime_artifact_sha256,
|
||||||
"runner_sha256": arguments.runner_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(
|
def _input_digests(
|
||||||
paths: ReferenceGraphRuntimePaths,
|
paths: ReferenceGraphRuntimePaths,
|
||||||
detector_profile: Path,
|
detector_profile: Path,
|
||||||
|
additive_low_step_profile: Path | None = None,
|
||||||
) -> dict[str, str]:
|
) -> dict[str, str]:
|
||||||
values = {
|
values = {
|
||||||
field.name: _sha256(getattr(paths, field.name))
|
field.name: _sha256(getattr(paths, field.name))
|
||||||
for field in fields(ReferenceGraphRuntimePaths)
|
for field in fields(ReferenceGraphRuntimePaths)
|
||||||
}
|
}
|
||||||
values["detector_profile"] = _sha256(detector_profile)
|
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()))
|
return dict(sorted(values.items()))
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -20,6 +20,7 @@ param(
|
|||||||
[double]$MinimumEffectiveWorldStateFps = 9.5,
|
[double]$MinimumEffectiveWorldStateFps = 9.5,
|
||||||
[ValidateRange(1.0, 10000.0)]
|
[ValidateRange(1.0, 10000.0)]
|
||||||
[double]$MaximumWorldStateCompletionP95Ms = 125.0,
|
[double]$MaximumWorldStateCompletionP95Ms = 125.0,
|
||||||
|
[switch]$AdditiveLowStep,
|
||||||
[string]$OutputRoot = (
|
[string]$OutputRoot = (
|
||||||
"D:\NDC_MISSIONCORE\runtime\results\m48n-native-reference-graph-shadow"
|
"D:\NDC_MISSIONCORE\runtime\results\m48n-native-reference-graph-shadow"
|
||||||
)
|
)
|
||||||
@@ -109,6 +110,15 @@ $expectedConfigs = [ordered]@{
|
|||||||
"8c3a5aa837da1f028f5998fb504a1381f9b2b68de6420a32160410b6dc0887c7"
|
"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()) {
|
foreach ($entry in $expectedConfigs.GetEnumerator()) {
|
||||||
$null = Assert-File (Join-Path $release $entry.Key) $entry.Value (
|
$null = Assert-File (Join-Path $release $entry.Key) $entry.Value (
|
||||||
"M48N config {0}" -f $entry.Key
|
"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"
|
throw "Canonical Triton must remain healthy during M48N shadow"
|
||||||
}
|
}
|
||||||
$canonicalTritonId = [string]$canonicalTriton.Id
|
$canonicalTritonId = [string]$canonicalTriton.Id
|
||||||
$tritonName = "ndc-mission-core-m48n-native-reference-graph-triton"
|
$tritonName = if ($AdditiveLowStep) {
|
||||||
$graphName = "ndc-mission-core-m48n-native-reference-graph"
|
"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)) {
|
foreach ($name in @($tritonName, $graphName)) {
|
||||||
if (& docker ps -a --format "{{.Names}}" --filter "name=^/$name$") {
|
if (& docker ps -a --format "{{.Names}}" --filter "name=^/$name$") {
|
||||||
throw "M48N candidate container $name already exists"
|
throw "M48N candidate container $name already exists"
|
||||||
@@ -263,6 +281,19 @@ try {
|
|||||||
throw "M48N Triton published a host port"
|
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 = @(
|
$arguments = @(
|
||||||
"run", "--name", $graphName,
|
"run", "--name", $graphName,
|
||||||
"--label", "com.nodedc.product=mission-core",
|
"--label", "com.nodedc.product=mission-core",
|
||||||
@@ -294,7 +325,7 @@ try {
|
|||||||
"--entrypoint", "python3",
|
"--entrypoint", "python3",
|
||||||
$image,
|
$image,
|
||||||
"/release/run_m48s_reference_graph_shadow_worker.py",
|
"/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",
|
"--baseline-profile", "/release/m4-recorded-realtime-baseline-v1.json",
|
||||||
"--detector-profile", "/release/rf-detr-large-native-kb4-risk-shadow-v0.json",
|
"--detector-profile", "/release/rf-detr-large-native-kb4-risk-shadow-v0.json",
|
||||||
"--geometry-profile", "/release/m4-geometry-association-v1.json",
|
"--geometry-profile", "/release/m4-geometry-association-v1.json",
|
||||||
@@ -331,7 +362,7 @@ try {
|
|||||||
"--output", "/output/result.json",
|
"--output", "/output/result.json",
|
||||||
"--progress", "/output/progress.jsonl",
|
"--progress", "/output/progress.jsonl",
|
||||||
"--frame-ledger", "/output/frames.jsonl"
|
"--frame-ledger", "/output/frames.jsonl"
|
||||||
)
|
) + $additiveArguments
|
||||||
& docker @arguments
|
& docker @arguments
|
||||||
Assert-LastExitCode "M48N native complete reference graph shadow"
|
Assert-LastExitCode "M48N native complete reference graph shadow"
|
||||||
foreach ($name in @("result.json", "frames.jsonl", "progress.jsonl")) {
|
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 import DeliveryEvidenceObserver, ReferencePerceptionGraphV2
|
||||||
from .graph_contracts import DeliveredFrame, GraphRunMode
|
from .graph_contracts import DeliveredFrame, GraphRunMode
|
||||||
|
from .m48_low_step_occupancy import (
|
||||||
|
M48AdditiveLowStepGeometryProvider,
|
||||||
|
load_m48_low_step_occupancy_profile,
|
||||||
|
)
|
||||||
from .motion import ClassIndependentMotionEstimator
|
from .motion import ClassIndependentMotionEstimator
|
||||||
from .providers import (
|
from .providers import (
|
||||||
DetectorProvider,
|
DetectorProvider,
|
||||||
@@ -122,6 +126,7 @@ def build_m48s_reference_graph_runtime(
|
|||||||
decode_timing_observer: DecodeTimingObserver | None = None,
|
decode_timing_observer: DecodeTimingObserver | None = None,
|
||||||
source_pacing_observer: SourcePacingObserver | None = None,
|
source_pacing_observer: SourcePacingObserver | None = None,
|
||||||
detector_timing_observer: DetectorTimingObserver | None = None,
|
detector_timing_observer: DetectorTimingObserver | None = None,
|
||||||
|
additive_low_step_profile: Path | None = None,
|
||||||
maximum_frames: int | None = None,
|
maximum_frames: int | None = None,
|
||||||
source_rate_hz: float | None = None,
|
source_rate_hz: float | None = None,
|
||||||
source_prefetch_capacity_frames: int = 64,
|
source_prefetch_capacity_frames: int = 64,
|
||||||
@@ -133,7 +138,9 @@ def build_m48s_reference_graph_runtime(
|
|||||||
pinned_files = {
|
pinned_files = {
|
||||||
ProviderRole.SOURCE: paths.baseline_profile,
|
ProviderRole.SOURCE: paths.baseline_profile,
|
||||||
ProviderRole.DETECTOR: detector_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.TEMPORAL: paths.temporal_motion_profile,
|
||||||
ProviderRole.MOTION: paths.temporal_motion_profile,
|
ProviderRole.MOTION: paths.temporal_motion_profile,
|
||||||
ProviderRole.ROLLING: paths.rolling_map_profile,
|
ProviderRole.ROLLING: paths.rolling_map_profile,
|
||||||
@@ -144,6 +151,11 @@ def build_m48s_reference_graph_runtime(
|
|||||||
|
|
||||||
load_m4_baseline(paths.baseline_profile)
|
load_m4_baseline(paths.baseline_profile)
|
||||||
geometry_profile = load_geometry_profile(paths.geometry_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)
|
temporal_motion_profile = load_temporal_motion_profile(paths.temporal_motion_profile)
|
||||||
rolling_map_profile = load_rolling_map_profile(paths.rolling_map_profile)
|
rolling_map_profile = load_rolling_map_profile(paths.rolling_map_profile)
|
||||||
threat_profile = load_replay_threat_profile(paths.threat_profile)
|
threat_profile = load_replay_threat_profile(paths.threat_profile)
|
||||||
@@ -204,11 +216,19 @@ def build_m48s_reference_graph_runtime(
|
|||||||
store,
|
store,
|
||||||
profile=threat_profile.body_frame,
|
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(
|
graph = ReferencePerceptionGraphV2(
|
||||||
config=config,
|
config=config,
|
||||||
source=source,
|
source=source,
|
||||||
detector=detector,
|
detector=detector,
|
||||||
geometry=Ravnoves00GeometryAssociationProvider(store=store),
|
geometry=geometry,
|
||||||
temporal=BoundedSpatialTemporalProvider(
|
temporal=BoundedSpatialTemporalProvider(
|
||||||
point_resolver=store,
|
point_resolver=store,
|
||||||
profile=temporal_motion_profile,
|
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
|
REPOSITORY_ROOT
|
||||||
/ "config/perception/m48n-rf-detr-native-reference-graph-shadow-v0.json"
|
/ "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:
|
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()
|
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:
|
def test_m48s_advisory_policy_is_bounded_distinct_and_commandless() -> None:
|
||||||
matrix = advisory_policy_matrix()
|
matrix = advisory_policy_matrix()
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user