---
phase: 01-eskf-core
plan: 02
type: execute
wave: 1
depends_on: []
files_modified:
  - src/gps_denied/core/coordinates.py
autonomous: true
requirements:
  - ESKF-06

must_haves:
  truths:
    - "pixel_to_gps uses camera intrinsic matrix K to unproject pixel to camera ray, not fake 0.1m/pixel scaling"
    - "Camera-to-body transform uses T_cam_body rotation (nadir-pointing camera mount)"
    - "Body-to-NED rotation uses a quaternion parameter (from ESKF attitude), not hardcoded identity"
    - "Ray-ground intersection at known altitude produces NED position, then converts to WGS84"
    - "gps_to_pixel is the inverse: WGS84 -> NED -> body -> camera -> pixel"
    - "transform_points applies a real 3x3 homography via cv2.perspectiveTransform"
  artifacts:
    - path: "src/gps_denied/core/coordinates.py"
      provides: "Full coordinate chain: pixel -> camera ray -> body -> NED -> WGS84"
      contains: "np.linalg.inv"
  key_links:
    - from: "src/gps_denied/core/coordinates.py"
      to: "numpy"
      via: "K matrix construction, quaternion rotation, ray-ground intersection"
      pattern: "np\\.array"
    - from: "src/gps_denied/core/coordinates.py"
      to: "cv2"
      via: "perspectiveTransform for homography application"
      pattern: "cv2\\.perspectiveTransform"
---

<objective>
Replace all FAKE Math stubs in CoordinateTransformer with the real pixel-to-GPS coordinate chain: pixel -> camera ray (K matrix) -> body frame (T_cam_body) -> NED (quaternion) -> WGS84.

Purpose: Without real coordinate transforms, all position estimates, object localization, and satellite matching produce wrong results. The 0.1m/pixel approximation ignores altitude, camera intrinsics, and UAV attitude entirely.

Output: Updated `src/gps_denied/core/coordinates.py` with real geometric projections.
</objective>

<execution_context>
@$HOME/.claude/get-shit-done/workflows/execute-plan.md
@$HOME/.claude/get-shit-done/templates/summary.md
</execution_context>

<context>
@.planning/PROJECT.md
@.planning/ROADMAP.md
@.planning/STATE.md
@.planning/codebase/CONVENTIONS.md
@_docs/01_solution/solution.md (lines 187-222 — coordinate system spec)
@src/gps_denied/core/coordinates.py
@src/gps_denied/schemas/__init__.py

<interfaces>
<!-- Key types the executor needs -->

From src/gps_denied/schemas/__init__.py:
```python
class GPSPoint(BaseModel):
    lat: float = Field(..., ge=-90, le=90)
    lon: float = Field(..., ge=-180, le=180)

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] | None = None
    distortion_coefficients: list[float] | None = None
```

From _docs/01_solution/solution.md (camera intrinsics):
- ADTI 20L V1 + 16mm lens: fx = fy = 16 * 5456 / 23.2 = 3763 pixels
- cx = 2728, cy = 1816
- Camera mount: nadir-pointing (Z-down), R_cam_body = Rx(180deg)
</interfaces>
</context>

<tasks>

<task type="auto">
  <name>Task 1: Replace fake coordinate transforms with real camera projection math</name>
  <files>src/gps_denied/core/coordinates.py</files>
  <read_first>
    - src/gps_denied/core/coordinates.py (current file — understand existing methods, ENU convention, OriginNotSetError)
    - src/gps_denied/schemas/__init__.py (CameraParameters fields: focal_length in mm, sensor_width/height in mm, resolution_width/height in pixels, principal_point optional)
    - _docs/01_solution/solution.md (lines 187-222 for reference frames: Camera C, Body B, NED N, WGS84; transformation chain; T_cam_body spec)
  </read_first>
  <action>
Modify `src/gps_denied/core/coordinates.py`. Keep all existing methods (set_enu_origin, get_enu_origin, gps_to_enu, enu_to_gps) UNCHANGED. Replace the implementations of `pixel_to_gps`, `gps_to_pixel`, `image_object_to_gps`, and `transform_points`.

Add imports at top:
```python
import cv2
import numpy as np
```

Add module-level helper function:

```python
def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray:
    """Build 3x3 camera intrinsic matrix K from CameraParameters."""
    fx = cam.focal_length * cam.resolution_width / cam.sensor_width
    fy = cam.focal_length * cam.resolution_height / cam.sensor_height
    if cam.principal_point is not None:
        cx, cy = cam.principal_point
    else:
        cx = cam.resolution_width / 2.0
        cy = cam.resolution_height / 2.0
    return np.array([
        [fx,  0, cx],
        [ 0, fy, cy],
        [ 0,  0,  1],
    ], dtype=np.float64)
```

Add module-level helper:

```python
def _cam_to_body_rotation() -> np.ndarray:
    """Camera-to-body rotation for nadir-pointing camera.

    Camera frame: Z forward (optical axis), X right, Y down (OpenCV convention).
    Body frame: X forward (nose), Y right (starboard), Z down.
    Camera points nadir: camera Z-axis aligns with body -Z (downward).

    The rotation is Rx(180deg): flips Y and Z axes.
    R_cam_body = [[1, 0, 0], [0, -1, 0], [0, 0, -1]]
    """
    return np.array([
        [1,  0,  0],
        [0, -1,  0],
        [0,  0, -1],
    ], dtype=np.float64)
```

Add module-level helper:

```python
def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray:
    """Convert [w, x, y, z] quaternion to 3x3 rotation matrix."""
    w, x, y, z = q
    return np.array([
        [1 - 2*(y*y + z*z), 2*(x*y - w*z),     2*(x*z + w*y)],
        [2*(x*y + w*z),     1 - 2*(x*x + z*z), 2*(y*z - w*x)],
        [2*(x*z - w*y),     2*(y*z + w*x),     1 - 2*(x*x + y*y)],
    ], dtype=np.float64)
```

Replace `pixel_to_gps` method:

```python
def pixel_to_gps(
    self,
    flight_id: str,
    pixel: tuple[float, float],
    frame_pose: dict,
    camera_params: CameraParameters,
    altitude: float,
    quaternion: np.ndarray | None = None,
) -> GPSPoint:
    """Unproject pixel to GPS via ray-ground intersection.

    Chain: pixel -> camera ray (K^-1) -> body (T_cam_body) -> ENU (quaternion) -> WGS84.

    Args:
        flight_id: Flight identifier for ENU origin lookup.
        pixel: (u, v) pixel coordinates.
        frame_pose: Dict with "position" key -> [east, north, up] in ENU meters.
        camera_params: Camera intrinsic parameters.
        altitude: UAV altitude above ground in meters (positive up).
        quaternion: [w, x, y, z] body-to-ENU rotation quaternion.
                    If None, assumes identity (level flight, north-facing).
    """
    # Step 1: Pixel -> camera ray
    K = _build_intrinsic_matrix(camera_params)
    K_inv = np.linalg.inv(K)
    pixel_h = np.array([pixel[0], pixel[1], 1.0])
    ray_cam = K_inv @ pixel_h  # direction in camera frame

    # Step 2: Camera -> body
    R_cam_body = _cam_to_body_rotation()
    ray_body = R_cam_body @ ray_cam

    # Step 3: Body -> ENU
    if quaternion is not None:
        R_body_enu = _quat_to_rotation_matrix(quaternion)
    else:
        R_body_enu = np.eye(3)
    ray_enu = R_body_enu @ ray_body

    # Step 4: Ray-ground intersection
    # UAV is at altitude (Up component), ground is at Up=0
    # Ray: P = P_uav + t * ray_enu
    # Ground plane: Up = 0 -> t = -altitude / ray_enu[2]
    if abs(ray_enu[2]) < 1e-10:
        # Ray parallel to ground — return UAV position projected
        frame_enu = frame_pose.get("position", [0, 0, 0])
        return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))

    t = -altitude / ray_enu[2]
    if t < 0:
        # Ray points upward — use UAV position
        frame_enu = frame_pose.get("position", [0, 0, 0])
        return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))

    frame_enu = frame_pose.get("position", [0, 0, 0])
    ground_east = frame_enu[0] + t * ray_enu[0]
    ground_north = frame_enu[1] + t * ray_enu[1]

    # Step 5: ENU -> WGS84
    return self.enu_to_gps(flight_id, (ground_east, ground_north, 0.0))
```

Replace `gps_to_pixel` method:

```python
def gps_to_pixel(
    self,
    flight_id: str,
    gps: GPSPoint,
    frame_pose: dict,
    camera_params: CameraParameters,
    altitude: float,
    quaternion: np.ndarray | None = None,
) -> tuple[float, float]:
    """Project GPS coordinate to image pixel.

    Inverse of pixel_to_gps: WGS84 -> ENU -> body -> camera -> pixel.
    """
    # Step 1: GPS -> ENU
    enu = self.gps_to_enu(flight_id, gps)

    # Step 2: ENU point relative to UAV
    frame_enu = frame_pose.get("position", [0, 0, 0])
    point_enu = np.array([
        enu[0] - frame_enu[0],
        enu[1] - frame_enu[1],
        -altitude,  # ground is at -altitude relative to UAV (UAV is at +altitude)
    ])

    # Step 3: ENU -> body
    if quaternion is not None:
        R_body_enu = _quat_to_rotation_matrix(quaternion)
    else:
        R_body_enu = np.eye(3)
    point_body = R_body_enu.T @ point_enu

    # Step 4: Body -> camera
    R_cam_body = _cam_to_body_rotation()
    point_cam = R_cam_body.T @ point_body

    # Step 5: Camera -> pixel
    if abs(point_cam[2]) < 1e-10:
        cx = camera_params.resolution_width / 2.0
        cy = camera_params.resolution_height / 2.0
        return (cx, cy)

    K = _build_intrinsic_matrix(camera_params)
    pixel_h = K @ (point_cam / point_cam[2])
    return (float(pixel_h[0]), float(pixel_h[1]))
```

Replace `image_object_to_gps` method:

```python
def image_object_to_gps(
    self,
    flight_id: str,
    frame_id: int,
    object_pixel: tuple[float, float],
    frame_pose: dict | None = None,
    camera_params: CameraParameters | None = None,
    altitude: float = 100.0,
    quaternion: np.ndarray | None = None,
) -> GPSPoint:
    """Convert object pixel coordinates to GPS using full projection chain.

    If frame_pose or camera_params are not provided, uses defaults for
    backward compatibility (level flight, ADTI 20L V1 camera).
    """
    if frame_pose is None:
        frame_pose = {"position": [0, 0, 0]}
    if camera_params is None:
        camera_params = CameraParameters(
            focal_length=16.0,
            sensor_width=23.2,
            sensor_height=15.4,
            resolution_width=5456,
            resolution_height=3632,
        )
    return self.pixel_to_gps(
        flight_id, object_pixel, frame_pose, camera_params, altitude, quaternion
    )
```

Replace `transform_points` method:

```python
def transform_points(
    self,
    points: list[tuple[float, float]],
    transformation: list[list[float]],
) -> list[tuple[float, float]]:
    """Apply 3x3 homography to a list of 2D points using cv2.perspectiveTransform."""
    if not points:
        return []
    H = np.array(transformation, dtype=np.float64)
    pts = np.array(points, dtype=np.float64).reshape(-1, 1, 2)
    transformed = cv2.perspectiveTransform(pts, H)
    return [(float(p[0][0]), float(p[0][1])) for p in transformed]
```

Per ESKF-06: complete coordinate chain replacing all FAKE Math stubs.
  </action>
  <verify>
    <automated>python -c "
import numpy as np
from gps_denied.core.coordinates import CoordinateTransformer, _build_intrinsic_matrix
from gps_denied.schemas import CameraParameters, GPSPoint

ct = CoordinateTransformer()
ct.set_enu_origin('f1', GPSPoint(lat=48.0, lon=37.0))

# Test: image center at altitude 600m with identity quaternion should project near UAV position
cam = CameraParameters(focal_length=16.0, sensor_width=23.2, sensor_height=15.4, resolution_width=5456, resolution_height=3632)
pose = {'position': [0, 0, 0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])

gps = ct.pixel_to_gps('f1', (2728.0, 1816.0), pose, cam, 600.0, q_identity)
# Center pixel should project to UAV nadir position
assert abs(gps.lat - 48.0) < 0.001, f'lat={gps.lat}'
assert abs(gps.lon - 37.0) < 0.001, f'lon={gps.lon}'

# Test K matrix
K = _build_intrinsic_matrix(cam)
assert K.shape == (3, 3)
fx_expected = 16.0 * 5456 / 23.2
assert abs(K[0, 0] - fx_expected) < 1.0, f'fx={K[0,0]}, expected={fx_expected}'

print('Coordinate chain tests OK')
"</automated>
  </verify>
  <acceptance_criteria>
    - src/gps_denied/core/coordinates.py contains `import numpy as np`
    - src/gps_denied/core/coordinates.py contains `import cv2`
    - src/gps_denied/core/coordinates.py contains `def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray`
    - src/gps_denied/core/coordinates.py contains `def _cam_to_body_rotation() -> np.ndarray`
    - src/gps_denied/core/coordinates.py contains `def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray`
    - src/gps_denied/core/coordinates.py contains `K_inv = np.linalg.inv(K)` in pixel_to_gps
    - src/gps_denied/core/coordinates.py contains `ray_cam = K_inv @ pixel_h` in pixel_to_gps
    - src/gps_denied/core/coordinates.py contains `R_cam_body @ ray_cam` in pixel_to_gps
    - src/gps_denied/core/coordinates.py contains `t = -altitude / ray_enu[2]` in pixel_to_gps (ray-ground intersection)
    - src/gps_denied/core/coordinates.py contains `cv2.perspectiveTransform` in transform_points
    - The string "FAKE Math" does NOT appear in the file
    - The string "0.1" scaling factor does NOT appear in pixel_to_gps
    - gps_to_enu and enu_to_gps methods are UNCHANGED from original
    - image_object_to_gps uses ADTI 20L V1 defaults: focal_length=16.0, sensor_width=23.2, resolution_width=5456
  </acceptance_criteria>
  <done>All FAKE Math stubs replaced. pixel_to_gps uses K^-1 unprojection, T_cam_body rotation, quaternion body-to-ENU, and ray-ground intersection. gps_to_pixel is the exact inverse. transform_points uses cv2.perspectiveTransform. image_object_to_gps uses real ADTI 20L V1 camera defaults.</done>
</task>

</tasks>

<verification>
1. `python -c "from gps_denied.core.coordinates import CoordinateTransformer, _build_intrinsic_matrix, _cam_to_body_rotation, _quat_to_rotation_matrix; print('Imports OK')"` — all new functions importable
2. `ruff check src/gps_denied/core/coordinates.py` — no lint errors
3. `grep -c "FAKE Math" src/gps_denied/core/coordinates.py` — returns 0 (all stubs removed)
</verification>

<success_criteria>
- pixel_to_gps implements real K^-1 unprojection with camera intrinsics
- Camera-to-body rotation R_cam_body accounts for nadir-pointing mount
- Body-to-ENU rotation uses quaternion parameter
- Ray-ground intersection uses altitude for ground plane
- Image center pixel projects to UAV nadir position (verified numerically)
- gps_to_pixel is the exact inverse of pixel_to_gps
- transform_points uses cv2.perspectiveTransform
- No FAKE Math stubs remain
</success_criteria>

<output>
After completion, create `.planning/phases/01-eskf-core/01-02-SUMMARY.md`
</output>