feat(perception): add dual evidence replay threat
This commit is contained in:
DCCONSTRUCTIONS
@@ -245,6 +245,48 @@ class RecordedGeometryStore:
|
||||
points.setflags(write=False)
|
||||
return points
|
||||
|
||||
def pose_values_for_frame(
|
||||
self,
|
||||
frame_id: str,
|
||||
) -> tuple[tuple[float, float, float], tuple[float, float, float, float]] | None:
|
||||
"""Return one verified replay pose without exposing the source archive."""
|
||||
|
||||
prefix = "frame-"
|
||||
if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit():
|
||||
raise GeometryProviderError("replay pose frame identity is invalid")
|
||||
frame_index = int(frame_id[len(prefix) :])
|
||||
if not 0 <= frame_index < self.profile.frame_count:
|
||||
raise GeometryProviderError("replay pose frame is outside the source profile")
|
||||
if not bool(self._source["sample_available"][frame_index]):
|
||||
return None
|
||||
position = np.asarray(
|
||||
self._source["pose_positions_map"][frame_index],
|
||||
dtype=np.float64,
|
||||
)
|
||||
orientation = np.asarray(
|
||||
self._source["pose_quaternions_map_from_lidar"][frame_index],
|
||||
dtype=np.float64,
|
||||
)
|
||||
if not np.isfinite(position).all() or not np.isfinite(orientation).all():
|
||||
raise GeometryProviderError("available replay pose is not finite")
|
||||
return (
|
||||
(float(position[0]), float(position[1]), float(position[2])),
|
||||
(
|
||||
float(orientation[0]),
|
||||
float(orientation[1]),
|
||||
float(orientation[2]),
|
||||
float(orientation[3]),
|
||||
),
|
||||
)
|
||||
|
||||
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"])
|
||||
)
|
||||
|
||||
def _validate(self) -> None:
|
||||
source_required = {
|
||||
"frame_indices",
|
||||
|
||||
@@ -73,14 +73,20 @@ def validate_threats(
|
||||
obstacle_map: LocalObstacleMap,
|
||||
threats: tuple[ThreatAssessment, ...],
|
||||
) -> None:
|
||||
component_ids = {
|
||||
evidence_ids = {
|
||||
obstacle.component_id for obstacle in (*obstacle_map.occupied, *obstacle_map.unknown)
|
||||
}
|
||||
evidence_ids.update(proposal.proposal_id for proposal in obstacle_map.camera_uncertainty)
|
||||
assessment_ids = [threat.assessment_id for threat in threats]
|
||||
assessed_ids = [threat.component_id for threat in threats]
|
||||
if len(set(assessment_ids)) != len(assessment_ids):
|
||||
raise GraphExecutionError("threat assessment identities are duplicated")
|
||||
if any(threat.component_id not in component_ids for threat in threats):
|
||||
if len(set(assessed_ids)) != len(assessed_ids):
|
||||
raise GraphExecutionError("threat evidence references are duplicated")
|
||||
if any(threat.component_id not in evidence_ids for threat in threats):
|
||||
raise GraphExecutionError("threat assessment references an unknown component")
|
||||
if set(assessed_ids) != evidence_ids:
|
||||
raise GraphExecutionError("threat assessment coverage is incomplete")
|
||||
|
||||
|
||||
__all__ = [
|
||||
|
||||
@@ -0,0 +1,782 @@
|
||||
"""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 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/v1"
|
||||
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v1"
|
||||
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = (
|
||||
"config/perception/m4-replay-threat-v1.json"
|
||||
)
|
||||
|
||||
|
||||
class ReplayThreatError(ValueError):
|
||||
"""A virtual rig, source pose or threat input is ambiguous or unsafe."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class ReplayPose:
|
||||
frame_id: str
|
||||
position_map_xyz_m: tuple[float, float, float]
|
||||
orientation_map_from_lidar_xyzw: tuple[float, float, float, float]
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if not self.frame_id:
|
||||
raise ReplayThreatError("replay pose frame id is empty")
|
||||
if (
|
||||
len(self.position_map_xyz_m) != 3
|
||||
or len(self.orientation_map_from_lidar_xyzw) != 4
|
||||
or not all(
|
||||
math.isfinite(value)
|
||||
for value in (
|
||||
*self.position_map_xyz_m,
|
||||
*self.orientation_map_from_lidar_xyzw,
|
||||
)
|
||||
)
|
||||
):
|
||||
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")
|
||||
|
||||
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)
|
||||
)
|
||||
values = tuple(
|
||||
float(sum(delta[row] * rotation[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 RecordedReplayPoseResolver:
|
||||
"""Adapt the verified geometry store to the source-neutral pose seam."""
|
||||
|
||||
def __init__(self, store: object) -> None:
|
||||
method = getattr(store, "pose_values_for_frame", None)
|
||||
if not callable(method):
|
||||
raise ReplayThreatError("recorded pose store does not expose verified poses")
|
||||
self._pose_values_for_frame = method
|
||||
|
||||
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,
|
||||
)
|
||||
|
||||
|
||||
@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
|
||||
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,
|
||||
*,
|
||||
pose_resolver: ReplayPoseResolver,
|
||||
profile: ReplayThreatProfile,
|
||||
) -> None:
|
||||
if profile.provider_id != self.provider_id:
|
||||
raise ReplayThreatError("threat provider identity changed")
|
||||
self.pose_resolver = pose_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")
|
||||
pose = self.pose_resolver.pose_for_frame(obstacle_map.frame_id)
|
||||
assessments = [
|
||||
self._metric_or_stale(obstacle_map.frame_id, obstacle, pose)
|
||||
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,
|
||||
pose: ReplayPose | None,
|
||||
) -> ThreatAssessment:
|
||||
if obstacle.state is not TemporalState.CURRENT:
|
||||
return self._unknown(
|
||||
frame_id,
|
||||
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:
|
||||
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)
|
||||
cells_body = tuple(
|
||||
pose.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, pose)
|
||||
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,
|
||||
)
|
||||
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
|
||||
reasons = [
|
||||
"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"]
|
||||
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 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_pose: ReplayPose,
|
||||
) -> 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_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:
|
||||
return None
|
||||
obstacle_delta = tuple(
|
||||
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]
|
||||
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)
|
||||
)
|
||||
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",
|
||||
"virtual_rig",
|
||||
"corridor",
|
||||
"policy",
|
||||
"authority",
|
||||
},
|
||||
"replay threat profile",
|
||||
)
|
||||
if (
|
||||
document["schema_version"] != REPLAY_THREAT_PROFILE_SCHEMA
|
||||
or document["provider_id"] != REPLAY_THREAT_PROVIDER_ID
|
||||
):
|
||||
raise ReplayThreatError("replay threat profile identity is incompatible")
|
||||
source = _object(document["source"], "threat source")
|
||||
calibration = _object(document["calibration"], "threat calibration")
|
||||
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(
|
||||
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",
|
||||
)
|
||||
_exact_keys(
|
||||
policy,
|
||||
{
|
||||
"camera_only_decision",
|
||||
"held_or_stale_decision",
|
||||
"semantic_class_used",
|
||||
"detector_identity_used",
|
||||
"absence_of_points_means_free",
|
||||
"geometry_only_is_eligible",
|
||||
},
|
||||
"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-binding-only"
|
||||
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,
|
||||
}
|
||||
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"),
|
||||
)
|
||||
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"),
|
||||
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 _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",
|
||||
"RecordedReplayPoseResolver",
|
||||
"ReplayPose",
|
||||
"ReplayPoseResolver",
|
||||
"ReplayThreatError",
|
||||
"ReplayThreatProfile",
|
||||
"VirtualCorridorProfile",
|
||||
"VirtualRigProfile",
|
||||
"load_replay_threat_profile",
|
||||
]
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,43 @@
|
||||
"""Command-line entrypoint for the local M4.6 replay threat run."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
from .threat_replay import build_threat_replay
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--repository-root", type=Path, required=True)
|
||||
parser.add_argument("--temporal-result-root", type=Path, required=True)
|
||||
parser.add_argument("--geometry-result-root", type=Path, required=True)
|
||||
parser.add_argument("--detector-result-root", type=Path, required=True)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
result = build_threat_replay(
|
||||
repository_root=args.repository_root,
|
||||
temporal_result_root=args.temporal_result_root,
|
||||
geometry_result_root=args.geometry_result_root,
|
||||
detector_result_root=args.detector_result_root,
|
||||
output_root=args.output_root,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"result_id": result.result_id,
|
||||
"result_root": str(result.result_root),
|
||||
"accepted": result.accepted,
|
||||
"metrics": result.metrics,
|
||||
},
|
||||
ensure_ascii=False,
|
||||
sort_keys=True,
|
||||
)
|
||||
)
|
||||
return 0 if result.accepted else 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user