mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 07:11:13 +00:00
docs: rewrite README for Stage 2 Phase 2 (hexagonal arch + AC + structlog)
- Reflects Stage 2 hexagonal layout: components/, hot_types/, obs/, pipeline/ - Updated architecture diagram with ports-and-adapters structure - Correct test count: 236 passed, 8 skipped (was: 216 from Stage 1) - New sections: AC traceability, taxonomy markers, per-env config, Roadmap table - ADR links to 0001-0004 - Component table with Protocol / dev / Jetson adapter columns - Removed stale Stage 1 next steps and sprint backlog Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
@@ -4,54 +4,59 @@
|
||||
|
||||
Замінює GPS-сигнал власною оцінкою позиції на основі відеопотоку (cuVSLAM), IMU та супутникових знімків. Позиція подається у польотний контролер ArduPilot у форматі `GPS_INPUT` через MAVLink при 5–10 Гц.
|
||||
|
||||
**Гілка розробки:** `stage2` | **Фаза:** 2/6 (завершено) | **Тести:** 236 passed, 8 skipped
|
||||
|
||||
---
|
||||
|
||||
## Архітектура
|
||||
## Архітектура (Stage 2 — Hexagonal / Ports-and-Adapters)
|
||||
|
||||
```
|
||||
IMU (MAVLink RAW_IMU) ──────────────────────────────────────────▶ ESKF.predict()
|
||||
│
|
||||
ADTI 20L V1 ──▶ ImageInputPipeline ──▶ ImageRotationManager │
|
||||
ADTI 20L V1 ──▶ pipeline/image_input ──▶ ImageRotationManager │
|
||||
│ │
|
||||
┌───────────────┼───────────────┐ │
|
||||
▼ ▼ ▼ │
|
||||
cuVSLAM/ORB VO GlobalPlaceRecog SatelliteData │
|
||||
(F07) (F08/Faiss) (F04) │
|
||||
components/vio components/gpr components/ │
|
||||
cuVSLAM (Jetson) Faiss+DINOv2 satellite_ │
|
||||
ORB-SLAM3 (dev) numpy (dev) matcher │
|
||||
│ │ │ │
|
||||
▼ ▼ ▼ │
|
||||
ESKF.update_vo() GSD norm MetricRefinement│
|
||||
│ (F09) │
|
||||
ESKF.update_vo() GPR norm MetricRefinement│
|
||||
│ (XFeat TRT) │
|
||||
└──────────────────────▶ ESKF.update_sat()│
|
||||
│
|
||||
ESKF state ◀──┘
|
||||
│
|
||||
┌───────────────┼──────────────┐
|
||||
▼ ▼ ▼
|
||||
MAVLinkBridge FactorGraph SSE Stream
|
||||
GPS_INPUT 5-10Hz (GTSAM ISAM2) → Ground Station
|
||||
components/mavlink_io core/factor_graph SSE stream
|
||||
GPS_INPUT 5-10Hz (GTSAM ISAM2) → ground station
|
||||
→ ArduPilot FC
|
||||
```
|
||||
|
||||
**State Machine** (`process_frame`):
|
||||
**Стейт-машина** (`FlightProcessor.process_frame`):
|
||||
```
|
||||
NORMAL ──(VO fail)──▶ LOST ──▶ RECOVERY ──(GPR+Metric ok)──▶ NORMAL
|
||||
```
|
||||
|
||||
**Правило залежностей:** `pipeline/orchestrator.py` імпортує лише Protocols. Тільки `pipeline/composition.py` (`build_pipeline(env)`) знає про конкретні адаптери.
|
||||
|
||||
---
|
||||
|
||||
## Стек
|
||||
|
||||
| Підсистема | Dev / CI | Jetson (production) |
|
||||
|-----------|--------|---------------------|
|
||||
| **Visual Odometry** | ORBVisualOdometry / CuVSLAMMonoDepthVisualOdometry (scaled ORB fallback) | CuVSLAMMonoDepthVisualOdometry (PyCuVSLAM v15 Mono-Depth — barometer as synthetic depth) |
|
||||
| **AI Inference** | MockInferenceEngine | TRTInferenceEngine (TensorRT FP16; INT8 disabled — broken for ViT on Jetson) |
|
||||
| **Place Recognition** | numpy L2 fallback (AnyLoc-VLAD-DINOv2 baseline) | Faiss GPU index + DINOv2-VLAD TRT FP16 |
|
||||
| **MAVLink** | MockMAVConnection | pymavlink over UART |
|
||||
| **ESKF** | numpy (15-state) | numpy (15-state) |
|
||||
| **Factor Graph** | Mock poses | GTSAM 4.3 ISAM2 (sprint 2 — ESKF-only sufficient for sprint 1) |
|
||||
|-----------|----------|---------------------|
|
||||
| **Visual Odometry** | `ORBVisualOdometry` / `CuVSLAMMonoDepthVO` | `CuVSLAMMonoDepthVO` (PyCuVSLAM v15, barometer як synthetic depth) |
|
||||
| **AI Inference** | `MockInferenceEngine` | `TRTInferenceEngine` (TensorRT FP16) |
|
||||
| **Place Recognition** | numpy L2 fallback | Faiss GPU + DINOv2-VLAD TRT FP16 |
|
||||
| **MAVLink** | `MockMAVConnection` | pymavlink over UART |
|
||||
| **ESKF** | numpy 15-state | numpy 15-state |
|
||||
| **Factor Graph** | stub | GTSAM 4.3 ISAM2 |
|
||||
| **Логування** | structlog ConsoleRenderer | structlog JSON (orjson) |
|
||||
| **API** | FastAPI + Pydantic v2 + SSE | FastAPI + Pydantic v2 + SSE |
|
||||
| **БД** | SQLite + SQLAlchemy 2 async | SQLite |
|
||||
| **Тести** | pytest + pytest-asyncio | — |
|
||||
|
||||
---
|
||||
|
||||
@@ -67,7 +72,7 @@ NORMAL ──(VO fail)──▶ LOST ──▶ RECOVERY ──(GPR+Metric ok)─
|
||||
```bash
|
||||
git clone https://github.com/azaion/gps-denied-onboard.git
|
||||
cd gps-denied-onboard
|
||||
git checkout stage1
|
||||
git checkout stage2
|
||||
|
||||
python3 -m venv .venv
|
||||
source .venv/bin/activate
|
||||
@@ -77,8 +82,8 @@ pip install -e ".[dev]"
|
||||
### Запуск
|
||||
|
||||
```bash
|
||||
# Прямий запуск
|
||||
uvicorn gps_denied.app:app --host 0.0.0.0 --port 8000
|
||||
# Прямий запуск (env=x86_dev за замовчуванням)
|
||||
ENV=x86_dev uvicorn gps_denied.app:app --host 0.0.0.0 --port 8000
|
||||
|
||||
# Docker
|
||||
docker compose up --build
|
||||
@@ -86,31 +91,31 @@ docker compose up --build
|
||||
|
||||
Сервер: `http://127.0.0.1:8000`
|
||||
|
||||
### Змінні середовища
|
||||
### Конфігурація
|
||||
|
||||
Вибір середовища задається змінною `ENV`:
|
||||
|
||||
```env
|
||||
ENV=x86_dev # за замовчуванням — ORB-SLAM3, моки, консольні логи
|
||||
ENV=jetson # cuVSLAM + TRT + pymavlink + JSON logs
|
||||
ENV=ci # усі моки, без hardware
|
||||
ENV=sitl # ArduPilot SITL
|
||||
```
|
||||
|
||||
Кожне середовище має overlay у `config/{env}.yaml`. Усі параметри — у `src/gps_denied/config.py`.
|
||||
|
||||
**Ключові змінні середовища:**
|
||||
|
||||
```env
|
||||
# Основні
|
||||
DB_URL=sqlite+aiosqlite:///./flight_data.db
|
||||
SATELLITE_TILE_DIR=.satellite_tiles
|
||||
MAVLINK_CONNECTION=serial:/dev/ttyTHS1:57600 # або tcp:host:port
|
||||
MAVLINK_OUTPUT_HZ=5.0
|
||||
MAVLINK_TELEMETRY_HZ=1.0
|
||||
|
||||
# ESKF тюнінг (опціонально)
|
||||
ESKF_VO_POSITION_NOISE=0.3
|
||||
ESKF_SATELLITE_MAX_AGE=30.0
|
||||
ESKF_MAHALANOBIS_THRESHOLD=16.27
|
||||
|
||||
# API
|
||||
API_HOST=127.0.0.1
|
||||
API_PORT=8000
|
||||
|
||||
# Моделі
|
||||
MODEL_WEIGHTS_DIR=weights
|
||||
```
|
||||
|
||||
Повний список: `src/gps_denied/config.py` (40+ параметрів з prefix `DB_`, `API_`, `TILES_`, `MODEL_`, `MAVLINK_`, `SATELLITE_`, `ESKF_`, `RECOVERY_`, `ROTATION_`).
|
||||
|
||||
---
|
||||
|
||||
## API
|
||||
@@ -121,111 +126,61 @@ MODEL_WEIGHTS_DIR=weights
|
||||
| `/flights` | POST | Створити політ |
|
||||
| `/flights/{id}` | GET | Деталі польоту |
|
||||
| `/flights/{flight_id}` | DELETE | Видалити політ |
|
||||
| `/flights/{flight_id}/images/batch` | POST | Батч зображень |
|
||||
| `/flights/{flight_id}/images/batch` | POST | Батч зображень → обробка |
|
||||
| `/flights/{flight_id}/user-fix` | POST | GPS-якір від оператора → ESKF update |
|
||||
| `/flights/{flight_id}/status` | GET | Статус обробки |
|
||||
| `/flights/{flight_id}/stream` | GET | SSE стрім (позиція + confidence) |
|
||||
| `/flights/{flight_id}/frames/{frame_id}/object-to-gps` | POST | Pixel → GPS (ray-ground проекція) |
|
||||
| `/flights/{flight_id}/waypoints/{waypoint_id}` | PUT | Оновити waypoint |
|
||||
| `/flights/{flight_id}/waypoints/batch` | PUT | Batch update waypoints |
|
||||
| `/flights/{flight_id}/frames/{frame_id}/object-to-gps` | POST | Pixel → GPS (ray-ground) |
|
||||
|
||||
---
|
||||
|
||||
## Тести
|
||||
|
||||
```bash
|
||||
# Всі тести
|
||||
python -m pytest -q
|
||||
# Всі тести (236 passed, 8 skipped)
|
||||
python -m pytest tests/ -q --ignore=tests/e2e
|
||||
|
||||
# Конкретний модуль
|
||||
python -m pytest tests/test_eskf.py -v
|
||||
python -m pytest tests/test_mavlink.py -v
|
||||
python -m pytest tests/test_accuracy.py -v
|
||||
# За категорією (taxonomy маркери)
|
||||
python -m pytest -m unit -q
|
||||
python -m pytest -m integration -q
|
||||
python -m pytest -m blackbox -q
|
||||
|
||||
# Тести прив'язані до Acceptance Criteria
|
||||
python -m pytest -m ac -q # тільки ac-marked тести
|
||||
python -m pytest -m ac --ac-dump # + таблиця покриття AC
|
||||
|
||||
# SITL (потребує ArduPilot SITL)
|
||||
docker compose -f docker-compose.sitl.yml up -d
|
||||
ARDUPILOT_SITL_HOST=localhost pytest tests/test_sitl_integration.py -v
|
||||
pytest -m sitl tests/ -v
|
||||
|
||||
# E2E пайплайн на публічних UAV-датасетах (EuRoC / VPAIR / MARS-LVIG)
|
||||
pytest tests/e2e/ -q # unit + skip-when-absent (швидко)
|
||||
pytest tests/e2e/ -m "e2e and not e2e_slow" -v # CI-tier з завантаженим датасетом
|
||||
pytest tests/e2e/ -m e2e_slow -v # nightly-tier (VPAIR sample, MARS-LVIG stress)
|
||||
|
||||
# EuRoC Machine Hall bundle — 12.6 GB, DOI 10.3929/ethz-b-000690084
|
||||
# Завантажити вручну (DSpace UI без прямого URL), розпакувати внутрішній
|
||||
# MH_0N_easy.zip у datasets/euroc/MH_0N/, щоб існував mav0/
|
||||
# SHA256 зашитий у DATASET_REGISTRY ("euroc_machine_hall") для верифікації
|
||||
|
||||
# VPAIR sample (fixed-wing, downward, 300-400 м) — form-gated на Zenodo
|
||||
# Розпакувати так, щоб datasets/vpair/sample/poses_query.txt існував
|
||||
# SHA256 зашитий у DATASET_REGISTRY ("vpair_sample") для верифікації
|
||||
|
||||
# Для автоматизованих entry (коли з'являться) — той самий CLI:
|
||||
python scripts/download_dataset.py <dataset_name>
|
||||
# E2E на публічних UAV-датасетах
|
||||
pytest tests/e2e/ -q # skip якщо датасет відсутній
|
||||
pytest tests/e2e/ -m "e2e and not e2e_slow" -v # CI-tier
|
||||
pytest tests/e2e/ -m e2e_slow -v # nightly-tier (VPAIR, MARS-LVIG)
|
||||
```
|
||||
|
||||
E2E-харнес гонить `FlightProcessor` як black-box через спільний `DatasetAdapter` (`src/gps_denied/testing/`). Датасети лежать у `./datasets/` (gitignored), тести пропускаються (не фейляться) коли датасету немає. Детально — у локальному design doc `.planning/brainstorms/2026-04-16-e2e-datasets-design.md` та плані `2026-04-16-e2e-datasets-plan.md`.
|
||||
**E2E результати (EuRoC MH_01–05, indoor):**
|
||||
|
||||
**Поточний статус реальних прогонів (2026-04-18):**
|
||||
| Датасет | Кадри | ESKF ATE RMSE | Статус |
|
||||
|---------|-------|---------------|--------|
|
||||
| EuRoC MH_01 (easy) | 100 | 0.205 м | PASS |
|
||||
| EuRoC MH_02 (easy) | 100 | 0.131 м | PASS |
|
||||
| EuRoC MH_03 (medium) | 100 | 0.008 м | PASS |
|
||||
| EuRoC MH_04 (difficult) | 100 | 0.009 м | PASS |
|
||||
| EuRoC MH_05 (difficult) | 100 | 0.007 м | PASS |
|
||||
| VPAIR sample (fixed-wing) | 200 | — | xfail (немає raw IMU) |
|
||||
|
||||
| Датасет | Кадри | ESKF ATE RMSE | GPS ATE | Статус |
|
||||
|---------|-------|---------------|---------|--------|
|
||||
| EuRoC MH_01 (easy) | 100 | **0.205 м** ✅ | xfail (indoor) | PASS |
|
||||
| EuRoC MH_02 (easy) | 100 | **0.131 м** ✅ | xfail (indoor) | PASS |
|
||||
| EuRoC MH_03 (medium) | 100 | **0.008 м** ✅ | xfail (indoor) | PASS |
|
||||
| EuRoC MH_04 (difficult) | 100 | **0.009 м** ✅ | xfail (indoor) | PASS |
|
||||
| EuRoC MH_05 (difficult) | 100 | **0.007 м** ✅ | xfail (indoor) | PASS |
|
||||
| VPAIR sample (fixed-wing, outdoor) | 200 | — | ~1770 км xfail | xfail |
|
||||
| MARS-LVIG (rotary, RTK) | — | — | — | skip (датасет відсутній) |
|
||||
|
||||
EuRoC: `vo_success=99/100`, `eskf_initialized=100/100`. GPS estimate xfail — indoor, satellite tiles не релевантні.
|
||||
VPAIR: ESKF не активний (немає raw IMU), VO без якоря розходиться. Outdoor — потенційно satellite matching допоможе.
|
||||
|
||||
### Покриття тестами (216 passed / 8 skipped — unit/component; EuRoC MH_01–05 e2e PASS)
|
||||
|
||||
| Файл тесту | Компонент | К-сть |
|
||||
|-------------|-----------|-------|
|
||||
| `test_schemas.py` | Pydantic схеми | 12 |
|
||||
| `test_database.py` | SQLAlchemy CRUD | 9 |
|
||||
| `test_api_flights.py` | REST endpoints | 5 |
|
||||
| `test_health.py` | Health check | 1 |
|
||||
| `test_eskf.py` | ESKF 15-state | 17 |
|
||||
| `test_coordinates.py` | ENU/GPS/pixel | 4 |
|
||||
| `test_satellite.py` | Тайли + Mercator | 8 |
|
||||
| `test_pipeline.py` | Image queue | 5 |
|
||||
| `test_rotation.py` | 360° ротації | 4 |
|
||||
| `test_models.py` | Model Manager + TRT | 6 |
|
||||
| `test_vo.py` | VO (ORB + cuVSLAM + Mono-Depth) | 16 |
|
||||
| `test_gpr.py` | Place Recognition + AnyLoc markers | 9 |
|
||||
| `test_metric.py` | Metric Refinement + GSD | 6 |
|
||||
| `test_graph.py` | Factor Graph (GTSAM) | 4 |
|
||||
| `test_chunk_manager.py` | Chunk lifecycle | 3 |
|
||||
| `test_recovery.py` | Recovery coordinator | 2 |
|
||||
| `test_processor_full.py` | State Machine | 4 |
|
||||
| `test_processor_pipe.py` | PIPE wiring (Phase 5) | 13 |
|
||||
| `test_mavlink.py` | MAVLink I/O bridge | 19 |
|
||||
| `test_gps_input_encoding.py` | GPS_INPUT field encoding (MAVLink #232) | 12 |
|
||||
| `test_acceptance.py` | AC сценарії + perf | 6 |
|
||||
| `test_accuracy.py` | Accuracy validation | 23 |
|
||||
| `test_sitl_integration.py` | SITL (skip без ArduPilot) | 8 |
|
||||
| | **Всього** | **216+8** |
|
||||
|
||||
---
|
||||
|
||||
## Benchmark валідації (Phase 7)
|
||||
### AC Traceability
|
||||
|
||||
```bash
|
||||
python scripts/benchmark_accuracy.py --frames 50
|
||||
# Звіт: 39 AC total — 14 covered, 21 pending-phase-3+, 4 deferred-hardware
|
||||
python scripts/gen_ac_traceability.py
|
||||
|
||||
# CI gate (виходить 0 якщо всі непокриті позначені pending/deferred)
|
||||
python scripts/gen_ac_traceability.py --check
|
||||
```
|
||||
|
||||
Результати на синтетичній траєкторії (20 м/с, 0.7 fps, шум VO 0.3 м, супутник кожні 5 кадрів):
|
||||
|
||||
| Критерій | Результат | Ліміт |
|
||||
|---------|-----------|-------|
|
||||
| 80% кадрів ≤ 50 м | ✅ 100% | ≥ 80% |
|
||||
| 60% кадрів ≤ 20 м | ✅ 100% | ≥ 60% |
|
||||
| p95 затримка | ✅ ~9 мс | < 400 мс |
|
||||
| VO дрейф за 1 км | ✅ ~11 м | < 100 м |
|
||||
Матриця: `.planning/AC-TRACEABILITY.md`
|
||||
|
||||
---
|
||||
|
||||
@@ -235,93 +190,133 @@ python scripts/benchmark_accuracy.py --frames 50
|
||||
gps-denied-onboard/
|
||||
├── src/gps_denied/
|
||||
│ ├── app.py # FastAPI factory + lifespan
|
||||
│ ├── config.py # Pydantic Settings
|
||||
│ ├── api/routers/flights.py # REST + SSE endpoints
|
||||
│ ├── core/
|
||||
│ ├── config.py # AppSettings / RuntimeConfig (pydantic-settings)
|
||||
│ ├── components/ # Hexagonal adapters (ports + impls)
|
||||
│ │ ├── vio/ # Visual Odometry
|
||||
│ │ ├── satellite_matcher/ # Tile loading + XFeat metric refinement
|
||||
│ │ ├── gpr/ # Global Place Recognition (Faiss/numpy)
|
||||
│ │ ├── mavlink_io/ # MAVLink bridge + mock
|
||||
│ │ ├── anchor_verifier/ # stub — Phase 3
|
||||
│ │ ├── safety_state/ # stub — Phase 3
|
||||
│ │ ├── flight_recorder/ # stub — Phase 4
|
||||
│ │ └── coordinate_transforms/ # stub — Phase 5
|
||||
│ ├── core/ # Concentrated math (без DI)
|
||||
│ │ ├── eskf.py # 15-state ESKF (IMU+VO+satellite fusion)
|
||||
│ │ ├── processor.py # FlightProcessor + process_frame
|
||||
│ │ ├── vo.py # ORBVisualOdometry / CuVSLAMVisualOdometry
|
||||
│ │ ├── mavlink.py # MAVLinkBridge → GPS_INPUT → ArduPilot
|
||||
│ │ ├── satellite.py # SatelliteDataManager (local z/x/y tiles)
|
||||
│ │ ├── gpr.py # GlobalPlaceRecognition (Faiss/numpy)
|
||||
│ │ ├── metric.py # MetricRefinement (LiteSAM/XFeat + GSD)
|
||||
│ │ ├── graph.py # FactorGraphOptimizer (GTSAM ISAM2)
|
||||
│ │ ├── coordinates.py # CoordinateTransformer (ENU↔GPS↔pixel)
|
||||
│ │ ├── models.py # ModelManager + TRTInferenceEngine
|
||||
│ │ ├── benchmark.py # AccuracyBenchmark + SyntheticTrajectory
|
||||
│ │ ├── pipeline.py # ImageInputPipeline
|
||||
│ │ ├── rotation.py # ImageRotationManager
|
||||
│ │ ├── factor_graph.py # FactorGraphOptimizer (GTSAM ISAM2)
|
||||
│ │ ├── coordinates.py # ENU↔GPS↔pixel transforms
|
||||
│ │ ├── chunk_manager.py # RouteChunkManager
|
||||
│ │ ├── recovery.py # FailureRecoveryCoordinator
|
||||
│ │ └── chunk_manager.py # RouteChunkManager
|
||||
│ ├── schemas/ # Pydantic схеми (eskf, mavlink, vo, ...)
|
||||
│ ├── db/ # SQLAlchemy ORM + async repository
|
||||
│ └── utils/mercator.py # Web Mercator tile utilities
|
||||
├── tests/ # 22 test модулі
|
||||
│ │ ├── rotation.py # ImageRotationManager
|
||||
│ │ └── models.py # ModelManager + TRTInferenceEngine
|
||||
│ ├── hot_types/ # @dataclass(slots=True, frozen=True) — hot path
|
||||
│ ├── obs/ # Observability (Phase 2)
|
||||
│ │ ├── logging_config.py # configure_logging(env) — JSON/console
|
||||
│ │ └── log_schemas.py # Pydantic v2 boundary log event schemas
|
||||
│ ├── pipeline/ # Orchestration layer
|
||||
│ │ ├── orchestrator.py # FlightProcessor + process_frame
|
||||
│ │ ├── composition.py # build_pipeline(env) — composition root
|
||||
│ │ ├── image_input.py
|
||||
│ │ ├── result_manager.py
|
||||
│ │ └── sse_streamer.py
|
||||
│ ├── api/routers/flights.py # REST endpoints + SSE
|
||||
│ ├── schemas/ # Pydantic REST schemas + shims до hot_types
|
||||
│ └── db/ # SQLAlchemy ORM + async repository
|
||||
├── tests/ # 37 test-модулів з pytestmark (236 passed)
|
||||
│ └── e2e/ # E2E на публічних UAV-датасетах
|
||||
├── config/ # Per-env YAML overlays
|
||||
│ ├── jetson.yaml
|
||||
│ ├── x86_dev.yaml
|
||||
│ ├── ci.yaml
|
||||
│ └── sitl.yaml
|
||||
├── scripts/
|
||||
│ └── benchmark_accuracy.py # CLI валідація точності
|
||||
├── Dockerfile # Multi-stage Python 3.11 image
|
||||
├── docker-compose.yml # Local dev
|
||||
├── docker-compose.sitl.yml # ArduPilot SITL harness
|
||||
├── .github/workflows/
|
||||
│ ├── ci.yml # lint + pytest + docker smoke (кожен push)
|
||||
│ └── sitl.yml # SITL integration (нічний / ручний)
|
||||
└── pyproject.toml
|
||||
│ ├── gen_ac_traceability.py # AC coverage report + CI gate
|
||||
│ └── benchmark_accuracy.py # Synthetic trajectory accuracy CLI
|
||||
├── _docs/
|
||||
│ ├── 00_problem/
|
||||
│ │ └── acceptance_criteria.md # 39 AC (AC-1.1..AC-8.6 + AC-NEW-1..8)
|
||||
│ └── 01_solution/decisions/ # ADRs 0001–0004
|
||||
├── Dockerfile
|
||||
├── docker-compose.yml
|
||||
├── docker-compose.sitl.yml
|
||||
└── .github/workflows/
|
||||
├── ci.yml # lint → {unit, integration, blackbox, ac-gate} → docker
|
||||
├── nightly.yml # sitl + e2e slow (cron 03:00 UTC)
|
||||
└── sitl.yml # SITL integration
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Компоненти
|
||||
## Компоненти (Stage 2)
|
||||
|
||||
| ID | Назва | Файл | Dev | Jetson |
|
||||
|----|-------|------|-----|--------|
|
||||
| F04 | Satellite Data Manager | `core/satellite.py` | local tiles | local tiles |
|
||||
| F05 | Image Input Pipeline | `core/pipeline.py` | ✅ | ✅ |
|
||||
| F06 | Image Rotation Manager | `core/rotation.py` | ✅ | ✅ |
|
||||
| F07 | Sequential Visual Odometry | `core/vo.py` | ORB / Mono-Depth (scaled ORB) | cuVSLAM Mono-Depth |
|
||||
| F08 | Global Place Recognition | `core/gpr.py` | numpy | Faiss GPU |
|
||||
| F09 | Metric Refinement | `core/metric.py` | Mock | LiteSAM/XFeat TRT |
|
||||
| F10 | Factor Graph Optimizer | `core/graph.py` | Mock | GTSAM ISAM2 |
|
||||
| F11 | Failure Recovery | `core/recovery.py` | ✅ | ✅ |
|
||||
| F12 | Route Chunk Manager | `core/chunk_manager.py` | ✅ | ✅ |
|
||||
| F13 | Coordinate Transformer | `core/coordinates.py` | ✅ | ✅ |
|
||||
| F16 | Model Manager | `core/models.py` | Mock | TRT engines |
|
||||
| F17 | ESKF Sensor Fusion | `core/eskf.py` | ✅ | ✅ |
|
||||
| F18 | MAVLink I/O Bridge | `core/mavlink.py` | Mock | pymavlink |
|
||||
| Компонент | Protocol | Dev adapter | Jetson adapter |
|
||||
|-----------|----------|-------------|----------------|
|
||||
| Visual Odometry | `vio/protocol.py` | `ORBVisualOdometry` | `CuVSLAMMonoDepthVO` |
|
||||
| Satellite Matcher | `satellite_matcher/protocol.py` | `LocalTileLoader` + `MetricRefinement` | те саме |
|
||||
| Place Recognition | `gpr/protocol.py` | `FaissGPR` (numpy fallback) | `FaissGPR` (GPU) |
|
||||
| MAVLink I/O | `mavlink_io/protocol.py` | `MockMAVConnection` | `MAVLinkBridge` (pymavlink) |
|
||||
| Anchor Verifier | `anchor_verifier/protocol.py` | stub | stub (Phase 3) |
|
||||
| Safety State | `safety_state/protocol.py` | stub | stub (Phase 3) |
|
||||
| Flight Recorder | `flight_recorder/protocol.py` | stub | stub (Phase 4) |
|
||||
| Coord. Transforms | `coordinate_transforms/protocol.py` | stub | stub (Phase 5) |
|
||||
|
||||
---
|
||||
|
||||
## Що залишилось (наступні кроки)
|
||||
## Acceptance Criteria
|
||||
|
||||
### Sprint 1 — виконано (2026-04-18)
|
||||
39 AC з ідентифікаторами `AC-N.M` / `AC-NEW-N` у `_docs/00_problem/acceptance_criteria.md`.
|
||||
|
||||
Див. план `docs/superpowers/plans/2026-04-18-sprint1-vo-migration.md` і design doc `docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md`.
|
||||
Ключові порогові значення:
|
||||
|
||||
- [x] **numpy pin** `>=1.26,<2.0` — NumPy 2.0 silently breaks GTSAM bindings (issue #2264)
|
||||
- [x] **CuVSLAMMonoDepthVisualOdometry** додано поряд з Inertial-варіантом. Dev/CI: ORB translation scaled by `depth_hint_m / 600.0`. Jetson: cuVSLAM Mono-Depth mode з barometric altitude як synthetic depth.
|
||||
- [x] **GlobalPlaceRecognition** явно маркований як AnyLoc-VLAD-DINOv2 baseline з selection rationale в docstring
|
||||
- [x] **GPS_INPUT encoding** покритий 12 unit-тестами проти `_eskf_to_gps_input` (degE7 lat/lon, ENU→NED velocity, ConfidenceTier → fix_type)
|
||||
- [x] **E2E regression guard** на EuRoC MH_01 для Mono-Depth (ATE 0.2046м, baseline незмінний)
|
||||
| Критерій | Поріг |
|
||||
|----------|-------|
|
||||
| Точність 80% кадрів | ≤ 50 м |
|
||||
| Точність 60% кадрів | ≤ 20 м |
|
||||
| End-to-end latency | < 400 мс |
|
||||
| Пам'ять (shared CPU/GPU) | < 8 GB |
|
||||
| Сторадж місії | < 64 GB |
|
||||
| GSD супутникового знімку | ≤ 0.5 м/px |
|
||||
| Час до першої позиції | ≤ 30 с |
|
||||
|
||||
### Sprint 2 — далі (захищений e2e-харнесом)
|
||||
---
|
||||
|
||||
1. **Wire CuVSLAMMonoDepthVisualOdometry через E2EHarness** — harness зараз хардкодить `ORBVisualOdometry()`; додати `vo_backend` параметр щоб прогнати Mono-Depth через pipeline
|
||||
2. **Колапс дуплікатного коду з `CuVSLAMVisualOdometry`** (Inertial варіант) — видалити Inertial mode після Jetson validation
|
||||
3. **VPAIR unblock** — xfail (1770 км ATE) блокований відсутністю raw IMU + mock satellite index. Реальні MapTiler tiles + Mono-Depth з GT-altitude розблокують.
|
||||
4. **DINOv2-VLAD TRT FP16 engine** — реальний descriptor замість numpy L2 fallback. Satellite tiles через MapTiler MBTiles (offline).
|
||||
5. **aero-vloc benchmark** на наших nadir кадрах — підтвердити R@1 DINOv2-VLAD перед фіксацією Faiss index design
|
||||
6. **Аудит solution.md** — звірити `_docs/01_solution/solution.md` з реальним кодом (Inertial → Mono-Depth)
|
||||
7. **Реструктуризація** — `src/gps_denied/*` → `src/*`
|
||||
## Архітектурні рішення (ADRs)
|
||||
|
||||
### Критичні блокери (перевірити до наступного коду)
|
||||
- **Flight Controller processor**: H743 ✅ / F405 ❌ (silently ignores GPS_INPUT). Запитати постачальника.
|
||||
- **IMU rate через MAVLink**: за замовчуванням ArduPilot 50 Hz, для Mono-Inertial потрібно ≥100 Hz (`SR2_RAW_SENS`). Для Mono-Depth не критично.
|
||||
| ADR | Рішення |
|
||||
|-----|---------|
|
||||
| [0001](_docs/01_solution/decisions/0001-e2e-dataset-strategy.md) | E2E dataset strategy (EuRoC / VPAIR / MARS-LVIG) |
|
||||
| [0002](_docs/01_solution/decisions/0002-hexagonal-architecture-stage2.md) | Hexagonal / ports-and-adapters для Stage 2 |
|
||||
| [0003](_docs/01_solution/decisions/0003-hot-path-dataclasses-vs-pydantic.md) | `@dataclass(slots=True, frozen=True)` на hot path; Pydantic лише на межах |
|
||||
| [0004](_docs/01_solution/decisions/0004-stage2-as-independent-iteration.md) | Stage 2 як незалежна ітерація зі своїми фазами 1–6 |
|
||||
|
||||
### On-device (Jetson Orin Nano Super)
|
||||
1. Офлайн завантаження тайлів для зони місії → `{tile_dir}/z/x/y.png`
|
||||
2. Конвертація моделей: LiteSAM/XFeat PyTorch → ONNX → TRT FP16
|
||||
3. Запуск SITL: `docker compose -f docker-compose.sitl.yml up`
|
||||
4. Польотні дані: записати GPS + відео → порівняти ESKF-траєкторію з ground truth
|
||||
5. Калібрування: camera intrinsics + IMU noise density для конкретного апарату
|
||||
---
|
||||
|
||||
## Roadmap Stage 2
|
||||
|
||||
| Фаза | Назва | Статус |
|
||||
|------|-------|--------|
|
||||
| 1 | Hexagonal Refactor | done (2026-05-11) |
|
||||
| 2 | AC Doc + Test Taxonomy + Observability Spine | done (2026-05-11) |
|
||||
| 3 | Safety Anchor State Machine + Geometry-Gated Anchor Verifier | planned |
|
||||
| 4 | Conditional VPR + Flight Data Recorder | planned |
|
||||
| 5 | MAVLink source labels + dual-channel scaffold | planned |
|
||||
| 6 | Azaion 10.05.2026 real-flight integration fixture | planned |
|
||||
|
||||
---
|
||||
|
||||
## Benchmark
|
||||
|
||||
```bash
|
||||
python scripts/benchmark_accuracy.py --frames 50
|
||||
```
|
||||
|
||||
Результати на синтетичній траєкторії (20 м/с, 0.7 fps, шум VO 0.3 м, супутник кожні 5 кадрів):
|
||||
|
||||
| Критерій | Результат | Ліміт |
|
||||
|---------|-----------|-------|
|
||||
| 80% кадрів ≤ 50 м | 100% | ≥ 80% |
|
||||
| 60% кадрів ≤ 20 м | 100% | ≥ 60% |
|
||||
| p95 latency | ~9 мс | < 400 мс |
|
||||
| VO дрейф за 1 км | ~11 м | < 100 м |
|
||||
|
||||
---
|
||||
|
||||
|
||||
Reference in New Issue
Block a user