gps-denied-onboard/components/factor_graph_optimizer/base.py

from abc import ABC, abstractmethod
from typing import Optional
import numpy as np

from models.core import Pose, GPSPoint
from models.results import OptimizationResult, RefinedFrameResult
from models.processing import RelativePose


class FactorGraphOptimizerBase(ABC):
    @abstractmethod
    async def initialize_graph(self, flight_id: str) -> bool:
        pass

    @abstractmethod
    async def add_odometry_factor(
        self,
        flight_id: str,
        from_frame: int,
        to_frame: int,
        relative_pose: RelativePose,
    ) -> bool:
        pass

    @abstractmethod
    async def add_gps_prior(
        self,
        flight_id: str,
        frame_id: int,
        gps: GPSPoint,
        covariance: np.ndarray,
    ) -> bool:
        pass

    @abstractmethod
    async def optimize(
        self, flight_id: str, max_iterations: int = 100
    ) -> OptimizationResult:
        pass

    @abstractmethod
    async def get_optimized_poses(
        self, flight_id: str
    ) -> list[RefinedFrameResult]:
        pass

    @abstractmethod
    async def get_pose(
        self, flight_id: str, frame_id: int
    ) -> Optional[Pose]:
        pass

    @abstractmethod
    async def marginalize_old_poses(
        self, flight_id: str, keep_last_n: int
    ) -> bool:
        pass