340 lines
12 KiB
Python
340 lines
12 KiB
Python
from __future__ import annotations
|
|
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
|
|
from k1link.perception.contracts import (
|
|
BoundingRegion2D,
|
|
CorridorIntersection,
|
|
GridCell,
|
|
HistorySample,
|
|
LocalObstacleMap,
|
|
MotionState,
|
|
ObjectProposal2D,
|
|
SourceAccounting,
|
|
TemporalObstacle,
|
|
TemporalState,
|
|
ThreatDecision,
|
|
)
|
|
from k1link.perception.geometry import RecordedGeometryStore
|
|
from k1link.perception.graph_validation import validate_threats
|
|
from k1link.perception.threat import (
|
|
DualEvidenceReplayThreatProvider,
|
|
RecordedReplayBodyFrameResolver,
|
|
ReplayBodyFrame,
|
|
load_replay_threat_profile,
|
|
)
|
|
|
|
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
|
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v2.json"
|
|
ROLLING_PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v3.json"
|
|
|
|
|
|
class _BodyFrames:
|
|
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
|
|
return ReplayBodyFrame(
|
|
frame_id=frame_id,
|
|
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 _obstacle(
|
|
component_id: str,
|
|
cell: GridCell,
|
|
*,
|
|
motion: MotionState,
|
|
history: tuple[tuple[str, int, tuple[float, float, float]], ...],
|
|
state: TemporalState = TemporalState.CURRENT,
|
|
semantic_hint: str | None = None,
|
|
) -> TemporalObstacle:
|
|
samples = tuple(
|
|
HistorySample(frame_id=frame_id, evidence_time_ns=time_ns, centroid_xyz_m=point)
|
|
for frame_id, time_ns, point in history
|
|
)
|
|
current = samples[-1]
|
|
return TemporalObstacle(
|
|
component_id=component_id,
|
|
identity_scope="ephemeral",
|
|
state=state,
|
|
ttl_ns=(3_000_000_000 if state is TemporalState.RETAINED else 750_000_000),
|
|
last_hit_ns=current.evidence_time_ns,
|
|
age_ns=0 if state is TemporalState.CURRENT else 100_000_000,
|
|
association_basis="test-spatial-support",
|
|
history=samples,
|
|
cells=() if state is TemporalState.EXPIRED else (cell,),
|
|
coordinate_frame=None if state is TemporalState.EXPIRED else "map",
|
|
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
|
|
),
|
|
motion_reason=(
|
|
"stale-support"
|
|
if state is not TemporalState.CURRENT
|
|
else "bounded-map-history-moving"
|
|
if motion is MotionState.MOVING
|
|
else "bounded-map-history-stationary"
|
|
if motion is MotionState.STATIONARY
|
|
else "insufficient-history"
|
|
),
|
|
semantic_hint=semantic_hint,
|
|
)
|
|
|
|
|
|
def _proposal(frame_id: str = "frame-000002") -> ObjectProposal2D:
|
|
return ObjectProposal2D(
|
|
proposal_id="proposal-camera-only",
|
|
source_id="RAVNOVES00",
|
|
frame_id=frame_id,
|
|
region=BoundingRegion2D(10.0, 10.0, 20.0, 20.0),
|
|
objectness=0.8,
|
|
provider_id="test-detector/v1",
|
|
model_id="test-model/v1",
|
|
preprocess_id="test-preprocess/v1",
|
|
semantic_hint="person",
|
|
)
|
|
|
|
|
|
def _map(
|
|
*,
|
|
occupied: tuple[TemporalObstacle, ...] = (),
|
|
unknown: tuple[TemporalObstacle, ...] = (),
|
|
camera: tuple[ObjectProposal2D, ...] = (),
|
|
frame_id: str = "frame-000002",
|
|
) -> LocalObstacleMap:
|
|
return LocalObstacleMap(
|
|
source_id="RAVNOVES00",
|
|
session_id="20260720T065719Z_viewer_live",
|
|
frame_id=frame_id,
|
|
graph_id="reference-perception-graph/v1",
|
|
generated_monotonic_ns=0,
|
|
output_age_ns=0,
|
|
occupied=occupied,
|
|
unknown=unknown,
|
|
camera_uncertainty=camera,
|
|
accounting=SourceAccounting(1, 1, 0, 0),
|
|
)
|
|
|
|
|
|
def test_replay_threat_profile_freezes_virtual_authority_and_dual_evidence_policy() -> None:
|
|
profile = load_replay_threat_profile(PROFILE_PATH)
|
|
|
|
assert profile.rig.body_length_m == 1.0
|
|
assert profile.rig.body_width_m == 0.6
|
|
assert profile.rig.nominal_sensor_height_m == 1.25
|
|
assert profile.corridor.forward_length_m == 8.0
|
|
assert profile.calibration_content_sha256 == (
|
|
"05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9"
|
|
)
|
|
|
|
|
|
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(
|
|
body_frame_resolver=_BodyFrames(),
|
|
profile=load_replay_threat_profile(PROFILE_PATH),
|
|
)
|
|
current_frame = "frame-000002"
|
|
critical = (
|
|
_obstacle(
|
|
"static-in-corridor",
|
|
GridCell(6, 0, 0),
|
|
motion=MotionState.STATIONARY,
|
|
history=(
|
|
("frame-000000", 0, (2.925, 0.225, 0.225)),
|
|
(current_frame, 300_000_000, (2.925, 0.225, 0.225)),
|
|
),
|
|
),
|
|
_obstacle(
|
|
"crossing",
|
|
GridCell(6, 3, 0),
|
|
motion=MotionState.MOVING,
|
|
history=(
|
|
("frame-000000", 0, (2.925, 2.575, 0.225)),
|
|
(current_frame, 300_000_000, (2.925, 1.575, 0.225)),
|
|
),
|
|
),
|
|
_obstacle(
|
|
"approaching",
|
|
GridCell(9, 0, 0),
|
|
motion=MotionState.MOVING,
|
|
history=(
|
|
("frame-000000", 0, (6.275, 0.225, 0.225)),
|
|
(current_frame, 300_000_000, (4.275, 0.225, 0.225)),
|
|
),
|
|
semantic_hint="car",
|
|
),
|
|
_obstacle(
|
|
"geometry-only",
|
|
GridCell(4, 0, 0),
|
|
motion=MotionState.STATIONARY,
|
|
history=(
|
|
("frame-000000", 0, (2.025, 0.225, 0.225)),
|
|
(current_frame, 300_000_000, (2.025, 0.225, 0.225)),
|
|
),
|
|
semantic_hint=None,
|
|
),
|
|
)
|
|
|
|
result = provider.assess(_map(occupied=critical))
|
|
|
|
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
|
|
)
|
|
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(
|
|
body_frame_resolver=_BodyFrames(),
|
|
profile=load_replay_threat_profile(PROFILE_PATH),
|
|
)
|
|
current_frame = "frame-000002"
|
|
clear = (
|
|
_obstacle(
|
|
"static-outside",
|
|
GridCell(6, 7, 0),
|
|
motion=MotionState.STATIONARY,
|
|
history=(
|
|
("frame-000000", 0, (2.925, 3.375, 0.225)),
|
|
(current_frame, 300_000_000, (2.925, 3.375, 0.225)),
|
|
),
|
|
),
|
|
_obstacle(
|
|
"receding-behind",
|
|
GridCell(-5, 0, 0),
|
|
motion=MotionState.MOVING,
|
|
history=(
|
|
("frame-000000", 0, (-1.025, 0.225, 0.225)),
|
|
(current_frame, 300_000_000, (-2.025, 0.225, 0.225)),
|
|
),
|
|
),
|
|
)
|
|
incomplete = _obstacle(
|
|
"unknown-motion",
|
|
GridCell(6, 7, 0),
|
|
motion=MotionState.UNKNOWN,
|
|
history=((current_frame, 300_000_000, (2.925, 3.375, 0.225)),),
|
|
)
|
|
held = _obstacle(
|
|
"occluded-held",
|
|
GridCell(6, 0, 0),
|
|
motion=MotionState.UNKNOWN,
|
|
history=((current_frame, 300_000_000, (2.925, 0.225, 0.225)),),
|
|
state=TemporalState.HELD,
|
|
)
|
|
obstacle_map = _map(
|
|
occupied=(*clear, incomplete),
|
|
unknown=(held,),
|
|
camera=(_proposal(),),
|
|
)
|
|
|
|
result = provider.assess(obstacle_map)
|
|
by_id = {item.component_id: item for item in result}
|
|
|
|
assert by_id["static-outside"].decision is ThreatDecision.NOT_THREAT
|
|
assert by_id["receding-behind"].decision is ThreatDecision.NOT_THREAT
|
|
assert by_id["unknown-motion"].decision is ThreatDecision.UNKNOWN
|
|
assert by_id["occluded-held"].decision is ThreatDecision.UNKNOWN
|
|
assert by_id["proposal-camera-only"].decision is ThreatDecision.UNKNOWN
|
|
assert by_id["proposal-camera-only"].closest_approach_m is None
|
|
validate_threats(obstacle_map, result)
|
|
|
|
|
|
def test_semantic_hint_and_ephemeral_component_name_do_not_change_threat_geometry() -> None:
|
|
provider = DualEvidenceReplayThreatProvider(
|
|
body_frame_resolver=_BodyFrames(),
|
|
profile=load_replay_threat_profile(PROFILE_PATH),
|
|
)
|
|
history = (
|
|
("frame-000000", 0, (2.925, 0.225, 0.225)),
|
|
("frame-000002", 300_000_000, (2.925, 0.225, 0.225)),
|
|
)
|
|
first = _obstacle(
|
|
"ephemeral-a",
|
|
GridCell(6, 0, 0),
|
|
motion=MotionState.STATIONARY,
|
|
history=history,
|
|
semantic_hint="car",
|
|
)
|
|
second = _obstacle(
|
|
"ephemeral-b",
|
|
GridCell(6, 0, 0),
|
|
motion=MotionState.STATIONARY,
|
|
history=history,
|
|
semantic_hint=None,
|
|
)
|
|
|
|
values = provider.assess(_map(occupied=(first, second)))
|
|
|
|
assert values[0].decision == values[1].decision
|
|
assert values[0].corridor_intersection == values[1].corridor_intersection
|
|
assert values[0].relative_speed_mps == values[1].relative_speed_mps
|
|
assert values[0].closest_approach_m == values[1].closest_approach_m
|
|
assert values[0].ttc_seconds == values[1].ttc_seconds
|
|
|
|
|
|
def test_retained_map_can_block_but_cannot_claim_clear_or_motion() -> None:
|
|
provider = DualEvidenceReplayThreatProvider(
|
|
body_frame_resolver=_BodyFrames(),
|
|
profile=load_replay_threat_profile(ROLLING_PROFILE_PATH),
|
|
)
|
|
retained_in = _obstacle(
|
|
"retained-in",
|
|
GridCell(6, 0, 0),
|
|
motion=MotionState.UNKNOWN,
|
|
history=(("frame-000002", 300_000_000, (2.925, 0.225, 0.225)),),
|
|
state=TemporalState.RETAINED,
|
|
)
|
|
retained_out = _obstacle(
|
|
"retained-out",
|
|
GridCell(6, 7, 0),
|
|
motion=MotionState.UNKNOWN,
|
|
history=(("frame-000002", 300_000_000, (2.925, 3.375, 0.225)),),
|
|
state=TemporalState.RETAINED,
|
|
)
|
|
|
|
values = provider.assess(_map(occupied=(retained_in, retained_out)))
|
|
by_id = {item.component_id: item for item in values}
|
|
|
|
assert by_id["retained-in"].decision is ThreatDecision.THREAT
|
|
assert by_id["retained-in"].relative_speed_mps is None
|
|
assert by_id["retained-out"].decision is ThreatDecision.UNKNOWN
|
|
assert by_id["retained-out"].corridor_intersection is CorridorIntersection.UNKNOWN
|