From c1b8e5937ee6f0ce21f73744086886eeff7fc7fa Mon Sep 17 00:00:00 2001 From: Yuzviak Date: Sat, 18 Apr 2026 14:49:43 +0300 Subject: [PATCH] feat(harness): init ESKF from adapter's first GT pose as synthetic GPS origin MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Wires a real CoordinateTransformer into the processor and seeds the ESKF with the dataset's first ground-truth lat/lon/alt before the frame loop. Result on EuRoC MH_01 (100 frames): eskf_initialized: 0/100 → 100/100 vo_success: 99/100 (unchanged) eskf_has_position: 100/100 Satellite measurements are now correctly rejected by the Mahalanobis gate (Δ² ~10⁶) because ORB produces unit-scale translations (scale_ambiguous=True) which drive the ESKF position to diverge rapidly. The gate is working as intended — the remaining issue is VO metric scale, not ESKF initialisation. Co-Authored-By: Claude Opus 4.7 (1M context) --- src/gps_denied/testing/harness.py | 14 ++++++++++++++ tests/e2e/test_euroc.py | 13 ++++++++----- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/src/gps_denied/testing/harness.py b/src/gps_denied/testing/harness.py index b006664..93cfd05 100644 --- a/src/gps_denied/testing/harness.py +++ b/src/gps_denied/testing/harness.py @@ -23,6 +23,7 @@ import cv2 import numpy as np from gps_denied.core.chunk_manager import RouteChunkManager +from gps_denied.core.coordinates import CoordinateTransformer from gps_denied.core.gpr import GlobalPlaceRecognition from gps_denied.core.graph import FactorGraphOptimizer from gps_denied.core.metric import MetricRefinement @@ -30,6 +31,7 @@ from gps_denied.core.models import ModelManager from gps_denied.core.processor import FlightProcessor from gps_denied.core.recovery import FailureRecoveryCoordinator from gps_denied.core.vo import ORBVisualOdometry +from gps_denied.schemas import GPSPoint from gps_denied.schemas.graph import FactorGraphConfig from gps_denied.testing.datasets.base import ( DatasetAdapter, @@ -88,6 +90,16 @@ class E2EHarness: for i, pose in enumerate(gt_poses): gt_by_idx[i] = pose + # Seed ESKF with first GT pose as synthetic GPS origin so the processor + # can propagate VO translations into a consistent ENU frame. + if gt_poses: + origin = gt_poses[0] + start_gps = GPSPoint(lat=origin.lat, lon=origin.lon) + processor._coord.set_enu_origin(self._flight_id, start_gps) # noqa: SLF001 + processor._init_eskf_for_flight( # noqa: SLF001 + self._flight_id, start_gps, altitude=origin.alt + ) + trace_fh = None if self._trace_path is not None: self._trace_path.parent.mkdir(parents=True, exist_ok=True) @@ -182,9 +194,11 @@ class E2EHarness: graph = FactorGraphOptimizer(FactorGraphConfig()) chunk_mgr = RouteChunkManager(graph) recovery = FailureRecoveryCoordinator(chunk_mgr, gpr, metric) + coord = CoordinateTransformer() proc.attach_components( vo=vo, gpr=gpr, metric=metric, graph=graph, recovery=recovery, chunk_mgr=chunk_mgr, + coord=coord, ) return proc diff --git a/tests/e2e/test_euroc.py b/tests/e2e/test_euroc.py index dc9f721..66753bb 100644 --- a/tests/e2e/test_euroc.py +++ b/tests/e2e/test_euroc.py @@ -38,9 +38,11 @@ async def test_euroc_mh01_rmse_within_ceiling(euroc_mh01_root: Path): result = await harness.run() if result.estimated_positions_enu.shape[0] == 0: pytest.xfail( - "Pipeline emits GPS estimates via fallback satellite matching but ESKF never " - "initialises (no start_gps call in harness). VO now engages at 99% with ORB. " - "Next: wire ESKF init with a synthetic GPS origin in the harness." + "Pipeline emits zero GPS estimates — ESKF+VO active (99/100 vo_success, " + "100/100 eskf_initialized) but satellite outlier rejection blocks all fixes. " + "Root cause: ORB scale_ambiguous=True → unit-scale VO translation → ESKF " + "position diverges → Mahalanobis gate rejects satellite measurements (~10^6 >> 16.3). " + "Next: apply VO scale from ESKF velocity or switch to metric VO backend." ) # Align lengths by truncating to shorter (estimates may lag GT at start) n = min(result.estimated_positions_enu.shape[0], result.ground_truth.shape[0]) @@ -51,7 +53,8 @@ async def test_euroc_mh01_rmse_within_ceiling(euroc_mh01_root: Path): if ate["rmse"] >= EUROC_MH01_RMSE_CEILING_M: pytest.xfail( f"ATE RMSE={ate['rmse']:.2f}m exceeds {EUROC_MH01_RMSE_CEILING_M}m ceiling. " - "VO engages (99%) but ESKF never initialises without a start_gps call in the " - "harness — estimates come from satellite fallback only." + "VO+ESKF both active but ORB translation is unit-scale (scale_ambiguous=True) → " + "ESKF position diverges rapidly → satellite Mahalanobis gate rejects all fixes. " + "Fix: recover metric scale from ESKF velocity or use cuVSLAM (metric VO)." ) assert ate["rmse"] < EUROC_MH01_RMSE_CEILING_M, f"ATE RMSE={ate['rmse']:.2f}m"