mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 01:26:37 +00:00
feat(phases 2-7): implement full GPS-denied navigation pipeline
Phase 2 — Visual Odometry: - ORBVisualOdometry (dev/CI), CuVSLAMVisualOdometry (Jetson) - TRTInferenceEngine (TensorRT FP16, conditional import) - create_vo_backend() factory Phase 3 — Satellite Matching + GPR: - SatelliteDataManager: local z/x/y tiles, ESKF ±3σ tile selection - GSD normalization (SAT-03), RANSAC inlier-ratio confidence (SAT-04) - GlobalPlaceRecognition: Faiss index + numpy fallback Phase 4 — MAVLink I/O: - MAVLinkBridge: GPS_INPUT 15+ fields, IMU callback, 1Hz telemetry - 3-consecutive-failure reloc request - MockMAVConnection for CI Phase 5 — Pipeline Wiring: - ESKF wired into process_frame: VO update → satellite update - CoordinateTransformer + SatelliteDataManager via DI - MAVLink state push per frame (PIPE-07) - Real pixel_to_gps via ray-ground projection (PIPE-06) - GTSAM ISAM2 update when available (PIPE-03) Phase 6 — Docker + CI: - Multi-stage Dockerfile (python:3.11-slim) - docker-compose.yml (dev), docker-compose.sitl.yml (ArduPilot SITL) - GitHub Actions: ci.yml (lint+pytest+docker smoke), sitl.yml (nightly) - tests/test_sitl_integration.py (8 tests, skip without SITL) Phase 7 — Accuracy Validation: - AccuracyBenchmark + SyntheticTrajectory - AC-PERF-1: 80% within 50m ✅ - AC-PERF-2: 60% within 20m ✅ - AC-PERF-3: p95 latency < 400ms ✅ - AC-PERF-4: VO drift 1km < 100m ✅ (actual ~11m) - scripts/benchmark_accuracy.py CLI Tests: 195 passed / 8 skipped Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
+64
-2
@@ -35,7 +35,69 @@ def test_retrieve_candidate_tiles(gpr):
|
||||
def test_retrieve_candidate_tiles_for_chunk(gpr):
|
||||
imgs = [np.zeros((200, 200, 3), dtype=np.uint8) for _ in range(5)]
|
||||
candidates = gpr.retrieve_candidate_tiles_for_chunk(imgs, top_k=3)
|
||||
|
||||
|
||||
assert len(candidates) == 3
|
||||
# Ensure they are sorted
|
||||
# Ensure they are sorted descending (GPR-03)
|
||||
assert candidates[0].similarity_score >= candidates[1].similarity_score
|
||||
|
||||
|
||||
# ---------------------------------------------------------------
|
||||
# GPR-01: Real Faiss index with file path
|
||||
# ---------------------------------------------------------------
|
||||
|
||||
def test_load_index_missing_file_falls_back(tmp_path):
|
||||
"""GPR-01: non-existent index path → numpy fallback, still usable."""
|
||||
from gps_denied.core.models import ModelManager
|
||||
from gps_denied.core.gpr import GlobalPlaceRecognition
|
||||
|
||||
g = GlobalPlaceRecognition(ModelManager())
|
||||
ok = g.load_index("f1", str(tmp_path / "nonexistent.index"))
|
||||
assert ok is True
|
||||
assert g._is_loaded is True
|
||||
# Should still answer queries
|
||||
img = np.zeros((200, 200, 3), dtype=np.uint8)
|
||||
cands = g.retrieve_candidate_tiles(img, top_k=3)
|
||||
assert len(cands) == 3
|
||||
|
||||
|
||||
def test_load_index_not_loaded_returns_empty():
|
||||
"""query_database before load_index → empty list (no crash)."""
|
||||
from gps_denied.core.models import ModelManager
|
||||
from gps_denied.core.gpr import GlobalPlaceRecognition
|
||||
|
||||
g = GlobalPlaceRecognition(ModelManager())
|
||||
desc = np.random.rand(4096).astype(np.float32)
|
||||
matches = g.query_database(desc, top_k=5)
|
||||
assert matches == []
|
||||
|
||||
|
||||
# ---------------------------------------------------------------
|
||||
# GPR-03: Ranking is deterministic (sorted by similarity)
|
||||
# ---------------------------------------------------------------
|
||||
|
||||
def test_rank_candidates_sorted(gpr):
|
||||
"""rank_candidates must return descending similarity order."""
|
||||
from gps_denied.schemas.gpr import TileCandidate
|
||||
from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas.satellite import TileBounds
|
||||
|
||||
dummy_bounds = TileBounds(
|
||||
nw=GPSPoint(lat=49.1, lon=32.0), ne=GPSPoint(lat=49.1, lon=32.1),
|
||||
sw=GPSPoint(lat=49.0, lon=32.0), se=GPSPoint(lat=49.0, lon=32.1),
|
||||
center=GPSPoint(lat=49.05, lon=32.05), gsd=0.6,
|
||||
)
|
||||
cands = [
|
||||
TileCandidate(tile_id="a", gps_center=GPSPoint(lat=49, lon=32), bounds=dummy_bounds, similarity_score=0.3, rank=3),
|
||||
TileCandidate(tile_id="b", gps_center=GPSPoint(lat=49, lon=32), bounds=dummy_bounds, similarity_score=0.9, rank=1),
|
||||
TileCandidate(tile_id="c", gps_center=GPSPoint(lat=49, lon=32), bounds=dummy_bounds, similarity_score=0.6, rank=2),
|
||||
]
|
||||
ranked = gpr.rank_candidates(cands)
|
||||
scores = [c.similarity_score for c in ranked]
|
||||
assert scores == sorted(scores, reverse=True)
|
||||
|
||||
|
||||
def test_descriptor_is_l2_normalised(gpr):
|
||||
"""DINOv2 descriptor returned by compute_location_descriptor is unit-norm."""
|
||||
img = np.random.randint(0, 255, (200, 200, 3), dtype=np.uint8)
|
||||
desc = gpr.compute_location_descriptor(img)
|
||||
assert np.isclose(np.linalg.norm(desc), 1.0, atol=1e-5)
|
||||
|
||||
Reference in New Issue
Block a user