gps-denied-onboard/docs/03_tests/23_vision_to_optimization_pipeline_spec.md

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