# System Integration Test: Vision to Optimization Pipeline

## Summary
Validate integration between vision layers (L1, L2, L3) and Factor Graph Optimizer.

## Components Under Test
- Sequential Visual Odometry (L1)
- Global Place Recognition (L2)
- Metric Refinement (L3)
- Factor Graph Optimizer
- Result Manager

## Test Scenario
Test the flow of vision estimates into factor graph optimization:
1. L1 provides relative pose factors
2. L3 provides absolute GPS factors
3. Factor graph fuses both into optimized trajectory
4. Results show improvement over individual layers

## Test Cases

### Test Case 1: Sequential Factors Only (L1)
- Process AD000001-AD000010 with L1 only
- Feed relative poses to factor graph
- Verify: Drift accumulates without GPS anchors

### Test Case 2: GPS Anchors Only (L3)
- Process same images with L3 only
- Feed absolute GPS to factor graph
- Verify: Accurate but no temporal smoothness

### Test Case 3: Fused L1 + L3 (Optimal)
- Process with both L1 and L3
- Factor graph fuses relative and absolute factors
- Verify: Better accuracy than L1-only, smoother than L3-only

### Test Case 4: L2 Recovery after L1 Failure
- Simulate L1 tracking loss
- L2 recovers global location
- L3 refines it
- Factor graph incorporates recovery

### Test Case 5: Robust Outlier Handling
- Include outlier measurement (268m jump)
- Verify robust kernel down-weights outlier
- Trajectory remains consistent

### Test Case 6: Incremental Updates
- Add images one by one
- Factor graph updates incrementally
- Verify past trajectory refined when new anchors arrive (AC-8)

## Success Criteria
- L1-only shows drift (errors grow over time)
- L3-only accurate but may be jagged
- L1+L3 fusion achieves best results
- Outliers handled without breaking trajectory
- Incremental updates work correctly
- Accuracy improves over single-layer estimates

## Maximum Expected Time
- L1-only (10 images): < 10 seconds
- L3-only (10 images): < 15 seconds
- Fused (10 images): < 20 seconds
- Total test: < 60 seconds

## Pass/Fail Criteria
**Passes If**: Factor graph successfully fuses vision estimates, accuracy improved, outliers handled
**Fails If**: Fusion fails, accuracy worse than single layer, outliers corrupt trajectory