# Integration Test: Factor Graph Optimizer (F10)

## Summary
Validate the Factor Graph Optimizer component using GTSAM to fuse sequential relative poses (L1) and absolute GPS anchors (L3) into a globally consistent trajectory, with native multi-chunk support for disconnected route segments.

## Component Under Test
**Component**: Factor Graph Optimizer (F10)
**Interface**: `IFactorGraphOptimizer`
**Dependencies**:
- F07 Sequential Visual Odometry - provides relative factors
- F09 Metric Refinement - provides absolute GPS factors
- F12 Route Chunk Manager - chunk lifecycle (F10 provides low-level graph ops)
- F13 Coordinate Transformer
- H02 GSD Calculator - scale resolution
- H03 Robust Kernels - outlier handling
- GTSAM library

## Detailed Description
This test validates that the Factor Graph Optimizer can:
1. Build and maintain a factor graph with pose nodes and measurement factors
2. Add relative pose factors from sequential tracking
3. Add absolute GPS factors from satellite anchoring
4. Apply robust cost functions (Huber/Cauchy) to handle outliers
5. Optimize the graph to produce globally consistent trajectory
6. Handle 350m outliers without catastrophic failure
7. Incrementally update the graph as new measurements arrive
8. Maintain Mean Reprojection Error (MRE) < 1.0 pixels

The optimizer is the "brain" of ASTRAL-Next, reconciling potentially conflicting measurements to produce the final trajectory.

## Input Data

### Test Case 1: Simple Sequential Chain
- **Images**: AD000001-AD000010 (10 images)
- **Input**: 9 relative pose factors from L1 (sequential pairs)
- **Input**: 3 absolute GPS factors from L3 (images 1, 5, 10)
- **Expected**: Smooth trajectory close to ground truth

### Test Case 2: With Outlier (AC-3)
- **Images**: AD000045-AD000050 (6 images, includes 268m outlier)
- **Input**: 5 relative pose factors (including outlier AD000047→AD000048)
- **Input**: 2 absolute GPS factors (images 45, 50)
- **Expected**: Robust kernel should down-weight outlier, trajectory remains consistent

### Test Case 3: Sharp Turn Recovery (AC-4)
- **Images**: AD000042, AD000044, AD000045, AD000046 (skip AD000043)
- **Input**: Sequential factors where available, GPS anchor at AD000044
- **Expected**: Graph should handle missing sequential link, rely on GPS anchor

### Test Case 4: Baseline Full Route
- **Images**: AD000001-AD000030 (30 images)
- **Input**: 29 relative factors, 6 GPS anchors (every 5th image)
- **Expected**: Meet AC-1 and AC-2 accuracy targets

### Test Case 5: Altitude Constraint Test
- **Images**: AD000010-AD000015
- **Input**: Relative factors with varying scale, altitude prior = 400m
- **Expected**: Optimizer should maintain consistent altitude ~400m

### Test Case 6: Incremental Update Test
- **Images**: AD000001-AD000020
- **Input**: Add measurements incrementally (simulate real-time operation)
- **Expected**: Trajectory should converge smoothly, past poses may be refined

### Test Case 7: Create Chunk Subgraph
- **Input**: flight_id, chunk_id, start_frame_id = 20
- **Expected**: 
  - create_chunk_subgraph() returns True
  - New subgraph created for chunk
  - Chunk isolated from main trajectory

### Test Case 8: Add Relative Factors to Chunk
- **Chunk**: chunk_2 with frames 20-30
- **Input**: 10 relative factors from VO
- **Expected**: 
  - add_relative_factor_to_chunk() returns True for each
  - Factors added to chunk's subgraph only
  - Main trajectory unaffected

### Test Case 9: Add Chunk Anchor
- **Chunk**: chunk_2 (frames 20-30, unanchored)
- **Input**: GPS anchor at frame 25
- **Expected**: 
  - add_chunk_anchor() returns True
  - Chunk can now be merged
  - Chunk optimization triggered

### Test Case 10: Optimize Chunk
- **Chunk**: chunk_2 with anchor
- **Input**: optimize_chunk(chunk_id, iterations=10)
- **Expected**: 
  - Returns OptimizationResult
  - converged = True
  - Chunk trajectory consistent
  - Other chunks unaffected

### Test Case 11: Merge Chunk Subgraphs
- **Chunks**: chunk_1 (frames 1-10), chunk_2 (frames 20-30, anchored)
- **Input**: merge_chunk_subgraphs(flight_id, chunk_2, chunk_1, transform)
- **Expected**: 
  - Returns True
  - chunk_2 merged into chunk_1
  - Sim(3) transform applied correctly
  - Global consistency maintained

### Test Case 12: Get Chunk Trajectory
- **Chunk**: chunk_2 with 10 frames
- **Input**: get_chunk_trajectory(flight_id, chunk_id)
- **Expected**: 
  - Returns Dict[int, Pose] with 10 frames
  - Poses in chunk's coordinate system

### Test Case 13: Optimize Global
- **Setup**: 3 chunks, 2 anchored, 1 merged
- **Input**: optimize_global(flight_id, iterations=50)
- **Expected**: 
  - All chunks optimized together
  - Global consistency achieved
  - Returns OptimizationResult with all frame IDs

### Test Case 14: Multi-Flight Isolation
- **Setup**: 2 flights processing simultaneously
- **Input**: Add factors to both flights
- **Expected**: 
  - Each flight's graph isolated
  - No cross-contamination
  - Independent optimization results

### Test Case 15: Delete Flight Graph
- **Setup**: Flight with complex trajectory and chunks
- **Input**: delete_flight_graph(flight_id)
- **Expected**: 
  - Returns True
  - All resources cleaned up
  - No memory leaks

## Expected Output

For each test case:
```json
{
  "success": true/false,
  "num_poses": <integer>,
  "num_factors": <integer>,
  "num_relative_factors": <integer>,
  "num_absolute_factors": <integer>,
  "optimization_iterations": <integer>,
  "final_error": <float>,
  "mean_reprojection_error_px": <float>,
  "optimized_trajectory": [
    {
      "image": "AD000001.jpg",
      "estimated_gps": [lat, lon],
      "ground_truth_gps": [lat, lon],
      "error_m": <float>,
      "covariance": [[...]]
    }
  ],
  "accuracy_stats": {
    "percent_under_50m": <float>,
    "percent_under_20m": <float>,
    "mean_error_m": <float>,
    "median_error_m": <float>,
    "rmse_m": <float>
  },
  "processing_time_ms": <float>
}
```

## Success Criteria

**Per Test Case**:

**Test Case 1 (Simple Chain)**:
- success = true
- percent_under_50m ≥ 90%
- mean_reprojection_error_px < 1.0
- processing_time_ms < 500ms

**Test Case 2 (With Outlier)**:
- success = true
- Outlier factor should have low weight in final solution
- Other poses should maintain accuracy (percent_under_50m ≥ 80%)
- mean_reprojection_error_px < 1.5

**Test Case 3 (Sharp Turn)**:
- success = true
- Gap between AD000042 and AD000044 should be bridged by GPS anchor
- percent_under_50m ≥ 75%
- processing_time_ms < 300ms

**Test Case 4 (Baseline)**:
- percent_under_50m ≥ 80% (meets AC-1)
- percent_under_20m ≥ 60% (meets AC-2)
- mean_reprojection_error_px < 1.0 (meets AC-10)
- processing_time_ms < 1000ms

**Test Case 5 (Altitude)**:
- All poses should have altitude within ±50m of 400m
- Scale drift should be prevented

**Test Case 6 (Incremental)**:
- Each incremental update completes in <100ms
- Final trajectory matches batch optimization (within 5m)

**Test Cases 7-9 (Chunk Creation & Factors)**:
- Chunk subgraph created successfully
- Factors added to correct chunk
- Chunk anchor enables merging

**Test Cases 10-11 (Chunk Optimization & Merging)**:
- Chunk optimizes independently
- Sim(3) transform applied correctly
- Merged trajectory globally consistent

**Test Cases 12-13 (Chunk Queries & Global)**:
- Chunk trajectory retrieved correctly
- Global optimization handles all chunks

**Test Cases 14-15 (Isolation & Cleanup)**:
- Multi-flight isolation maintained
- Resource cleanup complete

## Maximum Expected Time
- **Small graph (10 poses)**: < 500ms
- **Medium graph (30 poses)**: < 1000ms
- **Incremental update**: < 100ms per new pose
- **Create chunk subgraph**: < 10ms
- **Add factor to chunk**: < 5ms
- **Add chunk anchor**: < 50ms
- **Optimize chunk (10 frames)**: < 100ms
- **Merge chunks**: < 200ms
- **Optimize global (50 frames, 3 chunks)**: < 500ms
- **Total test suite**: < 60 seconds

## Test Execution Steps

1. **Setup Phase**:
   a. Initialize GTSAM optimizer with appropriate noise models
   b. Configure robust cost functions (Huber kernel with threshold 50m)
   c. Set up altitude prior constraint (400m ± 50m)
   d. Initialize coordinate transformer

2. **For Each Test Case**:
   a. Load ground truth GPS data
   b. Construct simulated L1 relative factors (or load from pre-computed)
   c. Construct simulated L3 absolute factors (with realistic noise)
   d. Build factor graph incrementally:
      - Add pose variables
      - Add relative factors between consecutive poses
      - Add absolute GPS factors at specified anchors
      - Add altitude prior factors
   e. Run optimization
   f. Extract optimized trajectory
   g. Compare against ground truth
   h. Calculate accuracy statistics

3. **Validation Phase**:
   a. Verify accuracy targets are met
   b. Check mean reprojection error
   c. Validate processing times
   d. Inspect covariances (uncertainty should be reasonable)
   e. Check that robust kernels activated for outliers

4. **Report Generation**:
   a. Trajectory visualization (if possible)
   b. Error distribution plots
   c. Factor residuals before/after optimization
   d. Timing breakdown
   e. Pass/fail determination

## Pass/Fail Criteria

**Overall Test Passes If**:
- At least 12 out of 15 test cases meet their individual success criteria
- Test Case 4 (Baseline) must pass (validates AC-1, AC-2, AC-10)
- Test Cases 7-11 (Chunk operations) must pass (validates multi-chunk architecture)
- No crashes or numerical instabilities
- Memory usage remains stable

**Test Fails If**:
- Test Case 4 fails to meet AC-1, AC-2, or AC-10
- Chunk creation/merging fails
- Multi-flight isolation violated
- More than 3 test cases fail completely
- Optimizer produces NaN or infinite values
- Processing time exceeds 2x maximum expected time
- Memory leak detected (>500MB growth)

## Additional Validation

**Robustness Analysis**:
1. **Outlier Rejection**: Verify that outlier factors have reduced weights after optimization
2. **Convergence**: Check that optimization converges within 50 iterations
3. **Consistency**: Run optimization multiple times with same input - should get identical results
4. **Covariance**: Verify that uncertainty grows in sections with no GPS anchors

**Edge Cases**:
1. **No GPS Anchors**: Graph with only relative factors - should drift but not crash
2. **No Relative Factors**: Graph with only GPS anchors - should produce disconnected poses
3. **Conflicting Measurements**: GPS anchor disagrees with relative factors by 100m - robust kernel should handle

**Performance Metrics**:
- **Optimization speedup**: Compare incremental (iSAM2) vs batch optimization
- **Scalability**: Test with 100 poses - should complete in <5 seconds
- **Memory efficiency**: Memory usage should scale linearly with number of poses

## Error Analysis
If test fails, analyze:
- Which factors have largest residuals
- Whether robust kernels are activating appropriately
- If altitude constraint is being respected
- Whether relative factors are consistent with absolute factors
- If numerical conditioning is good (check condition number of Hessian)
- Correlation between number of GPS anchors and accuracy

