# Coordinate Transformer

## Interface Definition

**Interface Name**: `ICoordinateTransformer`

### Interface Methods

```python
class ICoordinateTransformer(ABC):
    # ENU Origin Management
    @abstractmethod
    def set_enu_origin(self, flight_id: str, origin_gps: GPSPoint) -> None:
        pass
    
    @abstractmethod
    def get_enu_origin(self, flight_id: str) -> GPSPoint:
        pass
    
    @abstractmethod
    def gps_to_enu(self, flight_id: str, gps: GPSPoint) -> Tuple[float, float, float]:
        pass
    
    @abstractmethod
    def enu_to_gps(self, flight_id: str, enu: Tuple[float, float, float]) -> GPSPoint:
        pass
    
    # Pixel/GPS Conversions
    @abstractmethod
    def pixel_to_gps(self, flight_id: str, pixel: Tuple[float, float], frame_pose: Pose, camera_params: CameraParameters, altitude: float) -> GPSPoint:
        pass
    
    @abstractmethod
    def gps_to_pixel(self, flight_id: str, gps: GPSPoint, frame_pose: Pose, camera_params: CameraParameters, altitude: float) -> Tuple[float, float]:
        pass
    
    @abstractmethod
    def image_object_to_gps(self, flight_id: str, frame_id: int, object_pixel: Tuple[float, float]) -> GPSPoint:
        pass
    
    @abstractmethod
    def transform_points(self, points: List[Tuple[float, float]], transformation: np.ndarray) -> List[Tuple[float, float]]:
        pass
```

## Component Description

### Responsibilities
- Pixel-to-GPS coordinate conversions
- GPS-to-pixel inverse projections
- **Critical**: Convert object pixel coordinates (from external detection system) to GPS
- Handle multiple coordinate systems: WGS84, Web Mercator, ENU, image pixels, rotated coordinates
- Camera model integration for projection operations

**Note**: GSD calculations are delegated to H02 GSD Calculator helper.

### Scope
- Coordinate system transformations
- Camera projection mathematics
- Integration with Factor Graph poses
- Object localization (pixel → GPS)
- Support for external object detection system

## API Methods

### ENU Origin Management

#### `set_enu_origin(flight_id: str, origin_gps: GPSPoint) -> None`

**Description**: Sets the ENU (East-North-Up) coordinate system origin for a specific flight. Called once during flight creation using the flight's start_gps.

**Why ENU Origin?**: Factor graph optimization works in Cartesian coordinates (meters) for better numerical stability. ENU converts GPS (degrees) to local meters relative to an origin. See `helpers/enu_origin_explanation.md` for details.

**Called By**:
- F02.1 Flight Lifecycle Manager (during create_flight)

**Input**:
```python
flight_id: str  # Flight identifier
origin_gps: GPSPoint
    lat: float  # Origin latitude in WGS84 (flight start_gps)
    lon: float  # Origin longitude in WGS84 (flight start_gps)
```

**Processing Flow**:
1. Store origin_gps as ENU origin for flight_id
2. Precompute conversion factors for lat/lon to meters at origin latitude
3. All subsequent ENU operations for this flight use this origin

**Important**: 
- Each flight has its own ENU origin (the flight's start_gps)
- Origin remains constant for the entire flight duration
- All frames in the flight use the same origin for ENU conversions

**Test Cases**:
1. **Set origin**: Store origin GPS
2. **Subsequent gps_to_enu calls**: Use set origin
3. **Multiple flights**: Each flight has independent origin

---

#### `get_enu_origin(flight_id: str) -> GPSPoint`

**Description**: Returns the ENU origin for a specific flight.

**Called By**:
- F10 Factor Graph Optimizer (for coordinate checks)
- F14 Result Manager (for reference)

**Input**:
```python
flight_id: str
```

**Output**:
```python
GPSPoint: The ENU origin (flight start_gps)
```

**Error Conditions**:
- Raises `OriginNotSetError` if called before set_enu_origin() for this flight_id

**Test Cases**:
1. **After set_enu_origin**: Returns stored origin
2. **Before set_enu_origin**: Raises error

---

#### `gps_to_enu(flight_id: str, gps: GPSPoint) -> Tuple[float, float, float]`

**Description**: Converts GPS coordinates to ENU (East, North, Up) relative to the flight's ENU origin.

**Called By**:
- F10 Factor Graph Optimizer (for absolute factors)
- Internal (for pixel_to_gps)

**Input**:
```python
flight_id: str  # Flight identifier
gps: GPSPoint
    lat: float
    lon: float
```

**Output**:
```python
Tuple[float, float, float]: (east, north, up) in meters relative to flight's origin
```

**Algorithm**:
1. Compute delta_lat = gps.lat - origin.lat
2. Compute delta_lon = gps.lon - origin.lon
3. east = delta_lon × cos(origin.lat) × 111319.5  # meters/degree at equator
4. north = delta_lat × 111319.5
5. up = 0 (or computed from elevation if available)

**Test Cases**:
1. **Origin GPS**: Returns (0, 0, 0)
2. **1km East**: Returns (~1000, 0, 0)
3. **1km North**: Returns (0, ~1000, 0)
4. **Diagonal**: Returns correct east/north components

---

#### `enu_to_gps(flight_id: str, enu: Tuple[float, float, float]) -> GPSPoint`

**Description**: Converts ENU coordinates back to GPS using the flight's ENU origin.

**Called By**:
- F10 Factor Graph Optimizer (for get_trajectory)
- F14 Result Manager (for publishing GPS results)

**Input**:
```python
flight_id: str  # Flight identifier
enu: Tuple[float, float, float]  # (east, north, up) in meters relative to flight's origin
```

**Output**:
```python
GPSPoint: WGS84 coordinates
```

**Algorithm**:
1. delta_lon = east / (cos(origin.lat) × 111319.5)
2. delta_lat = north / 111319.5
3. lat = origin.lat + delta_lat
4. lon = origin.lon + delta_lon

**Test Cases**:
1. **Origin (0, 0, 0)**: Returns origin GPS
2. **Round-trip**: gps → enu → gps matches original (within precision)

---

### Pixel/GPS Conversions

### `pixel_to_gps(flight_id: str, pixel: Tuple[float, float], frame_pose: Pose, camera_params: CameraParameters, altitude: float) -> GPSPoint`

**Description**: Converts pixel coordinates to GPS using camera pose and ground plane assumption.

**Called By**:
- F02.2 Flight Processing Engine (for frame center GPS)
- Internal (for image_object_to_gps)

**Input**:
```python
flight_id: str  # Flight identifier (required for ENU origin lookup)
pixel: Tuple[float, float]  # (x, y) in image coordinates
frame_pose: Pose  # From Factor Graph (ENU coordinates)
camera_params: CameraParameters
altitude: float  # Ground altitude
```

**Output**:
```python
GPSPoint:
    lat: float
    lon: float
```

**Algorithm**:
1. Unproject pixel to 3D ray using H01 Camera Model
2. Intersect ray with ground plane at altitude
3. Transform 3D point from camera frame to ENU using frame_pose
4. Convert ENU to WGS84 GPS using enu_to_gps(flight_id, enu_point)

**Assumptions**:
- Ground plane assumption (terrain height negligible)
- Downward-pointing camera
- Known altitude

**Error Conditions**:
- None (always returns GPS, may be inaccurate if assumptions violated)

**Test Cases**:
1. **Image center**: Returns frame center GPS
2. **Image corner**: Returns GPS at corner
3. **Object pixel**: Returns object GPS
4. **Altitude variation**: Correct GPS at different altitudes

---

### `gps_to_pixel(flight_id: str, gps: GPSPoint, frame_pose: Pose, camera_params: CameraParameters, altitude: float) -> Tuple[float, float]`

**Description**: Inverse projection from GPS to image pixel coordinates.

**Called By**:
- Visualization tools (overlay GPS annotations)
- Testing/validation

**Input**:
```python
flight_id: str  # Flight identifier (required for ENU origin lookup)
gps: GPSPoint
frame_pose: Pose
camera_params: CameraParameters
altitude: float
```

**Output**:
```python
Tuple[float, float]: (x, y) pixel coordinates
```

**Algorithm**:
1. Convert GPS to ENU using gps_to_enu(flight_id, gps)
2. Transform ENU point to camera frame using frame_pose
3. Project 3D point to image plane using H01 Camera Model
4. Return pixel coordinates

**Test Cases**:
1. **Frame center GPS**: Returns image center pixel
2. **Out of view GPS**: Returns pixel outside image bounds

---

### `image_object_to_gps(flight_id: str, frame_id: int, object_pixel: Tuple[float, float]) -> GPSPoint`

**Description**: **Critical method** - Converts object pixel coordinates to GPS. Used for external object detection integration.

**Called By**:
- F02 Flight Processor (via convert_object_to_gps delegation from F01)
- F14 Result Manager (converts objects to GPS for output)

**Input**:
```python
flight_id: str  # Flight identifier (needed for ENU origin and factor graph)
frame_id: int  # Frame containing object
object_pixel: Tuple[float, float]  # Pixel coordinates from object detector
```

**Output**:
```python
GPSPoint: GPS coordinates of object center
```

**Processing Flow**:
1. Get frame_pose from F10 Factor Graph Optimizer.get_trajectory(flight_id)[frame_id]
2. Get camera_params from F17 Configuration Manager.get_flight_config(flight_id)
3. Get altitude from F17 Configuration Manager.get_flight_config(flight_id).altitude
4. Call pixel_to_gps(object_pixel, frame_pose, camera_params, altitude)
5. Use enu_to_gps(flight_id, enu_point) for final GPS conversion
6. Return GPS

**User Story**:
- External system detects object in UAV image at pixel (1024, 768)
- Calls F02.convert_object_to_gps(flight_id="abc", frame_id=237, pixel=(1024, 768))
- F02 delegates to F13.image_object_to_gps(flight_id="abc", frame_id=237, object_pixel=(1024, 768))
- Returns GPSPoint(lat=48.123, lon=37.456)
- Object GPS can be used for navigation, targeting, etc.

**Test Cases**:
1. **Object at image center**: Returns frame center GPS
2. **Object at corner**: Returns GPS with offset
3. **Multiple objects**: Each gets correct GPS
4. **Refined trajectory**: Object GPS updates after refinement

---

### `transform_points(points: List[Tuple[float, float]], transformation: np.ndarray) -> List[Tuple[float, float]]`

**Description**: Applies homography or affine transformation to list of points.

**Called By**:
- F06 Image Rotation Manager (for rotation transforms)
- F09 Metric Refinement (homography application)

**Input**:
```python
points: List[Tuple[float, float]]
transformation: np.ndarray  # 3×3 homography or 2×3 affine
```

**Output**:
```python
List[Tuple[float, float]]: Transformed points
```

**Processing Flow**:
1. Convert points to homogeneous coordinates
2. Apply transformation matrix
3. Normalize and return

**Test Cases**:
1. **Identity transform**: Points unchanged
2. **Rotation**: Points rotated correctly
3. **Homography**: Points transformed with perspective

## Integration Tests

### Test 1: Frame Center GPS Calculation
1. Get frame_pose from Factor Graph
2. pixel_to_gps(image_center) → GPS
3. Verify GPS matches expected location
4. Verify accuracy < 50m

### Test 2: Object Localization
1. External detector finds object at pixel (1500, 2000)
2. image_object_to_gps(frame_id, pixel) → GPS
3. Verify GPS correct
4. Multiple objects → all get correct GPS

### Test 3: Round-Trip Conversion
1. Start with GPS point
2. gps_to_pixel() → pixel
3. pixel_to_gps() → GPS
4. Verify GPS matches original (within tolerance)

### Test 4: GSD via H02
1. Call H02.compute_gsd() with known parameters
2. Verify matches expected value
3. Test at different altitudes

## Non-Functional Requirements

### Performance
- **pixel_to_gps**: < 5ms
- **gps_to_pixel**: < 5ms
- **image_object_to_gps**: < 10ms

### Accuracy
- **GPS accuracy**: Inherits from Factor Graph accuracy (~20m)
- **GSD calculation**: ±1% precision
- **Projection accuracy**: < 1 pixel error

### Reliability
- Handle edge cases (points outside image)
- Graceful handling of degenerate configurations
- Numerical stability

## Dependencies

### Internal Components
- **F10 Factor Graph Optimizer**: For frame poses via `get_trajectory(flight_id)` - required for `pixel_to_gps()` and `image_object_to_gps()` to get frame pose estimates.
- **F17 Configuration Manager**: For camera parameters via `get_flight_config(flight_id)`.
- **H01 Camera Model**: For projection/unprojection operations (`project()`, `unproject()`).
- **H02 GSD Calculator**: For GSD calculations in coordinate conversions.

**Note**: F13 uses internal `enu_to_gps()` and `gps_to_enu()` methods that rely on the ENU origin set via `set_enu_origin(flight_id, origin_gps)`. H06 Web Mercator Utils is NOT used for ENU conversions - ENU is a local Cartesian coordinate system.

### External Dependencies
- **numpy**: Matrix operations
- **opencv-python**: Homography operations (optional)

## Data Models

### Pose (from Factor Graph)
```python
class Pose(BaseModel):
    position: np.ndarray  # (3,) - [x, y, z] in ENU
    orientation: np.ndarray  # (4,) quaternion or (3,3) rotation
    timestamp: datetime
```

### CameraParameters
```python
class CameraParameters(BaseModel):
    focal_length: float  # mm
    sensor_width: float  # mm
    sensor_height: float  # mm
    resolution_width: int  # pixels
    resolution_height: int  # pixels
    principal_point: Tuple[float, float]  # (cx, cy)
    distortion_coefficients: Optional[List[float]]
```

### GPSPoint
```python
class GPSPoint(BaseModel):
    lat: float
    lon: float
```

### CoordinateFrame
```python
class CoordinateFrame(Enum):
    WGS84 = "wgs84"  # GPS coordinates
    ENU = "enu"  # East-North-Up local frame
    ECEF = "ecef"  # Earth-Centered Earth-Fixed
    IMAGE = "image"  # Image pixel coordinates
    CAMERA = "camera"  # Camera frame
```

