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

fix(m4): retain compact obstacle evidence

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-05 22:16:33 +03:00
parent 13ed096f80
commit aacc6dc43b
24 changed files with 981 additions and 410 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/perception/threat_replay.py
+149 -45
View File
@@ -66,7 +66,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, 1880)
VISUAL_GEOMETRY_REGRESSION_SEQUENCES: Final = (138, 274, 1880, 2584)
FRAME_1880_ENGINEERING_ANCHORS: Final = (
{
"anchor_id": "near-concrete-hemisphere",
@@ -83,6 +83,22 @@ FRAME_1880_ENGINEERING_ANCHORS: Final = (
"must_assert_threat": False,
},
)
FRAME_2584_ENGINEERING_ANCHORS: Final = (
{
"anchor_id": "near-compact-concrete-hemisphere",
"x_bounds_m": (1.5, 2.4),
"y_bounds_m": (-0.7, 0.2),
"z_bounds_m": (-0.1, 0.9),
"must_assert_threat": True,
},
{
"anchor_id": "far-concrete-hemisphere-occupancy",
"x_bounds_m": (2.8, 4.0),
"y_bounds_m": (1.2, 2.4),
"z_bounds_m": (-0.1, 1.0),
"must_assert_threat": False,
},
)
class ThreatReplayError(RuntimeError):
@@ -143,6 +159,7 @@ def build_threat_replay(
latencies_ms: list[float] = []
visual_count = 0
frame_1880_regression: dict[str, object] | None = None
frame_2584_regression: dict[str, object] | None = None
try:
temporal_frames_path = temporal.result_root / "frames.jsonl"
geometry_frames_path = geometry.result_root / "frames.jsonl"
@@ -188,13 +205,8 @@ def build_threat_replay(
"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"
)
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"
)
@@ -229,11 +241,8 @@ def build_threat_replay(
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, *rolling_retained, *unknown)
} | {
item.proposal_id for item in camera_uncertainty
}
item.component_id for item in (*current, *rolling_retained, *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(
@@ -248,9 +257,13 @@ def build_threat_replay(
if frame_count == 1880:
frame_1880_regression = _frame_1880_regression(
metric_rows,
body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
),
body_frame_resolver.body_frame_for_frame(packet.envelope.frame_id),
)
if frame_count == 2584:
frame_2584_regression = _frame_2584_regression(
metric_rows,
body_frame_resolver.body_frame_for_frame(packet.envelope.frame_id),
voxel_size_m=profile.corridor.occupied_voxel_size_m,
)
for item in assessments:
assessment_counts[item.decision.value] += 1
@@ -327,6 +340,7 @@ def build_threat_replay(
fixtures=fixtures,
body_frame=body_frame_resolver.qualification_summary(),
frame_1880_regression=frame_1880_regression,
frame_2584_regression=frame_2584_regression,
)
requirements = _requirements_v2(metrics, fixtures)
accepted = all(value is True for value in requirements.values())
@@ -488,9 +502,7 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
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)
_requirements_v2(metrics, fixtures) if is_v2 else _requirements_v1(metrics, fixtures)
)
if (
report.get("schema_version")
@@ -687,8 +699,7 @@ def _visual_frame(
"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
row.get("state") == TemporalState.RETAINED.value for row in metric_rows
),
"metric_obstacles": metric_visuals,
"camera_proposals": camera_rows,
@@ -778,22 +789,15 @@ def _frame_1880_regression(
"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])
),
"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")
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
)
and (item["must_assert_threat"] is False or item["decision"] == ThreatDecision.THREAT.value)
for item in anchors
)
return {
@@ -808,6 +812,100 @@ def _frame_1880_regression(
}
def _frame_2584_regression(
metric_rows: list[dict[str, object]],
body_frame: ReplayBodyFrame | None,
*,
voxel_size_m: float,
) -> dict[str, object]:
"""Bind both visible hemispheres to produced occupancy without injecting it."""
if body_frame is None:
raise ThreatReplayError("frame 2584 has no qualified body frame")
candidates: list[
tuple[dict[str, object], tuple[float, float, float], tuple[float, float, float]]
] = []
for row in metric_rows:
assessment = _object(row.get("assessment"), "frame 2584 assessment")
centroid_map = _array(row.get("centroid_map_xyz_m"), "frame 2584 centroid")
if len(centroid_map) != 3:
raise ThreatReplayError("frame 2584 centroid is invalid")
centroid_body = body_frame.map_point_to_body(
tuple(_number_value(value, "frame 2584 centroid") for value in centroid_map)
)
for raw_cell in _array(row.get("cells"), "frame 2584 cells"):
cell = _object(raw_cell, "frame 2584 cell")
point_map = tuple(
(_signed_integer(cell.get(key), f"frame 2584 cell {key}") + 0.5) * voxel_size_m
for key in ("x", "y", "z")
)
candidates.append(
(
row,
centroid_body,
body_frame.map_point_to_body(point_map),
)
)
if not row.get("cells"):
raise ThreatReplayError("frame 2584 metric component has no occupied cells")
if assessment.get("component_id") != row.get("component_id"):
raise ThreatReplayError("frame 2584 assessment identity changed")
anchors: list[dict[str, object]] = []
matched_ids: set[str] = set()
for raw_anchor in FRAME_2584_ENGINEERING_ANCHORS:
anchor = _object(raw_anchor, "frame 2584 engineering anchor")
x_bounds = _bounds(anchor.get("x_bounds_m"), "frame 2584 x bounds")
y_bounds = _bounds(anchor.get("y_bounds_m"), "frame 2584 y bounds")
z_bounds = _bounds(anchor.get("z_bounds_m"), "frame 2584 z bounds")
match = next(
(
(row, centroid, cell)
for row, centroid, cell in candidates
if row.get("component_id") not in matched_ids
and x_bounds[0] <= cell[0] <= x_bounds[1]
and y_bounds[0] <= cell[1] <= y_bounds[1]
and z_bounds[0] <= cell[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)
assessment = (
None
if match is None
else _object(match[0].get("assessment"), "frame 2584 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,
"component_state": None if match is None else match[0].get("state"),
"centroid_body_xyz_m": None if match is None else list(match[1]),
"matched_cell_body_xyz_m": None if match is None else list(match[2]),
"decision": None if assessment is None else 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": 2584,
"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,
"matching_basis": "produced-occupied-cell-inside-camera-reviewed-body-window",
"gate": "compact-and-merged-hemisphere-occupancy-regression",
}
class _FixtureBodyFrames:
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
return ReplayBodyFrame(
@@ -982,11 +1080,7 @@ def _fixture_obstacle(
component_id=component_id,
identity_scope="ephemeral",
state=state,
ttl_ns=(
3_000_000_000
if state is TemporalState.RETAINED
else 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",
@@ -1096,6 +1190,7 @@ def _metrics(
fixtures: dict[str, object],
body_frame: dict[str, object],
frame_1880_regression: dict[str, object] | None,
frame_2584_regression: dict[str, object] | None,
) -> dict[str, object]:
values = np.asarray(latencies_ms, dtype=np.float64)
return {
@@ -1116,6 +1211,7 @@ def _metrics(
"qualified_base_footprint_available": True,
"geometry_regression_sequences": list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES),
"frame_1880_regression": frame_1880_regression,
"frame_2584_regression": frame_2584_regression,
},
"fixtures": {
"passed": fixtures["passed_count"],
@@ -1156,8 +1252,7 @@ def _requirements_v1(
stale_cases = [
_object(item, "fixture")
for item in cases
if isinstance(item, dict)
and item.get("name") in {"occluded-held", "stale-expired"}
if isinstance(item, dict) and item.get("name") in {"occluded-held", "stale-expired"}
]
return {
"full_ravnoves00_replay_completed": (
@@ -1168,8 +1263,7 @@ def _requirements_v1(
),
"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(
@@ -1206,13 +1300,10 @@ def _requirements_v1(
== _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
)
>= 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 body_frame.get("forward") == "smoothed-slam-trajectory-validated-by-camera-axis"
and _number_value(
_object(
body_frame.get("camera_forward_alignment_deg"),
@@ -1308,6 +1399,19 @@ def _requirements_v2(
).get("required_threats_passed")
is True
),
"frame_2584_retains_compact_hemisphere_and_accounts_for_far_occupancy": (
isinstance(visual.get("frame_2584_regression"), dict)
and _object(
visual.get("frame_2584_regression"),
"frame 2584 regression",
).get("matched_anchor_count")
== len(FRAME_2584_ENGINEERING_ANCHORS)
and _object(
visual.get("frame_2584_regression"),
"frame 2584 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")