mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 02:26:37 +00:00
Yuzviak
094895b21b
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>
161 lines
5.8 KiB
Python
161 lines
5.8 KiB
Python
"""Tests for SatelliteDataManager (F04) — SAT-01/02 and mercator utils (H06)."""
|
||
|
||
import numpy as np
|
||
import pytest
|
||
|
||
from gps_denied.core.satellite import SatelliteDataManager
|
||
from gps_denied.schemas import GPSPoint
|
||
from gps_denied.utils import mercator
|
||
|
||
|
||
# ---------------------------------------------------------------
|
||
# Mercator utils
|
||
# ---------------------------------------------------------------
|
||
|
||
def test_latlon_to_tile():
|
||
lat, lon, zoom = 50.4501, 30.5234, 15
|
||
coords = mercator.latlon_to_tile(lat, lon, zoom)
|
||
assert coords.zoom == 15
|
||
assert coords.x > 0
|
||
assert coords.y > 0
|
||
|
||
|
||
def test_tile_to_latlon():
|
||
gps = mercator.tile_to_latlon(19131, 10927, 15)
|
||
assert 50.0 < gps.lat < 52.0
|
||
assert 30.0 < gps.lon < 31.0
|
||
|
||
|
||
def test_tile_bounds():
|
||
coords = mercator.TileCoords(x=19131, y=10927, zoom=15)
|
||
bounds = mercator.compute_tile_bounds(coords)
|
||
assert bounds.nw.lat > bounds.se.lat
|
||
assert bounds.nw.lon < bounds.se.lon
|
||
assert bounds.gsd > 0
|
||
|
||
|
||
# ---------------------------------------------------------------
|
||
# SAT-01: Local tile storage (no HTTP)
|
||
# ---------------------------------------------------------------
|
||
|
||
@pytest.fixture
|
||
def satellite_manager(tmp_path):
|
||
return SatelliteDataManager(tile_dir=str(tmp_path / "tiles"))
|
||
|
||
|
||
def test_load_local_tile_missing(satellite_manager):
|
||
"""Missing tile returns None — no crash."""
|
||
coords = mercator.TileCoords(x=0, y=0, zoom=12)
|
||
result = satellite_manager.load_local_tile(coords)
|
||
assert result is None
|
||
|
||
|
||
def test_save_and_load_local_tile(satellite_manager):
|
||
"""SAT-01: saved tile can be read back from the local directory."""
|
||
coords = mercator.TileCoords(x=19131, y=10927, zoom=15)
|
||
img = np.zeros((256, 256, 3), dtype=np.uint8)
|
||
img[:] = [0, 128, 255]
|
||
|
||
ok = satellite_manager.save_local_tile(coords, img)
|
||
assert ok is True
|
||
|
||
loaded = satellite_manager.load_local_tile(coords)
|
||
assert loaded is not None
|
||
assert loaded.shape == (256, 256, 3)
|
||
|
||
|
||
def test_mem_cache_hit(satellite_manager):
|
||
"""Tile loaded once should be served from memory on second request."""
|
||
coords = mercator.TileCoords(x=1, y=1, zoom=10)
|
||
img = np.ones((256, 256, 3), dtype=np.uint8) * 42
|
||
satellite_manager.save_local_tile(coords, img)
|
||
|
||
r1 = satellite_manager.load_local_tile(coords)
|
||
r2 = satellite_manager.load_local_tile(coords)
|
||
assert r1 is r2 # same object = came from mem cache
|
||
|
||
|
||
# ---------------------------------------------------------------
|
||
# SAT-02: ESKF ±3σ tile selection
|
||
# ---------------------------------------------------------------
|
||
|
||
def test_select_tiles_small_sigma(satellite_manager):
|
||
"""Very tight sigma → single tile covering the position."""
|
||
gps = GPSPoint(lat=50.45, lon=30.52)
|
||
tiles = satellite_manager.select_tiles_for_eskf_position(gps, sigma_h_m=1.0, zoom=18)
|
||
# Should produce at least the center tile
|
||
assert len(tiles) >= 1
|
||
for t in tiles:
|
||
assert t.zoom == 18
|
||
|
||
|
||
def test_select_tiles_large_sigma(satellite_manager):
|
||
"""Larger sigma → more tiles returned."""
|
||
gps = GPSPoint(lat=50.45, lon=30.52)
|
||
small = satellite_manager.select_tiles_for_eskf_position(gps, sigma_h_m=10.0, zoom=18)
|
||
large = satellite_manager.select_tiles_for_eskf_position(gps, sigma_h_m=200.0, zoom=18)
|
||
assert len(large) >= len(small)
|
||
|
||
|
||
def test_select_tiles_bounding_box(satellite_manager):
|
||
"""Selected tiles must span a bounding box that covers ±3σ."""
|
||
gps = GPSPoint(lat=49.0, lon=32.0)
|
||
sigma = 50.0 # 50 m → 3σ = 150 m
|
||
zoom = 18
|
||
tiles = satellite_manager.select_tiles_for_eskf_position(gps, sigma_h_m=sigma, zoom=zoom)
|
||
assert len(tiles) >= 1
|
||
# All returned tiles must be at the requested zoom
|
||
assert all(t.zoom == zoom for t in tiles)
|
||
|
||
|
||
# ---------------------------------------------------------------
|
||
# SAT-01: Mosaic assembly
|
||
# ---------------------------------------------------------------
|
||
|
||
def test_assemble_mosaic_single(satellite_manager):
|
||
"""Single tile → mosaic equals that tile (resized)."""
|
||
coords = mercator.TileCoords(x=10, y=10, zoom=15)
|
||
img = np.zeros((256, 256, 3), dtype=np.uint8)
|
||
mosaic, bounds = satellite_manager.assemble_mosaic([(coords, img)], target_size=256)
|
||
assert mosaic.shape == (256, 256, 3)
|
||
assert bounds.center is not None
|
||
|
||
|
||
def test_assemble_mosaic_2x2(satellite_manager):
|
||
"""2×2 tile grid assembles into a single mosaic."""
|
||
base = mercator.TileCoords(x=10, y=10, zoom=15)
|
||
tiles = [
|
||
(mercator.TileCoords(x=10, y=10, zoom=15), np.zeros((256, 256, 3), dtype=np.uint8)),
|
||
(mercator.TileCoords(x=11, y=10, zoom=15), np.zeros((256, 256, 3), dtype=np.uint8)),
|
||
(mercator.TileCoords(x=10, y=11, zoom=15), np.zeros((256, 256, 3), dtype=np.uint8)),
|
||
(mercator.TileCoords(x=11, y=11, zoom=15), np.zeros((256, 256, 3), dtype=np.uint8)),
|
||
]
|
||
mosaic, bounds = satellite_manager.assemble_mosaic(tiles, target_size=512)
|
||
assert mosaic.shape == (512, 512, 3)
|
||
|
||
|
||
def test_assemble_mosaic_empty(satellite_manager):
|
||
result = satellite_manager.assemble_mosaic([])
|
||
assert result is None
|
||
|
||
|
||
# ---------------------------------------------------------------
|
||
# Cache helpers (backward compat)
|
||
# ---------------------------------------------------------------
|
||
|
||
def test_cache_tile_compat(satellite_manager):
|
||
coords = mercator.TileCoords(x=100, y=100, zoom=12)
|
||
img = np.zeros((256, 256, 3), dtype=np.uint8)
|
||
assert satellite_manager.cache_tile("f1", coords, img) is True
|
||
cached = satellite_manager.get_cached_tile("f1", coords)
|
||
assert cached is not None
|
||
|
||
|
||
def test_grid_calculations(satellite_manager):
|
||
center = mercator.TileCoords(x=100, y=100, zoom=15)
|
||
grid = satellite_manager.get_tile_grid(center, 9)
|
||
assert len(grid) == 9
|
||
assert any(c.x == 100 and c.y == 100 for c in grid)
|
||
new_tiles = satellite_manager.expand_search_grid(center, 9, 25)
|
||
assert len(new_tiles) == 16 # 25 - 9
|