mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 05: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:
@@ -28,9 +28,11 @@ class ImageInputPipeline:
|
||||
# flight_id -> asyncio.Queue of ImageBatch
|
||||
self._queues: dict[str, asyncio.Queue] = {}
|
||||
self.max_queue_size = max_queue_size
|
||||
|
||||
|
||||
# In-memory tracking (in a real system, sync this with DB)
|
||||
self._status: dict[str, dict] = {}
|
||||
# Exact sequence → filename mapping (VO-05: no substring collision)
|
||||
self._sequence_map: dict[str, dict[int, str]] = {}
|
||||
|
||||
def _get_queue(self, flight_id: str) -> asyncio.Queue:
|
||||
if flight_id not in self._queues:
|
||||
@@ -50,7 +52,7 @@ class ImageInputPipeline:
|
||||
errors = []
|
||||
|
||||
num_images = len(batch.images)
|
||||
if num_images < 10:
|
||||
if num_images < 1:
|
||||
errors.append("Batch is empty")
|
||||
elif num_images > 100:
|
||||
errors.append("Batch too large")
|
||||
@@ -124,6 +126,8 @@ class ImageInputPipeline:
|
||||
metadata=meta
|
||||
)
|
||||
processed_images.append(img_data)
|
||||
# VO-05: record exact sequence→filename mapping
|
||||
self._sequence_map.setdefault(flight_id, {})[seq] = fn
|
||||
|
||||
# Store to disk
|
||||
self.store_images(flight_id, processed_images)
|
||||
@@ -161,19 +165,33 @@ class ImageInputPipeline:
|
||||
return img
|
||||
|
||||
def get_image_by_sequence(self, flight_id: str, sequence: int) -> ImageData | None:
|
||||
"""Retrieves a specific image by sequence number."""
|
||||
# For simplicity, we assume filenames follow "frame_{sequence:06d}.jpg"
|
||||
# But if the user uploaded custom files, we'd need a DB lookup.
|
||||
# Let's use a local map for this prototype if it's strictly required,
|
||||
# or search the directory.
|
||||
"""Retrieves a specific image by sequence number (exact match — VO-05)."""
|
||||
flight_dir = os.path.join(self.storage_dir, flight_id)
|
||||
if not os.path.exists(flight_dir):
|
||||
return None
|
||||
|
||||
# search
|
||||
|
||||
# Prefer the exact mapping built during process_next_batch
|
||||
fn = self._sequence_map.get(flight_id, {}).get(sequence)
|
||||
if fn:
|
||||
path = os.path.join(flight_dir, fn)
|
||||
img = cv2.imread(path)
|
||||
if img is not None:
|
||||
h, w = img.shape[:2]
|
||||
meta = ImageMetadata(
|
||||
sequence=sequence,
|
||||
filename=fn,
|
||||
dimensions=(w, h),
|
||||
file_size=os.path.getsize(path),
|
||||
timestamp=datetime.now(timezone.utc),
|
||||
)
|
||||
return ImageData(flight_id, sequence, fn, img, meta)
|
||||
|
||||
# Fallback: scan directory for exact filename patterns
|
||||
# (handles images stored before this process started)
|
||||
for fn in os.listdir(flight_dir):
|
||||
# very rough matching
|
||||
if str(sequence) in fn or fn.endswith(f"_{sequence:06d}.jpg"):
|
||||
base, _ = os.path.splitext(fn)
|
||||
# Accept only if the base name ends with exactly the padded sequence number
|
||||
if base.endswith(f"{sequence:06d}") or base == str(sequence):
|
||||
path = os.path.join(flight_dir, fn)
|
||||
img = cv2.imread(path)
|
||||
if img is not None:
|
||||
@@ -183,10 +201,10 @@ class ImageInputPipeline:
|
||||
filename=fn,
|
||||
dimensions=(w, h),
|
||||
file_size=os.path.getsize(path),
|
||||
timestamp=datetime.now(timezone.utc)
|
||||
timestamp=datetime.now(timezone.utc),
|
||||
)
|
||||
return ImageData(flight_id, sequence, fn, img, meta)
|
||||
|
||||
|
||||
return None
|
||||
|
||||
def get_processing_status(self, flight_id: str) -> ProcessingStatus:
|
||||
|
||||
Reference in New Issue
Block a user