mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 09:06: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>
119 lines
4.0 KiB
Python
119 lines
4.0 KiB
Python
"""Tests for Factor Graph Optimizer (F10)."""
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from gps_denied.core.graph import FactorGraphOptimizer
|
|
from gps_denied.schemas import GPSPoint
|
|
from gps_denied.schemas.graph import FactorGraphConfig
|
|
from gps_denied.schemas.vo import RelativePose
|
|
from gps_denied.schemas.metric import Sim3Transform
|
|
|
|
|
|
@pytest.fixture
|
|
def optimizer():
|
|
config = FactorGraphConfig()
|
|
return FactorGraphOptimizer(config)
|
|
|
|
|
|
def test_add_relative_factor(optimizer):
|
|
flight_id = "test_f_1"
|
|
|
|
# Needs initial node, using chunk creation wrapper to mock initial
|
|
optimizer.create_chunk_subgraph(flight_id, "ch_1", start_frame_id=0)
|
|
|
|
# Mock relative pose logic: in mock graph logic it's added via "add_relative_factor_to_chunk"
|
|
# OR we need an initial root via add_absolute/relative. Our mock for `add_relative` is simple.
|
|
|
|
rel1 = RelativePose(
|
|
translation=np.array([1.0, 0.0, 0.0]),
|
|
rotation=np.eye(3),
|
|
covariance=np.eye(6),
|
|
confidence=0.9,
|
|
inlier_count=100,
|
|
total_matches=120,
|
|
tracking_good=True
|
|
)
|
|
|
|
# Test global logic requires frame exist, let's inject a zero frame
|
|
optimizer._init_flight(flight_id)
|
|
optimizer._flights_state[flight_id]["poses"][0] = optimizer._chunks_state["ch_1"]["poses"][0]
|
|
|
|
res = optimizer.add_relative_factor(flight_id, 0, 1, rel1, np.eye(6))
|
|
assert res is True
|
|
|
|
traj = optimizer.get_trajectory(flight_id)
|
|
assert 1 in traj
|
|
assert np.allclose(traj[1].position, [1.0, 0.0, 0.0])
|
|
|
|
def test_add_absolute_factor(optimizer):
|
|
flight_id = "test_f_2"
|
|
optimizer._init_flight(flight_id)
|
|
|
|
# Inject frame 0 and frame 1
|
|
from gps_denied.schemas.graph import Pose
|
|
from datetime import datetime
|
|
optimizer._flights_state[flight_id]["poses"][0] = Pose(
|
|
frame_id=0, position=np.zeros(3), orientation=np.eye(3), timestamp=datetime.now()
|
|
)
|
|
optimizer._flights_state[flight_id]["poses"][1] = Pose(
|
|
frame_id=1, position=np.zeros(3), orientation=np.eye(3), timestamp=datetime.now()
|
|
)
|
|
|
|
# First GPS sets the ENU origin → position becomes [0,0,0]
|
|
origin_gps = GPSPoint(lat=49.0, lon=30.0)
|
|
optimizer.add_absolute_factor(flight_id, 0, origin_gps, np.eye(2), is_user_anchor=True)
|
|
assert np.allclose(optimizer.get_trajectory(flight_id)[0].position, [0, 0, 0])
|
|
|
|
# Second GPS at different coords → should produce non-zero ENU displacement
|
|
gps2 = GPSPoint(lat=49.1, lon=30.1)
|
|
res = optimizer.add_absolute_factor(flight_id, 1, gps2, np.eye(2), is_user_anchor=True)
|
|
assert res is True
|
|
|
|
traj = optimizer.get_trajectory(flight_id)
|
|
assert traj[1].position[1] > 1000 # ~11 km north
|
|
|
|
def test_optimize_and_retrieve(optimizer):
|
|
flight_id = "test_f_3"
|
|
optimizer._init_flight(flight_id)
|
|
|
|
res = optimizer.optimize(flight_id, 5)
|
|
assert res.converged is True
|
|
assert res.iterations_used == 5
|
|
|
|
traj = optimizer.get_trajectory(flight_id)
|
|
assert isinstance(traj, dict)
|
|
|
|
def test_chunk_merging(optimizer):
|
|
flight_id = "test_f_4"
|
|
c1 = "chunk1"
|
|
c2 = "chunk2"
|
|
|
|
assert optimizer.create_chunk_subgraph(flight_id, c1, 0) is True
|
|
assert optimizer.create_chunk_subgraph(flight_id, c2, 10) is True
|
|
|
|
rel = RelativePose(
|
|
translation=np.array([5.0, 0, 0]),
|
|
rotation=np.eye(3),
|
|
covariance=np.eye(6),
|
|
confidence=0.9,
|
|
inlier_count=100,
|
|
total_matches=120,
|
|
tracking_good=True
|
|
)
|
|
optimizer.add_relative_factor_to_chunk(flight_id, c2, 10, 11, rel, np.eye(6))
|
|
|
|
sim3 = Sim3Transform(translation=np.array([100.0, 0, 0]), rotation=np.eye(3), scale=1.0)
|
|
res = optimizer.merge_chunk_subgraphs(flight_id, c2, c1, sim3)
|
|
|
|
assert res is True
|
|
|
|
c1_poses = optimizer.get_chunk_trajectory(flight_id, c1)
|
|
# 10 and 11 should now be in c1, transformed!
|
|
assert 10 in c1_poses
|
|
assert 11 in c1_poses
|
|
|
|
# 100 translation
|
|
assert c1_poses[10].position[0] == 100.0
|
|
assert c1_poses[11].position[0] == 105.0
|