azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 11:21:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

refactor(01-07): factor_graph, pipeline pkg, testing/benchmark, Protocol ABCs

Browse Source 
- Create core/factor_graph.py: IFactorGraphOptimizer converted to Protocol
- Shim core/graph.py to re-export from core/factor_graph
- Create pipeline/ package: orchestrator, image_input, result_manager, sse_streamer
- Shim core/{processor,pipeline,results,sse}.py to re-export from pipeline/
- Create testing/benchmark.py; shim core/benchmark.py
- Convert IRouteChunkManager, IFailureRecoveryCoordinator, IModelManager, IImageMatcher to Protocol
- Update pyproject.toml ruff per-file-ignores to new paths
- All 216 tests pass (regression floor maintained)
This commit is contained in:
Yuzviak
2026-05-11 08:59:07 +03:00
parent 275c7b4642
commit 5a60c1ee2c
18 changed files with 1857 additions and 1836 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied/core/graph.py
+5 -364
View File
@@ -1,364 +1,5 @@
"""Factor Graph Optimizer (Component F10)."""
import logging
from abc import ABC, abstractmethod
from datetime import datetime, timezone
from typing import Dict
import numpy as np
try:
import gtsam
HAS_GTSAM = True
except ImportError:
HAS_GTSAM = False
from gps_denied.schemas import GPSPoint
from gps_denied.schemas.graph import FactorGraphConfig, OptimizationResult, Pose
from gps_denied.schemas.metric import Sim3Transform
from gps_denied.schemas.vo import RelativePose
logger = logging.getLogger(__name__)
class IFactorGraphOptimizer(ABC):
"""GTSAM-based factor graph optimizer."""
@abstractmethod
def add_relative_factor(self, flight_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
pass
@abstractmethod
def add_absolute_factor(self, flight_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray, is_user_anchor: bool) -> bool:
pass
@abstractmethod
def add_altitude_prior(self, flight_id: str, frame_id: int, altitude: float, covariance: float) -> bool:
pass
@abstractmethod
def optimize(self, flight_id: str, iterations: int) -> OptimizationResult:
pass
@abstractmethod
def get_trajectory(self, flight_id: str) -> Dict[int, Pose]:
pass
@abstractmethod
def get_marginal_covariance(self, flight_id: str, frame_id: int) -> np.ndarray:
pass
@abstractmethod
def create_chunk_subgraph(self, flight_id: str, chunk_id: str, start_frame_id: int) -> bool:
pass
@abstractmethod
def add_relative_factor_to_chunk(self, flight_id: str, chunk_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
pass
@abstractmethod
def add_chunk_anchor(self, flight_id: str, chunk_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray) -> bool:
pass
@abstractmethod
def merge_chunk_subgraphs(self, flight_id: str, new_chunk_id: str, main_chunk_id: str, transform: Sim3Transform) -> bool:
pass
@abstractmethod
def get_chunk_trajectory(self, flight_id: str, chunk_id: str) -> Dict[int, Pose]:
pass
@abstractmethod
def optimize_chunk(self, flight_id: str, chunk_id: str, iterations: int) -> OptimizationResult:
pass
@abstractmethod
def optimize_global(self, flight_id: str, iterations: int) -> OptimizationResult:
pass
@abstractmethod
def delete_flight_graph(self, flight_id: str) -> bool:
pass
class FactorGraphOptimizer(IFactorGraphOptimizer):
"""Implementation of F10 Factor Graph using GTSAM or Mock."""
def __init__(self, config: FactorGraphConfig):
self.config = config
# Keyed by flight_id
self._flights_state: Dict[str, dict] = {}
# Keyed by chunk_id
self._chunks_state: Dict[str, dict] = {}
# Per-flight ENU origin (set from first absolute GPS factor)
self._enu_origins: Dict[str, GPSPoint] = {}
def _init_flight(self, flight_id: str):
if flight_id not in self._flights_state:
self._flights_state[flight_id] = {
"graph": gtsam.NonlinearFactorGraph() if HAS_GTSAM else None,
"initial": gtsam.Values() if HAS_GTSAM else None,
"isam": gtsam.ISAM2() if HAS_GTSAM else None,
"poses": {},
"dirty": False
}
def _init_chunk(self, chunk_id: str):
if chunk_id not in self._chunks_state:
self._chunks_state[chunk_id] = {
"graph": gtsam.NonlinearFactorGraph() if HAS_GTSAM else None,
"initial": gtsam.Values() if HAS_GTSAM else None,
"isam": gtsam.ISAM2() if HAS_GTSAM else None,
"poses": {},
"dirty": False
}
# ================== MOCK IMPLEMENTATION ====================
# As GTSAM Python bindings can be extremely context-dependent and
# require proper ENU translation logic, we use an advanced Mock
# that satisfies the architectural design and typing for the backend.
def add_relative_factor(self, flight_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
self._init_flight(flight_id)
state = self._flights_state[flight_id]
# --- Mock: propagate position chain ---
if frame_i in state["poses"]:
prev_pose = state["poses"][frame_i]
new_pos = prev_pose.position + relative_pose.translation
state["poses"][frame_j] = Pose(
frame_id=frame_j,
position=new_pos,
orientation=np.eye(3),
timestamp=datetime.now(timezone.utc),
covariance=np.eye(6),
)
state["dirty"] = True
else:
return False
# --- GTSAM: add BetweenFactorPose3 ---
if HAS_GTSAM and state["graph"] is not None:
try:
cov6 = covariance if covariance.shape == (6, 6) else np.eye(6)
noise = gtsam.noiseModel.Gaussian.Covariance(cov6)
key_i = gtsam.symbol("x", frame_i)
key_j = gtsam.symbol("x", frame_j)
t = relative_pose.translation
between = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(float(t[0]), float(t[1]), float(t[2])))
state["graph"].add(gtsam.BetweenFactorPose3(key_i, key_j, between, noise))
if not state["initial"].exists(key_j):
if state["initial"].exists(key_i):
prev = state["initial"].atPose3(key_i)
pt = prev.translation()
new_t = gtsam.Point3(pt[0] + t[0], pt[1] + t[1], pt[2] + t[2])
else:
new_t = gtsam.Point3(float(t[0]), float(t[1]), float(t[2]))
state["initial"].insert(key_j, gtsam.Pose3(gtsam.Rot3(), new_t))
except Exception as exc:
logger.debug("GTSAM add_relative_factor failed: %s", exc)
return True
def _gps_to_enu(self, flight_id: str, gps: GPSPoint) -> np.ndarray:
"""Convert GPS to local ENU using per-flight origin."""
origin = self._enu_origins.get(flight_id)
if origin is None:
# First GPS factor sets the origin
self._enu_origins[flight_id] = gps
return np.zeros(3)
enu_x = (gps.lon - origin.lon) * 111000 * np.cos(np.radians(origin.lat))
enu_y = (gps.lat - origin.lat) * 111000
return np.array([enu_x, enu_y, 0.0])
def add_absolute_factor(self, flight_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray, is_user_anchor: bool) -> bool:
self._init_flight(flight_id)
state = self._flights_state[flight_id]
enu = self._gps_to_enu(flight_id, gps)
# --- Mock: update pose position ---
if frame_id in state["poses"]:
state["poses"][frame_id].position = enu
state["dirty"] = True
else:
return False
# --- GTSAM: add PriorFactorPose3 ---
if HAS_GTSAM and state["graph"] is not None:
try:
cov6 = covariance if covariance.shape == (6, 6) else np.eye(6)
noise = gtsam.noiseModel.Gaussian.Covariance(cov6)
key = gtsam.symbol("x", frame_id)
prior = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(float(enu[0]), float(enu[1]), float(enu[2])))
state["graph"].add(gtsam.PriorFactorPose3(key, prior, noise))
if not state["initial"].exists(key):
state["initial"].insert(key, prior)
except Exception as exc:
logger.debug("GTSAM add_absolute_factor failed: %s", exc)
return True
def add_altitude_prior(self, flight_id: str, frame_id: int, altitude: float, covariance: float) -> bool:
self._init_flight(flight_id)
state = self._flights_state[flight_id]
if frame_id in state["poses"]:
state["poses"][frame_id].position = np.array([
state["poses"][frame_id].position[0],
state["poses"][frame_id].position[1],
altitude,
])
state["dirty"] = True
return True
return False
def optimize(self, flight_id: str, iterations: int) -> OptimizationResult:
self._init_flight(flight_id)
state = self._flights_state[flight_id]
# --- PIPE-03: Real GTSAM ISAM2 update when available ---
if HAS_GTSAM and state["dirty"] and state["graph"] is not None:
try:
state["isam"].update(state["graph"], state["initial"])
estimate = state["isam"].calculateEstimate()
for fid in list(state["poses"].keys()):
key = gtsam.symbol("x", fid)
if estimate.exists(key):
pose = estimate.atPose3(key)
t = pose.translation()
state["poses"][fid].position = np.array([t[0], t[1], t[2]])
state["poses"][fid].orientation = np.array(pose.rotation().matrix())
# Reset for next incremental batch
state["graph"] = gtsam.NonlinearFactorGraph()
state["initial"] = gtsam.Values()
except Exception as exc:
logger.warning("GTSAM ISAM2 update failed: %s", exc)
state["dirty"] = False
return OptimizationResult(
converged=True,
final_error=0.1,
iterations_used=iterations,
optimized_frames=list(state["poses"].keys()),
mean_reprojection_error=0.5,
)
def get_trajectory(self, flight_id: str) -> Dict[int, Pose]:
if flight_id not in self._flights_state:
return {}
return self._flights_state[flight_id]["poses"]
def get_marginal_covariance(self, flight_id: str, frame_id: int) -> np.ndarray:
return np.eye(6)
# ================== CHUNK OPERATIONS =======================
def create_chunk_subgraph(self, flight_id: str, chunk_id: str, start_frame_id: int) -> bool:
self._init_chunk(chunk_id)
state = self._chunks_state[chunk_id]
state["poses"][start_frame_id] = Pose(
frame_id=start_frame_id,
position=np.zeros(3),
orientation=np.eye(3),
timestamp=datetime.now(timezone.utc),
covariance=np.eye(6)
)
return True
def add_relative_factor_to_chunk(self, flight_id: str, chunk_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
if chunk_id not in self._chunks_state:
return False
state = self._chunks_state[chunk_id]
if frame_i in state["poses"]:
prev_pose = state["poses"][frame_i]
new_pos = prev_pose.position + relative_pose.translation
state["poses"][frame_j] = Pose(
frame_id=frame_j,
position=new_pos,
orientation=np.eye(3),
timestamp=datetime.now(timezone.utc),
covariance=np.eye(6)
)
state["dirty"] = True
return True
return False
def add_chunk_anchor(self, flight_id: str, chunk_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray) -> bool:
if chunk_id not in self._chunks_state:
return False
state = self._chunks_state[chunk_id]
if frame_id in state["poses"]:
enu = self._gps_to_enu(flight_id, gps)
state["poses"][frame_id].position = enu
state["dirty"] = True
return True
return False
def merge_chunk_subgraphs(self, flight_id: str, new_chunk_id: str, main_chunk_id: str, transform: Sim3Transform) -> bool:
if new_chunk_id not in self._chunks_state or main_chunk_id not in self._chunks_state:
return False
new_state = self._chunks_state[new_chunk_id]
main_state = self._chunks_state[main_chunk_id]
# Apply Sim(3) transform effectively by copying poses
for f_id, p in new_state["poses"].items():
# mock sim3 transform
idx_pos = (transform.scale * (transform.rotation @ p.position)) + transform.translation
main_state["poses"][f_id] = Pose(
frame_id=f_id,
position=idx_pos,
orientation=np.eye(3),
timestamp=p.timestamp,
covariance=p.covariance
)
return True
def get_chunk_trajectory(self, flight_id: str, chunk_id: str) -> Dict[int, Pose]:
if chunk_id not in self._chunks_state:
return {}
return self._chunks_state[chunk_id]["poses"]
def optimize_chunk(self, flight_id: str, chunk_id: str, iterations: int) -> OptimizationResult:
if chunk_id not in self._chunks_state:
return OptimizationResult(converged=False, final_error=99.0, iterations_used=0, optimized_frames=[], mean_reprojection_error=99.0)
state = self._chunks_state[chunk_id]
state["dirty"] = False
return OptimizationResult(
converged=True,
final_error=0.1,
iterations_used=iterations,
optimized_frames=list(state["poses"].keys()),
mean_reprojection_error=0.5
)
def optimize_global(self, flight_id: str, iterations: int) -> OptimizationResult:
# Optimizes everything
self._init_flight(flight_id)
state = self._flights_state[flight_id]
state["dirty"] = False
return OptimizationResult(
converged=True,
final_error=0.1,
iterations_used=iterations,
optimized_frames=list(state["poses"].keys()),
mean_reprojection_error=0.5
)
def delete_flight_graph(self, flight_id: str) -> bool:
removed = False
if flight_id in self._flights_state:
del self._flights_state[flight_id]
removed = True
self._enu_origins.pop(flight_id, None)
return removed
"""Legacy import path. Phase 1 shim — code lives in core/factor_graph.py."""
from gps_denied.core.factor_graph import ( # noqa: F401
IFactorGraphOptimizer,
FactorGraphOptimizer,
)