azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-04-23 11:16:37 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
f7c8154a6fa3815a2c93866e4f3a5aeb4ca8a2dc
gps-denied-onboard/.planning/phases/01-eskf-core/01-02-PLAN.md
T

390 lines
14 KiB
Markdown
Raw Blame History

---
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>
Reference in New Issue View Git Blame Copy Permalink