feat: stage9 — Factor Graph and Chunks

tests/test_chunk_manager.py

@@ -0,0 +1,61 @@
+"""Tests for Route Chunk Manager (F12)."""
+
+import pytest
+import numpy as np
+
+from gps_denied.core.chunk_manager import RouteChunkManager
+from gps_denied.core.graph import FactorGraphOptimizer
+from gps_denied.schemas.graph import FactorGraphConfig
+from gps_denied.schemas.chunk import ChunkStatus
+from gps_denied.schemas.metric import Sim3Transform
+
+@pytest.fixture
+def chunk_manager():
+    opt = FactorGraphOptimizer(FactorGraphConfig())
+    return RouteChunkManager(opt)
+
+def test_create_and_get_chunk(chunk_manager):
+    flight_id = "flight123"
+    chunk = chunk_manager.create_new_chunk(flight_id, 0)
+    
+    assert chunk.is_active is True
+    assert chunk.start_frame_id == 0
+    assert chunk.flight_id == flight_id
+    
+    active = chunk_manager.get_active_chunk(flight_id)
+    assert active.chunk_id == chunk.chunk_id
+    
+def test_add_frame_to_chunk(chunk_manager):
+    flight = "fl2"
+    chunk_manager.create_new_chunk(flight, 100)
+    
+    assert chunk_manager.add_frame_to_chunk(flight, 101) is True
+    
+    active = chunk_manager.get_active_chunk(flight)
+    assert 101 in active.frames
+    assert len(active.frames) == 2
+
+def test_chunk_merging_logic(chunk_manager):
+    flight = "fl3"
+    c1 = chunk_manager.create_new_chunk(flight, 0)
+    c1_id = c1.chunk_id
+    
+    c2 = chunk_manager.create_new_chunk(flight, 50)
+    c2_id = c2.chunk_id
+    chunk_manager.add_frame_to_chunk(flight, 51)
+    
+    # c2 is active now, c1 is inactive
+    assert chunk_manager.get_active_chunk(flight).chunk_id == c2_id
+    
+    transform = Sim3Transform(translation=np.zeros(3), rotation=np.eye(3), scale=1)
+    
+    # merge c2 into c1
+    res = chunk_manager.merge_chunks(flight, c2_id, c1_id, transform)
+    
+    assert res is True
+    
+    # c2 frames moved to c1
+    assert 50 in c1.frames
+    assert len(c2.frames) == 0
+    assert c2.matching_status == ChunkStatus.MERGED
+    assert c2.is_active is False

tests/test_graph.py

@@ -0,0 +1,111 @@
+"""Tests for Factor Graph Optimizer (F10)."""
+
+import numpy as np
+import pytest
+
+from gps_denied.core.graph import FactorGraphOptimizer
+from gps_denied.schemas.flight import GPSPoint
+from gps_denied.schemas.graph import FactorGraphConfig
+from gps_denied.schemas.vo import RelativePose
+from gps_denied.schemas.metric import Sim3Transform
+
+
+@pytest.fixture
+def optimizer():
+    config = FactorGraphConfig()
+    return FactorGraphOptimizer(config)
+
+
+def test_add_relative_factor(optimizer):
+    flight_id = "test_f_1"
+    
+    # Needs initial node, using chunk creation wrapper to mock initial
+    optimizer.create_chunk_subgraph(flight_id, "ch_1", start_frame_id=0)
+    
+    # Mock relative pose logic: in mock graph logic it's added via "add_relative_factor_to_chunk" 
+    # OR we need an initial root via add_absolute/relative. Our mock for `add_relative` is simple.
+    
+    rel1 = RelativePose(
+        translation=np.array([1.0, 0.0, 0.0]),
+        rotation=np.eye(3),
+        covariance=np.eye(6),
+        confidence=0.9,
+        inlier_count=100,
+        total_matches=120,
+        tracking_good=True
+    )
+    
+    # Test global logic requires frame exist, let's inject a zero frame
+    optimizer._init_flight(flight_id)
+    optimizer._flights_state[flight_id]["poses"][0] = optimizer._chunks_state["ch_1"]["poses"][0]
+    
+    res = optimizer.add_relative_factor(flight_id, 0, 1, rel1, np.eye(6))
+    assert res is True
+    
+    traj = optimizer.get_trajectory(flight_id)
+    assert 1 in traj
+    assert np.allclose(traj[1].position, [1.0, 0.0, 0.0])
+
+def test_add_absolute_factor(optimizer):
+    flight_id = "test_f_2"
+    optimizer._init_flight(flight_id)
+    
+    # Inject 0
+    from gps_denied.schemas.graph import Pose
+    from datetime import datetime
+    optimizer._flights_state[flight_id]["poses"][0] = Pose(
+        frame_id=0, position=np.zeros(3), orientation=np.eye(3), timestamp=datetime.now()
+    )
+    
+    gps = GPSPoint(lat=49.1, lon=30.1)
+    res = optimizer.add_absolute_factor(flight_id, 0, gps, np.eye(2), is_user_anchor=True)
+    assert res is True
+    
+    # Verify modification
+    traj = optimizer.get_trajectory(flight_id)
+    # The x,y are roughly calcualted
+    assert traj[0].position[1] > 1000  # lat 49.1 > 49.0
+    
+def test_optimize_and_retrieve(optimizer):
+    flight_id = "test_f_3"
+    optimizer._init_flight(flight_id)
+    
+    res = optimizer.optimize(flight_id, 5)
+    assert res.converged is True
+    assert res.iterations_used == 5
+    
+    traj = optimizer.get_trajectory(flight_id)
+    assert isinstance(traj, dict)
+    
+def test_chunk_merging(optimizer):
+    flight_id = "test_f_4"
+    c1 = "chunk1"
+    c2 = "chunk2"
+    
+    assert optimizer.create_chunk_subgraph(flight_id, c1, 0) is True
+    assert optimizer.create_chunk_subgraph(flight_id, c2, 10) is True
+    
+    rel = RelativePose(
+        translation=np.array([5.0, 0, 0]),
+        rotation=np.eye(3),
+        covariance=np.eye(6),
+        confidence=0.9,
+        inlier_count=100,
+        total_matches=120,
+        tracking_good=True
+    )
+    optimizer.add_relative_factor_to_chunk(flight_id, c2, 10, 11, rel, np.eye(6))
+    
+    sim3 = Sim3Transform(translation=np.array([100.0, 0, 0]), rotation=np.eye(3), scale=1.0)
+    res = optimizer.merge_chunk_subgraphs(flight_id, c2, c1, sim3)
+    
+    assert res is True
+    
+    c1_poses = optimizer.get_chunk_trajectory(flight_id, c1)
+    # 10 and 11 should now be in c1, transformed!
+    assert 10 in c1_poses
+    assert 11 in c1_poses
+    
+    # 100 translation
+    assert c1_poses[10].position[0] == 100.0
+    assert c1_poses[11].position[0] == 105.0

tests/test_recovery.py

@@ -0,0 +1,62 @@
+"""Tests for Failure Recovery Coordinator (F11)."""
+
+import pytest
+import numpy as np
+
+from gps_denied.core.recovery import FailureRecoveryCoordinator
+from gps_denied.core.chunk_manager import RouteChunkManager
+from gps_denied.core.graph import FactorGraphOptimizer
+from gps_denied.core.gpr import GlobalPlaceRecognition
+from gps_denied.core.metric import MetricRefinement
+from gps_denied.core.models import ModelManager
+from gps_denied.schemas.graph import FactorGraphConfig
+
+@pytest.fixture
+def recovery():
+    opt = FactorGraphOptimizer(FactorGraphConfig())
+    cm = RouteChunkManager(opt)
+    
+    mm = ModelManager()
+    gpr = GlobalPlaceRecognition(mm)
+    gpr.load_index("test", "test")
+    
+    metric = MetricRefinement(mm)
+    return FailureRecoveryCoordinator(cm, gpr, metric)
+
+def test_handle_tracking_lost(recovery):
+    flight = "recovery_fl"
+    res = recovery.handle_tracking_lost(flight, 404)
+    
+    assert res is True
+    # active chunk should be 404
+    active = recovery.chunk_manager.get_active_chunk(flight)
+    assert active.start_frame_id == 404
+
+def test_process_chunk_recovery_success(recovery, monkeypatch):
+    # Mock LitSAM to guarantee match
+    def mock_align(*args, **kwargs):
+        from gps_denied.schemas.metric import ChunkAlignmentResult, Sim3Transform
+        from gps_denied.schemas.flight import GPSPoint
+        return ChunkAlignmentResult(
+            matched=True, chunk_id="x", chunk_center_gps=GPSPoint(lat=49, lon=30),
+            rotation_angle=0, confidence=0.9, inlier_count=50,
+            transform=Sim3Transform(translation=np.zeros(3), rotation=np.eye(3), scale=1),
+            reprojection_error=1.0
+        )
+        
+    monkeypatch.setattr(recovery.metric_refinement, "align_chunk_to_satellite", mock_align)
+    
+    flight = "rec2"
+    recovery.handle_tracking_lost(flight, 10)
+    chunk_id = recovery.chunk_manager.get_active_chunk(flight).chunk_id
+    
+    images = [np.zeros((256, 256, 3)) for _ in range(3)]
+    res = recovery.process_chunk_recovery(flight, chunk_id, images)
+    
+    assert res is True
+    
+    from gps_denied.schemas.chunk import ChunkStatus
+    active = recovery.chunk_manager.get_active_chunk(flight)
+    
+    assert active.has_anchor is True
+    assert active.matching_status == ChunkStatus.ANCHORED