mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 02:06:36 +00:00
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:
@@ -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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user