# Feature: Trajectory Optimization & Retrieval

## Name
Trajectory Optimization & Retrieval

## Description
Provides core optimization functionality using GTSAM's iSAM2 for incremental updates and Levenberg-Marquardt for batch optimization. Retrieves optimized trajectory poses and marginal covariances for uncertainty quantification. Supports both real-time incremental optimization (after each frame) and batch refinement (when new absolute factors added).

## Component APIs Implemented
- `optimize(flight_id, iterations) -> OptimizationResult`
- `get_trajectory(flight_id) -> Dict[int, Pose]`
- `get_marginal_covariance(flight_id, frame_id) -> np.ndarray`

## External Tools and Services
- **GTSAM**: iSAM2 incremental optimizer, Levenberg-Marquardt batch solver
- **numpy**: Matrix operations for covariance extraction

## Internal Methods

### `_run_isam2_update(flight_id, iterations) -> OptimizationResult`
Runs incremental iSAM2 optimization on flight's factor graph.
- Updates only affected nodes (fast, <100ms)
- Used for real-time frame-by-frame optimization
- Returns optimization statistics

### `_run_batch_optimization(flight_id, iterations) -> OptimizationResult`
Runs full Levenberg-Marquardt optimization on entire graph.
- Re-optimizes all poses (slower, ~500ms for 100 frames)
- Used when new absolute factors added for back-propagation
- Returns optimization statistics

### `_check_convergence(prev_error, curr_error, threshold) -> bool`
Checks if optimization has converged.
- Compares error reduction against threshold
- Returns True if error reduction < threshold

### `_extract_poses_from_graph(flight_id) -> Dict[int, Pose]`
Extracts all optimized poses from GTSAM Values.
- Converts GTSAM Pose3 to internal Pose model
- Includes position, orientation, timestamp

### `_compute_marginal_covariance(flight_id, frame_id) -> np.ndarray`
Computes marginal covariance for specific frame.
- Uses GTSAM Marginals class
- Returns 6x6 covariance matrix [x, y, z, roll, pitch, yaw]

### `_get_optimized_frames(flight_id, prev_values, curr_values) -> List[int]`
Determines which frames had pose updates.
- Compares previous and current values
- Returns list of frame IDs with significant changes

### `_compute_mean_reprojection_error(flight_id) -> float`
Computes mean reprojection error across all factors.
- Used for quality assessment
- Should be < 1.0 pixels per AC

## Unit Tests

### Test: optimize_incremental_updates_affected_nodes
- Arrange: Create graph with 10 frames, add new factor to frame 8
- Act: Call optimize with few iterations
- Assert: Only frames 8-10 significantly updated

### Test: optimize_batch_updates_all_frames
- Arrange: Create graph with drift, add absolute GPS at frame 50
- Act: Call optimize with batch mode
- Assert: All frames updated (back-propagation)

### Test: optimize_returns_convergence_status
- Arrange: Create well-constrained graph
- Act: Call optimize
- Assert: OptimizationResult.converged == True

### Test: get_trajectory_returns_all_poses
- Arrange: Create graph with 100 frames, optimize
- Act: Call get_trajectory
- Assert: Returns dict with 100 poses

### Test: get_trajectory_poses_in_enu
- Arrange: Create graph with known reference
- Act: Get trajectory
- Assert: Positions in ENU coordinates

### Test: get_marginal_covariance_returns_6x6
- Arrange: Create and optimize graph
- Act: Call get_marginal_covariance for frame
- Assert: Returns (6, 6) numpy array

### Test: get_marginal_covariance_reduces_after_absolute_factor
- Arrange: Create graph, get covariance for frame 50
- Act: Add absolute GPS factor at frame 50, re-optimize
- Assert: New covariance smaller than before

### Test: optimize_respects_iteration_limit
- Arrange: Create complex graph
- Act: Call optimize with iterations=5
- Assert: iterations_used <= 5

## Integration Tests

### Test: drift_correction_with_absolute_gps
1. Build trajectory with VO only (100 frames) - will drift
2. Add absolute GPS factor at frame 50
3. Optimize (batch mode)
4. Verify trajectory corrects
5. Verify frames 1-49 also corrected (back-propagation)

### Test: user_anchor_immediate_refinement
1. Build trajectory to frame 237
2. Add user anchor (high confidence, is_user_anchor=True)
3. Optimize
4. Verify trajectory snaps to anchor
5. Verify low covariance at anchor frame

### Test: performance_incremental_under_100ms
1. Create graph with 50 frames
2. Add new relative factor
3. Time incremental optimization
4. Assert: < 100ms

### Test: performance_batch_under_500ms
1. Create graph with 100 frames
2. Time batch optimization
3. Assert: < 500ms

