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

fix(perception): stabilize replay body frame

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-05 20:04:08 +03:00
parent de19229895
commit c70ad345ea
12 changed files with 783 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/perception/threat_replay.py
+124 -80
View File
@@ -35,7 +35,6 @@ from .contracts import (
from .detector_replay_contracts import DetectorReplayResult
from .detector_replay_result import read_detector_replay_result
from .geometry import RecordedGeometryStore
from .geometry_math import quaternion_xyzw_to_rotation_matrix
from .geometry_replay import GeometryReplayResult, read_geometry_replay_result
from .providers import SourcePacket
from .recorded_source import RecordedRavnoves00Source, ReplayPacing
@@ -43,8 +42,8 @@ from .temporal_replay import TemporalReplayResult, read_temporal_replay_result
from .threat import (
DEFAULT_REPLAY_THREAT_PROFILE_PATH,
DualEvidenceReplayThreatProvider,
RecordedReplayPoseResolver,
ReplayPose,
RecordedReplayBodyFrameResolver,
ReplayBodyFrame,
ReplayThreatProfile,
load_replay_threat_profile,
)
@@ -62,6 +61,7 @@ THREAT_REPLAY_REPORT_NAME: Final = "report.json"
THREAT_REPLAY_MANIFEST_NAME: Final = "manifest.json"
VISUAL_FRAME_COUNT: Final = 32
VISUAL_POINT_LIMIT: Final = 4_000
VISUAL_GEOMETRY_REGRESSION_SEQUENCES: Final = (138, 274)
class ThreatReplayError(RuntimeError):
@@ -87,25 +87,26 @@ def build_threat_replay(
output_root: Path,
) -> ThreatReplayResult:
repository = repository_root.resolve()
profile = load_replay_threat_profile(
repository / DEFAULT_REPLAY_THREAT_PROFILE_PATH
)
profile = load_replay_threat_profile(repository / DEFAULT_REPLAY_THREAT_PROFILE_PATH)
temporal = read_temporal_replay_result(temporal_result_root)
geometry = read_geometry_replay_result(geometry_result_root)
detector = read_detector_replay_result(detector_result_root)
_validate_upstream(profile, temporal, geometry, detector)
store = RecordedGeometryStore.from_repository(repository)
pose_resolver = RecordedReplayPoseResolver(store)
body_frame_resolver = RecordedReplayBodyFrameResolver(
store,
profile=profile.body_frame,
)
provider = DualEvidenceReplayThreatProvider(
pose_resolver=pose_resolver,
body_frame_resolver=body_frame_resolver,
profile=profile,
)
source = RecordedRavnoves00Source.from_repository(
repository,
pacing=ReplayPacing.UNCAPPED,
)
visual_sequences = _visual_sequences(store.available_frame_indices())
visual_sequences = _visual_sequences(body_frame_resolver.qualified_frame_indices())
root = output_root.expanduser().absolute()
root.mkdir(mode=0o700, parents=True, exist_ok=True)
@@ -189,13 +190,11 @@ def build_threat_replay(
)
frame_started_ns = time.perf_counter_ns()
assessments = provider.assess(obstacle_map)
latencies_ms.append(
(time.perf_counter_ns() - frame_started_ns) / 1_000_000
)
latencies_ms.append((time.perf_counter_ns() - frame_started_ns) / 1_000_000)
by_id = {item.component_id: item for item in assessments}
expected_ids = {
item.component_id for item in (*current, *unknown)
} | {item.proposal_id for item in camera_uncertainty}
expected_ids = {item.component_id for item in (*current, *unknown)} | {
item.proposal_id for item in camera_uncertainty
}
if set(by_id) != expected_ids:
raise ThreatReplayError("threat assessment coverage is incomplete")
camera_rows = _camera_rows(
@@ -204,8 +203,7 @@ def build_threat_replay(
by_id,
)
metric_rows = [
_metric_row(item, by_id[item.component_id])
for item in (*current, *unknown)
_metric_row(item, by_id[item.component_id]) for item in (*current, *unknown)
]
for item in assessments:
assessment_counts[item.decision.value] += 1
@@ -222,10 +220,8 @@ def build_threat_replay(
"sequence": frame_count,
"frame_id": packet.envelope.frame_id,
"source_time_ns": packet.envelope.timestamps.source_ns,
"source_available": (
packet.envelope.registered_point_increment.available
),
"pose_available": pose_resolver.pose_for_frame(
"source_available": (packet.envelope.registered_point_increment.available),
"body_frame_available": body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
)
is not None,
@@ -247,7 +243,9 @@ def build_threat_replay(
_visual_frame(
packet=packet,
store=store,
pose=pose_resolver.pose_for_frame(packet.envelope.frame_id),
body_frame=body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
),
metric_rows=metric_rows,
camera_rows=camera_rows,
profile=profile,
@@ -277,6 +275,7 @@ def build_threat_replay(
elapsed_ns=elapsed_ns,
visual_count=visual_count,
fixtures=fixtures,
body_frame=body_frame_resolver.qualification_summary(),
)
requirements = _requirements(metrics, fixtures)
accepted = all(value is True for value in requirements.values())
@@ -300,6 +299,12 @@ def build_threat_replay(
"source_pack_sha256": profile.source_pack_sha256,
"calibration_id": profile.calibration_id,
"calibration_content_sha256": profile.calibration_content_sha256,
"body_frame": {
"schema_version": profile.body_frame.schema_version,
"origin": profile.body_frame.origin,
"up": profile.body_frame.up,
"forward": profile.body_frame.forward,
},
"rig_profile_id": profile.rig.profile_id,
"corridor_profile_id": profile.corridor.profile_id,
"producer_sha256": _producer_hashes(repository),
@@ -326,13 +331,20 @@ def build_threat_replay(
profile.rig.body_width_m,
],
"nominal_sensor_height_m": profile.rig.nominal_sensor_height_m,
"body_frame": {
"origin": profile.body_frame.origin,
"up": profile.body_frame.up,
"forward": profile.body_frame.forward,
},
"forward_corridor_m": profile.corridor.forward_length_m,
"prediction_horizon_seconds": (
profile.corridor.prediction_horizon_seconds
),
"prediction_horizon_seconds": (profile.corridor.prediction_horizon_seconds),
},
"limitations": [
"The body and corridor are replay-simulated, not a measured physical mount.",
(
"The replay base_footprint uses SLAM trajectory and map gravity; "
"a mounted vehicle replaces it with calibrated T_body_from_sensor."
),
"The LiDAR archive is the vendor mapped point increment, not every raw beam.",
"TTC uses bounded constant-relative-velocity replay extrapolation.",
"Camera-only evidence remains unknown and cannot establish metric clearance.",
@@ -398,9 +410,7 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
):
raise ThreatReplayError("threat replay identity changed")
artifacts = _array(manifest.get("artifacts"), "threat artifacts")
by_role = {
_object(item, "threat artifact").get("role"): item for item in artifacts
}
by_role = {_object(item, "threat artifact").get("role"): item for item in artifacts}
expected = {
"threat-replay-frames": (THREAT_REPLAY_FRAMES_NAME, "frames_sha256"),
"threat-visual-frames": (THREAT_REPLAY_VISUALS_NAME, "visuals_sha256"),
@@ -420,9 +430,7 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
raise ThreatReplayError("threat artifact identity changed")
report = _read_json(paths["threat-replay-report"])
metrics = _object(identity.get("metrics"), "threat metrics")
requirements = _object(
identity.get("acceptance_requirements"), "threat requirements"
)
requirements = _object(identity.get("acceptance_requirements"), "threat requirements")
fixtures = _read_json(paths["threat-deterministic-fixtures"])
accepted = all(value is True for value in requirements.values())
if (
@@ -505,9 +513,7 @@ def _metric_row(
"motion_reason": obstacle.motion_reason,
"semantic_hint": obstacle.semantic_hint,
"centroid_map_xyz_m": (
None
if obstacle.last_centroid_xyz_m is None
else list(obstacle.last_centroid_xyz_m)
None if obstacle.last_centroid_xyz_m is None else list(obstacle.last_centroid_xyz_m)
),
"cells": [item.to_dict() for item in obstacle.cells],
"history": [item.to_dict() for item in obstacle.history],
@@ -552,9 +558,7 @@ def _camera_rows(
"occupied_support": geometry["occupied_support"],
"range_m": geometry["range_m"],
"geometry_reason_codes": geometry["reason_codes"],
"threat_decision": (
None if assessment is None else assessment.decision.value
),
"threat_decision": (None if assessment is None else assessment.decision.value),
"threat_reason_codes": (
[] if assessment is None else list(assessment.reason_codes)
),
@@ -567,21 +571,19 @@ def _visual_frame(
*,
packet: SourcePacket,
store: RecordedGeometryStore,
pose: ReplayPose | None,
body_frame: ReplayBodyFrame | None,
metric_rows: list[dict[str, object]],
camera_rows: list[dict[str, object]],
profile: ReplayThreatProfile,
) -> dict[str, object]:
if pose is None:
raise ThreatReplayError("visual frame has no source pose")
if body_frame is None:
raise ThreatReplayError("visual frame has no qualified body frame")
points = store.current_points(packet)
if points is None:
raise ThreatReplayError("visual frame has no current point cloud")
rotation = quaternion_xyzw_to_rotation_matrix(
pose.orientation_map_from_lidar_xyzw
)
position = np.asarray(pose.position_map_xyz_m, dtype=np.float64)
points_body = (points - position) @ rotation
basis = np.asarray(body_frame.basis_map_from_body, dtype=np.float64)
origin = np.asarray(body_frame.origin_map_xyz_m, dtype=np.float64)
points_body = (points - origin) @ basis
stride = max(1, math.ceil(points_body.shape[0] / VISUAL_POINT_LIMIT))
sampled = points_body[::stride][:VISUAL_POINT_LIMIT]
metric_visuals = []
@@ -590,7 +592,7 @@ def _visual_frame(
cells = row.get("cells")
if not isinstance(centroid, list) or not isinstance(cells, list):
continue
centroid_body = pose.map_point_to_body(
centroid_body = body_frame.map_point_to_body(
(float(centroid[0]), float(centroid[1]), float(centroid[2]))
)
cell_centers = []
@@ -602,11 +604,7 @@ def _visual_frame(
for key in ("x", "y", "z")
)
cell_centers.append(
list(
pose.map_point_to_body(
(point_map[0], point_map[1], point_map[2])
)
)
list(body_frame.map_point_to_body((point_map[0], point_map[1], point_map[2])))
)
metric_visuals.append(
{
@@ -628,6 +626,14 @@ def _visual_frame(
"point_cloud_sample_count": int(sampled.shape[0]),
"metric_obstacles": metric_visuals,
"camera_proposals": camera_rows,
"body_frame": {
"origin_map_xyz_m": list(body_frame.origin_map_xyz_m),
"basis_map_from_body": [list(row) for row in body_frame.basis_map_from_body],
"sensor_height_m": body_frame.sensor_height_m,
"surface_slope_deg": body_frame.surface_slope_deg,
"forward_source": body_frame.forward_source,
"camera_forward_alignment_deg": body_frame.camera_forward_alignment_deg,
},
"rig": {
"length_m": profile.rig.body_length_m,
"width_m": profile.rig.body_width_m,
@@ -636,30 +642,29 @@ def _visual_frame(
"corridor": {
"forward_length_m": profile.corridor.forward_length_m,
"rear_margin_m": profile.corridor.rear_margin_m,
"half_width_m": (
profile.rig.body_width_m / 2
+ profile.corridor.lateral_clearance_m
),
"prediction_horizon_seconds": (
profile.corridor.prediction_horizon_seconds
),
"half_width_m": (profile.rig.body_width_m / 2 + profile.corridor.lateral_clearance_m),
"prediction_horizon_seconds": (profile.corridor.prediction_horizon_seconds),
},
"authority": _false_authority(),
}
class _FixturePoses:
def pose_for_frame(self, frame_id: str) -> ReplayPose:
return ReplayPose(
class _FixtureBodyFrames:
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
return ReplayBodyFrame(
frame_id=frame_id,
position_map_xyz_m=(0.0, 0.0, 0.0),
orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
origin_map_xyz_m=(0.0, 0.0, 0.0),
basis_map_from_body=((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)),
sensor_height_m=1.25,
surface_slope_deg=0.0,
forward_source="fixture",
camera_forward_alignment_deg=0.0,
)
def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
provider = DualEvidenceReplayThreatProvider(
pose_resolver=_FixturePoses(),
body_frame_resolver=_FixtureBodyFrames(),
profile=profile,
)
frame_id = "frame-000002"
@@ -783,8 +788,7 @@ def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
"cases": cases,
"critical_case_count": sum(item["critical"] is True for item in cases),
"critical_false_not_threat_count": sum(
item["critical"] is True and item["actual"] == "not-threat"
for item in cases
item["critical"] is True and item["actual"] == "not-threat" for item in cases
),
"passed_count": sum(item["passed"] is True for item in cases),
"total_count": len(cases),
@@ -816,9 +820,7 @@ def _fixture_obstacle(
last_centroid_xyz_m=None if state is TemporalState.EXPIRED else last.centroid_xyz_m,
motion=motion if state is TemporalState.CURRENT else MotionState.UNKNOWN,
motion_confidence=(
0.0
if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN
else 1.0
0.0 if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN else 1.0
),
motion_reason=(
"stale-support"
@@ -912,6 +914,7 @@ def _metrics(
elapsed_ns: int,
visual_count: int,
fixtures: dict[str, object],
body_frame: dict[str, object],
) -> dict[str, object]:
values = np.asarray(latencies_ms, dtype=np.float64)
return {
@@ -920,6 +923,7 @@ def _metrics(
"decisions": dict(sorted(assessment_counts.items())),
"motion_decisions": dict(sorted(motion_decisions.items())),
"reason_counts": dict(sorted(reason_counts.items())),
"body_frame": body_frame,
"visual_evidence": {
"frame_count": visual_count,
"point_limit_per_frame": VISUAL_POINT_LIMIT,
@@ -928,14 +932,14 @@ def _metrics(
"point_cloud_available": True,
"metric_distance_available": True,
"virtual_corridor_available": True,
"qualified_base_footprint_available": True,
"geometry_regression_sequences": list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES),
},
"fixtures": {
"passed": fixtures["passed_count"],
"total": fixtures["total_count"],
"critical": fixtures["critical_case_count"],
"critical_false_not_threat": fixtures[
"critical_false_not_threat_count"
],
"critical_false_not_threat": fixtures["critical_false_not_threat_count"],
},
"runtime": {
"elapsed_ns": elapsed_ns,
@@ -955,12 +959,9 @@ def _requirements(
evidence = _object(metrics.get("evidence"), "evidence metrics")
decisions = _object(metrics.get("decisions"), "decision metrics")
visual = _object(metrics.get("visual_evidence"), "visual metrics")
total_evidence = sum(
_integer(value, "evidence count") for value in evidence.values()
)
total_decisions = sum(
_integer(value, "decision count") for value in decisions.values()
)
body_frame = _object(metrics.get("body_frame"), "body frame metrics")
total_evidence = sum(_integer(value, "evidence count") for value in evidence.values())
total_decisions = sum(_integer(value, "decision count") for value in decisions.values())
cases = _array(fixtures.get("cases"), "fixture cases")
camera_case = next(
(
@@ -984,8 +985,7 @@ def _requirements(
),
"camera_only_is_unknown_never_safe": camera_case.get("actual") == "unknown",
"held_and_stale_are_unknown_never_safe": (
len(stale_cases) == 2
and all(item.get("actual") == "unknown" for item in stale_cases)
len(stale_cases) == 2 and all(item.get("actual") == "unknown" for item in stale_cases)
),
"geometry_only_evidence_is_assessed": (
_integer(
@@ -1012,8 +1012,29 @@ def _requirements(
"point_cloud_available",
"metric_distance_available",
"virtual_corridor_available",
"qualified_base_footprint_available",
)
)
and visual.get("geometry_regression_sequences")
== list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES)
),
"body_frame_is_grounded_gravity_stable_and_route_aligned": (
body_frame.get("available")
== _integer(body_frame.get("qualified"), "qualified body frames")
+ _integer(body_frame.get("rejected"), "rejected body frames")
and _integer(body_frame.get("qualified"), "qualified body frames")
>= math.ceil(_integer(body_frame.get("available"), "available body frames") * 0.95)
and body_frame.get("origin") == "local-surface-vertical-projection"
and body_frame.get("up") == "vendor-slam-map-gravity-axis"
and body_frame.get("forward") == "smoothed-slam-trajectory-validated-by-camera-axis"
and _number_value(
_object(
body_frame.get("camera_forward_alignment_deg"),
"body alignment metrics",
).get("maximum"),
"maximum body alignment",
)
<= 25.0
),
"physical_collision_and_actuation_authority_remain_false": (
fixtures.get("authority") == _false_authority()
@@ -1062,6 +1083,23 @@ def _visual_sequences(available: tuple[int, ...]) -> frozenset[int]:
available[round(index * (len(available) - 1) / (VISUAL_FRAME_COUNT - 1))]
for index in range(VISUAL_FRAME_COUNT)
}
available_set = frozenset(available)
for anchor in VISUAL_GEOMETRY_REGRESSION_SEQUENCES:
if anchor not in available_set:
raise ThreatReplayError("geometry regression frame is not qualified")
if anchor in selected:
continue
replaceable = selected.difference(
{
available[0],
available[-1],
*VISUAL_GEOMETRY_REGRESSION_SEQUENCES,
}
)
if not replaceable:
raise ThreatReplayError("visual regression sample cannot be inserted")
selected.remove(min(replaceable, key=lambda value: abs(value - anchor)))
selected.add(anchor)
if len(selected) != VISUAL_FRAME_COUNT:
raise ThreatReplayError("visual sample selection is not unique")
return frozenset(selected)
@@ -1189,6 +1227,12 @@ def _integer(value: object, label: str) -> int:
return value
def _number_value(value: object, label: str) -> float:
if not isinstance(value, int | float) or isinstance(value, bool) or not math.isfinite(value):
raise ThreatReplayError(f"{label} is not finite")
return float(value)
def _signed_integer(value: object, label: str) -> int:
if not isinstance(value, int) or isinstance(value, bool):
raise ThreatReplayError(f"{label} must be an integer")