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:
Yuzviak
2026-04-02 17:00:41 +03:00
parent a15bef5c01
commit 094895b21b
40 changed files with 4572 additions and 497 deletions
+53 -6
View File
@@ -5,7 +5,7 @@ import pytest
from gps_denied.core.metric import MetricRefinement
from gps_denied.core.models import ModelManager
from gps_denied.schemas.flight import GPSPoint
from gps_denied.schemas import GPSPoint
from gps_denied.schemas.metric import AlignmentResult, ChunkAlignmentResult
from gps_denied.schemas.satellite import TileBounds
@@ -39,22 +39,69 @@ def test_extract_gps_from_alignment(metric, bounds):
assert np.isclose(gps.lon, 32.5)
def test_align_to_satellite(metric, bounds, monkeypatch):
# Monkeypatch random to ensure matched=True and high inliers
def mock_infer(*args, **kwargs):
H = np.eye(3, dtype=np.float64)
return {"homography": H, "inlier_count": 80, "confidence": 0.8}
return {"homography": H, "inlier_count": 80, "total_correspondences": 100, "confidence": 0.8, "reprojection_error": 1.0}
engine = metric.model_manager.get_inference_engine("LiteSAM")
monkeypatch.setattr(engine, "infer", mock_infer)
uav = np.zeros((256, 256, 3))
sat = np.zeros((256, 256, 3))
res = metric.align_to_satellite(uav, sat, bounds)
assert res is not None
assert isinstance(res, AlignmentResult)
assert res.matched is True
assert res.inlier_count == 80
# SAT-04: confidence = inlier_ratio
assert np.isclose(res.confidence, 80 / 100)
# ---------------------------------------------------------------
# SAT-03: GSD normalization
# ---------------------------------------------------------------
def test_normalize_gsd_downsamples(metric):
"""UAV frame at 0.16 m/px downsampled to satellite 0.6 m/px."""
uav = np.zeros((480, 640, 3), dtype=np.uint8)
out = metric.normalize_gsd(uav, uav_gsd_mpp=0.16, sat_gsd_mpp=0.6)
# Should be roughly 640 * (0.16/0.6) ≈ 170 wide
assert out.shape[1] < 640
assert out.shape[0] < 480
def test_normalize_gsd_no_downscale_needed(metric):
"""UAV GSD already coarser than satellite → image unchanged."""
uav = np.zeros((256, 256, 3), dtype=np.uint8)
out = metric.normalize_gsd(uav, uav_gsd_mpp=0.8, sat_gsd_mpp=0.6)
assert out.shape == uav.shape
def test_normalize_gsd_zero_args(metric):
"""Zero GSD args → image returned unchanged (guard against divide-by-zero)."""
uav = np.zeros((100, 100, 3), dtype=np.uint8)
out = metric.normalize_gsd(uav, uav_gsd_mpp=0.0, sat_gsd_mpp=0.6)
assert out.shape == uav.shape
# ---------------------------------------------------------------
# SAT-04: confidence = inlier ratio via align_to_satellite
# ---------------------------------------------------------------
def test_align_confidence_is_inlier_ratio(metric, bounds, monkeypatch):
"""SAT-04: returned confidence must equal inlier_count / total_correspondences."""
def mock_infer(*args, **kwargs):
H = np.eye(3, dtype=np.float64)
return {"homography": H, "inlier_count": 60, "total_correspondences": 150,
"confidence": 0.4, "reprojection_error": 1.0}
engine = metric.model_manager.get_inference_engine("LiteSAM")
monkeypatch.setattr(engine, "infer", mock_infer)
res = metric.align_to_satellite(np.zeros((256, 256, 3)), np.zeros((256, 256, 3)), bounds)
if res is not None:
assert np.isclose(res.confidence, 60 / 150)
def test_align_chunk_to_satellite(metric, bounds, monkeypatch):
def mock_infer(*args, **kwargs):