1086 lines
39 KiB
Python
1086 lines
39 KiB
Python
"""Replay-only virtual-corridor threat assessment for Mission Core M4.6.
|
|
|
|
The provider consumes the canonical object map and a source-bound replay pose.
|
|
It never reads semantic class or detector identity when calculating geometry,
|
|
motion, corridor intersection, closest approach or TTC.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import math
|
|
from collections.abc import Iterable
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
from typing import Final, Protocol
|
|
|
|
from .contracts import (
|
|
CorridorIntersection,
|
|
LocalObstacleMap,
|
|
MotionState,
|
|
QualificationState,
|
|
TemporalObstacle,
|
|
TemporalState,
|
|
ThreatAssessment,
|
|
ThreatDecision,
|
|
)
|
|
from .geometry_math import quaternion_xyzw_to_rotation_matrix
|
|
|
|
REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v3"
|
|
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v3"
|
|
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = "config/perception/m4-replay-threat-v3.json"
|
|
LEGACY_REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v2"
|
|
LEGACY_REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v2"
|
|
|
|
type Vector3 = tuple[float, float, float]
|
|
type Matrix3 = tuple[Vector3, Vector3, Vector3]
|
|
|
|
|
|
class ReplayThreatError(ValueError):
|
|
"""A virtual rig, source pose or threat input is ambiguous or unsafe."""
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class ReplayBodyFrame:
|
|
frame_id: str
|
|
origin_map_xyz_m: Vector3
|
|
basis_map_from_body: Matrix3
|
|
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 body frame id is empty")
|
|
if (
|
|
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.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 body frame is not finite")
|
|
columns: Matrix3 = (
|
|
(
|
|
self.basis_map_from_body[0][0],
|
|
self.basis_map_from_body[1][0],
|
|
self.basis_map_from_body[2][0],
|
|
),
|
|
(
|
|
self.basis_map_from_body[0][1],
|
|
self.basis_map_from_body[1][1],
|
|
self.basis_map_from_body[2][1],
|
|
),
|
|
(
|
|
self.basis_map_from_body[0][2],
|
|
self.basis_map_from_body[1][2],
|
|
self.basis_map_from_body[2][2],
|
|
),
|
|
)
|
|
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: Vector3,
|
|
) -> Vector3:
|
|
delta = _vector3(
|
|
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: Vector3,
|
|
) -> Vector3:
|
|
values = tuple(
|
|
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 ReplayBodyFrameResolver(Protocol):
|
|
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame | None: ...
|
|
|
|
|
|
class RecordedReplayBodyFrameResolver:
|
|
"""Derive a ground-level virtual body frame from verified replay evidence."""
|
|
|
|
def __init__(self, store: object, *, profile: VirtualBodyFrameProfile) -> None:
|
|
method = getattr(store, "replay_body_frame_inputs", None)
|
|
if not callable(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 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 = _vector3(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 = _vector3(value / normal_norm for value in plane[:3])
|
|
if ground_normal[2] < 0.0:
|
|
ground_normal = _vector3(-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 = _vector3(float(calibration[2, index]) for index in range(3))
|
|
camera_forward_map = _vector3(
|
|
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 = _vector3(
|
|
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 = _vector3(position[index] - vertical_height * up[index] for index in range(3))
|
|
basis: Matrix3 = (
|
|
(forward[0], left[0], up[0]),
|
|
(forward[1], left[1], up[1]),
|
|
(forward[2], left[2], up[2]),
|
|
)
|
|
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:
|
|
profile_id: str
|
|
body_length_m: float
|
|
body_width_m: float
|
|
lidar_reference: str
|
|
nominal_sensor_height_m: float
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class VirtualCorridorProfile:
|
|
profile_id: str
|
|
forward_length_m: float
|
|
rear_margin_m: float
|
|
lateral_clearance_m: float
|
|
prediction_horizon_seconds: float
|
|
occupied_voxel_size_m: float
|
|
minimum_motion_span_seconds: float
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class ReplayThreatProfile:
|
|
profile_id: str
|
|
provider_id: str
|
|
source_id: str
|
|
session_id: str
|
|
temporal_result_id: str
|
|
temporal_frames_sha256: str
|
|
geometry_result_id: str
|
|
geometry_frames_sha256: str
|
|
detector_result_id: str
|
|
detector_frames_sha256: str
|
|
source_pack_id: str
|
|
source_pack_sha256: str
|
|
calibration_id: str
|
|
calibration_content_sha256: str
|
|
body_frame: VirtualBodyFrameProfile
|
|
rig: VirtualRigProfile
|
|
corridor: VirtualCorridorProfile
|
|
profile_sha256: str
|
|
|
|
|
|
class DualEvidenceReplayThreatProvider:
|
|
"""Assess metric and nonmetric evidence without choosing a primary sensor."""
|
|
|
|
provider_id: str = REPLAY_THREAT_PROVIDER_ID
|
|
|
|
def __init__(
|
|
self,
|
|
*,
|
|
body_frame_resolver: ReplayBodyFrameResolver,
|
|
profile: ReplayThreatProfile,
|
|
) -> None:
|
|
if profile.provider_id not in {
|
|
self.provider_id,
|
|
LEGACY_REPLAY_THREAT_PROVIDER_ID,
|
|
}:
|
|
raise ReplayThreatError("threat provider identity changed")
|
|
self.body_frame_resolver = body_frame_resolver
|
|
self.profile = profile
|
|
|
|
def assess(self, obstacle_map: LocalObstacleMap) -> tuple[ThreatAssessment, ...]:
|
|
if (
|
|
obstacle_map.source_id != self.profile.source_id
|
|
or obstacle_map.session_id != self.profile.session_id
|
|
):
|
|
raise ReplayThreatError("obstacle map escaped the threat profile")
|
|
body_frame = self.body_frame_resolver.body_frame_for_frame(obstacle_map.frame_id)
|
|
assessments = [
|
|
self._metric_or_stale(obstacle_map.frame_id, obstacle, body_frame)
|
|
for obstacle in (*obstacle_map.occupied, *obstacle_map.unknown)
|
|
]
|
|
assessments.extend(
|
|
self._camera_only(obstacle_map.frame_id, proposal.proposal_id)
|
|
for proposal in obstacle_map.camera_uncertainty
|
|
)
|
|
return tuple(assessments)
|
|
|
|
def _metric_or_stale(
|
|
self,
|
|
frame_id: str,
|
|
obstacle: TemporalObstacle,
|
|
body_frame: ReplayBodyFrame | None,
|
|
) -> ThreatAssessment:
|
|
if obstacle.state not in {TemporalState.CURRENT, TemporalState.RETAINED}:
|
|
return self._unknown(
|
|
frame_id,
|
|
obstacle.component_id,
|
|
("stale-evidence", f"temporal-state-{obstacle.state.value}"),
|
|
)
|
|
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 = body_frame.map_point_to_body(obstacle.last_centroid_xyz_m)
|
|
cells_body = tuple(
|
|
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,
|
|
(cell.z + 0.5) * self.profile.corridor.occupied_voxel_size_m,
|
|
)
|
|
)
|
|
for cell in obstacle.cells
|
|
)
|
|
velocity_body = self._relative_velocity_body(obstacle, body_frame)
|
|
corridor_entry = _first_corridor_entry_seconds(
|
|
cells_body,
|
|
velocity_body,
|
|
rig=self.profile.rig,
|
|
corridor=self.profile.corridor,
|
|
)
|
|
current_intersection = _intersects_corridor_now(
|
|
cells_body,
|
|
rig=self.profile.rig,
|
|
corridor=self.profile.corridor,
|
|
)
|
|
retained = obstacle.state is TemporalState.RETAINED
|
|
motion_complete = (
|
|
not retained
|
|
and 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
|
|
reasons = [
|
|
(
|
|
"retained-corridor-intersection"
|
|
if retained
|
|
else "current-corridor-intersection"
|
|
)
|
|
if current_intersection
|
|
else "predicted-corridor-intersection",
|
|
"metric-lidar-geometry",
|
|
]
|
|
elif motion_complete:
|
|
intersection = CorridorIntersection.CLEAR
|
|
decision = ThreatDecision.NOT_THREAT
|
|
reasons = ["predicted-corridor-clear", "metric-lidar-geometry"]
|
|
elif retained:
|
|
intersection = CorridorIntersection.UNKNOWN
|
|
decision = ThreatDecision.UNKNOWN
|
|
reasons = [
|
|
"retained-map-occupancy-outside-current-corridor",
|
|
"metric-lidar-geometry",
|
|
"absence-of-republication-is-not-free",
|
|
]
|
|
else:
|
|
intersection = CorridorIntersection.UNKNOWN
|
|
decision = ThreatDecision.UNKNOWN
|
|
reasons = ["motion-incomplete", "metric-lidar-geometry"]
|
|
|
|
closest = _closest_body_clearance_m(
|
|
cells_body,
|
|
velocity_body,
|
|
rig=self.profile.rig,
|
|
horizon_seconds=self.profile.corridor.prediction_horizon_seconds,
|
|
)
|
|
ttc = _first_body_entry_seconds(
|
|
cells_body,
|
|
velocity_body,
|
|
rig=self.profile.rig,
|
|
voxel_size_m=self.profile.corridor.occupied_voxel_size_m,
|
|
horizon_seconds=self.profile.corridor.prediction_horizon_seconds,
|
|
)
|
|
relative_speed = _closing_speed_mps(centroid_body, velocity_body)
|
|
if retained:
|
|
reasons.append("motion-retained-map-increment-no-current-motion")
|
|
elif velocity_body is None:
|
|
reasons.append(f"motion-{obstacle.motion_reason}")
|
|
else:
|
|
reasons.append(f"motion-{obstacle.motion.value}")
|
|
if obstacle.semantic_hint is None:
|
|
reasons.append("geometry-only-evidence")
|
|
else:
|
|
reasons.append("camera-lidar-associated-evidence")
|
|
return ThreatAssessment(
|
|
assessment_id=_assessment_id(frame_id, obstacle.component_id),
|
|
component_id=obstacle.component_id,
|
|
rig_profile_id=self.profile.rig.profile_id,
|
|
corridor_profile_id=self.profile.corridor.profile_id,
|
|
qualification=QualificationState.QUALIFIED,
|
|
relative_speed_mps=relative_speed,
|
|
closest_approach_m=closest,
|
|
ttc_seconds=ttc,
|
|
corridor_intersection=intersection,
|
|
decision=decision,
|
|
reason_codes=tuple(reasons),
|
|
)
|
|
|
|
def _relative_velocity_body(
|
|
self,
|
|
obstacle: TemporalObstacle,
|
|
current_body_frame: ReplayBodyFrame,
|
|
) -> tuple[float, float] | None:
|
|
if len(obstacle.history) < 2:
|
|
return None
|
|
first = obstacle.history[0]
|
|
last = obstacle.history[-1]
|
|
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_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 = _vector3(
|
|
last.centroid_xyz_m[index] - first.centroid_xyz_m[index] for index in range(3)
|
|
)
|
|
rig_delta = _vector3(
|
|
last_body_frame.origin_map_xyz_m[index] - first_body_frame.origin_map_xyz_m[index]
|
|
for index in range(3)
|
|
)
|
|
relative_map = _vector3(
|
|
(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:
|
|
return self._unknown(
|
|
frame_id,
|
|
proposal_id,
|
|
("camera-only-no-metric-geometry", "absence-of-lidar-is-not-safe"),
|
|
)
|
|
|
|
def _unknown(
|
|
self,
|
|
frame_id: str,
|
|
component_id: str,
|
|
reasons: tuple[str, ...],
|
|
) -> ThreatAssessment:
|
|
return ThreatAssessment(
|
|
assessment_id=_assessment_id(frame_id, component_id),
|
|
component_id=component_id,
|
|
rig_profile_id=self.profile.rig.profile_id,
|
|
corridor_profile_id=self.profile.corridor.profile_id,
|
|
qualification=QualificationState.UNQUALIFIED,
|
|
relative_speed_mps=None,
|
|
closest_approach_m=None,
|
|
ttc_seconds=None,
|
|
corridor_intersection=CorridorIntersection.UNKNOWN,
|
|
decision=ThreatDecision.UNKNOWN,
|
|
reason_codes=reasons,
|
|
)
|
|
|
|
|
|
def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
|
|
if not path.is_file() or path.is_symlink():
|
|
raise ReplayThreatError("replay threat profile is not a regular file")
|
|
raw = path.read_bytes()
|
|
try:
|
|
document = _object(json.loads(raw), "replay threat profile")
|
|
except json.JSONDecodeError as exc:
|
|
raise ReplayThreatError("replay threat profile JSON is invalid") from exc
|
|
_exact_keys(
|
|
document,
|
|
{
|
|
"schema_version",
|
|
"profile_id",
|
|
"provider_id",
|
|
"source",
|
|
"calibration",
|
|
"body_frame",
|
|
"virtual_rig",
|
|
"corridor",
|
|
"policy",
|
|
"authority",
|
|
},
|
|
"replay threat profile",
|
|
)
|
|
identity = (document["schema_version"], document["provider_id"])
|
|
if identity not in {
|
|
(REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID),
|
|
(LEGACY_REPLAY_THREAT_PROFILE_SCHEMA, LEGACY_REPLAY_THREAT_PROVIDER_ID),
|
|
}:
|
|
raise ReplayThreatError("replay threat profile identity is incompatible")
|
|
is_v3 = identity == (REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID)
|
|
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")
|
|
authority = _object(document["authority"], "threat authority")
|
|
_exact_keys(
|
|
source,
|
|
{
|
|
"source_id",
|
|
"session_id",
|
|
"temporal_result_id",
|
|
"temporal_frames_sha256",
|
|
"geometry_result_id",
|
|
"geometry_frames_sha256",
|
|
"detector_result_id",
|
|
"detector_frames_sha256",
|
|
"source_pack_id",
|
|
"source_pack_sha256",
|
|
},
|
|
"threat source",
|
|
)
|
|
_exact_keys(
|
|
calibration,
|
|
{"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,
|
|
{
|
|
"profile_id",
|
|
"body_length_m",
|
|
"body_width_m",
|
|
"lidar_reference",
|
|
"nominal_sensor_height_m",
|
|
"physical_mount_claimed",
|
|
},
|
|
"virtual rig",
|
|
)
|
|
_exact_keys(
|
|
corridor,
|
|
{
|
|
"profile_id",
|
|
"forward_length_m",
|
|
"rear_margin_m",
|
|
"lateral_clearance_m",
|
|
"prediction_horizon_seconds",
|
|
"occupied_voxel_size_m",
|
|
"minimum_motion_span_seconds",
|
|
},
|
|
"virtual corridor",
|
|
)
|
|
policy_keys = {
|
|
"camera_only_decision",
|
|
"held_or_stale_decision",
|
|
"semantic_class_used",
|
|
"detector_identity_used",
|
|
"absence_of_points_means_free",
|
|
"geometry_only_is_eligible",
|
|
}
|
|
if is_v3:
|
|
policy_keys.add("retained_map_intersection_decision")
|
|
_exact_keys(policy, policy_keys, "threat policy")
|
|
_exact_keys(
|
|
authority,
|
|
{
|
|
"mode",
|
|
"physical_live",
|
|
"physical_collision_accepted",
|
|
"commands_enabled",
|
|
"actuation_allowed",
|
|
"navigation_or_safety_accepted",
|
|
},
|
|
"threat authority",
|
|
)
|
|
if (
|
|
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 != {
|
|
"camera_only_decision": "unknown",
|
|
"held_or_stale_decision": "unknown",
|
|
"semantic_class_used": False,
|
|
"detector_identity_used": False,
|
|
"absence_of_points_means_free": False,
|
|
"geometry_only_is_eligible": True,
|
|
**(
|
|
{"retained_map_intersection_decision": "threat"}
|
|
if is_v3
|
|
else {}
|
|
),
|
|
}
|
|
or authority
|
|
!= {
|
|
"mode": "replay-simulated",
|
|
"physical_live": False,
|
|
"physical_collision_accepted": False,
|
|
"commands_enabled": False,
|
|
"actuation_allowed": False,
|
|
"navigation_or_safety_accepted": False,
|
|
}
|
|
):
|
|
raise ReplayThreatError("replay threat policy or authority is unsafe")
|
|
virtual_rig = VirtualRigProfile(
|
|
profile_id=_string(rig, "profile_id"),
|
|
body_length_m=_positive_number(rig, "body_length_m"),
|
|
body_width_m=_positive_number(rig, "body_width_m"),
|
|
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"),
|
|
occupied_voxel_size_m=_positive_number(corridor, "occupied_voxel_size_m"),
|
|
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")
|
|
for value in (
|
|
source.get("temporal_frames_sha256"),
|
|
source.get("geometry_frames_sha256"),
|
|
source.get("detector_frames_sha256"),
|
|
source.get("source_pack_sha256"),
|
|
calibration.get("content_identity_sha256"),
|
|
):
|
|
_sha256(value, "threat evidence digest")
|
|
return ReplayThreatProfile(
|
|
profile_id=_string(document, "profile_id"),
|
|
provider_id=_string(document, "provider_id"),
|
|
source_id=_string(source, "source_id"),
|
|
session_id=_string(source, "session_id"),
|
|
temporal_result_id=_string(source, "temporal_result_id"),
|
|
temporal_frames_sha256=_string(source, "temporal_frames_sha256"),
|
|
geometry_result_id=_string(source, "geometry_result_id"),
|
|
geometry_frames_sha256=_string(source, "geometry_frames_sha256"),
|
|
detector_result_id=_string(source, "detector_result_id"),
|
|
detector_frames_sha256=_string(source, "detector_frames_sha256"),
|
|
source_pack_id=_string(source, "source_pack_id"),
|
|
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(),
|
|
)
|
|
|
|
|
|
def _assessment_id(frame_id: str, component_id: str) -> str:
|
|
digest = hashlib.sha256(f"{frame_id}\0{component_id}".encode()).hexdigest()
|
|
return f"threat-{digest}"
|
|
|
|
|
|
def _corridor_bounds(
|
|
rig: VirtualRigProfile,
|
|
corridor: VirtualCorridorProfile,
|
|
*,
|
|
expansion_m: float,
|
|
) -> tuple[float, float, float, float]:
|
|
return (
|
|
-(rig.body_length_m / 2 + corridor.rear_margin_m + expansion_m),
|
|
rig.body_length_m / 2 + corridor.forward_length_m + expansion_m,
|
|
-(rig.body_width_m / 2 + corridor.lateral_clearance_m + expansion_m),
|
|
rig.body_width_m / 2 + corridor.lateral_clearance_m + expansion_m,
|
|
)
|
|
|
|
|
|
def _body_bounds(
|
|
rig: VirtualRigProfile,
|
|
*,
|
|
expansion_m: float,
|
|
) -> tuple[float, float, float, float]:
|
|
return (
|
|
-(rig.body_length_m / 2 + expansion_m),
|
|
rig.body_length_m / 2 + expansion_m,
|
|
-(rig.body_width_m / 2 + expansion_m),
|
|
rig.body_width_m / 2 + expansion_m,
|
|
)
|
|
|
|
|
|
def _intersects_corridor_now(
|
|
cells_body: tuple[tuple[float, float, float], ...],
|
|
*,
|
|
rig: VirtualRigProfile,
|
|
corridor: VirtualCorridorProfile,
|
|
) -> bool:
|
|
expansion = corridor.occupied_voxel_size_m * math.sqrt(2) / 2
|
|
bounds = _corridor_bounds(rig, corridor, expansion_m=expansion)
|
|
return any(_inside((point[0], point[1]), bounds) for point in cells_body)
|
|
|
|
|
|
def _first_corridor_entry_seconds(
|
|
cells_body: tuple[tuple[float, float, float], ...],
|
|
velocity_body: tuple[float, float] | None,
|
|
*,
|
|
rig: VirtualRigProfile,
|
|
corridor: VirtualCorridorProfile,
|
|
) -> float | None:
|
|
if velocity_body is None:
|
|
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)
|
|
valid = [
|
|
entry
|
|
for entry in entries
|
|
if entry is not None and entry <= corridor.prediction_horizon_seconds
|
|
]
|
|
return None if not valid else round(min(valid), 12)
|
|
|
|
|
|
def _first_body_entry_seconds(
|
|
cells_body: tuple[tuple[float, float, float], ...],
|
|
velocity_body: tuple[float, float] | None,
|
|
*,
|
|
rig: VirtualRigProfile,
|
|
voxel_size_m: float,
|
|
horizon_seconds: float,
|
|
) -> float | None:
|
|
if velocity_body is None:
|
|
return None
|
|
expansion = voxel_size_m * math.sqrt(2) / 2
|
|
bounds = _body_bounds(rig, expansion_m=expansion)
|
|
valid = [
|
|
entry
|
|
for point in cells_body
|
|
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)
|
|
|
|
|
|
def _ray_box_entry(
|
|
point: tuple[float, float],
|
|
velocity: tuple[float, float],
|
|
bounds: tuple[float, float, float, float],
|
|
) -> float | None:
|
|
t_min = 0.0
|
|
t_max = math.inf
|
|
for coordinate, speed, lower, upper in (
|
|
(point[0], velocity[0], bounds[0], bounds[1]),
|
|
(point[1], velocity[1], bounds[2], bounds[3]),
|
|
):
|
|
if abs(speed) < 1e-12:
|
|
if coordinate < lower or coordinate > upper:
|
|
return None
|
|
continue
|
|
first = (lower - coordinate) / speed
|
|
second = (upper - coordinate) / speed
|
|
near, far = min(first, second), max(first, second)
|
|
t_min = max(t_min, near)
|
|
t_max = min(t_max, far)
|
|
if t_min > t_max:
|
|
return None
|
|
return max(0.0, t_min) if t_max >= 0.0 else None
|
|
|
|
|
|
def _closest_body_clearance_m(
|
|
cells_body: tuple[tuple[float, float, float], ...],
|
|
velocity_body: tuple[float, float] | None,
|
|
*,
|
|
rig: VirtualRigProfile,
|
|
horizon_seconds: float,
|
|
) -> float:
|
|
bounds = _body_bounds(rig, expansion_m=0.0)
|
|
candidates = {0.0, horizon_seconds}
|
|
if velocity_body is not None:
|
|
speed_squared = velocity_body[0] ** 2 + velocity_body[1] ** 2
|
|
if speed_squared > 1e-12:
|
|
for point in cells_body:
|
|
candidates.add(
|
|
min(
|
|
horizon_seconds,
|
|
max(
|
|
0.0,
|
|
-(point[0] * velocity_body[0] + point[1] * velocity_body[1])
|
|
/ speed_squared,
|
|
),
|
|
)
|
|
)
|
|
for coordinate, speed, lower, upper in (
|
|
(point[0], velocity_body[0], bounds[0], bounds[1]),
|
|
(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)))
|
|
velocity = velocity_body or (0.0, 0.0)
|
|
clearance = min(
|
|
_point_box_distance(
|
|
(point[0] + velocity[0] * time_s, point[1] + velocity[1] * time_s),
|
|
bounds,
|
|
)
|
|
for point in cells_body
|
|
for time_s in candidates
|
|
)
|
|
return round(clearance, 12)
|
|
|
|
|
|
def _point_box_distance(
|
|
point: tuple[float, float],
|
|
bounds: tuple[float, float, float, float],
|
|
) -> float:
|
|
dx = max(bounds[0] - point[0], 0.0, point[0] - bounds[1])
|
|
dy = max(bounds[2] - point[1], 0.0, point[1] - bounds[3])
|
|
return math.hypot(dx, dy)
|
|
|
|
|
|
def _closing_speed_mps(
|
|
centroid_body: tuple[float, float, float],
|
|
velocity_body: tuple[float, float] | None,
|
|
) -> float | None:
|
|
if velocity_body is None:
|
|
return None
|
|
distance = math.hypot(centroid_body[0], centroid_body[1])
|
|
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,
|
|
12,
|
|
)
|
|
|
|
|
|
def _inside(
|
|
point: tuple[float, float],
|
|
bounds: tuple[float, float, float, float],
|
|
) -> bool:
|
|
return bounds[0] <= point[0] <= bounds[1] and bounds[2] <= point[1] <= bounds[3]
|
|
|
|
|
|
def _object(value: object, label: str) -> dict[str, object]:
|
|
if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
|
|
raise ReplayThreatError(f"{label} must be an object")
|
|
return value
|
|
|
|
|
|
def _exact_keys(document: dict[str, object], expected: set[str], label: str) -> None:
|
|
if set(document) != expected:
|
|
raise ReplayThreatError(f"{label} fields are incompatible")
|
|
|
|
|
|
def _string(document: dict[str, object], key: str) -> str:
|
|
value = document.get(key)
|
|
if not isinstance(value, str) or not value:
|
|
raise ReplayThreatError(f"{key} must be a nonempty string")
|
|
return value
|
|
|
|
|
|
def _positive_number(document: dict[str, object], key: str) -> float:
|
|
value = _number(document, key)
|
|
if value <= 0.0:
|
|
raise ReplayThreatError(f"{key} must be positive")
|
|
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 _vector3(values: Iterable[float]) -> Vector3:
|
|
first, second, third = values
|
|
return float(first), float(second), float(third)
|
|
|
|
|
|
def _dot(first: Vector3, second: Vector3) -> float:
|
|
return sum(first[index] * second[index] for index in range(3))
|
|
|
|
|
|
def _cross(
|
|
first: Vector3,
|
|
second: Vector3,
|
|
) -> Vector3:
|
|
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: Vector3,
|
|
) -> Vector3 | None:
|
|
norm = math.sqrt(_dot(value, value))
|
|
if norm < 1e-9:
|
|
return None
|
|
return value[0] / norm, value[1] / norm, value[2] / norm
|
|
|
|
|
|
def _reject(
|
|
value: Vector3,
|
|
normal: Vector3,
|
|
) -> Vector3:
|
|
along = _dot(value, normal)
|
|
return (
|
|
value[0] - along * normal[0],
|
|
value[1] - along * normal[1],
|
|
value[2] - along * normal[2],
|
|
)
|
|
|
|
|
|
def _angle_degrees(
|
|
first: Vector3,
|
|
second: Vector3,
|
|
) -> 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:
|
|
raise ReplayThreatError(f"{key} must be nonnegative")
|
|
return value
|
|
|
|
|
|
def _number(document: dict[str, object], key: str) -> float:
|
|
value = document.get(key)
|
|
if not isinstance(value, int | float) or isinstance(value, bool) or not math.isfinite(value):
|
|
raise ReplayThreatError(f"{key} must be a finite number")
|
|
return float(value)
|
|
|
|
|
|
def _sha256(value: object, label: str) -> None:
|
|
if (
|
|
not isinstance(value, str)
|
|
or len(value) != 64
|
|
or any(character not in "0123456789abcdef" for character in value)
|
|
):
|
|
raise ReplayThreatError(f"{label} is invalid")
|
|
|
|
|
|
__all__ = [
|
|
"DEFAULT_REPLAY_THREAT_PROFILE_PATH",
|
|
"DualEvidenceReplayThreatProvider",
|
|
"REPLAY_THREAT_PROFILE_SCHEMA",
|
|
"REPLAY_THREAT_PROVIDER_ID",
|
|
"RecordedReplayBodyFrameResolver",
|
|
"ReplayBodyFrame",
|
|
"ReplayBodyFrameResolver",
|
|
"ReplayThreatError",
|
|
"ReplayThreatProfile",
|
|
"VirtualBodyFrameProfile",
|
|
"VirtualCorridorProfile",
|
|
"VirtualRigProfile",
|
|
"load_replay_threat_profile",
|
|
]
|