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

fix(perception): stabilize replay body frame

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-05 20:04:08 +03:00
parent de19229895
commit c70ad345ea
12 changed files with 783 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/control-station/src/core/laboratory/m4ReplayThreat.ts
+46
View File
@@ -32,6 +32,15 @@ export interface M4ThreatReplayResult {
providerLatencyP95Ms: number;
providerLatencyMaxMs: number;
};
bodyFrame: {
available: number;
qualified: number;
rejected: number;
cameraForwardAlignmentDeg: {
p95: number;
maximum: number;
};
};
reasonCounts: Readonly<Record<string, number>>;
};
configuration: {
@@ -39,6 +48,11 @@ export interface M4ThreatReplayResult {
nominalSensorHeightM: number;
forwardCorridorM: number;
predictionHorizonSeconds: number;
bodyFrame: {
origin: "local-surface-vertical-projection";
up: "vendor-slam-map-gravity-axis";
forward: "smoothed-slam-trajectory-validated-by-camera-axis";
};
};
limitations: readonly string[];
}
@@ -254,7 +268,13 @@ export async function fetchM4ThreatReplayResult({
const evidence = object(metrics.evidence, "M4.6 evidence");
const fixtures = object(metrics.fixtures, "M4.6 fixtures");
const runtime = object(metrics.runtime, "M4.6 runtime");
const bodyFrame = object(metrics.body_frame, "M4.6 body frame");
const cameraAlignment = object(
bodyFrame.camera_forward_alignment_deg,
"M4.6 camera alignment",
);
const configuration = object(item.configuration, "M4.6 configuration");
const configuredBodyFrame = object(configuration.body_frame, "M4.6 configured body frame");
const sourceResultIds = object(item.source_result_ids, "M4.6 sources");
return {
resultId: resultId(item.result_id),
@@ -290,6 +310,15 @@ export async function fetchM4ThreatReplayResult({
providerLatencyP95Ms: number(runtime.provider_latency_p95_ms, "M4.6 p95"),
providerLatencyMaxMs: number(runtime.provider_latency_max_ms, "M4.6 max"),
},
bodyFrame: {
available: integer(bodyFrame.available, "M4.6 available body frames"),
qualified: integer(bodyFrame.qualified, "M4.6 qualified body frames"),
rejected: integer(bodyFrame.rejected, "M4.6 rejected body frames"),
cameraForwardAlignmentDeg: {
p95: number(cameraAlignment.p95, "M4.6 body frame camera alignment p95"),
maximum: number(cameraAlignment.maximum, "M4.6 body frame camera alignment maximum"),
},
},
reasonCounts: Object.fromEntries(
Object.entries(object(metrics.reason_counts, "M4.6 reasons")).map(
([key, value]) => [key, integer(value, `M4.6 ${key}`)],
@@ -301,6 +330,23 @@ export async function fetchM4ThreatReplayResult({
nominalSensorHeightM: number(configuration.nominal_sensor_height_m, "M4.6 height"),
forwardCorridorM: number(configuration.forward_corridor_m, "M4.6 corridor"),
predictionHorizonSeconds: number(configuration.prediction_horizon_seconds, "M4.6 horizon"),
bodyFrame: {
origin: exact(
configuredBodyFrame.origin,
"local-surface-vertical-projection",
"M4.6 body frame origin",
),
up: exact(
configuredBodyFrame.up,
"vendor-slam-map-gravity-axis",
"M4.6 body frame up",
),
forward: exact(
configuredBodyFrame.forward,
"smoothed-slam-trajectory-validated-by-camera-axis",
"M4.6 body frame forward",
),
},
},
limitations: array(item.limitations, "M4.6 limitations").map((value) => text(value, "M4.6 limitation")),
};
apps/control-station/src/workspaces/laboratory/M4ReplayThreatResult.tsx
+9 -5
View File
@@ -41,6 +41,10 @@ export function M4ReplayThreatResultView({
label: "Коридор",
value: `${result.configuration.forwardCorridorM} м · horizon ${result.configuration.predictionHorizonSeconds} с`,
},
{
label: "Опорная СК",
value: `SLAM gravity · route-forward · ${metrics.bodyFrame.qualified}/${metrics.bodyFrame.available} qualified`,
},
{
label: "Визуал",
value: "4489-frame VIDEO · 32 exact CAMERA/3D/PLAN samples",
@@ -48,14 +52,14 @@ export function M4ReplayThreatResultView({
]}
brief={{
question: "Может ли единый слой обнаруживать потенциальное препятствие по двум независимым источникам, не теряя LiDAR-only объекты и не объявляя camera-only наблюдение безопасным?",
approach: "Все 4489 кадров RAVNOVES00 повторно пропущены через неизменяемые detector, metric geometry и temporal ledgers. Geometry-only объекты получают метрическую оценку; camera-only и stale/held остаются unknown. Отдельная матрица из 9 детерминированных сценариев проверяет статические, сближающиеся и расходящиеся случаи.",
approach: "Все 4489 кадров RAVNOVES00 повторно пропущены через неизменяемые detector, metric geometry и temporal ledgers. Виртуальный base_footprint привязан к gravity-оси SLAM map и направлению сглаженной траектории, проверенному camera extrinsic. Geometry-only объекты получают метрическую оценку; camera-only и stale/held остаются unknown.",
principalResult: `${metrics.evidence.currentMetric.toLocaleString("ru-RU")} current metric и ${metrics.evidence.cameraOnly.toLocaleString("ru-RU")} camera-only наблюдений учтены; ${metrics.reasonCounts["geometry-only-evidence"]?.toLocaleString("ru-RU") ?? "0"} geometry-only оценок не потеряны. Критические fixtures: ${metrics.fixtures.passed}/${metrics.fixtures.total}, ложных safe: ${metrics.fixtures.criticalFalseNotThreat}.`,
limitation: "Корпус и коридор пока виртуальные, replay не является live-проходом или физическим collision test. Постоянная скорость — ограниченная модель, а independent object truth остаётся следующим gate.",
limitation: "Корпус и коридор пока виртуальные, replay не является live-проходом или физическим collision test. На машине виртуальная привязка должна замениться измеренным rigid T_body_from_sensor; independent object truth остаётся следующим gate.",
}}
method={{
completeness: "complete",
executionClass: "hybrid",
pipelineId: "dual-evidence-replay-threat/v1",
pipelineId: "dual-evidence-replay-threat/v2",
components: [
{
kind: "source",
@@ -127,8 +131,8 @@ export function M4ReplayThreatResultView({
},
]}
conclusion={{
proved: "На неизменяемом RAVNOVES00 каждый metric, stale/held и camera-only объект получил ровно одну консервативную оценку. Geometry-only препятствия участвуют в threat-решении без класса, camera-only и просроченные данные не превращаются в safe. Видео, точные camera samples и метрическое 3D-доказательство доступны в одном viewer.",
notProved: "Не доказаны live realtime, измеренная геометрия физического корпуса, независимая object-level правильность, навигационная или safety-пригодность и выдача команд.",
proved: `На неизменяемом RAVNOVES00 каждый metric, stale/held и camera-only объект получил ровно одну консервативную оценку. ${metrics.bodyFrame.qualified}/${metrics.bodyFrame.available} доступных body frames квалифицированы без переноса handheld roll/pitch на SLAM-мир; camera/route alignment p95 ${formatNumber(metrics.bodyFrame.cameraForwardAlignmentDeg.p95, 1)}°. Видео и точное 3D-доказательство доступны в одном viewer.`,
notProved: "Не доказаны live realtime, измеренный T_body_from_sensor и геометрия физического корпуса, независимая object-level правильность, навигационная или safety-пригодность и выдача команд.",
decision: "Сохранить dual-evidence provider как канонический replay seam и переходить к независимому object-centric gate; физическую геометрию и live/actuation authority не смешивать с дальнейшей CV-разработкой.",
}}
/>
apps/control-station/test/m4ReplayThreat.test.mjs
+16 -2
View File
@@ -54,8 +54,8 @@ test("M4.6 decodes accepted dual-evidence result without physical authority", as
created_at_utc: "2026-08-05T15:36:01.553Z",
status: "accepted",
profile_id: "m4-ravnoves00-virtual-corridor/v1",
rig_profile_id: "virtual-handheld-body-1000x600/v1",
corridor_profile_id: "ravnoves00-forward-corridor-8m/v1",
rig_profile_id: "virtual-base-footprint-1000x600/v2",
corridor_profile_id: "ravnoves00-forward-corridor-8m/v2",
source_result_ids: {
detector: `m4-detector-replay-${"b".repeat(64)}`,
geometry: `m4-geometry-replay-${"c".repeat(64)}`,
@@ -71,6 +71,12 @@ test("M4.6 decodes accepted dual-evidence result without physical authority", as
provider_latency_p95_ms: 19.8,
provider_latency_max_ms: 194.3,
},
body_frame: {
available: 3928,
qualified: 3861,
rejected: 67,
camera_forward_alignment_deg: { p95: 8.439, maximum: 24.252 },
},
reason_counts: { "geometry-only-evidence": 21958 },
},
configuration: {
@@ -78,6 +84,11 @@ test("M4.6 decodes accepted dual-evidence result without physical authority", as
nominal_sensor_height_m: 1.25,
forward_corridor_m: 8,
prediction_horizon_seconds: 5,
body_frame: {
origin: "local-surface-vertical-projection",
up: "vendor-slam-map-gravity-axis",
forward: "smoothed-slam-trajectory-validated-by-camera-axis",
},
},
limitations: ["replay only"],
accepted: true,
@@ -90,7 +101,10 @@ test("M4.6 decodes accepted dual-evidence result without physical authority", as
assert.equal(result.resultId, resultId);
assert.equal(result.metrics.evidence.currentMetric, 27299);
assert.equal(result.metrics.fixtures.criticalFalseNotThreat, 0);
assert.equal(result.metrics.bodyFrame.qualified, 3861);
assert.equal(result.metrics.bodyFrame.cameraForwardAlignmentDeg.p95, 8.439);
assert.deepEqual(result.configuration.virtualBodyM, [1, 0.6]);
assert.equal(result.configuration.bodyFrame.up, "vendor-slam-map-gravity-axis");
});
test("M4.6 binds exact CAMERA and metric 3D evidence to one replay frame", async () => {
config/perception/m4-replay-threat-v1.json → config/perception/m4-replay-threat-v2.json
+17 -6
View File
@@ -1,7 +1,7 @@
{
"schema_version": "missioncore.replay-threat-profile/v1",
"profile_id": "m4-ravnoves00-virtual-corridor/v1",
"provider_id": "dual-evidence-replay-threat/v1",
"schema_version": "missioncore.replay-threat-profile/v2",
"profile_id": "m4-ravnoves00-virtual-corridor/v2",
"provider_id": "dual-evidence-replay-threat/v2",
"source": {
"source_id": "RAVNOVES00",
"session_id": "20260720T065719Z_viewer_live",
@@ -17,10 +17,21 @@
"calibration": {
"calibration_id": "camera-1-kb4-05f3ad9b",
"content_identity_sha256": "05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9",
"usage": "projection-binding-only"
"usage": "projection-and-forward-axis-binding"
},
"body_frame": {
"schema_version": "missioncore.replay-body-frame-profile/v1",
"origin": "local-surface-vertical-projection",
"up": "vendor-slam-map-gravity-axis",
"forward": "smoothed-slam-trajectory-validated-by-camera-axis",
"trajectory_half_window_frames": 20,
"minimum_trajectory_displacement_m": 0.2,
"maximum_camera_route_misalignment_deg": 25.0,
"maximum_sensor_height_deviation_m": 0.45,
"maximum_surface_slope_deg": 10.0
},
"virtual_rig": {
"profile_id": "virtual-handheld-body-1000x600/v1",
"profile_id": "virtual-base-footprint-1000x600/v2",
"body_length_m": 1.0,
"body_width_m": 0.6,
"lidar_reference": "virtual-body-center",
@@ -28,7 +39,7 @@
"physical_mount_claimed": false
},
"corridor": {
"profile_id": "ravnoves00-forward-corridor-8m/v1",
"profile_id": "ravnoves00-forward-corridor-8m/v2",
"forward_length_m": 8.0,
"rear_margin_m": 0.5,
"lateral_clearance_m": 0.2,
docs/22_MILESTONE_4_OBJECT_CENTRIC_RECORDED_REALTIME_CV.md
+41 -15
View File
@@ -917,7 +917,8 @@ the following M4.6 replay-only threat phase.
### 2026-08-05 — M4.6 dual-evidence replay threat
M4.6 is closed by `k1link.perception.threat` and the immutable replay builder in
M4.6 is closed by the corrected v2 implementation in
`k1link.perception.threat` and the immutable replay builder in
`k1link.perception.threat_replay`:
- `DualEvidenceReplayThreatProvider` consumes the canonical `LocalObstacleMap`;
@@ -931,37 +932,62 @@ M4.6 is closed by `k1link.perception.threat` and the immutable replay builder in
- the versioned replay profile fixes a virtual `1.0 × 0.6 m` body, nominal
`1.25 m` sensor height, `8 m` forward corridor and `5 s` bounded prediction
horizon; all documents retain `replay-simulated`, physical-collision false and
actuation false authority.
actuation false authority;
- the collision frame is a gravity-stable virtual `base_footprint`: its vertical
origin comes from the recorded local surface, its up axis remains the vendor
SLAM map gravity axis, and its forward axis follows the smoothed recorded
trajectory while being checked against the calibrated camera optical axis;
- local surface height, slope or route/camera disagreement outside the admitted
bounds rejects that replay frame instead of rotating the world or silently
calculating a corridor from unqualified geometry.
The accepted immutable result is
`m4-threat-replay-7e1613a3ea35638b5ea7a3f7c1c78fe9eba1a3adae540b652dec167f815d45b2`:
The original result
`m4-threat-replay-7e1613a3ea35638b5ea7a3f7c1c78fe9eba1a3adae540b652dec167f815d45b2`
is withdrawn and superseded. It incorrectly used the instantaneous LiDAR frame
as a virtual body frame, assumed LiDAR `+X` was vehicle forward even though the
recorded K1 calibration places camera-forward near LiDAR `-Y`, and rendered the
SLAM world with the handheld sensor roll and pitch. Its acceptance only proved
artifact availability, not body/corridor geometric validity.
The corrected accepted immutable result is
`m4-threat-replay-78a06d96c4db5263dc63fc4e6e067c07fc81370d3f5085ff43361af89cec1e9e`:
- `4,489 / 4,489` frames completed, zero failed;
- `27,299` current metric, `37,995` stale/held and `10,158` camera-only evidence
publications were each assessed exactly once;
- decisions: `8,010 threat`, `6,610 not-threat`, `60,832 unknown`;
- `21,958` geometry-only assessments remained in the decision path without a
- `3,928` source-bound body-frame inputs were available, `3,861` qualified and
`67` were rejected: `65` for unqualified sensor height and `2` for excessive
route/camera disagreement; `561` source-unavailable frames remain explicitly
accounted for;
- calibrated camera-forward versus route-forward agreement was `8.439°` p95,
with `24.252°` as the maximum accepted value under the fixed `25°` limit;
- decisions: `2,716 threat`, `10,700 not-threat`, `62,036 unknown`;
- `21,690` geometry-only assessments remained in the decision path without a
class requirement;
- deterministic fixtures passed `9 / 9`; all four critical fixtures avoided a
false `not-threat` outcome;
- local uncapped execution measured `132.812 FPS`; provider latency was
`3.932 ms` p50 and `17.567 ms` p95;
- local uncapped execution measured `278.601 FPS`; provider latency was
`1.656 ms` p50 and `6.099 ms` p95;
- deterministic frame, visual and fixture ledgers are sealed by SHA-256
`bf690358efb45c323db7172251074b33c3ef7ede6ae99bd8d3da53cfba86b142`,
`fb022c6efd84f27c0916a6c87887443c9b43993ac4b1f9910332433152533dea`
and `e217b61f3e8cf444f2620c0d815c18b2131eaafca29352bf12f78e05db96ee13`.
`d55e7651f0b16a62c6b61c5cb2358dd8dff87dbfa57a59e9ec350bc38b156bc1`,
`957c35d46ae30143beb6b2f26f8f722853ef2a1e91a41d5dc1a03fbf723a54e0`
and `ffa6f6a0f82faa7b6304aca5d8a62e1bb2730b484929d20d66005db9a2b4fa20`.
The standard LAB catalog exposes the exact result with a common evidence viewer:
full recorded VIDEO, exact CAMERA samples with ranges/unknown boxes, and the same
32 synchronized LiDAR point-cloud samples in interactive 3D and plan view. The
recorded box overlay was extracted from E46C into a reusable component rather
than copied into an M4-specific renderer. Visual availability is evidence for
inspection, not independent ground truth.
than copied into an M4-specific renderer. Regression frames `138` and `274`,
which exposed the original rotated-world defect, are mandatory members of the
visual ledger. Visual availability is evidence for inspection, not independent
ground truth.
M4.6 does not close moving/static correctness or object-presence correctness;
those remain the independent M4.8 gate. It also does not authorize a physical
mount, live K1, navigation, collision safety or commands. M4.7 is now the next
implementation phase.
mount, live K1, navigation, collision safety or commands. On a physical vehicle,
the replay-derived virtual frame must be replaced by one measured rigid
`T_body_from_sensor`; this does not change the downstream obstacle or threat
contracts. M4.7 is now the next implementation phase.
## Implementation order
docs/adr/0040-dual-evidence-replay-threat-boundary.md
+29
View File
@@ -17,6 +17,13 @@ qualified LiDAR-to-body mount. A recorded threat experiment therefore needs an
explicit virtual geometry without weakening the physical rig contract in ADR
0035.
The first M4.6 implementation incorrectly treated the instantaneous LiDAR frame
as the virtual body frame. The K1 calibration proves that camera-forward is near
LiDAR `-Y`, not `+X`, and the handheld pose contains real roll and pitch. That
made the replay corridor approximately 90 degrees off the route and rotated the
SLAM world with the operator's hand. Result `m4-threat-replay-7e1613...` is
superseded and is not admissible M4.6 evidence.
## Decision
Mission Core threat assessment consumes two independent evidence paths:
@@ -48,6 +55,26 @@ only with `replay-simulated` authority. They do not populate or qualify
`missioncore.rig-geometry/v1`, and they cannot support physical collision,
navigation, safety or actuation claims.
The virtual collision frame is a gravity-stable `base_footprint`, not the
instantaneous sensor frame:
- the K1 vendor SLAM map remains the stable world in which mapped points live;
- the rolling local-surface model supplies only the vertical ground origin and
a quality check, not a permanent level-world assumption;
- forward is the smoothed SLAM trajectory tangent and is independently checked
against the calibrated camera optical axis;
- unavailable height, excessive local slope or camera/route disagreement makes
that frame unqualified instead of silently rotating the corridor;
- a mounted vehicle replaces this replay-only derivation with one measured,
rigid `T_body_from_sensor`; the detector, obstacle map and threat policy do not
change.
The sensor may therefore be mounted at a non-level angle or noncentral position
as long as it is rigid and its one-time body extrinsic is known. Vehicle roll
and pitch do not corrupt the SLAM map; a future 3D swept-volume planner may use
`base_link`, while the current 2D corridor remains explicitly tied to
`base_footprint`.
## Evidence and presentation
The accepted replay must publish immutable frame, fixture, report and visual
@@ -57,6 +84,8 @@ ledgers. Visual evidence uses the common LAB viewer and reusable renderers:
- exact camera samples with metric range or explicit missing range;
- synchronized point cloud, occupied cells, virtual body and corridor in 3D and
plan view;
- mandatory regression frames `138` and `274`, which exposed the original
sensor/body-axis failure;
- visible threat/not-threat/unknown and `replay-simulated` authority.
Visuals are an inspection surface, not ground truth. Independent object-centric
scripts/build_m4_worker_shadow_artifact.py
+1 -1
View File
@@ -25,7 +25,7 @@ WHEEL_NAME = "nodedc_mission_core-0.1.0-py3-none-any.whl"
RUNNER_NAME = RUNNER.name
PATCH_ID = re.compile(r"^[A-Za-z0-9._-]{1,96}$")
EXPECTED_BASELINE_SHA256 = "ea10359339e6cce31b5780a2710299771cab7cc0c1c2a2b56a1621f786b31fa8"
EXPECTED_WHEEL_SHA256 = "94ed4b7e70471d343eafa9728ce1bd496551a6ee2c2d9e3e67fa5c6c2eadfc5b"
EXPECTED_WHEEL_SHA256 = "fad22ce1b3ed926e0208ed95c767c01d61dd83a3a4af47607aa84a2d377e5bce"
PAYLOAD_FILES = (
RUNNER_NAME,
WHEEL_NAME,
src/k1link/perception/geometry.py
+84 -7
View File
@@ -38,9 +38,7 @@ from .recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference
GEOMETRY_PROFILE_SCHEMA: Final = "missioncore.geometry-association-profile/v1"
GEOMETRY_PROVIDER_ID: Final = "ravnoves00-geometry-association/v1"
DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path(
"config/perception/m4-geometry-association-v1.json"
)
DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path("config/perception/m4-geometry-association-v1.json")
FloatArray = npt.NDArray[np.float64]
UInt8Array = npt.NDArray[np.uint8]
@@ -85,6 +83,20 @@ class GeometryFrame:
return int(self.points_map.shape[0])
@dataclass(frozen=True, slots=True)
class ReplayBodyFrameInputs:
"""Verified inputs required to derive one replay-only virtual body frame."""
sensor_position_map: FloatArray
sensor_orientation_map_from_lidar_xyzw: FloatArray
ground_plane_coefficients_map: FloatArray
sensor_height_m: float
surface_slope_deg: float
trajectory_start_position_map: FloatArray
trajectory_end_position_map: FloatArray
t_camera_from_lidar: FloatArray
@dataclass(frozen=True, slots=True)
class GeometryProviderSnapshot:
input_frames: int
@@ -279,13 +291,78 @@ class RecordedGeometryStore:
),
)
def replay_body_frame_inputs(
self,
frame_id: str,
*,
trajectory_half_window_frames: int,
) -> ReplayBodyFrameInputs | None:
"""Return source-bound pose, surface and route evidence without inventing axes."""
if trajectory_half_window_frames < 1:
raise GeometryProviderError("trajectory half-window must be positive")
prefix = "frame-"
if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit():
raise GeometryProviderError("replay body frame identity is invalid")
frame_index = int(frame_id[len(prefix) :])
if not 0 <= frame_index < self.profile.frame_count:
raise GeometryProviderError("replay body frame is outside the source profile")
if not bool(self._source["sample_available"][frame_index]) or not bool(
self._surface["frame_valid"][frame_index]
):
return None
required = {
"plane_coefficients_map",
"sensor_height_m",
"slope_deg",
}
if not required.issubset(self._surface):
raise GeometryProviderError("local surface lacks replay body-frame evidence")
first = max(0, frame_index - trajectory_half_window_frames)
last = min(self.profile.frame_count, frame_index + trajectory_half_window_frames + 1)
available = np.flatnonzero(self._source["sample_available"][first:last]) + first
if available.size == 0:
return None
values = ReplayBodyFrameInputs(
sensor_position_map=np.asarray(
self._source["pose_positions_map"][frame_index], dtype=np.float64
),
sensor_orientation_map_from_lidar_xyzw=np.asarray(
self._source["pose_quaternions_map_from_lidar"][frame_index],
dtype=np.float64,
),
ground_plane_coefficients_map=np.asarray(
self._surface["plane_coefficients_map"][frame_index],
dtype=np.float64,
),
sensor_height_m=float(self._surface["sensor_height_m"][frame_index]),
surface_slope_deg=float(self._surface["slope_deg"][frame_index]),
trajectory_start_position_map=np.asarray(
self._source["pose_positions_map"][int(available[0])], dtype=np.float64
),
trajectory_end_position_map=np.asarray(
self._source["pose_positions_map"][int(available[-1])], dtype=np.float64
),
t_camera_from_lidar=np.asarray(self._source["t_camera_from_lidar"], dtype=np.float64),
)
if not all(
np.isfinite(value).all()
for value in (
values.sensor_position_map,
values.sensor_orientation_map_from_lidar_xyzw,
values.ground_plane_coefficients_map,
values.trajectory_start_position_map,
values.trajectory_end_position_map,
values.t_camera_from_lidar,
)
) or not math.isfinite(values.sensor_height_m + values.surface_slope_deg):
raise GeometryProviderError("replay body-frame evidence is not finite")
return values
def available_frame_indices(self) -> tuple[int, ...]:
"""Expose the immutable availability partition for deterministic sampling."""
return tuple(
int(index)
for index in np.flatnonzero(self._source["sample_available"])
)
return tuple(int(index) for index in np.flatnonzero(self._source["sample_available"]))
def _validate(self) -> None:
source_required = {
src/k1link/perception/threat.py
+349 -101
View File
@@ -26,11 +26,9 @@ from .contracts import (
)
from .geometry_math import quaternion_xyzw_to_rotation_matrix
REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v1"
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v1"
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = (
"config/perception/m4-replay-threat-v1.json"
)
REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v2"
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v2"
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = "config/perception/m4-replay-threat-v2.json"
class ReplayThreatError(ValueError):
@@ -38,72 +36,242 @@ class ReplayThreatError(ValueError):
@dataclass(frozen=True, slots=True)
class ReplayPose:
class ReplayBodyFrame:
frame_id: str
position_map_xyz_m: tuple[float, float, float]
orientation_map_from_lidar_xyzw: tuple[float, float, float, float]
origin_map_xyz_m: tuple[float, float, float]
basis_map_from_body: tuple[
tuple[float, float, float],
tuple[float, float, float],
tuple[float, float, float],
]
sensor_height_m: float
surface_slope_deg: float
forward_source: str
camera_forward_alignment_deg: float
def __post_init__(self) -> None:
if not self.frame_id:
raise ReplayThreatError("replay pose frame id is empty")
raise ReplayThreatError("replay body frame id is empty")
if (
len(self.position_map_xyz_m) != 3
or len(self.orientation_map_from_lidar_xyzw) != 4
len(self.origin_map_xyz_m) != 3
or len(self.basis_map_from_body) != 3
or any(len(row) != 3 for row in self.basis_map_from_body)
or not all(
math.isfinite(value)
for value in (
*self.position_map_xyz_m,
*self.orientation_map_from_lidar_xyzw,
*self.origin_map_xyz_m,
*(value for row in self.basis_map_from_body for value in row),
self.sensor_height_m,
self.surface_slope_deg,
self.camera_forward_alignment_deg,
)
)
):
raise ReplayThreatError("replay pose is not finite")
norm = math.sqrt(sum(value * value for value in self.orientation_map_from_lidar_xyzw))
if norm < 1e-9:
raise ReplayThreatError("replay pose orientation has no usable norm")
raise ReplayThreatError("replay body frame is not finite")
columns = tuple(
tuple(self.basis_map_from_body[row][column] for row in range(3)) for column in range(3)
)
if (
not self.forward_source
or self.sensor_height_m <= 0.0
or self.surface_slope_deg < 0.0
or self.camera_forward_alignment_deg < 0.0
or any(abs(_dot(column, column) - 1.0) > 1e-6 for column in columns)
or any(
abs(_dot(columns[first], columns[second])) > 1e-6
for first, second in ((0, 1), (0, 2), (1, 2))
)
or _dot(_cross(columns[0], columns[1]), columns[2]) < 1.0 - 1e-6
):
raise ReplayThreatError("replay body frame basis is invalid")
def map_point_to_body(
self,
point_map_xyz_m: tuple[float, float, float],
) -> tuple[float, float, float]:
rotation = quaternion_xyzw_to_rotation_matrix(
self.orientation_map_from_lidar_xyzw
)
delta = tuple(
point_map_xyz_m[index] - self.position_map_xyz_m[index]
for index in range(3)
)
delta = tuple(point_map_xyz_m[index] - self.origin_map_xyz_m[index] for index in range(3))
return self.map_vector_to_body(delta)
def map_vector_to_body(
self,
vector_map_xyz_m: tuple[float, float, float],
) -> tuple[float, float, float]:
values = tuple(
float(sum(delta[row] * rotation[row, column] for row in range(3)))
float(
sum(
vector_map_xyz_m[row] * self.basis_map_from_body[row][column]
for row in range(3)
)
)
for column in range(3)
)
return values[0], values[1], values[2]
class ReplayPoseResolver(Protocol):
def pose_for_frame(self, frame_id: str) -> ReplayPose | None: ...
class ReplayBodyFrameResolver(Protocol):
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame | None: ...
class RecordedReplayPoseResolver:
"""Adapt the verified geometry store to the source-neutral pose seam."""
class RecordedReplayBodyFrameResolver:
"""Derive a ground-level virtual body frame from verified replay evidence."""
def __init__(self, store: object) -> None:
method = getattr(store, "pose_values_for_frame", None)
def __init__(self, store: object, *, profile: VirtualBodyFrameProfile) -> None:
method = getattr(store, "replay_body_frame_inputs", None)
if not callable(method):
raise ReplayThreatError("recorded pose store does not expose verified poses")
self._pose_values_for_frame = method
raise ReplayThreatError("recorded geometry store lacks body-frame evidence")
available = getattr(store, "available_frame_indices", None)
if not callable(available):
raise ReplayThreatError("recorded geometry store lacks availability evidence")
self._inputs_for_frame = method
self._available_frame_indices = available
self.profile = profile
self._cache: dict[str, tuple[ReplayBodyFrame | None, str]] = {}
def pose_for_frame(self, frame_id: str) -> ReplayPose | None:
values = self._pose_values_for_frame(frame_id)
if values is None:
return None
position, orientation = values
return ReplayPose(
frame_id=frame_id,
position_map_xyz_m=position,
orientation_map_from_lidar_xyzw=orientation,
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame | None:
return self._resolve(frame_id)[0]
def qualified_frame_indices(self) -> tuple[int, ...]:
return tuple(
index
for index in self._available_frame_indices()
if self.body_frame_for_frame(f"frame-{index:06d}") is not None
)
def qualification_summary(self) -> dict[str, object]:
indices = self._available_frame_indices()
for index in indices:
self._resolve(f"frame-{index:06d}")
reasons: dict[str, int] = {}
frames: list[ReplayBodyFrame] = []
for frame, reason in self._cache.values():
reasons[reason] = reasons.get(reason, 0) + 1
if frame is not None:
frames.append(frame)
alignments = sorted(item.camera_forward_alignment_deg for item in frames)
return {
"available": len(indices),
"qualified": len(frames),
"rejected": len(indices) - len(frames),
"reason_counts": dict(sorted(reasons.items())),
"camera_forward_alignment_deg": {
"maximum": max(alignments) if alignments else None,
"p95": _percentile(alignments, 0.95),
},
"origin": self.profile.origin,
"up": self.profile.up,
"forward": self.profile.forward,
}
def _resolve(self, frame_id: str) -> tuple[ReplayBodyFrame | None, str]:
cached = self._cache.get(frame_id)
if cached is not None:
return cached
inputs = self._inputs_for_frame(
frame_id,
trajectory_half_window_frames=self.profile.trajectory_half_window_frames,
)
if inputs is None:
return self._store(frame_id, None, "source-or-surface-unavailable")
position = tuple(float(value) for value in inputs.sensor_position_map)
plane = tuple(float(value) for value in inputs.ground_plane_coefficients_map)
normal_norm = math.sqrt(sum(value * value for value in plane[:3]))
if normal_norm < 1e-9:
return self._store(frame_id, None, "ground-normal-invalid")
ground_normal = tuple(value / normal_norm for value in plane[:3])
if ground_normal[2] < 0.0:
ground_normal = tuple(-value for value in ground_normal)
plane = tuple(-value for value in plane)
sensor_height = _dot(position, ground_normal) + plane[3] / normal_norm
if (
abs(sensor_height - inputs.sensor_height_m) > 0.05
or abs(sensor_height - self.profile.nominal_sensor_height_m)
> self.profile.maximum_sensor_height_deviation_m
):
return self._store(frame_id, None, "sensor-height-unqualified")
if inputs.surface_slope_deg > self.profile.maximum_surface_slope_deg:
return self._store(frame_id, None, "surface-slope-unqualified")
# The collision corridor lives in a gravity-stable base_footprint frame.
# Local terrain locates that footprint vertically but must not rotate the
# SLAM world when a handheld or mounted sensor rolls and pitches.
up = (0.0, 0.0, 1.0)
vertical_denominator = _dot(ground_normal, up)
if vertical_denominator < 1e-6:
return self._store(frame_id, None, "ground-projection-invalid")
vertical_height = sensor_height / vertical_denominator
rotation = quaternion_xyzw_to_rotation_matrix(inputs.sensor_orientation_map_from_lidar_xyzw)
calibration = inputs.t_camera_from_lidar
camera_forward_lidar = tuple(float(calibration[2, index]) for index in range(3))
camera_forward_map = tuple(
float(sum(rotation[row, column] * camera_forward_lidar[column] for column in range(3)))
for row in range(3)
)
camera_forward = _normalize(_reject(camera_forward_map, up))
if camera_forward is None:
return self._store(frame_id, None, "camera-forward-invalid")
route = tuple(
float(
inputs.trajectory_end_position_map[index]
- inputs.trajectory_start_position_map[index]
)
for index in range(3)
)
route_on_ground = _reject(route, up)
if (
math.sqrt(_dot(route_on_ground, route_on_ground))
>= self.profile.minimum_trajectory_displacement_m
):
forward = _normalize(route_on_ground)
assert forward is not None
forward_source = "smoothed-trajectory-tangent"
alignment = _angle_degrees(forward, camera_forward)
if alignment > self.profile.maximum_camera_route_misalignment_deg:
return self._store(frame_id, None, "camera-route-misaligned")
else:
forward = camera_forward
forward_source = "calibrated-camera-forward-fallback"
alignment = 0.0
left = _normalize(_cross(up, forward))
if left is None:
return self._store(frame_id, None, "body-left-invalid")
forward = _normalize(_cross(left, up))
assert forward is not None
origin = tuple(position[index] - vertical_height * up[index] for index in range(3))
basis = tuple((forward[row], left[row], up[row]) for row in range(3))
frame = ReplayBodyFrame(
frame_id=frame_id,
origin_map_xyz_m=origin,
basis_map_from_body=basis,
sensor_height_m=sensor_height,
surface_slope_deg=float(inputs.surface_slope_deg),
forward_source=forward_source,
camera_forward_alignment_deg=alignment,
)
return self._store(frame_id, frame, "qualified")
def _store(
self,
frame_id: str,
frame: ReplayBodyFrame | None,
reason: str,
) -> tuple[ReplayBodyFrame | None, str]:
value = (frame, reason)
self._cache[frame_id] = value
return value
@dataclass(frozen=True, slots=True)
class VirtualBodyFrameProfile:
schema_version: str
origin: str
up: str
forward: str
trajectory_half_window_frames: int
minimum_trajectory_displacement_m: float
maximum_camera_route_misalignment_deg: float
maximum_sensor_height_deviation_m: float
maximum_surface_slope_deg: float
nominal_sensor_height_m: float
@dataclass(frozen=True, slots=True)
class VirtualRigProfile:
@@ -141,6 +309,7 @@ class ReplayThreatProfile:
source_pack_sha256: str
calibration_id: str
calibration_content_sha256: str
body_frame: VirtualBodyFrameProfile
rig: VirtualRigProfile
corridor: VirtualCorridorProfile
profile_sha256: str
@@ -154,12 +323,12 @@ class DualEvidenceReplayThreatProvider:
def __init__(
self,
*,
pose_resolver: ReplayPoseResolver,
body_frame_resolver: ReplayBodyFrameResolver,
profile: ReplayThreatProfile,
) -> None:
if profile.provider_id != self.provider_id:
raise ReplayThreatError("threat provider identity changed")
self.pose_resolver = pose_resolver
self.body_frame_resolver = body_frame_resolver
self.profile = profile
def assess(self, obstacle_map: LocalObstacleMap) -> tuple[ThreatAssessment, ...]:
@@ -168,9 +337,9 @@ class DualEvidenceReplayThreatProvider:
or obstacle_map.session_id != self.profile.session_id
):
raise ReplayThreatError("obstacle map escaped the threat profile")
pose = self.pose_resolver.pose_for_frame(obstacle_map.frame_id)
body_frame = self.body_frame_resolver.body_frame_for_frame(obstacle_map.frame_id)
assessments = [
self._metric_or_stale(obstacle_map.frame_id, obstacle, pose)
self._metric_or_stale(obstacle_map.frame_id, obstacle, body_frame)
for obstacle in (*obstacle_map.occupied, *obstacle_map.unknown)
]
assessments.extend(
@@ -183,7 +352,7 @@ class DualEvidenceReplayThreatProvider:
self,
frame_id: str,
obstacle: TemporalObstacle,
pose: ReplayPose | None,
body_frame: ReplayBodyFrame | None,
) -> ThreatAssessment:
if obstacle.state is not TemporalState.CURRENT:
return self._unknown(
@@ -191,16 +360,16 @@ class DualEvidenceReplayThreatProvider:
obstacle.component_id,
("stale-evidence", f"temporal-state-{obstacle.state.value}"),
)
if pose is None or obstacle.last_centroid_xyz_m is None or not obstacle.cells:
if body_frame is None or obstacle.last_centroid_xyz_m is None or not obstacle.cells:
return self._unknown(
frame_id,
obstacle.component_id,
("current-pose-or-metric-geometry-unavailable",),
)
centroid_body = pose.map_point_to_body(obstacle.last_centroid_xyz_m)
centroid_body = body_frame.map_point_to_body(obstacle.last_centroid_xyz_m)
cells_body = tuple(
pose.map_point_to_body(
body_frame.map_point_to_body(
(
(cell.x + 0.5) * self.profile.corridor.occupied_voxel_size_m,
(cell.y + 0.5) * self.profile.corridor.occupied_voxel_size_m,
@@ -209,7 +378,7 @@ class DualEvidenceReplayThreatProvider:
)
for cell in obstacle.cells
)
velocity_body = self._relative_velocity_body(obstacle, pose)
velocity_body = self._relative_velocity_body(obstacle, body_frame)
corridor_entry = _first_corridor_entry_seconds(
cells_body,
velocity_body,
@@ -221,9 +390,7 @@ class DualEvidenceReplayThreatProvider:
rig=self.profile.rig,
corridor=self.profile.corridor,
)
motion_complete = (
obstacle.motion is not MotionState.UNKNOWN and velocity_body is not None
)
motion_complete = obstacle.motion is not MotionState.UNKNOWN and velocity_body is not None
if current_intersection or (motion_complete and corridor_entry is not None):
intersection = CorridorIntersection.INTERSECTS
decision = ThreatDecision.THREAT
@@ -281,7 +448,7 @@ class DualEvidenceReplayThreatProvider:
def _relative_velocity_body(
self,
obstacle: TemporalObstacle,
current_pose: ReplayPose,
current_body_frame: ReplayBodyFrame,
) -> tuple[float, float] | None:
if len(obstacle.history) < 2:
return None
@@ -290,29 +457,25 @@ class DualEvidenceReplayThreatProvider:
span_seconds = (last.evidence_time_ns - first.evidence_time_ns) / 1_000_000_000
if span_seconds < self.profile.corridor.minimum_motion_span_seconds:
return None
first_pose = self.pose_resolver.pose_for_frame(first.frame_id)
last_pose = self.pose_resolver.pose_for_frame(last.frame_id)
if first_pose is None or last_pose is None or last.frame_id != current_pose.frame_id:
first_body_frame = self.body_frame_resolver.body_frame_for_frame(first.frame_id)
last_body_frame = self.body_frame_resolver.body_frame_for_frame(last.frame_id)
if (
first_body_frame is None
or last_body_frame is None
or last.frame_id != current_body_frame.frame_id
):
return None
obstacle_delta = tuple(
last.centroid_xyz_m[index] - first.centroid_xyz_m[index]
for index in range(3)
last.centroid_xyz_m[index] - first.centroid_xyz_m[index] for index in range(3)
)
rig_delta = tuple(
last_pose.position_map_xyz_m[index] - first_pose.position_map_xyz_m[index]
last_body_frame.origin_map_xyz_m[index] - first_body_frame.origin_map_xyz_m[index]
for index in range(3)
)
relative_map = tuple(
(obstacle_delta[index] - rig_delta[index]) / span_seconds
for index in range(3)
)
rotation = quaternion_xyzw_to_rotation_matrix(
current_pose.orientation_map_from_lidar_xyzw
)
body = tuple(
float(sum(relative_map[row] * rotation[row, column] for row in range(3)))
for column in range(3)
(obstacle_delta[index] - rig_delta[index]) / span_seconds for index in range(3)
)
body = current_body_frame.map_vector_to_body(relative_map)
return body[0], body[1]
def _camera_only(self, frame_id: str, proposal_id: str) -> ThreatAssessment:
@@ -359,6 +522,7 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
"provider_id",
"source",
"calibration",
"body_frame",
"virtual_rig",
"corridor",
"policy",
@@ -373,6 +537,7 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
raise ReplayThreatError("replay threat profile identity is incompatible")
source = _object(document["source"], "threat source")
calibration = _object(document["calibration"], "threat calibration")
body_frame = _object(document["body_frame"], "virtual body frame")
rig = _object(document["virtual_rig"], "virtual rig")
corridor = _object(document["corridor"], "virtual corridor")
policy = _object(document["policy"], "threat policy")
@@ -398,6 +563,21 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
{"calibration_id", "content_identity_sha256", "usage"},
"threat calibration",
)
_exact_keys(
body_frame,
{
"schema_version",
"origin",
"up",
"forward",
"trajectory_half_window_frames",
"minimum_trajectory_displacement_m",
"maximum_camera_route_misalignment_deg",
"maximum_sensor_height_deviation_m",
"maximum_surface_slope_deg",
},
"virtual body frame",
)
_exact_keys(
rig,
{
@@ -448,7 +628,11 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
"threat authority",
)
if (
calibration.get("usage") != "projection-binding-only"
calibration.get("usage") != "projection-and-forward-axis-binding"
or body_frame.get("schema_version") != "missioncore.replay-body-frame-profile/v1"
or body_frame.get("origin") != "local-surface-vertical-projection"
or body_frame.get("up") != "vendor-slam-map-gravity-axis"
or body_frame.get("forward") != "smoothed-slam-trajectory-validated-by-camera-axis"
or rig.get("physical_mount_claimed") is not False
or policy
!= {
@@ -477,18 +661,34 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
lidar_reference=_string(rig, "lidar_reference"),
nominal_sensor_height_m=_positive_number(rig, "nominal_sensor_height_m"),
)
body_frame_profile = VirtualBodyFrameProfile(
schema_version=_string(body_frame, "schema_version"),
origin=_string(body_frame, "origin"),
up=_string(body_frame, "up"),
forward=_string(body_frame, "forward"),
trajectory_half_window_frames=_positive_integer(
body_frame, "trajectory_half_window_frames"
),
minimum_trajectory_displacement_m=_positive_number(
body_frame, "minimum_trajectory_displacement_m"
),
maximum_camera_route_misalignment_deg=_positive_number(
body_frame, "maximum_camera_route_misalignment_deg"
),
maximum_sensor_height_deviation_m=_positive_number(
body_frame, "maximum_sensor_height_deviation_m"
),
maximum_surface_slope_deg=_positive_number(body_frame, "maximum_surface_slope_deg"),
nominal_sensor_height_m=virtual_rig.nominal_sensor_height_m,
)
virtual_corridor = VirtualCorridorProfile(
profile_id=_string(corridor, "profile_id"),
forward_length_m=_positive_number(corridor, "forward_length_m"),
rear_margin_m=_nonnegative_number(corridor, "rear_margin_m"),
lateral_clearance_m=_nonnegative_number(corridor, "lateral_clearance_m"),
prediction_horizon_seconds=_positive_number(
corridor, "prediction_horizon_seconds"
),
prediction_horizon_seconds=_positive_number(corridor, "prediction_horizon_seconds"),
occupied_voxel_size_m=_positive_number(corridor, "occupied_voxel_size_m"),
minimum_motion_span_seconds=_positive_number(
corridor, "minimum_motion_span_seconds"
),
minimum_motion_span_seconds=_positive_number(corridor, "minimum_motion_span_seconds"),
)
if virtual_rig.lidar_reference != "virtual-body-center":
raise ReplayThreatError("virtual LiDAR reference is unsupported")
@@ -515,6 +715,7 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
source_pack_sha256=_string(source, "source_pack_sha256"),
calibration_id=_string(calibration, "calibration_id"),
calibration_content_sha256=_string(calibration, "content_identity_sha256"),
body_frame=body_frame_profile,
rig=virtual_rig,
corridor=virtual_corridor,
profile_sha256=hashlib.sha256(raw).hexdigest(),
@@ -575,10 +776,7 @@ def _first_corridor_entry_seconds(
return None
expansion = corridor.occupied_voxel_size_m * math.sqrt(2) / 2
bounds = _corridor_bounds(rig, corridor, expansion_m=expansion)
entries = (
_ray_box_entry((point[0], point[1]), velocity_body, bounds)
for point in cells_body
)
entries = (_ray_box_entry((point[0], point[1]), velocity_body, bounds) for point in cells_body)
valid = [
entry
for entry in entries
@@ -602,8 +800,7 @@ def _first_body_entry_seconds(
valid = [
entry
for point in cells_body
if (entry := _ray_box_entry((point[0], point[1]), velocity_body, bounds))
is not None
if (entry := _ray_box_entry((point[0], point[1]), velocity_body, bounds)) is not None
and entry <= horizon_seconds
]
return None if not valid else round(min(valid), 12)
@@ -652,10 +849,7 @@ def _closest_body_clearance_m(
horizon_seconds,
max(
0.0,
-(
point[0] * velocity_body[0]
+ point[1] * velocity_body[1]
)
-(point[0] * velocity_body[0] + point[1] * velocity_body[1])
/ speed_squared,
),
)
@@ -665,12 +859,8 @@ def _closest_body_clearance_m(
(point[1], velocity_body[1], bounds[2], bounds[3]),
):
if abs(speed) > 1e-12:
candidates.add(
min(horizon_seconds, max(0.0, (lower - coordinate) / speed))
)
candidates.add(
min(horizon_seconds, max(0.0, (upper - coordinate) / speed))
)
candidates.add(min(horizon_seconds, max(0.0, (lower - coordinate) / speed)))
candidates.add(min(horizon_seconds, max(0.0, (upper - coordinate) / speed)))
velocity = velocity_body or (0.0, 0.0)
clearance = min(
_point_box_distance(
@@ -702,11 +892,7 @@ def _closing_speed_mps(
if distance < 1e-9:
return round(math.hypot(*velocity_body), 12)
return round(
-(
centroid_body[0] * velocity_body[0]
+ centroid_body[1] * velocity_body[1]
)
/ distance,
-(centroid_body[0] * velocity_body[0] + centroid_body[1] * velocity_body[1]) / distance,
12,
)
@@ -743,6 +929,67 @@ def _positive_number(document: dict[str, object], key: str) -> float:
return value
def _positive_integer(document: dict[str, object], key: str) -> int:
value = document.get(key)
if not isinstance(value, int) or isinstance(value, bool) or value <= 0:
raise ReplayThreatError(f"{key} must be a positive integer")
return value
def _dot(
first: tuple[float, float, float],
second: tuple[float, float, float],
) -> float:
return sum(first[index] * second[index] for index in range(3))
def _cross(
first: tuple[float, float, float],
second: tuple[float, float, float],
) -> tuple[float, float, float]:
return (
first[1] * second[2] - first[2] * second[1],
first[2] * second[0] - first[0] * second[2],
first[0] * second[1] - first[1] * second[0],
)
def _normalize(
value: tuple[float, float, float],
) -> tuple[float, float, float] | None:
norm = math.sqrt(_dot(value, value))
if norm < 1e-9:
return None
return tuple(item / norm for item in value)
def _reject(
value: tuple[float, float, float],
normal: tuple[float, float, float],
) -> tuple[float, float, float]:
along = _dot(value, normal)
return tuple(value[index] - along * normal[index] for index in range(3))
def _angle_degrees(
first: tuple[float, float, float],
second: tuple[float, float, float],
) -> float:
return math.degrees(math.acos(max(-1.0, min(1.0, _dot(first, second)))))
def _percentile(values: list[float], fraction: float) -> float | None:
if not values:
return None
position = fraction * (len(values) - 1)
lower = math.floor(position)
upper = math.ceil(position)
if lower == upper:
return values[lower]
weight = position - lower
return values[lower] * (1.0 - weight) + values[upper] * weight
def _nonnegative_number(document: dict[str, object], key: str) -> float:
value = _number(document, key)
if value < 0.0:
@@ -771,11 +1018,12 @@ __all__ = [
"DualEvidenceReplayThreatProvider",
"REPLAY_THREAT_PROFILE_SCHEMA",
"REPLAY_THREAT_PROVIDER_ID",
"RecordedReplayPoseResolver",
"ReplayPose",
"ReplayPoseResolver",
"RecordedReplayBodyFrameResolver",
"ReplayBodyFrame",
"ReplayBodyFrameResolver",
"ReplayThreatError",
"ReplayThreatProfile",
"VirtualBodyFrameProfile",
"VirtualCorridorProfile",
"VirtualRigProfile",
"load_replay_threat_profile",
src/k1link/perception/threat_replay.py
+124 -80
View File
@@ -35,7 +35,6 @@ from .contracts import (
from .detector_replay_contracts import DetectorReplayResult
from .detector_replay_result import read_detector_replay_result
from .geometry import RecordedGeometryStore
from .geometry_math import quaternion_xyzw_to_rotation_matrix
from .geometry_replay import GeometryReplayResult, read_geometry_replay_result
from .providers import SourcePacket
from .recorded_source import RecordedRavnoves00Source, ReplayPacing
@@ -43,8 +42,8 @@ from .temporal_replay import TemporalReplayResult, read_temporal_replay_result
from .threat import (
DEFAULT_REPLAY_THREAT_PROFILE_PATH,
DualEvidenceReplayThreatProvider,
RecordedReplayPoseResolver,
ReplayPose,
RecordedReplayBodyFrameResolver,
ReplayBodyFrame,
ReplayThreatProfile,
load_replay_threat_profile,
)
@@ -62,6 +61,7 @@ THREAT_REPLAY_REPORT_NAME: Final = "report.json"
THREAT_REPLAY_MANIFEST_NAME: Final = "manifest.json"
VISUAL_FRAME_COUNT: Final = 32
VISUAL_POINT_LIMIT: Final = 4_000
VISUAL_GEOMETRY_REGRESSION_SEQUENCES: Final = (138, 274)
class ThreatReplayError(RuntimeError):
@@ -87,25 +87,26 @@ def build_threat_replay(
output_root: Path,
) -> ThreatReplayResult:
repository = repository_root.resolve()
profile = load_replay_threat_profile(
repository / DEFAULT_REPLAY_THREAT_PROFILE_PATH
)
profile = load_replay_threat_profile(repository / DEFAULT_REPLAY_THREAT_PROFILE_PATH)
temporal = read_temporal_replay_result(temporal_result_root)
geometry = read_geometry_replay_result(geometry_result_root)
detector = read_detector_replay_result(detector_result_root)
_validate_upstream(profile, temporal, geometry, detector)
store = RecordedGeometryStore.from_repository(repository)
pose_resolver = RecordedReplayPoseResolver(store)
body_frame_resolver = RecordedReplayBodyFrameResolver(
store,
profile=profile.body_frame,
)
provider = DualEvidenceReplayThreatProvider(
pose_resolver=pose_resolver,
body_frame_resolver=body_frame_resolver,
profile=profile,
)
source = RecordedRavnoves00Source.from_repository(
repository,
pacing=ReplayPacing.UNCAPPED,
)
visual_sequences = _visual_sequences(store.available_frame_indices())
visual_sequences = _visual_sequences(body_frame_resolver.qualified_frame_indices())
root = output_root.expanduser().absolute()
root.mkdir(mode=0o700, parents=True, exist_ok=True)
@@ -189,13 +190,11 @@ def build_threat_replay(
)
frame_started_ns = time.perf_counter_ns()
assessments = provider.assess(obstacle_map)
latencies_ms.append(
(time.perf_counter_ns() - frame_started_ns) / 1_000_000
)
latencies_ms.append((time.perf_counter_ns() - frame_started_ns) / 1_000_000)
by_id = {item.component_id: item for item in assessments}
expected_ids = {
item.component_id for item in (*current, *unknown)
} | {item.proposal_id for item in camera_uncertainty}
expected_ids = {item.component_id for item in (*current, *unknown)} | {
item.proposal_id for item in camera_uncertainty
}
if set(by_id) != expected_ids:
raise ThreatReplayError("threat assessment coverage is incomplete")
camera_rows = _camera_rows(
@@ -204,8 +203,7 @@ def build_threat_replay(
by_id,
)
metric_rows = [
_metric_row(item, by_id[item.component_id])
for item in (*current, *unknown)
_metric_row(item, by_id[item.component_id]) for item in (*current, *unknown)
]
for item in assessments:
assessment_counts[item.decision.value] += 1
@@ -222,10 +220,8 @@ def build_threat_replay(
"sequence": frame_count,
"frame_id": packet.envelope.frame_id,
"source_time_ns": packet.envelope.timestamps.source_ns,
"source_available": (
packet.envelope.registered_point_increment.available
),
"pose_available": pose_resolver.pose_for_frame(
"source_available": (packet.envelope.registered_point_increment.available),
"body_frame_available": body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
)
is not None,
@@ -247,7 +243,9 @@ def build_threat_replay(
_visual_frame(
packet=packet,
store=store,
pose=pose_resolver.pose_for_frame(packet.envelope.frame_id),
body_frame=body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
),
metric_rows=metric_rows,
camera_rows=camera_rows,
profile=profile,
@@ -277,6 +275,7 @@ def build_threat_replay(
elapsed_ns=elapsed_ns,
visual_count=visual_count,
fixtures=fixtures,
body_frame=body_frame_resolver.qualification_summary(),
)
requirements = _requirements(metrics, fixtures)
accepted = all(value is True for value in requirements.values())
@@ -300,6 +299,12 @@ def build_threat_replay(
"source_pack_sha256": profile.source_pack_sha256,
"calibration_id": profile.calibration_id,
"calibration_content_sha256": profile.calibration_content_sha256,
"body_frame": {
"schema_version": profile.body_frame.schema_version,
"origin": profile.body_frame.origin,
"up": profile.body_frame.up,
"forward": profile.body_frame.forward,
},
"rig_profile_id": profile.rig.profile_id,
"corridor_profile_id": profile.corridor.profile_id,
"producer_sha256": _producer_hashes(repository),
@@ -326,13 +331,20 @@ def build_threat_replay(
profile.rig.body_width_m,
],
"nominal_sensor_height_m": profile.rig.nominal_sensor_height_m,
"body_frame": {
"origin": profile.body_frame.origin,
"up": profile.body_frame.up,
"forward": profile.body_frame.forward,
},
"forward_corridor_m": profile.corridor.forward_length_m,
"prediction_horizon_seconds": (
profile.corridor.prediction_horizon_seconds
),
"prediction_horizon_seconds": (profile.corridor.prediction_horizon_seconds),
},
"limitations": [
"The body and corridor are replay-simulated, not a measured physical mount.",
(
"The replay base_footprint uses SLAM trajectory and map gravity; "
"a mounted vehicle replaces it with calibrated T_body_from_sensor."
),
"The LiDAR archive is the vendor mapped point increment, not every raw beam.",
"TTC uses bounded constant-relative-velocity replay extrapolation.",
"Camera-only evidence remains unknown and cannot establish metric clearance.",
@@ -398,9 +410,7 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
):
raise ThreatReplayError("threat replay identity changed")
artifacts = _array(manifest.get("artifacts"), "threat artifacts")
by_role = {
_object(item, "threat artifact").get("role"): item for item in artifacts
}
by_role = {_object(item, "threat artifact").get("role"): item for item in artifacts}
expected = {
"threat-replay-frames": (THREAT_REPLAY_FRAMES_NAME, "frames_sha256"),
"threat-visual-frames": (THREAT_REPLAY_VISUALS_NAME, "visuals_sha256"),
@@ -420,9 +430,7 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
raise ThreatReplayError("threat artifact identity changed")
report = _read_json(paths["threat-replay-report"])
metrics = _object(identity.get("metrics"), "threat metrics")
requirements = _object(
identity.get("acceptance_requirements"), "threat requirements"
)
requirements = _object(identity.get("acceptance_requirements"), "threat requirements")
fixtures = _read_json(paths["threat-deterministic-fixtures"])
accepted = all(value is True for value in requirements.values())
if (
@@ -505,9 +513,7 @@ def _metric_row(
"motion_reason": obstacle.motion_reason,
"semantic_hint": obstacle.semantic_hint,
"centroid_map_xyz_m": (
None
if obstacle.last_centroid_xyz_m is None
else list(obstacle.last_centroid_xyz_m)
None if obstacle.last_centroid_xyz_m is None else list(obstacle.last_centroid_xyz_m)
),
"cells": [item.to_dict() for item in obstacle.cells],
"history": [item.to_dict() for item in obstacle.history],
@@ -552,9 +558,7 @@ def _camera_rows(
"occupied_support": geometry["occupied_support"],
"range_m": geometry["range_m"],
"geometry_reason_codes": geometry["reason_codes"],
"threat_decision": (
None if assessment is None else assessment.decision.value
),
"threat_decision": (None if assessment is None else assessment.decision.value),
"threat_reason_codes": (
[] if assessment is None else list(assessment.reason_codes)
),
@@ -567,21 +571,19 @@ def _visual_frame(
*,
packet: SourcePacket,
store: RecordedGeometryStore,
pose: ReplayPose | None,
body_frame: ReplayBodyFrame | None,
metric_rows: list[dict[str, object]],
camera_rows: list[dict[str, object]],
profile: ReplayThreatProfile,
) -> dict[str, object]:
if pose is None:
raise ThreatReplayError("visual frame has no source pose")
if body_frame is None:
raise ThreatReplayError("visual frame has no qualified body frame")
points = store.current_points(packet)
if points is None:
raise ThreatReplayError("visual frame has no current point cloud")
rotation = quaternion_xyzw_to_rotation_matrix(
pose.orientation_map_from_lidar_xyzw
)
position = np.asarray(pose.position_map_xyz_m, dtype=np.float64)
points_body = (points - position) @ rotation
basis = np.asarray(body_frame.basis_map_from_body, dtype=np.float64)
origin = np.asarray(body_frame.origin_map_xyz_m, dtype=np.float64)
points_body = (points - origin) @ basis
stride = max(1, math.ceil(points_body.shape[0] / VISUAL_POINT_LIMIT))
sampled = points_body[::stride][:VISUAL_POINT_LIMIT]
metric_visuals = []
@@ -590,7 +592,7 @@ def _visual_frame(
cells = row.get("cells")
if not isinstance(centroid, list) or not isinstance(cells, list):
continue
centroid_body = pose.map_point_to_body(
centroid_body = body_frame.map_point_to_body(
(float(centroid[0]), float(centroid[1]), float(centroid[2]))
)
cell_centers = []
@@ -602,11 +604,7 @@ def _visual_frame(
for key in ("x", "y", "z")
)
cell_centers.append(
list(
pose.map_point_to_body(
(point_map[0], point_map[1], point_map[2])
)
)
list(body_frame.map_point_to_body((point_map[0], point_map[1], point_map[2])))
)
metric_visuals.append(
{
@@ -628,6 +626,14 @@ def _visual_frame(
"point_cloud_sample_count": int(sampled.shape[0]),
"metric_obstacles": metric_visuals,
"camera_proposals": camera_rows,
"body_frame": {
"origin_map_xyz_m": list(body_frame.origin_map_xyz_m),
"basis_map_from_body": [list(row) for row in body_frame.basis_map_from_body],
"sensor_height_m": body_frame.sensor_height_m,
"surface_slope_deg": body_frame.surface_slope_deg,
"forward_source": body_frame.forward_source,
"camera_forward_alignment_deg": body_frame.camera_forward_alignment_deg,
},
"rig": {
"length_m": profile.rig.body_length_m,
"width_m": profile.rig.body_width_m,
@@ -636,30 +642,29 @@ def _visual_frame(
"corridor": {
"forward_length_m": profile.corridor.forward_length_m,
"rear_margin_m": profile.corridor.rear_margin_m,
"half_width_m": (
profile.rig.body_width_m / 2
+ profile.corridor.lateral_clearance_m
),
"prediction_horizon_seconds": (
profile.corridor.prediction_horizon_seconds
),
"half_width_m": (profile.rig.body_width_m / 2 + profile.corridor.lateral_clearance_m),
"prediction_horizon_seconds": (profile.corridor.prediction_horizon_seconds),
},
"authority": _false_authority(),
}
class _FixturePoses:
def pose_for_frame(self, frame_id: str) -> ReplayPose:
return ReplayPose(
class _FixtureBodyFrames:
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
return ReplayBodyFrame(
frame_id=frame_id,
position_map_xyz_m=(0.0, 0.0, 0.0),
orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
origin_map_xyz_m=(0.0, 0.0, 0.0),
basis_map_from_body=((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)),
sensor_height_m=1.25,
surface_slope_deg=0.0,
forward_source="fixture",
camera_forward_alignment_deg=0.0,
)
def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
provider = DualEvidenceReplayThreatProvider(
pose_resolver=_FixturePoses(),
body_frame_resolver=_FixtureBodyFrames(),
profile=profile,
)
frame_id = "frame-000002"
@@ -783,8 +788,7 @@ def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
"cases": cases,
"critical_case_count": sum(item["critical"] is True for item in cases),
"critical_false_not_threat_count": sum(
item["critical"] is True and item["actual"] == "not-threat"
for item in cases
item["critical"] is True and item["actual"] == "not-threat" for item in cases
),
"passed_count": sum(item["passed"] is True for item in cases),
"total_count": len(cases),
@@ -816,9 +820,7 @@ def _fixture_obstacle(
last_centroid_xyz_m=None if state is TemporalState.EXPIRED else last.centroid_xyz_m,
motion=motion if state is TemporalState.CURRENT else MotionState.UNKNOWN,
motion_confidence=(
0.0
if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN
else 1.0
0.0 if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN else 1.0
),
motion_reason=(
"stale-support"
@@ -912,6 +914,7 @@ def _metrics(
elapsed_ns: int,
visual_count: int,
fixtures: dict[str, object],
body_frame: dict[str, object],
) -> dict[str, object]:
values = np.asarray(latencies_ms, dtype=np.float64)
return {
@@ -920,6 +923,7 @@ def _metrics(
"decisions": dict(sorted(assessment_counts.items())),
"motion_decisions": dict(sorted(motion_decisions.items())),
"reason_counts": dict(sorted(reason_counts.items())),
"body_frame": body_frame,
"visual_evidence": {
"frame_count": visual_count,
"point_limit_per_frame": VISUAL_POINT_LIMIT,
@@ -928,14 +932,14 @@ def _metrics(
"point_cloud_available": True,
"metric_distance_available": True,
"virtual_corridor_available": True,
"qualified_base_footprint_available": True,
"geometry_regression_sequences": list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES),
},
"fixtures": {
"passed": fixtures["passed_count"],
"total": fixtures["total_count"],
"critical": fixtures["critical_case_count"],
"critical_false_not_threat": fixtures[
"critical_false_not_threat_count"
],
"critical_false_not_threat": fixtures["critical_false_not_threat_count"],
},
"runtime": {
"elapsed_ns": elapsed_ns,
@@ -955,12 +959,9 @@ def _requirements(
evidence = _object(metrics.get("evidence"), "evidence metrics")
decisions = _object(metrics.get("decisions"), "decision metrics")
visual = _object(metrics.get("visual_evidence"), "visual metrics")
total_evidence = sum(
_integer(value, "evidence count") for value in evidence.values()
)
total_decisions = sum(
_integer(value, "decision count") for value in decisions.values()
)
body_frame = _object(metrics.get("body_frame"), "body frame metrics")
total_evidence = sum(_integer(value, "evidence count") for value in evidence.values())
total_decisions = sum(_integer(value, "decision count") for value in decisions.values())
cases = _array(fixtures.get("cases"), "fixture cases")
camera_case = next(
(
@@ -984,8 +985,7 @@ def _requirements(
),
"camera_only_is_unknown_never_safe": camera_case.get("actual") == "unknown",
"held_and_stale_are_unknown_never_safe": (
len(stale_cases) == 2
and all(item.get("actual") == "unknown" for item in stale_cases)
len(stale_cases) == 2 and all(item.get("actual") == "unknown" for item in stale_cases)
),
"geometry_only_evidence_is_assessed": (
_integer(
@@ -1012,8 +1012,29 @@ def _requirements(
"point_cloud_available",
"metric_distance_available",
"virtual_corridor_available",
"qualified_base_footprint_available",
)
)
and visual.get("geometry_regression_sequences")
== list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES)
),
"body_frame_is_grounded_gravity_stable_and_route_aligned": (
body_frame.get("available")
== _integer(body_frame.get("qualified"), "qualified body frames")
+ _integer(body_frame.get("rejected"), "rejected body frames")
and _integer(body_frame.get("qualified"), "qualified body frames")
>= math.ceil(_integer(body_frame.get("available"), "available body frames") * 0.95)
and body_frame.get("origin") == "local-surface-vertical-projection"
and body_frame.get("up") == "vendor-slam-map-gravity-axis"
and body_frame.get("forward") == "smoothed-slam-trajectory-validated-by-camera-axis"
and _number_value(
_object(
body_frame.get("camera_forward_alignment_deg"),
"body alignment metrics",
).get("maximum"),
"maximum body alignment",
)
<= 25.0
),
"physical_collision_and_actuation_authority_remain_false": (
fixtures.get("authority") == _false_authority()
@@ -1062,6 +1083,23 @@ def _visual_sequences(available: tuple[int, ...]) -> frozenset[int]:
available[round(index * (len(available) - 1) / (VISUAL_FRAME_COUNT - 1))]
for index in range(VISUAL_FRAME_COUNT)
}
available_set = frozenset(available)
for anchor in VISUAL_GEOMETRY_REGRESSION_SEQUENCES:
if anchor not in available_set:
raise ThreatReplayError("geometry regression frame is not qualified")
if anchor in selected:
continue
replaceable = selected.difference(
{
available[0],
available[-1],
*VISUAL_GEOMETRY_REGRESSION_SEQUENCES,
}
)
if not replaceable:
raise ThreatReplayError("visual regression sample cannot be inserted")
selected.remove(min(replaceable, key=lambda value: abs(value - anchor)))
selected.add(anchor)
if len(selected) != VISUAL_FRAME_COUNT:
raise ThreatReplayError("visual sample selection is not unique")
return frozenset(selected)
@@ -1189,6 +1227,12 @@ def _integer(value: object, label: str) -> int:
return value
def _number_value(value: object, label: str) -> float:
if not isinstance(value, int | float) or isinstance(value, bool) or not math.isfinite(value):
raise ThreatReplayError(f"{label} is not finite")
return float(value)
def _signed_integer(value: object, label: str) -> int:
if not isinstance(value, int) or isinstance(value, bool):
raise ThreatReplayError(f"{label} must be an integer")
tests/test_m4_threat_replay_result.py
+17 -21
View File
@@ -8,10 +8,7 @@ from k1link.perception.threat_replay import read_threat_replay_result
from k1link.web.m4_threat_replay_api import build_m4_threat_replay_router
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
RESULT_ID = (
"m4-threat-replay-"
"7e1613a3ea35638b5ea7a3f7c1c78fe9eba1a3adae540b652dec167f815d45b2"
)
RESULT_ID = "m4-threat-replay-78a06d96c4db5263dc63fc4e6e067c07fc81370d3f5085ff43361af89cec1e9e"
RESULTS_ROOT = REPOSITORY_ROOT / ".runtime/compute-experiments/m4/replay-threat"
@@ -35,9 +32,9 @@ def test_full_source_threat_result_closes_m4_6_contract() -> None:
"stale-or-held": 37995,
}
assert result.metrics["decisions"] == {
"not-threat": 6610,
"threat": 8010,
"unknown": 60832,
"not-threat": 10700,
"threat": 2716,
"unknown": 62036,
}
assert result.metrics["fixtures"] == {
"critical": 4,
@@ -53,10 +50,10 @@ def test_threat_result_is_content_bound_and_visual_evidence_is_complete() -> Non
assert isinstance(identity, dict)
assert identity["frames_sha256"] == (
"bf690358efb45c323db7172251074b33c3ef7ede6ae99bd8d3da53cfba86b142"
"d55e7651f0b16a62c6b61c5cb2358dd8dff87dbfa57a59e9ec350bc38b156bc1"
)
assert identity["visuals_sha256"] == (
"fb022c6efd84f27c0916a6c87887443c9b43993ac4b1f9910332433152533dea"
"957c35d46ae30143beb6b2f26f8f722853ef2a1e91a41d5dc1a03fbf723a54e0"
)
visual = result.metrics["visual_evidence"]
assert isinstance(visual, dict)
@@ -69,24 +66,27 @@ def test_threat_result_is_content_bound_and_visual_evidence_is_complete() -> Non
"point_cloud_available",
"metric_distance_available",
"virtual_corridor_available",
"qualified_base_footprint_available",
)
)
assert visual["geometry_regression_sequences"] == [138, 274]
body_frame = result.metrics["body_frame"]
assert body_frame["qualified"] == 3861
assert body_frame["rejected"] == 67
assert body_frame["camera_forward_alignment_deg"]["p95"] < 9.0
def test_m4_6_lab_api_projects_report_and_exact_visual_frame() -> None:
list_results = _endpoint("/api/v1/laboratory/m4-threat/results")
list_visuals = _endpoint(
"/api/v1/laboratory/m4-threat/results/{result_id}/visuals"
)
get_visual = _endpoint(
"/api/v1/laboratory/m4-threat/results/{result_id}/visuals/{ordinal}"
)
list_visuals = _endpoint("/api/v1/laboratory/m4-threat/results/{result_id}/visuals")
get_visual = _endpoint("/api/v1/laboratory/m4-threat/results/{result_id}/visuals/{ordinal}")
catalog = list_results(limit=1)
assert catalog["items"][0]["result_id"] == RESULT_ID
assert catalog["items"][0]["authority"] == "replay-simulated"
visuals = list_visuals(RESULT_ID)
assert len(visuals["items"]) == 32
assert [item["sequence"] for item in visuals["items"][:3]] == [62, 138, 274]
frame = get_visual(RESULT_ID, 1)
assert frame["schema_version"] == "missioncore.perception-threat-visual-frame/v1"
assert frame["point_cloud_sample_count"] > 0
@@ -98,16 +98,12 @@ def test_m4_6_lab_api_projects_report_and_exact_visual_frame() -> None:
def test_m4_6_video_overlay_covers_the_exact_recorded_camera_timeline() -> None:
get_overlay = _endpoint(
"/api/v1/laboratory/m4-threat/results/{result_id}/video-overlay"
)
get_overlay = _endpoint("/api/v1/laboratory/m4-threat/results/{result_id}/video-overlay")
overlay = get_overlay(RESULT_ID)
assert overlay["frame_count"] == 4489
assert overlay["recorded_source"]["session_id"] == (
"20260720T065719Z_viewer_live"
)
assert overlay["recorded_source"]["session_id"] == ("20260720T065719Z_viewer_live")
assert overlay["frames"][0]["frame_index"] == 0
assert overlay["frames"][-1]["frame_index"] == 4488
assert overlay["authority"] == "replay-simulated"
tests/test_replay_threat_provider.py
+50 -18
View File
@@ -2,6 +2,8 @@ from __future__ import annotations
from pathlib import Path
import pytest
from k1link.perception.contracts import (
BoundingRegion2D,
CorridorIntersection,
@@ -15,23 +17,29 @@ from k1link.perception.contracts import (
TemporalState,
ThreatDecision,
)
from k1link.perception.geometry import RecordedGeometryStore
from k1link.perception.graph_validation import validate_threats
from k1link.perception.threat import (
DualEvidenceReplayThreatProvider,
ReplayPose,
RecordedReplayBodyFrameResolver,
ReplayBodyFrame,
load_replay_threat_profile,
)
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v1.json"
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v2.json"
class _Poses:
def pose_for_frame(self, frame_id: str) -> ReplayPose:
return ReplayPose(
class _BodyFrames:
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
return ReplayBodyFrame(
frame_id=frame_id,
position_map_xyz_m=(0.0, 0.0, 0.0),
orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
origin_map_xyz_m=(0.0, 0.0, 0.0),
basis_map_from_body=((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)),
sensor_height_m=1.25,
surface_slope_deg=0.0,
forward_source="fixture",
camera_forward_alignment_deg=0.0,
)
@@ -63,9 +71,7 @@ def _obstacle(
last_centroid_xyz_m=None if state is TemporalState.EXPIRED else current.centroid_xyz_m,
motion=MotionState.UNKNOWN if state is not TemporalState.CURRENT else motion,
motion_confidence=(
0.0
if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN
else 1.0
0.0 if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN else 1.0
),
motion_reason=(
"stale-support"
@@ -127,9 +133,35 @@ def test_replay_threat_profile_freezes_virtual_authority_and_dual_evidence_polic
)
def test_recorded_body_frame_is_grounded_and_does_not_inherit_handheld_roll_pitch() -> None:
profile = load_replay_threat_profile(PROFILE_PATH)
resolver = RecordedReplayBodyFrameResolver(
RecordedGeometryStore.from_repository(REPOSITORY_ROOT),
profile=profile.body_frame,
)
start = resolver.body_frame_for_frame("frame-000000")
middle = resolver.body_frame_for_frame("frame-000138")
later = resolver.body_frame_for_frame("frame-000274")
assert start is None # the opening surface height is not qualified evidence
assert middle is not None and later is not None
assert tuple(row[2] for row in middle.basis_map_from_body) == (0.0, 0.0, 1.0)
assert tuple(row[2] for row in later.basis_map_from_body) == (0.0, 0.0, 1.0)
assert middle.sensor_height_m == pytest.approx(1.2509065924)
assert later.sensor_height_m == pytest.approx(1.2838213430)
assert middle.camera_forward_alignment_deg < 7.0
assert later.camera_forward_alignment_deg < 2.0
summary = resolver.qualification_summary()
assert summary["available"] == 3928
assert summary["qualified"] == 3861
assert summary["rejected"] == 67
def test_static_crossing_approaching_and_geometry_only_critical_cases_are_never_safe() -> None:
provider = DualEvidenceReplayThreatProvider(
pose_resolver=_Poses(),
body_frame_resolver=_BodyFrames(),
profile=load_replay_threat_profile(PROFILE_PATH),
)
current_frame = "frame-000002"
@@ -179,17 +211,17 @@ def test_static_crossing_approaching_and_geometry_only_critical_cases_are_never_
assert {item.decision for item in result} == {ThreatDecision.THREAT}
assert all(item.corridor_intersection is CorridorIntersection.INTERSECTS for item in result)
assert (
next(item for item in result if item.component_id == "approaching").ttc_seconds
is not None
next(item for item in result if item.component_id == "approaching").ttc_seconds is not None
)
assert (
"geometry-only-evidence"
in next(item for item in result if item.component_id == "geometry-only").reason_codes
)
assert "geometry-only-evidence" in next(
item for item in result if item.component_id == "geometry-only"
).reason_codes
def test_receding_and_static_outside_are_clear_but_incomplete_evidence_is_unknown() -> None:
provider = DualEvidenceReplayThreatProvider(
pose_resolver=_Poses(),
body_frame_resolver=_BodyFrames(),
profile=load_replay_threat_profile(PROFILE_PATH),
)
current_frame = "frame-000002"
@@ -246,7 +278,7 @@ def test_receding_and_static_outside_are_clear_but_incomplete_evidence_is_unknow
def test_semantic_hint_and_ephemeral_component_name_do_not_change_threat_geometry() -> None:
provider = DualEvidenceReplayThreatProvider(
pose_resolver=_Poses(),
body_frame_resolver=_BodyFrames(),
profile=load_replay_threat_profile(PROFILE_PATH),
)
history = (