docs(01-eskf-core): create phase plan — 3 plans in 2 waves

2026-04-23 09:46:37 +00:00 · 2026-04-01 22:11:57 +03:00
parent d2b431f17f
commit bf9bef19c8
5 changed files with 1212 additions and 2 deletions
@@ -0,0 +1,389 @@
+---
+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>