# Feature: Geometric Pose Estimation

## Description
Estimates camera motion from matched keypoints using Essential Matrix decomposition. Orchestrates the full visual odometry pipeline and provides tracking quality assessment. Pure geometric computation (non-ML).

## Component APIs Implemented
- `compute_relative_pose(prev_image: np.ndarray, curr_image: np.ndarray) -> Optional[RelativePose]`
- `estimate_motion(matches: Matches, camera_params: CameraParameters) -> Optional[Motion]`

## External Tools and Services
- **opencv-python**: Essential Matrix estimation via RANSAC, matrix decomposition
- **numpy**: Matrix operations, coordinate normalization
- **F17 Configuration Manager**: Camera parameters (focal length, principal point)
- **H01 Camera Model**: Coordinate normalization utilities

## Internal Methods

### `_normalize_keypoints(keypoints: np.ndarray, camera_params: CameraParameters) -> np.ndarray`
Normalizes pixel coordinates to camera-centered coordinates using intrinsic matrix K.
```
normalized = K^(-1) @ [x, y, 1]^T
```

### `_estimate_essential_matrix(points1: np.ndarray, points2: np.ndarray) -> Tuple[Optional[np.ndarray], np.ndarray]`
RANSAC-based Essential Matrix estimation. Returns E matrix and inlier mask.
- Uses cv2.findEssentialMat with RANSAC
- Requires minimum 8 point correspondences
- Returns None if insufficient inliers

### `_decompose_essential_matrix(E: np.ndarray, points1: np.ndarray, points2: np.ndarray, camera_params: CameraParameters) -> Tuple[np.ndarray, np.ndarray]`
Decomposes Essential Matrix into rotation R and translation t.
- Uses cv2.recoverPose
- Selects correct solution via cheirality check
- Translation is unit vector (scale ambiguous)

### `_compute_tracking_quality(inlier_count: int, total_matches: int) -> Tuple[float, bool]`
Computes confidence score and tracking_good flag:
- **Good**: inlier_count > 50, inlier_ratio > 0.5 → confidence > 0.8
- **Degraded**: inlier_count 20-50 → confidence 0.4-0.8
- **Lost**: inlier_count < 20 → tracking_good = False

### `_build_relative_pose(motion: Motion, matches: Matches) -> RelativePose`
Constructs RelativePose dataclass from motion estimate and match statistics.

## Unit Tests

### Test: Keypoint Normalization
- Input: Pixel coordinates and camera params (fx=1000, cx=640, cy=360)
- Verify: Output centered at principal point, scaled by focal length

### Test: Essential Matrix Estimation - Good Data
- Input: 100+ inlier correspondences from known motion
- Verify: Returns valid Essential Matrix
- Verify: det(E) ≈ 0
- Verify: Singular values satisfy 2σ₁ ≈ σ₂, σ₃ ≈ 0

### Test: Essential Matrix Estimation - Insufficient Points
- Input: <8 point correspondences
- Verify: Returns None

### Test: Essential Matrix Decomposition
- Input: Valid Essential Matrix from known motion
- Verify: Returns valid rotation (det(R) = 1, R^T R = I)
- Verify: Translation is unit vector (||t|| = 1)

### Test: Tracking Quality - Good
- Input: inlier_count=100, total_matches=150
- Verify: tracking_good=True
- Verify: confidence > 0.8

### Test: Tracking Quality - Degraded
- Input: inlier_count=30, total_matches=50
- Verify: tracking_good=True
- Verify: 0.4 < confidence < 0.8

### Test: Tracking Quality - Lost
- Input: inlier_count=10, total_matches=20
- Verify: tracking_good=False

### Test: Scale Ambiguity Marker
- Input: Any valid motion estimate
- Verify: translation vector has unit norm (||t|| = 1)
- Verify: scale_ambiguous flag is True

### Test: Pure Rotation Handling
- Input: Matches from pure rotational motion (no translation)
- Verify: Returns valid pose
- Verify: translation ≈ [0, 0, 0] or arbitrary unit vector

### Test: Forward Motion
- Input: Matches from forward camera motion
- Verify: translation z-component is positive

## Integration Tests

### Test: Full Pipeline - Normal Flight
- Input: Consecutive frames with 50% overlap
- Verify: Returns valid RelativePose
- Verify: inlier_count > 100
- Verify: Total time < 150ms

### Test: Full Pipeline - Low Overlap
- Input: Frames with 5% overlap
- Verify: Returns valid RelativePose
- Verify: inlier_count > 20

### Test: Full Pipeline - Tracking Loss
- Input: Non-overlapping frames (sharp turn)
- Verify: Returns None
- Verify: No exception raised

### Test: Configuration Manager Integration
- Verify: Successfully retrieves camera_params from F17
- Verify: Parameters match expected resolution and focal length

### Test: Camera Model Integration
- Verify: H01 normalization produces correct coordinates
- Verify: Consistent with opencv undistortion

### Test: Pipeline Orchestration
- Verify: extract_and_match called once (combined inference)
- Verify: estimate_motion called with correct params
- Verify: Returns RelativePose with all fields populated

### Test: Agricultural Environment
- Input: Wheat field images with repetitive texture
- Verify: Pipeline succeeds with reasonable inlier count

### Test: Known Motion Validation
- Input: Synthetic image pair with known ground truth motion
- Verify: Estimated rotation within ±2° of ground truth
- Verify: Estimated translation direction within ±5° of ground truth

