diff --git a/apps/control-station/src/core/laboratory/m4ReplayThreat.ts b/apps/control-station/src/core/laboratory/m4ReplayThreat.ts index cf21708..0a14c67 100644 --- a/apps/control-station/src/core/laboratory/m4ReplayThreat.ts +++ b/apps/control-station/src/core/laboratory/m4ReplayThreat.ts @@ -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>; }; 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")), }; diff --git a/apps/control-station/src/workspaces/laboratory/M4ReplayThreatResult.tsx b/apps/control-station/src/workspaces/laboratory/M4ReplayThreatResult.tsx index 4406b6a..1ffbe8f 100644 --- a/apps/control-station/src/workspaces/laboratory/M4ReplayThreatResult.tsx +++ b/apps/control-station/src/workspaces/laboratory/M4ReplayThreatResult.tsx @@ -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-разработкой.", }} /> diff --git a/apps/control-station/test/m4ReplayThreat.test.mjs b/apps/control-station/test/m4ReplayThreat.test.mjs index e574e0d..85edbd8 100644 --- a/apps/control-station/test/m4ReplayThreat.test.mjs +++ b/apps/control-station/test/m4ReplayThreat.test.mjs @@ -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 () => { diff --git a/config/perception/m4-replay-threat-v1.json b/config/perception/m4-replay-threat-v2.json similarity index 71% rename from config/perception/m4-replay-threat-v1.json rename to config/perception/m4-replay-threat-v2.json index fabb745..31168ac 100644 --- a/config/perception/m4-replay-threat-v1.json +++ b/config/perception/m4-replay-threat-v2.json @@ -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, diff --git a/docs/22_MILESTONE_4_OBJECT_CENTRIC_RECORDED_REALTIME_CV.md b/docs/22_MILESTONE_4_OBJECT_CENTRIC_RECORDED_REALTIME_CV.md index b3606da..0e37454 100644 --- a/docs/22_MILESTONE_4_OBJECT_CENTRIC_RECORDED_REALTIME_CV.md +++ b/docs/22_MILESTONE_4_OBJECT_CENTRIC_RECORDED_REALTIME_CV.md @@ -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 diff --git a/docs/adr/0040-dual-evidence-replay-threat-boundary.md b/docs/adr/0040-dual-evidence-replay-threat-boundary.md index 1244229..de94499 100644 --- a/docs/adr/0040-dual-evidence-replay-threat-boundary.md +++ b/docs/adr/0040-dual-evidence-replay-threat-boundary.md @@ -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 diff --git a/scripts/build_m4_worker_shadow_artifact.py b/scripts/build_m4_worker_shadow_artifact.py index 5ec04d7..d3e0d75 100644 --- a/scripts/build_m4_worker_shadow_artifact.py +++ b/scripts/build_m4_worker_shadow_artifact.py @@ -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, diff --git a/src/k1link/perception/geometry.py b/src/k1link/perception/geometry.py index e0bb81f..e941d10 100644 --- a/src/k1link/perception/geometry.py +++ b/src/k1link/perception/geometry.py @@ -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 = { diff --git a/src/k1link/perception/threat.py b/src/k1link/perception/threat.py index e2df007..7a5b7de 100644 --- a/src/k1link/perception/threat.py +++ b/src/k1link/perception/threat.py @@ -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", diff --git a/src/k1link/perception/threat_replay.py b/src/k1link/perception/threat_replay.py index 2343a09..b2e8415 100644 --- a/src/k1link/perception/threat_replay.py +++ b/src/k1link/perception/threat_replay.py @@ -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") diff --git a/tests/test_m4_threat_replay_result.py b/tests/test_m4_threat_replay_result.py index fd301f9..c404130 100644 --- a/tests/test_m4_threat_replay_result.py +++ b/tests/test_m4_threat_replay_result.py @@ -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" diff --git a/tests/test_replay_threat_provider.py b/tests/test_replay_threat_provider.py index 2dcc7f5..2bf8808 100644 --- a/tests/test_replay_threat_provider.py +++ b/tests/test_replay_threat_provider.py @@ -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 = (