fix: P0+P1 audit — memory leak, hardcoded camera/GPS, lifespan init, background processing, batch validation, ABC interfaces

src/gps_denied/core/graph.py

@@ -3,7 +3,7 @@
 import logging
 from abc import ABC, abstractmethod
 from typing import Dict, List, Optional
-from datetime import datetime
+from datetime import datetime, timezone
 
 import numpy as np
 
@@ -87,10 +87,11 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
     def __init__(self, config: FactorGraphConfig):
         self.config = config
         # Keyed by flight_id
-        # Value structure: {"graph": graph, "initial": values, "isam": isam2_obj, "poses": {frame_id: Pose}}
         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:
@@ -134,26 +135,32 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
                 frame_id=frame_j,
                 position=new_pos,
                 orientation=new_orientation,
-                timestamp=datetime.now(),
+                timestamp=datetime.now(timezone.utc),
                 covariance=np.eye(6)
             )
             state["dirty"] = True
             return True
         return False
 
+    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]
         
-        # Mock GPS to ENU mapping (1 degree lat ~= 111km)
-        # Assuming origin is some coordinate
-        enu_x = (gps.lon - 30.0) * 111000 * np.cos(np.radians(49.0))
-        enu_y = (gps.lat - 49.0) * 111000
-        enu_z = 0.0
+        enu = self._gps_to_enu(flight_id, gps)
         
         if frame_id in state["poses"]:
-            # Hard snap
-            state["poses"][frame_id].position = np.array([enu_x, enu_y, enu_z])
+            state["poses"][frame_id].position = enu
             state["dirty"] = True
             return True
         return False
@@ -163,7 +170,11 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
         state = self._flights_state[flight_id]
         
         if frame_id in state["poses"]:
-            state["poses"][frame_id].position[2] = altitude
+            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
@@ -201,7 +212,7 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
             frame_id=start_frame_id,
             position=np.zeros(3),
             orientation=np.eye(3),
-            timestamp=datetime.now(),
+            timestamp=datetime.now(timezone.utc),
             covariance=np.eye(6)
         )
         return True
@@ -219,7 +230,7 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
                 frame_id=frame_j,
                 position=new_pos,
                 orientation=np.eye(3),
-                timestamp=datetime.now(),
+                timestamp=datetime.now(timezone.utc),
                 covariance=np.eye(6)
             )
             state["dirty"] = True
@@ -232,9 +243,8 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
             
         state = self._chunks_state[chunk_id]
         if frame_id in state["poses"]:
-            # Snap logic for mock
-            state["poses"][frame_id].position[0] = (gps.lon - 30.0) * 111000 * np.cos(np.radians(49.0))
-            state["poses"][frame_id].position[1] = (gps.lat - 49.0) * 111000
+            enu = self._gps_to_enu(flight_id, gps)
+            state["poses"][frame_id].position = enu
             state["dirty"] = True
             return True
         return False
@@ -296,7 +306,9 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
         )
 
     def delete_flight_graph(self, flight_id: str) -> bool:
+        removed = False
         if flight_id in self._flights_state:
             del self._flights_state[flight_id]
-            return True
-        return False
+            removed = True
+        self._enu_origins.pop(flight_id, None)
+        return removed