# Feature: Chunk Subgraph Operations

## Name
Chunk Subgraph Operations

## Description
Manages chunk-level factor graph subgraphs for handling tracking loss recovery. Creates independent subgraphs for route chunks (map fragments), adds factors to chunk-specific subgraphs, anchors chunks with GPS measurements, and optimizes chunks independently. F10 provides low-level factor graph operations only; F12 owns chunk metadata (status, is_active, etc.).

## Component APIs Implemented
- `create_chunk_subgraph(flight_id, chunk_id, start_frame_id) -> bool`
- `add_relative_factor_to_chunk(flight_id, chunk_id, frame_i, frame_j, relative_pose, covariance) -> bool`
- `add_chunk_anchor(flight_id, chunk_id, frame_id, gps, covariance) -> bool`
- `get_chunk_trajectory(flight_id, chunk_id) -> Dict[int, Pose]`
- `optimize_chunk(flight_id, chunk_id, iterations) -> OptimizationResult`

## External Tools and Services
- **GTSAM**: Subgraph management, BetweenFactor, PriorFactor
- **H03 Robust Kernels**: Huber kernel for chunk factors

## Internal Methods

### `_create_subgraph(flight_id, chunk_id) -> gtsam.NonlinearFactorGraph`
Creates new GTSAM subgraph for chunk.
- Initializes empty factor graph
- Stores in chunk subgraph dictionary

### `_initialize_chunk_origin(flight_id, chunk_id, start_frame_id) -> bool`
Initializes first frame pose in chunk's local coordinate system.
- Sets origin pose at identity (or relative to previous chunk)
- Adds prior factor for origin frame

### `_add_factor_to_subgraph(flight_id, chunk_id, factor) -> bool`
Adds factor to chunk's specific subgraph.
- Verifies chunk exists
- Appends factor to chunk's graph
- Marks chunk as needing optimization

### `_get_chunk_subgraph(flight_id, chunk_id) -> Optional[gtsam.NonlinearFactorGraph]`
Retrieves chunk's subgraph if exists.
- Returns None if chunk not found

### `_extract_chunk_poses(flight_id, chunk_id) -> Dict[int, Pose]`
Extracts all optimized poses from chunk's subgraph.
- Poses in chunk's local coordinate system
- Converts GTSAM Pose3 to internal Pose model

### `_optimize_subgraph(flight_id, chunk_id, iterations) -> OptimizationResult`
Runs Levenberg-Marquardt on chunk's isolated subgraph.
- Optimizes only chunk's factors
- Does not affect other chunks
- Returns optimization result

### `_anchor_chunk_to_global(flight_id, chunk_id, frame_id, enu_position) -> bool`
Adds GPS anchor to chunk's subgraph as PriorFactor.
- Converts GPS to ENU (may need separate reference or global reference)
- Adds prior factor for anchor frame
- Returns success status

### `_track_chunk_frames(flight_id, chunk_id, frame_id) -> bool`
Tracks which frames belong to which chunk.
- Updates frame-to-chunk mapping
- Used for factor routing

## Unit Tests

### Test: create_chunk_subgraph_returns_true
- Arrange: Valid flight_id
- Act: Call create_chunk_subgraph
- Assert: Returns True, subgraph created

### Test: create_chunk_subgraph_initializes_origin
- Arrange: Create chunk
- Act: Check subgraph
- Assert: Has prior factor for start_frame_id

### Test: add_relative_factor_to_chunk_success
- Arrange: Create chunk
- Act: Add relative factor to chunk
- Assert: Returns True, factor in chunk's subgraph

### Test: add_relative_factor_to_chunk_nonexistent_chunk
- Arrange: No chunk created
- Act: Add relative factor to nonexistent chunk
- Assert: Returns False

### Test: add_chunk_anchor_success
- Arrange: Create chunk with frames
- Act: Add GPS anchor
- Assert: Returns True, prior factor added

### Test: get_chunk_trajectory_returns_poses
- Arrange: Create chunk, add factors, optimize
- Act: Call get_chunk_trajectory
- Assert: Returns dict with chunk poses

### Test: get_chunk_trajectory_empty_chunk
- Arrange: Create chunk without factors
- Act: Call get_chunk_trajectory
- Assert: Returns dict with only origin pose

### Test: optimize_chunk_success
- Arrange: Create chunk with factors
- Act: Call optimize_chunk
- Assert: Returns OptimizationResult with converged=True

### Test: optimize_chunk_isolation
- Arrange: Create chunk_1 and chunk_2 with factors
- Act: Optimize chunk_1
- Assert: chunk_2 poses unchanged

### Test: multiple_chunks_simultaneous
- Arrange: Create 3 chunks
- Act: Add factors to each
- Assert: All chunks maintain independent state

## Integration Tests

### Test: multi_chunk_creation_and_isolation
1. Create chunk_1 with frames 1-10
2. Create chunk_2 with frames 20-30 (disconnected)
3. Add relative factors to each chunk
4. Verify chunks optimized independently
5. Verify factors isolated to respective chunks

### Test: chunk_anchoring
1. Create chunk with frames 1-10
2. Add relative factors
3. Add chunk_anchor at frame 5
4. Optimize chunk
5. Verify trajectory consistent with anchor
6. Verify has_anchor reflected (via F12 query)

### Test: chunk_trajectory_local_coordinates
1. Create chunk
2. Add relative factors
3. Get chunk trajectory
4. Verify poses in chunk's local coordinate system
5. Verify origin at identity or start frame

