feat(perception): add persistent lidar motion evidence

2026-07-24 12:49:11 +03:00
parent e6dd8c2cf1
commit 230cba4b21
10 changed files with 2496 additions and 4 deletions
@@ -0,0 +1,302 @@
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+
+from k1link.compute.occupancy_motion import (
+    PersistentSupportTracker,
+    SupportMeasurement,
+    _apply_occupancy_evidence,
+    evaluate_persistent_support_benchmark,
+    read_persistent_support_benchmark,
+    read_persistent_support_profile,
+)
+
+
+def _profile() -> dict[str, object]:
+    root = Path(__file__).resolve().parents[1]
+    profile, digest = read_persistent_support_profile(
+        root / "experiments" / "perception" / "e25_persistent_support_profile.json"
+    )
+    assert len(digest) == 64
+    return profile
+
+
+def _support(x: float, y: float = 2.0) -> np.ndarray:
+    return np.asarray(
+        [
+            [x - 0.2, y - 0.2, 0.4],
+            [x + 0.2, y - 0.2, 0.5],
+            [x - 0.2, y + 0.2, 0.6],
+            [x + 0.2, y + 0.2, 0.7],
+        ],
+        dtype=np.float64,
+    )
+
+
+def test_e25_profile_is_bounded_and_has_no_control_authority() -> None:
+    profile = _profile()
+
+    assert profile["source"]["coordinate_frame"] == "k1-map"
+    assert profile["bounds"]["maximum_tracks"] == 192
+    assert profile["occupancy"]["maximum_snapshots_per_track"] == 32
+    assert profile["authority"] == {
+        "commands_enabled": False,
+        "navigation_or_safety_accepted": False,
+    }
+
+
+def test_e25_static_support_survives_visible_surface_center_jitter() -> None:
+    tracker = PersistentSupportTracker(_profile())
+    evidence = {}
+    for frame in range(30):
+        jitter = 0.18 if frame % 2 else -0.18
+        evidence = tracker.observe(
+            track_id=250001,
+            source_track_id=10,
+            group="vehicle",
+            session_seconds=frame * 0.1,
+            points_map=_support(10.0 + jitter),
+        )
+
+    assert evidence["motion_state"] == "static"
+    assert evidence["occupancy_overlap_fraction"] is not None
+    assert evidence["occupancy_overlap_fraction"] >= 0.55
+
+
+def test_e25_coherent_translation_becomes_dynamic() -> None:
+    tracker = PersistentSupportTracker(_profile())
+    evidence = {}
+    for frame in range(20):
+        evidence = tracker.observe(
+            track_id=250002,
+            source_track_id=20,
+            group="person",
+            session_seconds=frame * 0.1,
+            points_map=_support(4.0 + frame * 0.5),
+        )
+
+    assert evidence["motion_state"] == "dynamic"
+    assert evidence["speed_mps"] is not None
+    assert evidence["speed_mps"] > 1.0
+    assert evidence["occupancy_overlap_fraction"] <= 0.25
+
+
+def test_e25_does_not_claim_motion_without_older_support() -> None:
+    tracker = PersistentSupportTracker(_profile())
+
+    evidence = tracker.observe(
+        track_id=250003,
+        source_track_id=30,
+        group="vehicle",
+        session_seconds=0.0,
+        points_map=_support(8.0),
+    )
+
+    assert evidence["motion_state"] == "unknown"
+    assert evidence["motion_confidence"] == 0.0
+
+
+def test_e25_current_support_is_not_attached_to_an_e24_held_hypothesis() -> None:
+    tracker = PersistentSupportTracker(_profile())
+    measurement = SupportMeasurement(
+        source_track_id=40,
+        label="car",
+        group="vehicle",
+        score=0.9,
+        points_map=_support(10.0),
+        source_indices=np.asarray([1, 2, 3, 4], dtype=np.int64),
+    )
+    _apply_occupancy_evidence(
+        [
+            {
+                "track_id": 250040,
+                "source_track_id": 40,
+                "temporal_status": "e24-observed",
+            }
+        ],
+        {40: measurement},
+        tracker,
+        0.0,
+    )
+    result = _apply_occupancy_evidence(
+        [
+            {
+                "track_id": 250041,
+                "source_track_id": 40,
+                "temporal_status": "e24-observed",
+            },
+            {
+                "track_id": 250040,
+                "source_track_id": 40,
+                "temporal_status": "e24-held-prediction",
+            },
+        ],
+        {40: measurement},
+        tracker,
+        0.1,
+    )
+
+    assert result[0]["occupancy_evidence_current"] is True
+    assert result[1]["occupancy_evidence_current"] is False
+    assert tracker.snapshot()["support_observations"] == 2
+
+
+def test_e25_target_benchmark_cannot_be_satisfied_by_another_object() -> None:
+    event = {
+        "id": "target-five",
+        "kind": "target-motion",
+        "window_seconds": [10.0, 12.0],
+        "class_group": "vehicle",
+        "target_source_track_ids": [5],
+        "expected_motion": "dynamic",
+        "minimum_hits": 2,
+        "minimum_span_seconds": 0.1,
+        "minimum_coverage_fraction": 0.5,
+    }
+    source_rows = [
+        {
+            "session_seconds": 10.0 + index * 0.2,
+            "objects": [
+                {
+                    "track_id": 5,
+                    "association_group": "vehicle",
+                }
+            ],
+        }
+        for index in range(3)
+    ]
+    wrong_fusion_rows = [
+        {
+            "session_seconds": 10.0 + index * 0.2,
+            "objects": [
+                {
+                    "track_id": 250099,
+                    "source_track_id": 99,
+                    "association_group": "vehicle",
+                    "motion_state": "dynamic",
+                    "occupancy_evidence_current": True,
+                }
+            ],
+        }
+        for index in range(3)
+    ]
+
+    result = evaluate_persistent_support_benchmark(
+        source_rows,
+        wrong_fusion_rows,
+        {"events": [event]},
+    )
+
+    assert result["passed"] is False
+    assert result["events"][0]["evidence_observations"] == 0
+
+
+def test_e25_benchmark_deduplicates_world_hypotheses_for_one_source_object() -> None:
+    event = {
+        "id": "target-five",
+        "kind": "target-motion",
+        "window_seconds": [10.0, 12.0],
+        "class_group": "vehicle",
+        "target_source_track_ids": [5],
+        "expected_motion": "dynamic",
+        "minimum_hits": 2,
+        "minimum_span_seconds": 0.1,
+        "minimum_coverage_fraction": 0.5,
+    }
+    source_rows = [
+        {
+            "frame_index": index,
+            "session_seconds": 10.0 + index * 0.2,
+            "objects": [{"track_id": 5, "association_group": "vehicle"}],
+        }
+        for index in range(3)
+    ]
+    fusion_rows = [
+        {
+            "frame_index": index,
+            "session_seconds": 10.0 + index * 0.2,
+            "objects": [
+                {
+                    "track_id": world_track,
+                    "source_track_id": 5,
+                    "association_group": "vehicle",
+                    "motion_state": "dynamic",
+                    "motion_confidence": 0.8,
+                    "occupancy_cell_count": 4,
+                    "occupancy_support_observations": 6,
+                    "occupancy_evidence_current": True,
+                }
+                for world_track in (250005, 250006)
+            ],
+        }
+        for index in range(3)
+    ]
+
+    result = evaluate_persistent_support_benchmark(
+        source_rows,
+        fusion_rows,
+        {"events": [event]},
+    )
+
+    assert result["events"][0]["coverage_fraction"] == 1.0
+    assert result["events"][0]["evidence_observations"] == 3
+    assert result["events"][0]["duplicate_evidence_observations"] == 3
+
+
+def test_e25_static_control_rejects_excess_false_dynamic_evidence() -> None:
+    event = {
+        "id": "parked-vehicles",
+        "kind": "static-control",
+        "window_seconds": [20.0, 22.0],
+        "class_group": "vehicle",
+        "expected_motion": "static",
+        "minimum_source_observations": 3,
+        "minimum_coverage_fraction": 0.5,
+        "minimum_classified_fraction": 0.5,
+        "maximum_false_dynamic_fraction": 0.1,
+    }
+    source_rows = [
+        {
+            "session_seconds": 20.0 + index * 0.2,
+            "objects": [{"track_id": 7, "association_group": "vehicle"}],
+        }
+        for index in range(3)
+    ]
+    fusion_rows = [
+        {
+            "session_seconds": 20.0 + index * 0.2,
+            "objects": [
+                {
+                    "track_id": 250007,
+                    "source_track_id": 7,
+                    "association_group": "vehicle",
+                    "motion_state": "dynamic",
+                    "occupancy_evidence_current": True,
+                }
+            ],
+        }
+        for index in range(3)
+    ]
+
+    result = evaluate_persistent_support_benchmark(
+        source_rows,
+        fusion_rows,
+        {"events": [event]},
+    )
+
+    assert result["passed"] is False
+    assert result["events"][0]["false_dynamic_fraction"] == 1.0
+
+
+def test_e25_benchmark_contract_uses_reviewed_source_track_bindings() -> None:
+    root = Path(__file__).resolve().parents[1]
+    benchmark, digest = read_persistent_support_benchmark(
+        root / "experiments" / "perception" / "e25_motion_benchmark.json"
+    )
+    target_events = [event for event in benchmark["events"] if event["kind"] == "target-motion"]
+
+    assert len(digest) == 64
+    assert target_events
+    assert all(event["target_source_track_ids"] for event in target_events)