# Feature: Core Factor Management

## Name
Core Factor Management

## Description
Provides the fundamental building blocks for factor graph construction by adding relative pose measurements (from VO), absolute GPS measurements (from LiteSAM or user anchors), and altitude priors. Handles scale resolution for monocular VO via GSD computation and applies robust kernels (Huber/Cauchy) for outlier handling.

## Component APIs Implemented
- `add_relative_factor(flight_id, frame_i, frame_j, relative_pose, covariance) -> bool`
- `add_absolute_factor(flight_id, frame_id, gps, covariance, is_user_anchor) -> bool`
- `add_altitude_prior(flight_id, frame_id, altitude, covariance) -> bool`

## External Tools and Services
- **GTSAM**: BetweenFactor, PriorFactor, UnaryFactor creation
- **H02 GSD Calculator**: Scale resolution for monocular VO (GSD computation)
- **H03 Robust Kernels**: Huber/Cauchy loss functions for outlier handling
- **F17 Configuration Manager**: Camera parameters, altitude, frame spacing

## Internal Methods

### `_scale_relative_translation(flight_id, translation, frame_i, frame_j) -> np.ndarray`
Scales unit translation vector from VO using GSD and expected displacement.
- Retrieves altitude and camera params from F17
- Calls H02.compute_gsd() to get ground sampling distance
- Computes expected_displacement = frame_spacing × GSD
- Returns scaled_translation = unit_translation × expected_displacement

### `_create_between_factor(frame_i, frame_j, pose, covariance) -> gtsam.BetweenFactor`
Creates GTSAM BetweenFactor for relative pose measurement.
- Creates Pose3 from translation and rotation
- Wraps with robust kernel (Huber) for outlier handling
- Returns configured factor

### `_create_prior_factor(frame_id, position, covariance, is_hard_constraint) -> gtsam.PriorFactor`
Creates GTSAM PriorFactor for absolute position constraint.
- Converts GPS (lat/lon) to ENU coordinates
- Sets covariance based on source (5m for user anchor, 20-50m for LiteSAM)
- Returns configured factor

### `_create_altitude_factor(frame_id, altitude, covariance) -> gtsam.UnaryFactor`
Creates GTSAM UnaryFactor for Z-coordinate constraint.
- Creates soft constraint on altitude
- Prevents scale drift in monocular VO

### `_apply_robust_kernel(factor, kernel_type, threshold) -> gtsam.Factor`
Wraps factor with robust kernel for outlier handling.
- Supports Huber (default) and Cauchy kernels
- Critical for 350m outlier handling

### `_gps_to_enu(gps, reference_origin) -> np.ndarray`
Converts GPS coordinates to local ENU (East-North-Up) frame.
- Uses reference origin (first frame or flight start GPS)
- Returns (x, y, z) in meters

### `_ensure_graph_initialized(flight_id) -> bool`
Ensures factor graph exists for flight, creates if needed.
- Creates new GTSAM iSAM2 instance if not exists
- Initializes reference origin for ENU conversion

## Unit Tests

### Test: add_relative_factor_scales_translation
- Arrange: Mock H02, F17 with known GSD values
- Act: Call add_relative_factor with unit translation
- Assert: Factor added with correctly scaled translation

### Test: add_relative_factor_applies_huber_kernel
- Arrange: Create factor graph
- Act: Add relative factor
- Assert: Factor has Huber kernel applied

### Test: add_absolute_factor_converts_gps_to_enu
- Arrange: Set reference origin
- Act: Add absolute factor with known GPS
- Assert: Factor position in correct ENU coordinates

### Test: add_absolute_factor_sets_covariance_by_source
- Arrange: Create factor graph
- Act: Add user anchor (is_user_anchor=True)
- Assert: Covariance is 5m (high confidence)

### Test: add_absolute_factor_litesam_covariance
- Arrange: Create factor graph
- Act: Add LiteSAM match (is_user_anchor=False)
- Assert: Covariance is 20-50m

### Test: add_altitude_prior_creates_soft_constraint
- Arrange: Create factor graph
- Act: Add altitude prior
- Assert: UnaryFactor created with correct covariance

### Test: add_relative_factor_returns_false_on_invalid_flight
- Arrange: No graph for flight_id
- Act: Call add_relative_factor
- Assert: Returns False (or creates graph per implementation)

### Test: scale_resolution_uses_f17_config
- Arrange: Mock F17 with specific altitude, camera params
- Act: Add relative factor
- Assert: H02 called with correct parameters from F17

## Integration Tests

### Test: incremental_trajectory_building
1. Initialize graph with first frame
2. Add 100 relative factors from VO
3. Add 100 altitude priors
4. Verify all factors added successfully
5. Verify graph structure correct

### Test: outlier_handling_350m
1. Add normal relative factors (10 frames)
2. Add 350m outlier factor (simulating tilt error)
3. Add more normal factors
4. Verify outlier factor weight reduced by Huber kernel
5. Verify graph remains stable

### Test: mixed_factor_types
1. Add relative factors
2. Add absolute GPS factors
3. Add altitude priors
4. Verify all factor types coexist correctly
5. Verify graph ready for optimization

