mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 02:26:37 +00:00
add chunking
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,521 @@
|
||||
# Image Rotation Manager
|
||||
|
||||
## Interface Definition
|
||||
|
||||
**Interface Name**: `IImageRotationManager`
|
||||
|
||||
### Interface Methods
|
||||
|
||||
```python
|
||||
class IImageRotationManager(ABC):
|
||||
@abstractmethod
|
||||
def rotate_image_360(self, image: np.ndarray, angle: float) -> np.ndarray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def try_rotation_steps(self, flight_id: str, image: np.ndarray, satellite_tile: np.ndarray) -> Optional[RotationResult]:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def calculate_precise_angle(self, homography: np.ndarray, initial_angle: float) -> float:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_current_heading(self, flight_id: str) -> Optional[float]:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def update_heading(self, flight_id: str, heading: float) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def detect_sharp_turn(self, flight_id: str, new_heading: float) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def requires_rotation_sweep(self, flight_id: str) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def rotate_chunk_360(self, chunk_images: List[np.ndarray], angle: float) -> List[np.ndarray]:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def try_chunk_rotation_steps(self, chunk_images: List[np.ndarray], satellite_tile: np.ndarray) -> Optional[RotationResult]:
|
||||
pass
|
||||
```
|
||||
|
||||
## Component Description
|
||||
|
||||
### Responsibilities
|
||||
- Handle UAV image rotation preprocessing for LiteSAM
|
||||
- **Critical**: LiteSAM fails if images rotated >45°, requires preprocessing
|
||||
- Perform 30° step rotation sweeps (12 rotations: 0°, 30°, 60°, ..., 330°)
|
||||
- Track UAV heading angle across flight
|
||||
- Calculate precise rotation angle from homography point correspondences
|
||||
- Detect sharp turns requiring rotation sweep
|
||||
- Pre-rotate images to known heading for subsequent frames
|
||||
- **Chunk rotation operations (rotate all images in chunk)**
|
||||
- **Chunk rotation sweeps for LiteSAM matching**
|
||||
|
||||
### Scope
|
||||
- Image rotation operations
|
||||
- UAV heading tracking and history
|
||||
- Sharp turn detection
|
||||
- Rotation sweep coordination with LiteSAM matching
|
||||
- Precise angle calculation from homography
|
||||
- **Chunk-level rotation (all images rotated by same angle)**
|
||||
|
||||
## API Methods
|
||||
|
||||
### `rotate_image_360(image: np.ndarray, angle: float) -> np.ndarray`
|
||||
|
||||
**Description**: Rotates an image by specified angle around center.
|
||||
|
||||
**Called By**:
|
||||
- Internal (during rotation sweep)
|
||||
- H07 Image Rotation Utils (may delegate to)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
image: np.ndarray # Input image (H×W×3)
|
||||
angle: float # Rotation angle in degrees (0-360)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
np.ndarray # Rotated image (same dimensions)
|
||||
```
|
||||
|
||||
**Processing Details**:
|
||||
- Rotation around image center
|
||||
- Preserves image dimensions
|
||||
- Fills borders with black or extrapolation
|
||||
|
||||
**Error Conditions**:
|
||||
- None (always returns rotated image)
|
||||
|
||||
**Test Cases**:
|
||||
1. **Rotate 90°**: Image rotated correctly
|
||||
2. **Rotate 0°**: Image unchanged
|
||||
3. **Rotate 180°**: Image inverted
|
||||
4. **Rotate 45°**: Diagonal rotation
|
||||
|
||||
---
|
||||
|
||||
### `try_rotation_steps(flight_id: str, image: np.ndarray, satellite_tile: np.ndarray) -> Optional[RotationResult]`
|
||||
|
||||
**Description**: Performs 30° rotation sweep, trying LiteSAM match for each rotation.
|
||||
|
||||
**Called By**:
|
||||
- Internal (when requires_rotation_sweep() returns True)
|
||||
- Main processing loop (first frame or sharp turn)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
flight_id: str
|
||||
image: np.ndarray # UAV image
|
||||
satellite_tile: np.ndarray # Satellite reference tile
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
RotationResult:
|
||||
matched: bool
|
||||
initial_angle: float # Best matching step angle (0, 30, 60, ...)
|
||||
precise_angle: float # Refined angle from homography
|
||||
confidence: float
|
||||
homography: np.ndarray
|
||||
```
|
||||
|
||||
**Algorithm**:
|
||||
```
|
||||
For angle in [0°, 30°, 60°, 90°, 120°, 150°, 180°, 210°, 240°, 270°, 300°, 330°]:
|
||||
rotated_image = rotate_image_360(image, angle)
|
||||
result = LiteSAM.align_to_satellite(rotated_image, satellite_tile)
|
||||
if result.matched and result.confidence > threshold:
|
||||
precise_angle = calculate_precise_angle(result.homography, angle)
|
||||
update_heading(flight_id, precise_angle)
|
||||
return RotationResult(matched=True, initial_angle=angle, precise_angle=precise_angle, ...)
|
||||
return None # No match found
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. For each 30° step:
|
||||
- Rotate image
|
||||
- Call F09 Metric Refinement (LiteSAM)
|
||||
- Check if match found
|
||||
2. If match found:
|
||||
- Calculate precise angle from homography
|
||||
- Update UAV heading
|
||||
- Return result
|
||||
3. If no match:
|
||||
- Return None (triggers progressive search expansion)
|
||||
|
||||
**Error Conditions**:
|
||||
- Returns `None`: No match found in any rotation
|
||||
- This is expected behavior (leads to progressive search)
|
||||
|
||||
**Test Cases**:
|
||||
1. **Match at 60°**: Finds match, returns result
|
||||
2. **Match at 0°**: No rotation needed, finds match
|
||||
3. **No match**: All 12 rotations tried, returns None
|
||||
4. **Multiple matches**: Returns best confidence
|
||||
|
||||
---
|
||||
|
||||
### `calculate_precise_angle(homography: np.ndarray, initial_angle: float) -> float`
|
||||
|
||||
**Description**: Calculates precise rotation angle from homography matrix point shifts.
|
||||
|
||||
**Called By**:
|
||||
- Internal (after LiteSAM match in rotation sweep)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
homography: np.ndarray # 3×3 homography matrix from LiteSAM
|
||||
initial_angle: float # 30° step angle that matched
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
float: Precise rotation angle (e.g., 62.3° refined from 60° step)
|
||||
```
|
||||
|
||||
**Algorithm**:
|
||||
1. Extract rotation component from homography
|
||||
2. Calculate angle from rotation matrix
|
||||
3. Refine initial_angle with delta from homography
|
||||
|
||||
**Uses**: H07 Image Rotation Utils for angle calculation
|
||||
|
||||
**Error Conditions**:
|
||||
- Falls back to initial_angle if calculation fails
|
||||
|
||||
**Test Cases**:
|
||||
1. **Refine 60°**: Returns 62.5° (small delta)
|
||||
2. **Refine 0°**: Returns 3.2° (small rotation)
|
||||
3. **Invalid homography**: Returns initial_angle
|
||||
|
||||
---
|
||||
|
||||
### `get_current_heading(flight_id: str) -> Optional[float]`
|
||||
|
||||
**Description**: Gets current UAV heading angle for a flight.
|
||||
|
||||
**Called By**:
|
||||
- F06 Internal (to check if pre-rotation needed)
|
||||
- Main processing loop (before LiteSAM)
|
||||
- F11 Failure Recovery Coordinator (logging)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
flight_id: str
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
Optional[float]: Heading angle in degrees (0-360), or None if not initialized
|
||||
```
|
||||
|
||||
**Error Conditions**:
|
||||
- Returns `None`: First frame, heading not yet determined
|
||||
|
||||
**Test Cases**:
|
||||
1. **After first frame**: Returns heading angle
|
||||
2. **Before first frame**: Returns None
|
||||
3. **During flight**: Returns current heading
|
||||
|
||||
---
|
||||
|
||||
### `update_heading(flight_id: str, heading: float) -> bool`
|
||||
|
||||
**Description**: Updates UAV heading angle after successful match.
|
||||
|
||||
**Called By**:
|
||||
- Internal (after rotation sweep match)
|
||||
- Internal (after normal LiteSAM match with small rotation delta)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
flight_id: str
|
||||
heading: float # New heading angle (0-360)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if updated successfully
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Normalize angle to 0-360 range
|
||||
2. Add to heading history (last 10 headings)
|
||||
3. Update current_heading for flight
|
||||
4. Persist to database (optional)
|
||||
|
||||
**Test Cases**:
|
||||
1. **Update heading**: Sets new heading
|
||||
2. **Angle normalization**: 370° → 10°
|
||||
3. **History tracking**: Maintains last 10 headings
|
||||
|
||||
---
|
||||
|
||||
### `detect_sharp_turn(flight_id: str, new_heading: float) -> bool`
|
||||
|
||||
**Description**: Detects if UAV made a sharp turn (>45° heading change).
|
||||
|
||||
**Called By**:
|
||||
- Internal (before deciding if rotation sweep needed)
|
||||
- Main processing loop
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
flight_id: str
|
||||
new_heading: float # Proposed new heading
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if sharp turn detected (>45° change)
|
||||
```
|
||||
|
||||
**Algorithm**:
|
||||
```python
|
||||
current = get_current_heading(flight_id)
|
||||
if current is None:
|
||||
return False
|
||||
delta = abs(new_heading - current)
|
||||
if delta > 180: # Handle wraparound
|
||||
delta = 360 - delta
|
||||
return delta > 45
|
||||
```
|
||||
|
||||
**Test Cases**:
|
||||
1. **Small turn**: 60° → 75° → False (15° delta)
|
||||
2. **Sharp turn**: 60° → 120° → True (60° delta)
|
||||
3. **Wraparound**: 350° → 20° → False (30° delta)
|
||||
4. **180° turn**: 0° → 180° → True
|
||||
|
||||
---
|
||||
|
||||
### `requires_rotation_sweep(flight_id: str) -> bool`
|
||||
|
||||
**Description**: Determines if rotation sweep is needed for current frame.
|
||||
|
||||
**Called By**:
|
||||
- Main processing loop (before each frame)
|
||||
- F11 Failure Recovery Coordinator (after tracking loss)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
flight_id: str
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if rotation sweep required
|
||||
```
|
||||
|
||||
**Conditions for sweep**:
|
||||
1. **First frame**: heading not initialized
|
||||
2. **Sharp turn detected**: >45° heading change from VO
|
||||
3. **Tracking loss**: LiteSAM failed to match in previous frame
|
||||
4. **User flag**: Manual trigger (rare)
|
||||
|
||||
**Test Cases**:
|
||||
1. **First frame**: Returns True
|
||||
2. **Second frame, no turn**: Returns False
|
||||
3. **Sharp turn detected**: Returns True
|
||||
4. **Tracking loss**: Returns True
|
||||
|
||||
---
|
||||
|
||||
### `rotate_chunk_360(chunk_images: List[np.ndarray], angle: float) -> List[np.ndarray]`
|
||||
|
||||
**Description**: Rotates all images in a chunk by the same angle.
|
||||
|
||||
**Called By**:
|
||||
- Internal (during try_chunk_rotation_steps)
|
||||
- F11 Failure Recovery Coordinator (chunk rotation sweeps)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
chunk_images: List[np.ndarray] # 5-20 images from chunk
|
||||
angle: float # Rotation angle in degrees (0-360)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
List[np.ndarray] # Rotated images (same dimensions)
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. For each image in chunk:
|
||||
- rotate_image_360(image, angle) → rotated_image
|
||||
2. Return list of rotated images
|
||||
|
||||
**Performance**:
|
||||
- Rotation time: ~20ms × N images
|
||||
- For 10 images: ~200ms total
|
||||
|
||||
**Test Cases**:
|
||||
1. **Rotate chunk**: All images rotated correctly
|
||||
2. **Angle consistency**: All images rotated by same angle
|
||||
3. **Image preservation**: Original images unchanged
|
||||
|
||||
---
|
||||
|
||||
### `try_chunk_rotation_steps(chunk_images: List[np.ndarray], satellite_tile: np.ndarray) -> Optional[RotationResult]`
|
||||
|
||||
**Description**: Performs 30° rotation sweep on entire chunk, trying LiteSAM match for each rotation.
|
||||
|
||||
**Called By**:
|
||||
- F11 Failure Recovery Coordinator (chunk LiteSAM matching with rotation)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
chunk_images: List[np.ndarray] # Chunk images
|
||||
satellite_tile: np.ndarray # Reference satellite tile
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
RotationResult:
|
||||
matched: bool
|
||||
initial_angle: float # Best matching step angle (0, 30, 60, ...)
|
||||
precise_angle: float # Refined angle from homography
|
||||
confidence: float
|
||||
homography: np.ndarray
|
||||
```
|
||||
|
||||
**Algorithm**:
|
||||
```
|
||||
For angle in [0°, 30°, 60°, 90°, 120°, 150°, 180°, 210°, 240°, 270°, 300°, 330°]:
|
||||
rotated_chunk = rotate_chunk_360(chunk_images, angle)
|
||||
result = LiteSAM.align_chunk_to_satellite(rotated_chunk, satellite_tile)
|
||||
if result.matched and result.confidence > threshold:
|
||||
precise_angle = calculate_precise_angle(result.homography, angle)
|
||||
return RotationResult(matched=True, initial_angle=angle, precise_angle=precise_angle, ...)
|
||||
return None # No match found
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. For each 30° step:
|
||||
- Rotate all chunk images
|
||||
- Call F09 Metric Refinement.align_chunk_to_satellite()
|
||||
- Check if match found
|
||||
2. If match found:
|
||||
- Calculate precise angle from homography
|
||||
- Return RotationResult
|
||||
3. If no match:
|
||||
- Return None
|
||||
|
||||
**Performance**:
|
||||
- 12 rotations × chunk LiteSAM (~60ms) = ~720ms
|
||||
- Acceptable for chunk matching (async operation)
|
||||
|
||||
**Test Cases**:
|
||||
1. **Match at 60°**: Finds match, returns result
|
||||
2. **Match at 0°**: No rotation needed, finds match
|
||||
3. **No match**: All 12 rotations tried, returns None
|
||||
4. **Multiple matches**: Returns best confidence
|
||||
|
||||
## Integration Tests
|
||||
|
||||
### Test 1: First Frame Rotation Sweep
|
||||
1. First frame arrives (no heading set)
|
||||
2. requires_rotation_sweep() → True
|
||||
3. try_rotation_steps() → rotates 12 times
|
||||
4. Match found at 60° step
|
||||
5. calculate_precise_angle() → 62.3°
|
||||
6. update_heading(62.3°)
|
||||
7. Subsequent frames use 62.3° heading
|
||||
|
||||
### Test 2: Normal Frame Processing
|
||||
1. Heading known (90°)
|
||||
2. requires_rotation_sweep() → False
|
||||
3. Pre-rotate image to 90°
|
||||
4. LiteSAM match succeeds with small delta (+2.5°)
|
||||
5. update_heading(92.5°)
|
||||
|
||||
### Test 3: Sharp Turn Detection
|
||||
1. UAV heading 45°
|
||||
2. Next frame shows 120° heading (from VO estimate)
|
||||
3. detect_sharp_turn() → True (75° delta)
|
||||
4. requires_rotation_sweep() → True
|
||||
5. Perform rotation sweep → find match at 120° step
|
||||
|
||||
### Test 4: Tracking Loss Recovery
|
||||
1. LiteSAM fails to match (no overlap after turn)
|
||||
2. requires_rotation_sweep() → True
|
||||
3. try_rotation_steps() with all 12 rotations
|
||||
4. Match found → heading updated
|
||||
|
||||
### Test 5: Chunk Rotation Sweeps
|
||||
1. Build chunk with 10 images (unknown orientation)
|
||||
2. try_chunk_rotation_steps() with satellite tile
|
||||
3. Match found at 120° step
|
||||
4. Precise angle calculated (122.5°)
|
||||
5. Verify all images rotated consistently
|
||||
|
||||
## Non-Functional Requirements
|
||||
|
||||
### Performance
|
||||
- **rotate_image_360**: < 20ms per rotation
|
||||
- **try_rotation_steps**: < 1.2 seconds (12 rotations × 100ms LiteSAM)
|
||||
- **calculate_precise_angle**: < 10ms
|
||||
- **get_current_heading**: < 1ms
|
||||
- **update_heading**: < 5ms
|
||||
|
||||
### Accuracy
|
||||
- **Angle precision**: ±0.5° for precise angle calculation
|
||||
- **Sharp turn detection**: 100% accuracy for >45° turns
|
||||
|
||||
### Reliability
|
||||
- Rotation sweep always completes all 12 steps
|
||||
- Graceful handling of no-match scenarios
|
||||
- Heading history preserved across failures
|
||||
|
||||
## Dependencies
|
||||
|
||||
### Internal Components
|
||||
- **F09 Metric Refinement**: For LiteSAM matching during rotation sweep and chunk matching
|
||||
- **H07 Image Rotation Utils**: For image rotation and angle calculations
|
||||
- **F12 Route Chunk Manager**: For chunk image retrieval
|
||||
|
||||
### External Dependencies
|
||||
- **opencv-python**: Image rotation (`cv2.warpAffine`)
|
||||
- **numpy**: Matrix operations
|
||||
|
||||
## Data Models
|
||||
|
||||
### RotationResult
|
||||
```python
|
||||
class RotationResult(BaseModel):
|
||||
matched: bool
|
||||
initial_angle: float # 30° step angle (0, 30, 60, ...)
|
||||
precise_angle: float # Refined angle from homography
|
||||
confidence: float
|
||||
homography: np.ndarray
|
||||
inlier_count: int
|
||||
```
|
||||
|
||||
### HeadingHistory
|
||||
```python
|
||||
class HeadingHistory(BaseModel):
|
||||
flight_id: str
|
||||
current_heading: float
|
||||
heading_history: List[float] # Last 10 headings
|
||||
last_update: datetime
|
||||
sharp_turns: int # Count of sharp turns detected
|
||||
```
|
||||
|
||||
### RotationConfig
|
||||
```python
|
||||
class RotationConfig(BaseModel):
|
||||
step_angle: float = 30.0 # Degrees
|
||||
sharp_turn_threshold: float = 45.0 # Degrees
|
||||
confidence_threshold: float = 0.7 # For accepting match
|
||||
history_size: int = 10 # Number of headings to track
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user