feat: stage9 — Factor Graph and Chunks

src/gps_denied/schemas/chunk.py

@@ -0,0 +1,32 @@
+"""Schemas for Route Chunk Manager (F12)."""
+
+from enum import Enum
+from typing import List, Optional
+
+from pydantic import BaseModel
+
+from gps_denied.schemas.flight import GPSPoint
+
+
+class ChunkStatus(str, Enum):
+    """Lifecycle status of a chunk."""
+    UNANCHORED = "unanchored"
+    MATCHING = "matching"
+    ANCHORED = "anchored"
+    MERGED = "merged"
+
+
+class ChunkHandle(BaseModel):
+    """Metadata handle for an independent subset of the factor graph."""
+    chunk_id: str
+    flight_id: str
+    start_frame_id: int
+    end_frame_id: Optional[int] = None
+    frames: List[int] = []
+    
+    is_active: bool = True
+    has_anchor: bool = False
+    
+    anchor_frame_id: Optional[int] = None
+    anchor_gps: Optional[GPSPoint] = None
+    matching_status: ChunkStatus = ChunkStatus.UNANCHORED

src/gps_denied/schemas/graph.py

@@ -0,0 +1,36 @@
+"""Schemas for Factor Graph Optimizer (F10)."""
+
+from datetime import datetime
+from typing import List, Optional
+
+import numpy as np
+from pydantic import BaseModel
+
+
+class Pose(BaseModel):
+    """Optimized pose representation."""
+    model_config = {"arbitrary_types_allowed": True}
+
+    frame_id: int
+    position: np.ndarray  # (3,)
+    orientation: np.ndarray  # (3, 3) matrix or (4,) quaternion
+    timestamp: datetime
+    covariance: Optional[np.ndarray] = None  # (6, 6)
+
+
+class OptimizationResult(BaseModel):
+    """Result of GTSAM optimization step."""
+    converged: bool
+    final_error: float
+    iterations_used: int
+    optimized_frames: List[int]
+    mean_reprojection_error: float
+
+
+class FactorGraphConfig(BaseModel):
+    """Configuration for GTSAM operations."""
+    robust_kernel_type: str = "Huber"
+    huber_threshold: float = 1.0  # pixels
+    altitude_prior_noise: float = 5.0  # meters
+    user_anchor_noise: float = 2.0  # meters
+    litesam_anchor_noise: float = 20.0  # meters