# Sequential Visual Odometry

## Interface Definition

**Interface Name**: `ISequentialVisualOdometry`

### Interface Methods

```python
class ISequentialVisualOdometry(ABC):
    @abstractmethod
    def compute_relative_pose(self, prev_image: np.ndarray, curr_image: np.ndarray) -> Optional[RelativePose]:
        pass
    
    @abstractmethod
    def extract_and_match(self, image1: np.ndarray, image2: np.ndarray) -> Matches:
        pass
    
    @abstractmethod
    def estimate_motion(self, matches: Matches, camera_params: CameraParameters) -> Optional[Motion]:
        pass
```

**Note**: F07 is chunk-agnostic. It only computes relative poses between images. The caller (F02.2 Flight Processing Engine) determines which chunk the frames belong to and routes factors to the appropriate subgraph via F12 → F10.

## Component Description

### Responsibilities
- Combined SuperPoint+LightGlue neural network inference for extraction and matching
- Handle <5% overlap scenarios via LightGlue attention mechanism
- Estimate relative pose (translation + rotation) between frames
- Return relative pose factors for Factor Graph Optimizer
- Detect tracking loss (low inlier count)

### Scope
- Frame-to-frame visual odometry
- Feature-based motion estimation using combined neural network
- Handles low overlap and challenging agricultural environments
- Provides relative measurements for trajectory optimization
- **Chunk-agnostic**: F07 doesn't know about chunks. Caller (F02.2) routes results to appropriate chunk subgraph.

### Architecture Notes
- Uses combined SuperPoint+LightGlue TensorRT model (single inference pass)
- Reference: [D_VINS](https://github.com/kajo-kurisu/D_VINS/) for TensorRT optimization patterns
- Model outputs matched keypoints directly from two input images

## API Methods

### `compute_relative_pose(prev_image: np.ndarray, curr_image: np.ndarray) -> Optional[RelativePose]`

**Description**: Computes relative camera pose between consecutive frames.

**Called By**:
- Main processing loop (per-frame)

**Input**:
```python
prev_image: np.ndarray  # Previous frame (t-1)
curr_image: np.ndarray  # Current frame (t)
```

**Output**:
```python
RelativePose:
    translation: np.ndarray  # (x, y, z) in meters
    rotation: np.ndarray  # 3×3 rotation matrix or quaternion
    confidence: float  # 0.0 to 1.0
    inlier_count: int
    total_matches: int
    tracking_good: bool
```

**Processing Flow**:
1. extract_and_match(prev_image, curr_image) → matches
2. estimate_motion(matches, camera_params) → motion
3. Return RelativePose

**Tracking Quality Indicators**:
- **Good tracking**: inlier_count > 50, inlier_ratio > 0.5
- **Degraded tracking**: inlier_count 20-50
- **Tracking loss**: inlier_count < 20

**Error Conditions**:
- Returns `None`: Tracking lost (insufficient matches)

**Test Cases**:
1. **Good overlap (>50%)**: Returns reliable pose
2. **Low overlap (5-10%)**: Still succeeds with LightGlue
3. **<5% overlap**: May return None (tracking loss)
4. **Agricultural texture**: Handles repetitive patterns

---

### `extract_and_match(image1: np.ndarray, image2: np.ndarray) -> Matches`

**Description**: Single-pass neural network inference combining SuperPoint feature extraction and LightGlue matching.

**Called By**:
- Internal (during compute_relative_pose)

**Input**:
```python
image1: np.ndarray  # First image (H×W×3 or H×W)
image2: np.ndarray  # Second image (H×W×3 or H×W)
```

**Output**:
```python
Matches:
    matches: np.ndarray  # (M, 2) - indices [idx1, idx2]
    scores: np.ndarray  # (M,) - match confidence scores
    keypoints1: np.ndarray  # (M, 2) - matched keypoints from image 1
    keypoints2: np.ndarray  # (M, 2) - matched keypoints from image 2
```

**Processing Details**:
- Uses F16 Model Manager to get combined SuperPoint+LightGlue TensorRT engine
- Single inference pass processes both images
- Converts to grayscale internally if needed
- SuperPoint extracts keypoints + 256-dim descriptors
- LightGlue performs attention-based matching with adaptive depth
- "Dustbin" mechanism handles unmatched features

**Performance** (TensorRT on RTX 2060):
- Combined inference: ~50-80ms (vs ~65-115ms separate)
- Faster for high-overlap pairs (adaptive depth exits early)

**Test Cases**:
1. **High overlap**: ~35ms, 500+ matches
2. **Low overlap (<5%)**: ~80ms, 20-50 matches
3. **No overlap**: Few or no matches (< 10)

---

### `estimate_motion(matches: Matches, camera_params: CameraParameters) -> Optional[Motion]`

**Description**: Estimates camera motion from matched keypoints using Essential Matrix.

**Called By**:
- Internal (during compute_relative_pose)

**Input**:
```python
matches: Matches
camera_params: CameraParameters:
    focal_length: float
    principal_point: Tuple[float, float]
    resolution: Tuple[int, int]
```

**Output**:
```python
Motion:
    translation: np.ndarray  # (x, y, z) - unit vector (scale ambiguous)
    rotation: np.ndarray  # 3×3 rotation matrix
    inliers: np.ndarray  # Boolean mask of inlier matches
    inlier_count: int
```

**Algorithm**:
1. Normalize keypoint coordinates using camera intrinsics
2. Estimate Essential Matrix using RANSAC
3. Decompose Essential Matrix → [R, t]
4. Return motion with inlier mask

**Scale Ambiguity**:
- Monocular VO has inherent scale ambiguity
- Translation is unit vector (direction only, magnitude = 1)
- **F07 does NOT resolve scale** - it only outputs unit translation vectors
- Scale resolution is handled by F10 Factor Graph Optimizer, which uses:
  - Altitude priors (soft constraints)
  - GSD-based expected displacement calculations (via H02)
  - Absolute GPS anchors from F09 Metric Refinement

**Critical Handoff to F10**:
The caller (F02.2) must pass the unit translation to F10 for scale resolution:
```python
vo_result = F07.compute_relative_pose(prev_image, curr_image)
# vo_result.translation is a UNIT VECTOR (||t|| = 1)

# F02.2 passes to F10 which scales using:
# 1. altitude = F17.get_operational_altitude(flight_id)
# 2. gsd = H02.compute_gsd(altitude, camera_params)
# 3. expected_displacement = frame_spacing * gsd
# 4. scaled_translation = vo_result.translation * expected_displacement
F10.add_relative_factor(flight_id, frame_i, frame_j, vo_result, covariance)
```

**Error Conditions**:
- Returns `None`: Insufficient inliers (< 8 points for Essential Matrix)

**Test Cases**:
1. **Good matches**: Returns motion with high inlier count
2. **Low inliers**: May return None
3. **Degenerate motion**: Handles pure rotation

## Integration Tests

### Test 1: Normal Flight Sequence
1. Load consecutive frames with 50% overlap
2. compute_relative_pose() → returns valid pose
3. Verify translation direction reasonable
4. Verify inlier_count > 100

### Test 2: Low Overlap Scenario
1. Load frames with 5% overlap
2. compute_relative_pose() → still succeeds
3. Verify inlier_count > 20
4. Verify combined model finds matches despite low overlap

### Test 3: Tracking Loss
1. Load frames with 0% overlap (sharp turn)
2. compute_relative_pose() → returns None
3. Verify tracking_good = False
4. Trigger global place recognition

### Test 4: Agricultural Texture
1. Load images of wheat fields (repetitive texture)
2. compute_relative_pose() → SuperPoint handles better than SIFT
3. Verify match quality

### Test 5: VO with Chunk Routing
1. Create chunk_1 and chunk_2 via F12
2. compute_relative_pose() for frames in chunk_1, F02.2 routes to chunk_1
3. compute_relative_pose() for frames in chunk_2, F02.2 routes to chunk_2
4. Verify F02.2 calls F12.add_frame_to_chunk() with correct chunk_id
5. Verify chunks optimized independently via F10

## Non-Functional Requirements

### Performance
- **compute_relative_pose**: < 150ms total
  - Combined SP+LG inference: ~50-80ms
  - Motion estimation: ~10ms
- **Frame rate**: 5-10 FPS processing (meets <5s requirement)

### Accuracy
- **Relative rotation**: ±2° error
- **Relative translation direction**: ±5° error
- **Inlier ratio**: >50% for good tracking

### Reliability
- Handle 100m spacing between frames
- Survive temporary tracking degradation
- Recover from brief occlusions

## Dependencies

### Internal Components
- **F16 Model Manager**: For combined SuperPoint+LightGlue TensorRT model
- **F17 Configuration Manager**: For camera parameters
- **H01 Camera Model**: For coordinate normalization
- **H05 Performance Monitor**: For timing measurements

**Note**: F07 is chunk-agnostic and does NOT depend on F10 Factor Graph Optimizer. F07 only computes relative poses between images and returns them to the caller (F02.2). The caller (F02.2) determines which chunk the frames belong to and routes factors to the appropriate subgraph via F12 → F10.

### External Dependencies
- **Combined SuperPoint+LightGlue**: Single TensorRT engine for extraction + matching
- **opencv-python**: Essential Matrix estimation
- **numpy**: Matrix operations

## Data Models

### Matches
```python
class Matches(BaseModel):
    matches: np.ndarray  # (M, 2) - pairs of indices
    scores: np.ndarray  # (M,) - match confidence
    keypoints1: np.ndarray  # (M, 2)
    keypoints2: np.ndarray  # (M, 2)
```

### RelativePose
```python
class RelativePose(BaseModel):
    translation: np.ndarray  # (3,) - unit vector
    rotation: np.ndarray  # (3, 3) or (4,) quaternion
    confidence: float
    inlier_count: int
    total_matches: int
    tracking_good: bool
    scale_ambiguous: bool = True
    chunk_id: Optional[str] = None
```

### Motion
```python
class Motion(BaseModel):
    translation: np.ndarray  # (3,)
    rotation: np.ndarray  # (3, 3)
    inliers: np.ndarray  # Boolean mask
    inlier_count: int
```

### CameraParameters
```python
class CameraParameters(BaseModel):
    focal_length: float
    principal_point: Tuple[float, float]
    resolution: Tuple[int, int]
```
