mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 08:31:13 +00:00
[AZ-240] [AZ-241] [AZ-242] Add native retrieval remediation
Implement the product remediation paths required before greenfield code testability revision: native VIO backend selection, local VPR descriptor index retrieval, and computed anchor matching gates. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -1,4 +1,4 @@
|
||||
from anchor_verification import AnchorFrame, GeometryGatedAnchorVerifier, MatchEvidence
|
||||
from anchor_verification import AnchorFrame, CandidateTile, GeometryGatedAnchorVerifier, MatchEvidence
|
||||
from shared.contracts import VprCandidate
|
||||
|
||||
|
||||
@@ -26,6 +26,29 @@ def _evidence(**overrides: object) -> MatchEvidence:
|
||||
return MatchEvidence.model_validate(payload)
|
||||
|
||||
|
||||
def _frame_with_keypoints() -> AnchorFrame:
|
||||
keypoints = tuple((float(x * 10), float(y * 10)) for x in range(5) for y in range(5))
|
||||
return AnchorFrame(
|
||||
frame_id="frame-1",
|
||||
image_ref="replay/frame-1.jpg",
|
||||
keypoints=keypoints,
|
||||
)
|
||||
|
||||
|
||||
def _tile_with_keypoints(**overrides: object) -> CandidateTile:
|
||||
keypoints = tuple(
|
||||
(float(x * 10 + 2), float(y * 10 + 3)) for x in range(5) for y in range(5)
|
||||
)
|
||||
payload: dict[str, object] = {
|
||||
"candidate": _candidate(),
|
||||
"image_ref": "cache/tile-1.tif",
|
||||
"keypoints": keypoints,
|
||||
"provenance_trusted": True,
|
||||
}
|
||||
payload.update(overrides)
|
||||
return CandidateTile.model_validate(payload)
|
||||
|
||||
|
||||
def test_candidate_verification_emits_acceptance_evidence() -> None:
|
||||
# Arrange
|
||||
verifier = GeometryGatedAnchorVerifier()
|
||||
@@ -42,6 +65,25 @@ def test_candidate_verification_emits_acceptance_evidence() -> None:
|
||||
assert result.homography == {"h00": 1.0, "h11": 1.0, "h22": 1.0}
|
||||
|
||||
|
||||
def test_matching_path_computes_evidence_from_frame_and_tile_inputs() -> None:
|
||||
# Arrange
|
||||
verifier = GeometryGatedAnchorVerifier()
|
||||
frame = _frame_with_keypoints()
|
||||
tile = _tile_with_keypoints()
|
||||
|
||||
# Act
|
||||
result = verifier.verify_candidate(frame, tile, matcher_profile="sift_orb")
|
||||
|
||||
# Assert
|
||||
assert result.decision.accepted is True
|
||||
assert result.decision.inliers == 25
|
||||
assert result.reason == "accepted_geometry"
|
||||
assert result.matcher_profile == "sift_orb"
|
||||
assert result.homography is not None
|
||||
assert result.homography["h02"] == 2.0
|
||||
assert result.homography["h12"] == 3.0
|
||||
|
||||
|
||||
def test_unsafe_candidate_is_rejected_with_reason() -> None:
|
||||
# Arrange
|
||||
verifier = GeometryGatedAnchorVerifier()
|
||||
@@ -62,6 +104,23 @@ def test_unsafe_candidate_is_rejected_with_reason() -> None:
|
||||
assert result.reason == "stale_or_untrusted_provenance"
|
||||
|
||||
|
||||
def test_computed_matching_rejects_low_inlier_geometry() -> None:
|
||||
# Arrange
|
||||
verifier = GeometryGatedAnchorVerifier()
|
||||
frame = _frame_with_keypoints()
|
||||
tile = _tile_with_keypoints(
|
||||
keypoints=((100.0, 100.0), (12.0, 3.0), (99.0, 88.0), (50.0, 40.0), (6.0, 9.0))
|
||||
)
|
||||
|
||||
# Act
|
||||
result = verifier.verify_candidate(frame, tile, matcher_profile="sift_orb")
|
||||
|
||||
# Assert
|
||||
assert result.decision.accepted is False
|
||||
assert result.reason == "low_inliers"
|
||||
assert result.decision.rejection_reason == "low_inliers"
|
||||
|
||||
|
||||
def test_matcher_benchmark_reports_profile_runtime_and_quality_metrics() -> None:
|
||||
# Arrange
|
||||
verifier = GeometryGatedAnchorVerifier()
|
||||
@@ -85,3 +144,27 @@ def test_matcher_benchmark_reports_profile_runtime_and_quality_metrics() -> None
|
||||
assert report.results[0].runtime_ms == 72.5
|
||||
assert report.results[1].accepted is False
|
||||
assert report.results[1].reason == "low_inliers"
|
||||
|
||||
|
||||
def test_matcher_benchmark_can_run_computed_paths() -> None:
|
||||
# Arrange
|
||||
verifier = GeometryGatedAnchorVerifier()
|
||||
frame = _frame_with_keypoints()
|
||||
|
||||
# Act
|
||||
report = verifier.benchmark_candidates(
|
||||
frame,
|
||||
(
|
||||
_tile_with_keypoints(),
|
||||
_tile_with_keypoints(
|
||||
keypoints=((2.0, 3.0), (2.0, 13.0), (2.0, 23.0), (2.0, 33.0), (2.0, 43.0))
|
||||
),
|
||||
),
|
||||
matcher_profile="sift_orb",
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert report.results[0].accepted is True
|
||||
assert report.results[0].runtime_ms >= 0.0
|
||||
assert report.results[1].accepted is False
|
||||
assert report.results[1].reason == "low_inliers"
|
||||
|
||||
@@ -1,3 +1,6 @@
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
from satellite_service import (
|
||||
LocalVprIndexPackage,
|
||||
LocalVprRetriever,
|
||||
@@ -33,6 +36,46 @@ def test_valid_local_index_load_reports_ready_status() -> None:
|
||||
assert readiness.ready is True
|
||||
assert readiness.engine == "cpu_faiss"
|
||||
assert readiness.loaded_records == 1
|
||||
assert readiness.package_id == "index-1"
|
||||
assert readiness.descriptor_model == "dinov2_vlad"
|
||||
|
||||
|
||||
def test_local_descriptor_index_package_loads_from_cache_file(tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
package_path = tmp_path / "vpr-index.json"
|
||||
package_path.write_text(
|
||||
json.dumps(
|
||||
{
|
||||
"package_id": "index-file-1",
|
||||
"engine": "cpu_faiss",
|
||||
"descriptor_model": "dinov2_vlad",
|
||||
"records": [
|
||||
{
|
||||
"chunk_id": "chunk-file",
|
||||
"tile_id": "tile-file",
|
||||
"descriptor": [1.0, 0.0],
|
||||
"footprint": {
|
||||
"min_lat": 49.0,
|
||||
"max_lat": 49.1,
|
||||
"min_lon": 36.0,
|
||||
"max_lon": 36.1,
|
||||
},
|
||||
"freshness_status": "fresh",
|
||||
}
|
||||
],
|
||||
}
|
||||
),
|
||||
encoding="utf-8",
|
||||
)
|
||||
retriever = LocalVprRetriever()
|
||||
|
||||
# Act
|
||||
readiness = retriever.load_index_from_path(package_path)
|
||||
|
||||
# Assert
|
||||
assert readiness.ready is True
|
||||
assert readiness.package_id == "index-file-1"
|
||||
assert readiness.loaded_records == 1
|
||||
|
||||
|
||||
def test_loaded_index_returns_bounded_candidates_with_freshness() -> None:
|
||||
@@ -65,12 +108,35 @@ def test_loaded_index_returns_bounded_candidates_with_freshness() -> None:
|
||||
|
||||
# Assert
|
||||
assert result.degraded is False
|
||||
assert result.retrieval_path == "local_descriptor_index"
|
||||
assert result.latency_ms is not None
|
||||
assert len(result.candidates) == 1
|
||||
assert result.candidates[0].chunk_id == "chunk-best"
|
||||
assert result.candidates[0].tile_id == "tile-best"
|
||||
assert result.candidates[0].freshness_status == "fresh"
|
||||
|
||||
|
||||
def test_loaded_index_requires_query_descriptor() -> None:
|
||||
# Arrange
|
||||
retriever = LocalVprRetriever()
|
||||
retriever.load_index(LocalVprIndexPackage(package_id="index-1", records=(_record(),)))
|
||||
request = RelocalizationRequest(
|
||||
frame_id="frame-1",
|
||||
image_ref="replay/frame-1.jpg",
|
||||
trigger_reason="covariance_growth",
|
||||
top_k=1,
|
||||
)
|
||||
|
||||
# Act
|
||||
result = retriever.retrieve(request)
|
||||
|
||||
# Assert
|
||||
assert result.ready is True
|
||||
assert result.degraded is True
|
||||
assert result.error is not None
|
||||
assert result.error.cause == "query_descriptor_missing"
|
||||
|
||||
|
||||
def test_missing_index_degrades_with_explicit_no_candidate_result() -> None:
|
||||
# Arrange
|
||||
retriever = LocalVprRetriever()
|
||||
@@ -92,6 +158,34 @@ def test_missing_index_degrades_with_explicit_no_candidate_result() -> None:
|
||||
assert result.error.cause == "index_not_loaded"
|
||||
|
||||
|
||||
def test_invalid_index_package_degrades_with_explicit_error(tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
package_path = tmp_path / "invalid-index.json"
|
||||
package_path.write_text("{not-json", encoding="utf-8")
|
||||
retriever = LocalVprRetriever()
|
||||
|
||||
# Act
|
||||
readiness = retriever.load_index_from_path(package_path)
|
||||
result = retriever.retrieve(
|
||||
RelocalizationRequest(
|
||||
frame_id="frame-1",
|
||||
image_ref="replay/frame-1.jpg",
|
||||
trigger_reason="cold_start",
|
||||
top_k=3,
|
||||
query_descriptor=(1.0, 0.0),
|
||||
)
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert readiness.ready is False
|
||||
assert readiness.error is not None
|
||||
assert readiness.error.cause == "index_package_invalid"
|
||||
assert result.ready is False
|
||||
assert result.degraded is True
|
||||
assert result.error is not None
|
||||
assert result.error.cause == "index_package_invalid"
|
||||
|
||||
|
||||
def test_descriptor_fidelity_gate_rejects_large_optimized_delta() -> None:
|
||||
# Arrange
|
||||
retriever = LocalVprRetriever()
|
||||
|
||||
@@ -1,5 +1,52 @@
|
||||
from shared.contracts import FramePacket, TelemetrySample
|
||||
from vio_adapter import LocalVioAdapter, VioInputPacket
|
||||
from vio_adapter import LocalVioAdapter, NativeVioBackend, VioBackendEstimate, VioInputPacket
|
||||
|
||||
|
||||
class RecordingNativeRunner:
|
||||
def __init__(self) -> None:
|
||||
self.initialized = False
|
||||
self.estimate_calls = 0
|
||||
|
||||
def initialize(self) -> None:
|
||||
self.initialized = True
|
||||
|
||||
def estimate(
|
||||
self,
|
||||
frame: FramePacket,
|
||||
telemetry_window: tuple[TelemetrySample, ...],
|
||||
) -> VioBackendEstimate:
|
||||
self.estimate_calls += 1
|
||||
return VioBackendEstimate(
|
||||
timestamp_ns=frame.timestamp_ns,
|
||||
relative_pose={"x_m": 12.0, "y_m": -1.5, "z_m": 0.2, "yaw_rad": 0.3},
|
||||
velocity_mps=(4.0, 0.5, 0.0),
|
||||
tracking_quality=0.77,
|
||||
bias_estimate={"sample_count": float(len(telemetry_window)), "gyro_bias": 0.01},
|
||||
covariance_hint=[[0.4, 0.0, 0.0], [0.0, 0.4, 0.0], [0.0, 0.0, 0.8]],
|
||||
)
|
||||
|
||||
|
||||
class FailingNativeRunner:
|
||||
def __init__(self, fail_on: str) -> None:
|
||||
self._fail_on = fail_on
|
||||
|
||||
def initialize(self) -> None:
|
||||
if self._fail_on == "initialize":
|
||||
raise RuntimeError("engine package missing")
|
||||
|
||||
def estimate(
|
||||
self,
|
||||
frame: FramePacket,
|
||||
telemetry_window: tuple[TelemetrySample, ...],
|
||||
) -> VioBackendEstimate:
|
||||
if self._fail_on == "estimate":
|
||||
raise RuntimeError("engine lost tracking")
|
||||
return VioBackendEstimate(
|
||||
timestamp_ns=frame.timestamp_ns,
|
||||
relative_pose={"x_m": 0.0},
|
||||
velocity_mps=(0.0, 0.0, 0.0),
|
||||
tracking_quality=1.0,
|
||||
)
|
||||
|
||||
|
||||
def _frame(**overrides: object) -> FramePacket:
|
||||
@@ -42,6 +89,60 @@ def test_valid_synchronized_packet_emits_vio_state() -> None:
|
||||
assert result.health.state == "ready"
|
||||
|
||||
|
||||
def test_configured_native_backend_path_emits_vio_state() -> None:
|
||||
# Arrange
|
||||
runner = RecordingNativeRunner()
|
||||
adapter = LocalVioAdapter(backend=NativeVioBackend(runner, backend_name="basalt"))
|
||||
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
||||
|
||||
# Act
|
||||
result = adapter.process(packet)
|
||||
|
||||
# Assert
|
||||
assert runner.initialized is True
|
||||
assert runner.estimate_calls == 1
|
||||
assert result.error is None
|
||||
assert result.state_packet is not None
|
||||
assert result.state_packet.relative_pose["x_m"] == 12.0
|
||||
assert result.state_packet.velocity_mps == (4.0, 0.5, 0.0)
|
||||
assert result.health.backend_name == "basalt"
|
||||
assert result.processing_latency_ms is not None
|
||||
|
||||
|
||||
def test_native_backend_initialization_failure_sets_failed_health() -> None:
|
||||
# Arrange
|
||||
adapter = LocalVioAdapter(
|
||||
backend=NativeVioBackend(FailingNativeRunner("initialize"), backend_name="basalt")
|
||||
)
|
||||
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
||||
|
||||
# Act
|
||||
result = adapter.process(packet)
|
||||
|
||||
# Assert
|
||||
assert result.state_packet is None
|
||||
assert result.health.state == "failed"
|
||||
assert result.error is not None
|
||||
assert result.error.cause == "backend_initialization_failed"
|
||||
|
||||
|
||||
def test_native_backend_runtime_failure_sets_failed_health() -> None:
|
||||
# Arrange
|
||||
adapter = LocalVioAdapter(
|
||||
backend=NativeVioBackend(FailingNativeRunner("estimate"), backend_name="basalt")
|
||||
)
|
||||
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
||||
|
||||
# Act
|
||||
result = adapter.process(packet)
|
||||
|
||||
# Assert
|
||||
assert result.state_packet is None
|
||||
assert result.health.state == "failed"
|
||||
assert result.error is not None
|
||||
assert result.error.cause == "backend_runtime_failed"
|
||||
|
||||
|
||||
def test_timestamp_mismatch_is_explicit_validation_error() -> None:
|
||||
# Arrange
|
||||
adapter = LocalVioAdapter(timestamp_tolerance_ns=1_000)
|
||||
|
||||
Reference in New Issue
Block a user