# Feature: Chunk Merging & Global Optimization

## Name
Chunk Merging & Global Optimization

## Description
Provides Sim(3) similarity transformation for merging chunk subgraphs and global optimization across all chunks. Handles scale, rotation, and translation differences between chunks (critical for monocular VO with scale ambiguity). Called by F12 Route Chunk Manager after chunk matching succeeds.

## Component APIs Implemented
- `merge_chunk_subgraphs(flight_id, new_chunk_id, main_chunk_id, transform: Sim3Transform) -> bool`
- `optimize_global(flight_id, iterations) -> OptimizationResult`

## External Tools and Services
- **GTSAM**: Graph merging, global optimization
- **numpy/scipy**: Sim(3) transformation computation

## Internal Methods

### `_apply_sim3_transform(poses: Dict[int, Pose], transform: Sim3Transform) -> Dict[int, Pose]`
Applies Sim(3) similarity transformation to all poses.
- Translation: p' = s * R * p + t
- Rotation: R' = R_transform * R_original
- Scale: All positions scaled by transform.scale
- Critical for merging chunks with different scales

### `_merge_subgraphs(flight_id, source_chunk_id, dest_chunk_id) -> bool`
Merges source chunk's subgraph into destination chunk's subgraph.
- Copies all factors from source to destination
- Updates node keys to avoid conflicts
- Preserves factor graph structure

### `_update_frame_to_chunk_mapping(flight_id, source_chunk_id, dest_chunk_id) -> bool`
Updates frame-to-chunk mapping after merge.
- All frames from source_chunk now belong to dest_chunk
- Used for routing future factors

### `_transform_chunk_factors(flight_id, chunk_id, transform: Sim3Transform) -> bool`
Transforms all factors in chunk's subgraph by Sim(3).
- Updates BetweenFactor translations with scale
- Updates PriorFactor positions
- Preserves covariances (may need adjustment for scale)

### `_validate_merge_preconditions(flight_id, new_chunk_id, main_chunk_id) -> bool`
Validates that merge can proceed.
- Verifies both chunks exist
- Verifies new_chunk has anchor (required for merge)
- Returns False if preconditions not met

### `_collect_all_chunk_subgraphs(flight_id) -> List[gtsam.NonlinearFactorGraph]`
Collects all chunk subgraphs for global optimization.
- Returns list of all subgraphs
- Filters out merged/inactive chunks (via F12)

### `_run_global_optimization(flight_id, iterations) -> OptimizationResult`
Runs optimization across all anchored chunks.
- Transforms chunks to global coordinate system
- Runs Levenberg-Marquardt on combined graph
- Updates all chunk trajectories

### `_compute_sim3_from_correspondences(source_poses, dest_poses) -> Sim3Transform`
Computes Sim(3) transformation from pose correspondences.
- Uses Horn's method or similar for rigid + scale estimation
- Returns translation, rotation, scale

### `_update_all_chunk_trajectories(flight_id) -> bool`
Updates all chunk trajectories after global optimization.
- Extracts poses from global graph
- Distributes to chunk-specific storage

## Unit Tests

### Test: merge_chunk_subgraphs_success
- Arrange: Create chunk_1 (anchored), chunk_2 (with anchor)
- Act: Call merge_chunk_subgraphs with Sim3 transform
- Assert: Returns True, chunks merged

### Test: merge_chunk_subgraphs_unanchored_fails
- Arrange: Create chunk_1, chunk_2 (no anchor)
- Act: Call merge_chunk_subgraphs
- Assert: Returns False

### Test: merge_applies_sim3_transform
- Arrange: Create chunks with known poses, define transform
- Act: Merge chunks
- Assert: new_chunk poses transformed correctly

### Test: merge_updates_frame_mapping
- Arrange: Create chunks, merge
- Act: Query frame-to-chunk mapping
- Assert: new_chunk frames now belong to main_chunk

### Test: optimize_global_returns_result
- Arrange: Create multiple anchored chunks
- Act: Call optimize_global
- Assert: Returns OptimizationResult with all frames

### Test: optimize_global_achieves_consistency
- Arrange: Create chunks with overlapping anchors
- Act: Optimize global
- Assert: Anchor frames align within tolerance

### Test: sim3_transform_handles_scale
- Arrange: Poses with scale=2.0 difference
- Act: Apply Sim(3) transform
- Assert: Scaled positions match expected

### Test: sim3_transform_handles_rotation
- Arrange: Poses with 90° rotation difference
- Act: Apply Sim(3) transform
- Assert: Rotated poses match expected

## Integration Tests

### Test: chunk_anchoring_and_merging
1. Create chunk_1 (frames 1-10), chunk_2 (frames 20-30)
2. Add relative factors to each
3. Add chunk_anchor to chunk_2 (frame 25)
4. Optimize chunk_2 → local consistency improved
5. Merge chunk_2 into chunk_1 with Sim(3) transform
6. Optimize global → both chunks globally consistent
7. Verify final trajectory coherent across chunks

### Test: simultaneous_multi_chunk_processing
1. Create 3 chunks simultaneously (disconnected segments)
2. Process frames in each chunk independently
3. Anchor each chunk asynchronously
4. Merge chunks as anchors become available
5. Verify final global trajectory consistent

### Test: global_optimization_performance
1. Create 5 chunks with 20 frames each
2. Anchor all chunks
3. Time optimize_global
4. Assert: < 500ms for 100 total frames

### Test: scale_resolution_across_chunks
1. Create chunk_1 with scale drift (monocular VO)
2. Create chunk_2 with different scale drift
3. Anchor both chunks
4. Merge with Sim(3) accounting for scale
5. Verify merged trajectory has consistent scale

