feat(harness): init ESKF from adapter's first GT pose as synthetic GPS origin

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) <noreply@anthropic.com>
This commit is contained in:
Yuzviak
2026-04-18 14:49:43 +03:00
committed by Maksym Yuzviak
parent 2ccd7be6fb
commit c1b8e5937e
2 changed files with 22 additions and 5 deletions
+14
View File
@@ -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