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

fix(perception): reconstruct rolling occupancy from K1 increments

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-05 21:12:47 +03:00
parent c70ad345ea
commit 7aa3ce55c0
16 changed files with 1489 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/perception/threat_replay.py
+329 -22
View File
@@ -53,6 +53,11 @@ THREAT_REPLAY_FRAME_SCHEMA: Final = "missioncore.perception-threat-replay-frame/
THREAT_REPLAY_VISUAL_SCHEMA: Final = "missioncore.perception-threat-visual-frame/v1"
THREAT_REPLAY_FIXTURE_SCHEMA: Final = "missioncore.perception-threat-fixtures/v1"
THREAT_REPLAY_REPORT_SCHEMA: Final = "missioncore.perception-threat-replay-report/v1"
THREAT_REPLAY_SCHEMA_V2: Final = "missioncore.perception-threat-replay-result/v2"
THREAT_REPLAY_FRAME_SCHEMA_V2: Final = "missioncore.perception-threat-replay-frame/v2"
THREAT_REPLAY_VISUAL_SCHEMA_V2: Final = "missioncore.perception-threat-visual-frame/v2"
THREAT_REPLAY_FIXTURE_SCHEMA_V2: Final = "missioncore.perception-threat-fixtures/v2"
THREAT_REPLAY_REPORT_SCHEMA_V2: Final = "missioncore.perception-threat-replay-report/v2"
THREAT_REPLAY_RESULT_PREFIX: Final = "m4-threat-replay-"
THREAT_REPLAY_FRAMES_NAME: Final = "frames.jsonl"
THREAT_REPLAY_VISUALS_NAME: Final = "visual-frames.jsonl"
@@ -61,7 +66,23 @@ 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)
VISUAL_GEOMETRY_REGRESSION_SEQUENCES: Final = (138, 274, 1880)
FRAME_1880_ENGINEERING_ANCHORS: Final = (
{
"anchor_id": "near-concrete-hemisphere",
"x_bounds_m": (0.3, 1.2),
"y_bounds_m": (-0.8, 0.2),
"z_bounds_m": (-0.1, 0.9),
"must_assert_threat": True,
},
{
"anchor_id": "far-concrete-hemisphere",
"x_bounds_m": (1.5, 2.7),
"y_bounds_m": (0.6, 1.7),
"z_bounds_m": (-0.1, 0.9),
"must_assert_threat": False,
},
)
class ThreatReplayError(RuntimeError):
@@ -121,6 +142,7 @@ def build_threat_replay(
reason_counts: Counter[str] = Counter()
latencies_ms: list[float] = []
visual_count = 0
frame_1880_regression: dict[str, object] | None = None
try:
temporal_frames_path = temporal.result_root / "frames.jsonl"
geometry_frames_path = geometry.result_root / "frames.jsonl"
@@ -159,6 +181,20 @@ def build_threat_replay(
for key in ("held", "expired")
for value in _array(temporal_frame.get(key), f"{key} obstacles")
)
rolling_retained = tuple(
TemporalObstacle.from_dict(value)
for value in _array(
temporal_frame.get("rolling_retained"),
"rolling retained obstacles",
)
)
if any(
item.state is not TemporalState.RETAINED
for item in rolling_retained
):
raise ThreatReplayError(
"temporal replay rolling map escaped retained state"
)
geometry_observations = _array(
geometry_frame.get("observations"), "geometry observations"
)
@@ -183,7 +219,7 @@ def build_threat_replay(
graph_id="reference-perception-graph/v1",
generated_monotonic_ns=0,
output_age_ns=0,
occupied=current,
occupied=(*current, *rolling_retained),
unknown=unknown,
camera_uncertainty=camera_uncertainty,
accounting=SourceAccounting(1, 1, 0, 0),
@@ -192,7 +228,10 @@ def build_threat_replay(
assessments = provider.assess(obstacle_map)
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)} | {
expected_ids = {
item.component_id
for item in (*current, *rolling_retained, *unknown)
} | {
item.proposal_id for item in camera_uncertainty
}
if set(by_id) != expected_ids:
@@ -203,12 +242,21 @@ 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, *rolling_retained, *unknown)
]
if frame_count == 1880:
frame_1880_regression = _frame_1880_regression(
metric_rows,
body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
),
)
for item in assessments:
assessment_counts[item.decision.value] += 1
reason_counts.update(item.reason_codes)
evidence_counts["current-metric"] += len(current)
evidence_counts["rolling-map-retained"] += len(rolling_retained)
evidence_counts["stale-or-held"] += len(unknown)
evidence_counts["camera-only"] += len(camera_uncertainty)
for obstacle in current:
@@ -216,7 +264,7 @@ def build_threat_replay(
f"{obstacle.motion.value}:{by_id[obstacle.component_id].decision.value}"
] += 1
frame_document = {
"schema_version": THREAT_REPLAY_FRAME_SCHEMA,
"schema_version": THREAT_REPLAY_FRAME_SCHEMA_V2,
"sequence": frame_count,
"frame_id": packet.envelope.frame_id,
"source_time_ns": packet.envelope.timestamps.source_ns,
@@ -232,6 +280,8 @@ def build_threat_replay(
"metric_obstacles": len(metric_rows),
"camera_proposals": len(proposals),
"camera_only": len(camera_uncertainty),
"current_increment_metric": len(current),
"rolling_map_retained": len(rolling_retained),
"assessments": len(assessments),
},
"authority": _false_authority(),
@@ -276,14 +326,15 @@ def build_threat_replay(
visual_count=visual_count,
fixtures=fixtures,
body_frame=body_frame_resolver.qualification_summary(),
frame_1880_regression=frame_1880_regression,
)
requirements = _requirements(metrics, fixtures)
requirements = _requirements_v2(metrics, fixtures)
accepted = all(value is True for value in requirements.values())
frames_sha256 = _file_sha256(frames_path)
visuals_sha256 = _file_sha256(visuals_path)
fixtures_sha256 = _file_sha256(fixtures_path)
identity = {
"schema_version": THREAT_REPLAY_SCHEMA,
"schema_version": THREAT_REPLAY_SCHEMA_V2,
"profile_id": profile.profile_id,
"profile_sha256": profile.profile_sha256,
"provider_id": provider.provider_id,
@@ -319,7 +370,7 @@ def build_threat_replay(
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
result_id = f"{THREAT_REPLAY_RESULT_PREFIX}{identity_sha256}"
report = {
"schema_version": THREAT_REPLAY_REPORT_SCHEMA,
"schema_version": THREAT_REPLAY_REPORT_SCHEMA_V2,
"result_id": result_id,
"identity_sha256": identity_sha256,
"status": "accepted" if accepted else "rejected",
@@ -355,7 +406,7 @@ def build_threat_replay(
report_path = staging / THREAT_REPLAY_REPORT_NAME
_write_json(report_path, report)
manifest = {
"schema_version": THREAT_REPLAY_SCHEMA,
"schema_version": THREAT_REPLAY_SCHEMA_V2,
"result_id": result_id,
"identity_sha256": identity_sha256,
"identity": identity,
@@ -400,11 +451,14 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
},
"threat replay manifest",
)
schema_version = manifest.get("schema_version")
if schema_version not in {THREAT_REPLAY_SCHEMA, THREAT_REPLAY_SCHEMA_V2}:
raise ThreatReplayError("threat replay schema is incompatible")
is_v2 = schema_version == THREAT_REPLAY_SCHEMA_V2
identity = _object(manifest.get("identity"), "threat replay identity")
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
if (
manifest.get("schema_version") != THREAT_REPLAY_SCHEMA
or manifest.get("result_id") != resolved.name
manifest.get("result_id") != resolved.name
or manifest.get("identity_sha256") != identity_sha256
or resolved.name != f"{THREAT_REPLAY_RESULT_PREFIX}{identity_sha256}"
):
@@ -433,8 +487,14 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
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())
expected_requirements = (
_requirements_v2(metrics, fixtures)
if is_v2
else _requirements_v1(metrics, fixtures)
)
if (
report.get("schema_version") != THREAT_REPLAY_REPORT_SCHEMA
report.get("schema_version")
!= (THREAT_REPLAY_REPORT_SCHEMA_V2 if is_v2 else THREAT_REPLAY_REPORT_SCHEMA)
or report.get("result_id") != resolved.name
or report.get("identity_sha256") != identity_sha256
or report.get("metrics") != metrics
@@ -443,13 +503,14 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
or identity.get("authority") != _false_authority()
or manifest.get("accepted") is not accepted
or identity.get("accepted") is not accepted
or requirements != _requirements(metrics, fixtures)
or requirements != expected_requirements
):
raise ThreatReplayError("threat replay report or acceptance changed")
_validate_ledgers(
paths["threat-replay-frames"],
paths["threat-visual-frames"],
metrics,
is_v2=is_v2,
)
return ThreatReplayResult(
result_id=resolved.name,
@@ -617,13 +678,18 @@ def _visual_frame(
}
)
return {
"schema_version": THREAT_REPLAY_VISUAL_SCHEMA,
"schema_version": THREAT_REPLAY_VISUAL_SCHEMA_V2,
"sequence": packet.envelope.sequence,
"frame_id": packet.envelope.frame_id,
"source_time_ns": packet.envelope.timestamps.source_ns,
"point_cloud_body_xyz_m": np.round(sampled, 6).tolist(),
"point_cloud_source_count": int(points.shape[0]),
"point_cloud_sample_count": int(sampled.shape[0]),
"point_cloud_layer": "current-increment",
"rolling_map_component_count": sum(
row.get("state") == TemporalState.RETAINED.value
for row in metric_rows
),
"metric_obstacles": metric_visuals,
"camera_proposals": camera_rows,
"body_frame": {
@@ -649,6 +715,99 @@ def _visual_frame(
}
def _frame_1880_regression(
metric_rows: list[dict[str, object]],
body_frame: ReplayBodyFrame | None,
) -> dict[str, object]:
if body_frame is None:
raise ThreatReplayError("frame 1880 has no qualified body frame")
retained_threats = 0
retained_components = 0
retained_rows: list[tuple[dict[str, object], tuple[float, float, float]]] = []
for row in metric_rows:
if row.get("state") != TemporalState.RETAINED.value:
continue
retained_components += 1
assessment = _object(row.get("assessment"), "frame 1880 assessment")
if assessment.get("decision") == ThreatDecision.THREAT.value:
retained_threats += 1
centroid_map = _array(
row.get("centroid_map_xyz_m"),
"frame 1880 retained centroid",
)
if len(centroid_map) != 3:
raise ThreatReplayError("frame 1880 retained centroid is invalid")
centroid_values = tuple(
_number_value(value, "frame 1880 centroid") for value in centroid_map
)
centroid_body = body_frame.map_point_to_body(
(centroid_values[0], centroid_values[1], centroid_values[2])
)
retained_rows.append((row, centroid_body))
anchors: list[dict[str, object]] = []
matched_ids: set[str] = set()
for raw_anchor in FRAME_1880_ENGINEERING_ANCHORS:
anchor = _object(raw_anchor, "frame 1880 engineering anchor")
x_bounds = _bounds(anchor.get("x_bounds_m"), "frame 1880 x bounds")
y_bounds = _bounds(anchor.get("y_bounds_m"), "frame 1880 y bounds")
z_bounds = _bounds(anchor.get("z_bounds_m"), "frame 1880 z bounds")
match = next(
(
(row, centroid)
for row, centroid in retained_rows
if row.get("component_id") not in matched_ids
and x_bounds[0] <= centroid[0] <= x_bounds[1]
and y_bounds[0] <= centroid[1] <= y_bounds[1]
and z_bounds[0] <= centroid[2] <= z_bounds[1]
),
None,
)
component_id = None if match is None else str(match[0]["component_id"])
if component_id is not None:
matched_ids.add(component_id)
anchor_assessment = (
None
if match is None
else _object(match[0].get("assessment"), "frame 1880 anchor assessment")
)
anchors.append(
{
"anchor_id": anchor["anchor_id"],
"bounds_body_xyz_m": [list(x_bounds), list(y_bounds), list(z_bounds)],
"must_assert_threat": anchor["must_assert_threat"],
"matched": match is not None,
"component_id": component_id,
"centroid_body_xyz_m": (
None if match is None else list(match[1])
),
"decision": (
None
if anchor_assessment is None
else anchor_assessment.get("decision")
),
}
)
required_threats_passed = all(
item["matched"] is True
and (
item["must_assert_threat"] is False
or item["decision"] == ThreatDecision.THREAT.value
)
for item in anchors
)
return {
"sequence": 1880,
"retained_components": retained_components,
"retained_threat_components": retained_threats,
"engineering_anchors": anchors,
"matched_anchor_count": sum(item["matched"] is True for item in anchors),
"required_threats_passed": required_threats_passed,
"camera_visible_hemispheres_independent_truth": False,
"gate": "two-visible-hemisphere-regression",
}
class _FixtureBodyFrames:
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
return ReplayBodyFrame(
@@ -766,6 +925,19 @@ def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
),
ThreatDecision.UNKNOWN,
),
_fixture_case(
provider,
"retained-in-corridor",
_fixture_obstacle(
"fixture-retained-in",
GridCell(6, 0, 0),
MotionState.UNKNOWN,
((frame_id, 200_000_000, (2.925, 0.225, 0.225)),),
state=TemporalState.RETAINED,
),
ThreatDecision.THREAT,
critical=True,
),
_fixture_camera_case(provider, frame_id),
_fixture_case(
provider,
@@ -784,7 +956,7 @@ def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
),
]
return {
"schema_version": THREAT_REPLAY_FIXTURE_SCHEMA,
"schema_version": THREAT_REPLAY_FIXTURE_SCHEMA_V2,
"cases": cases,
"critical_case_count": sum(item["critical"] is True for item in cases),
"critical_false_not_threat_count": sum(
@@ -810,7 +982,11 @@ def _fixture_obstacle(
component_id=component_id,
identity_scope="ephemeral",
state=state,
ttl_ns=750_000_000,
ttl_ns=(
3_000_000_000
if state is TemporalState.RETAINED
else 750_000_000
),
last_hit_ns=last.evidence_time_ns,
age_ns=0 if state is TemporalState.CURRENT else 100_000_000,
association_basis="deterministic-fixture",
@@ -849,8 +1025,12 @@ def _fixture_case(
graph_id="reference-perception-graph/v1",
generated_monotonic_ns=0,
output_age_ns=0,
occupied=(obstacle,) if obstacle.state is TemporalState.CURRENT else (),
unknown=(obstacle,) if obstacle.state is not TemporalState.CURRENT else (),
occupied=(obstacle,)
if obstacle.state in {TemporalState.CURRENT, TemporalState.RETAINED}
else (),
unknown=(obstacle,)
if obstacle.state not in {TemporalState.CURRENT, TemporalState.RETAINED}
else (),
camera_uncertainty=(),
accounting=SourceAccounting(1, 1, 0, 0),
)
@@ -915,6 +1095,7 @@ def _metrics(
visual_count: int,
fixtures: dict[str, object],
body_frame: dict[str, object],
frame_1880_regression: dict[str, object] | None,
) -> dict[str, object]:
values = np.asarray(latencies_ms, dtype=np.float64)
return {
@@ -934,6 +1115,7 @@ def _metrics(
"virtual_corridor_available": True,
"qualified_base_footprint_available": True,
"geometry_regression_sequences": list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES),
"frame_1880_regression": frame_1880_regression,
},
"fixtures": {
"passed": fixtures["passed_count"],
@@ -951,7 +1133,102 @@ def _metrics(
}
def _requirements(
def _requirements_v1(
metrics: dict[str, object],
fixtures: dict[str, object],
) -> dict[str, bool]:
frames = _object(metrics.get("frames"), "frame metrics")
evidence = _object(metrics.get("evidence"), "evidence metrics")
decisions = _object(metrics.get("decisions"), "decision metrics")
visual = _object(metrics.get("visual_evidence"), "visual metrics")
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(
(
_object(item, "fixture")
for item in cases
if isinstance(item, dict) and item.get("name") == "camera-only"
),
{},
)
stale_cases = [
_object(item, "fixture")
for item in cases
if isinstance(item, dict)
and item.get("name") in {"occluded-held", "stale-expired"}
]
return {
"full_ravnoves00_replay_completed": (
frames.get("total") == 4489 and frames.get("failed") == 0
),
"every_metric_or_camera_evidence_received_one_assessment": (
total_evidence == total_decisions and total_evidence > 0
),
"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)
),
"geometry_only_evidence_is_assessed": (
_integer(
_object(metrics.get("reason_counts"), "reason metrics").get(
"geometry-only-evidence", 0
),
"geometry-only count",
)
> 0
),
"deterministic_fixture_matrix_passed": (
fixtures.get("passed_count") == fixtures.get("total_count") == 9
),
"zero_critical_fixture_false_not_threat": (
fixtures.get("critical_false_not_threat_count") == 0
),
"visual_video_camera_cloud_distance_and_corridor_are_available": (
visual.get("frame_count") == VISUAL_FRAME_COUNT
and all(
visual.get(key) is True
for key in (
"video_overlay_available",
"camera_boxes_available",
"point_cloud_available",
"metric_distance_available",
"virtual_corridor_available",
"qualified_base_footprint_available",
)
)
and visual.get("geometry_regression_sequences") == [138, 274]
),
"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()
),
}
def _requirements_v2(
metrics: dict[str, object],
fixtures: dict[str, object],
) -> dict[str, bool]:
@@ -997,7 +1274,7 @@ def _requirements(
> 0
),
"deterministic_fixture_matrix_passed": (
fixtures.get("passed_count") == fixtures.get("total_count") == 9
fixtures.get("passed_count") == fixtures.get("total_count") == 10
),
"zero_critical_fixture_false_not_threat": (
fixtures.get("critical_false_not_threat_count") == 0
@@ -1018,6 +1295,19 @@ def _requirements(
and visual.get("geometry_regression_sequences")
== list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES)
),
"frame_1880_retains_two_hemispheres_and_blocks_near_corridor": (
isinstance(visual.get("frame_1880_regression"), dict)
and _object(
visual.get("frame_1880_regression"),
"frame 1880 regression",
).get("matched_anchor_count")
== len(FRAME_1880_ENGINEERING_ANCHORS)
and _object(
visual.get("frame_1880_regression"),
"frame 1880 regression",
).get("required_threats_passed")
is True
),
"body_frame_is_grounded_gravity_stable_and_route_aligned": (
body_frame.get("available")
== _integer(body_frame.get("qualified"), "qualified body frames")
@@ -1046,12 +1336,15 @@ def _validate_ledgers(
frames_path: Path,
visuals_path: Path,
metrics: dict[str, object],
*,
is_v2: bool,
) -> None:
frame_count = 0
assessment_count = 0
for sequence, frame in enumerate(_read_jsonl(frames_path)):
if (
frame.get("schema_version") != THREAT_REPLAY_FRAME_SCHEMA
frame.get("schema_version")
!= (THREAT_REPLAY_FRAME_SCHEMA_V2 if is_v2 else THREAT_REPLAY_FRAME_SCHEMA)
or frame.get("sequence") != sequence
or frame.get("authority") != _false_authority()
):
@@ -1071,7 +1364,11 @@ def _validate_ledgers(
for value in _object(metrics.get("decisions"), "decisions").values()
)
or len(visuals) != VISUAL_FRAME_COUNT
or any(item.get("schema_version") != THREAT_REPLAY_VISUAL_SCHEMA for item in visuals)
or any(
item.get("schema_version")
!= (THREAT_REPLAY_VISUAL_SCHEMA_V2 if is_v2 else THREAT_REPLAY_VISUAL_SCHEMA)
for item in visuals
)
):
raise ThreatReplayError("threat replay ledger and metrics disagree")
@@ -1233,6 +1530,16 @@ def _number_value(value: object, label: str) -> float:
return float(value)
def _bounds(value: object, label: str) -> tuple[float, float]:
if not isinstance(value, tuple) or len(value) != 2:
raise ThreatReplayError(f"{label} must contain two values")
lower = _number_value(value[0], label)
upper = _number_value(value[1], label)
if lower >= upper:
raise ThreatReplayError(f"{label} must be ordered")
return lower, upper
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")