azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-04-22 22:46:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat(tests): add comprehensive ESKF + coordinate chain tests (ESKF-01..06)

Browse Source 
test_eskf.py: 18 tests covering initialization, IMU prediction, VO/satellite
updates, confidence tiers, and full fusion integration.
test_coordinates.py: 17 new tests for K matrix, ray-ground intersection,
pixel-GPS roundtrips, and cv2.perspectiveTransform homography.

All 35 tests pass.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Yuzviak
2026-04-01 23:52:58 +03:00
parent dccadd4bf4
commit 2e5436a6c7
2 changed files with 638 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_coordinates.py
+321 -16
View File
@@ -1,8 +1,15 @@
"""Tests for CoordinateTransformer (F13)."""
"""Tests for CoordinateTransformer (F13) — Real coordinate chain (ESKF-06)."""
import numpy as np
import pytest
from gps_denied.core.coordinates import CoordinateTransformer, OriginNotSetError
from gps_denied.core.coordinates import (
CoordinateTransformer,
OriginNotSetError,
_build_intrinsic_matrix,
_cam_to_body_rotation,
_quat_to_rotation_matrix,
)
from gps_denied.schemas import CameraParameters, GPSPoint
@@ -58,7 +65,7 @@ def test_pixel_to_gps_flow(transformer):
fid = "flight_123"
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin(fid, origin)
cam = CameraParameters(
focal_length=25.0,
sensor_width=23.5,
@@ -66,19 +73,317 @@ def test_pixel_to_gps_flow(transformer):
resolution_width=4000,
resolution_height=3000,
)
# Image center should yield the frame center (mock implementation logic)
pixel = (2000.0, 1500.0)
# Image center should yield approximately the frame center
cx = cam.resolution_width / 2.0
cy = cam.resolution_height / 2.0
pixel = (cx, cy)
pose = {"position": [0, 0, 0]}
gps_res = transformer.pixel_to_gps(fid, pixel, pose, cam, 100.0)
assert gps_res.lat == origin.lat
assert gps_res.lon == origin.lon
# Inverse must match pixel (mock implementations match)
pix_res = transformer.gps_to_pixel(fid, gps_res, pose, cam, 100.0)
assert pix_res == pixel
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
gps_res = transformer.pixel_to_gps(fid, pixel, pose, cam, 100.0, quaternion=q_identity)
assert pytest.approx(gps_res.lat, abs=1e-4) == origin.lat
assert pytest.approx(gps_res.lon, abs=1e-4) == origin.lon
# Inverse must match pixel
pix_res = transformer.gps_to_pixel(fid, gps_res, pose, cam, 100.0, quaternion=q_identity)
assert abs(pix_res[0] - pixel[0]) < 1.0
assert abs(pix_res[1] - pixel[1]) < 1.0
# And image_object_to_gps should work
obj_gps = transformer.image_object_to_gps(fid, 1, pixel)
assert obj_gps.lat == origin.lat
obj_gps = transformer.image_object_to_gps(
fid, 1, pixel, frame_pose=pose, camera_params=cam, altitude=100.0, quaternion=q_identity
)
assert pytest.approx(obj_gps.lat, abs=1e-4) == origin.lat
# ============================================================================
# ESKF-06: Real Coordinate Chain Tests
# ============================================================================
class TestIntrinsicMatrix:
"""Tests for camera intrinsic matrix construction."""
def test_build_intrinsic_matrix_adti_20l_v1(self):
"""Test K matrix for ADTI 20L V1 camera with 16mm lens."""
cam = CameraParameters(
focal_length=16.0,
sensor_width=23.2,
sensor_height=15.4,
resolution_width=5456,
resolution_height=3632,
)
K = _build_intrinsic_matrix(cam)
# Expected focal lengths
fx_expected = 16.0 * 5456 / 23.2
fy_expected = 16.0 * 3632 / 15.4
assert K.shape == (3, 3)
assert abs(K[0, 0] - fx_expected) < 1.0 # fx
assert abs(K[1, 1] - fy_expected) < 1.0 # fy
assert abs(K[0, 2] - 2728.0) < 1.0 # cx (center)
assert abs(K[1, 2] - 1816.0) < 1.0 # cy (center)
assert K[2, 2] == 1.0
def test_build_intrinsic_matrix_custom_principal_point(self):
"""Test K matrix with custom principal point."""
cam = CameraParameters(
focal_length=16.0,
sensor_width=23.2,
sensor_height=15.4,
resolution_width=5456,
resolution_height=3632,
principal_point=(2700.0, 1800.0),
)
K = _build_intrinsic_matrix(cam)
assert abs(K[0, 2] - 2700.0) < 0.1 # cx
assert abs(K[1, 2] - 1800.0) < 0.1 # cy
class TestCameraToBodyRotation:
"""Tests for camera-to-body rotation matrix."""
def test_cam_to_body_is_180_x_rotation(self):
"""Test that camera-to-body rotation is Rx(180deg)."""
R_cam_body = _cam_to_body_rotation()
# Should flip Y and Z axes, keep X
assert R_cam_body.shape == (3, 3)
assert R_cam_body[0, 0] == 1.0
assert R_cam_body[1, 1] == -1.0
assert R_cam_body[2, 2] == -1.0
assert np.allclose(np.linalg.det(R_cam_body), 1.0) # det = 1 (proper rotation)
class TestQuaternionRotation:
"""Tests for quaternion to rotation matrix conversion."""
def test_identity_quaternion(self):
"""Test identity quaternion [1, 0, 0, 0] produces identity matrix."""
q = np.array([1.0, 0.0, 0.0, 0.0])
R = _quat_to_rotation_matrix(q)
assert np.allclose(R, np.eye(3))
def test_90_degree_z_rotation(self):
"""Test 90-degree rotation around Z axis."""
# q = [cos(45deg), 0, 0, sin(45deg)] for 90-degree rotation around Z
angle = np.pi / 4 # 45 degrees
q = np.array([np.cos(angle), 0.0, 0.0, np.sin(angle)])
R = _quat_to_rotation_matrix(q)
# Should rotate [1, 0, 0] to approximately [0, 1, 0]
x_axis = np.array([1.0, 0.0, 0.0])
rotated = R @ x_axis
expected = np.array([0.0, 1.0, 0.0])
assert np.allclose(rotated, expected, atol=0.01)
class TestPixelToGPSChain:
"""Tests for full pixel-to-GPS projection chain (ESKF-06)."""
def test_image_center_nadir_projection(self):
"""Test that image center pixel projects to UAV nadir position."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
cam = CameraParameters(
focal_length=16.0,
sensor_width=23.2,
sensor_height=15.4,
resolution_width=5456,
resolution_height=3632,
)
# Center pixel with identity quaternion (level flight)
pixel = (2728.0, 1816.0)
pose = {"position": [0.0, 0.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
gps = transformer.pixel_to_gps(
"f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
)
# Center pixel should project to UAV nadir (origin)
assert abs(gps.lat - 48.0) < 0.001
assert abs(gps.lon - 37.0) < 0.001
def test_pixel_offset_produces_ground_offset(self):
"""Test that off-center pixel offsets project to corresponding ground distance."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
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.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
altitude = 600.0
# Center pixel
center_pixel = (2728.0, 1816.0)
center_gps = transformer.pixel_to_gps(
"f1", center_pixel, pose, cam, altitude=altitude, quaternion=q_identity
)
# Offset pixel (100 pixels to the right)
offset_pixel = (2828.0, 1816.0)
offset_gps = transformer.pixel_to_gps(
"f1", offset_pixel, pose, cam, altitude=altitude, quaternion=q_identity
)
# Longitude difference should be non-zero (right = east = longer)
assert offset_gps.lon != center_gps.lon
def test_altitude_scaling_is_linear(self):
"""Test that doubling altitude doubles ground distance from nadir."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
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.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
pixel = (2728.0 + 100, 1816.0) # Offset pixel
# Project at altitude 300m
gps_300 = transformer.pixel_to_gps(
"f1", pixel, pose, cam, altitude=300.0, quaternion=q_identity
)
# Project at altitude 600m
gps_600 = transformer.pixel_to_gps(
"f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
)
# Distance from origin should scale with altitude (roughly)
# At 600m, ground distance should be ~2x that at 300m
dist_300 = abs(gps_300.lon - 37.0)
dist_600 = abs(gps_600.lon - 37.0)
ratio = dist_600 / dist_300 if dist_300 > 0 else 1.0
assert 1.8 < ratio < 2.2 # Allow some numerical tolerance
class TestGPSToPixelInverse:
"""Tests for GPS-to-pixel projection (inverse of pixel-to-GPS)."""
def test_gps_to_pixel_is_inverse(self):
"""Test that gps_to_pixel is the exact inverse of pixel_to_gps."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
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.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
original_pixel = (2728.0 + 50, 1816.0 - 30)
# pixel -> GPS
gps = transformer.pixel_to_gps(
"f1", original_pixel, pose, cam, altitude=600.0, quaternion=q_identity
)
# GPS -> pixel
recovered_pixel = transformer.gps_to_pixel(
"f1", gps, pose, cam, altitude=600.0, quaternion=q_identity
)
# Should recover original pixel
assert abs(recovered_pixel[0] - original_pixel[0]) < 1.0
assert abs(recovered_pixel[1] - original_pixel[1]) < 1.0
def test_roundtrip_with_quaternion(self):
"""Test pixel-GPS-pixel roundtrip with non-identity quaternion."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
cam = CameraParameters(
focal_length=16.0,
sensor_width=23.2,
sensor_height=15.4,
resolution_width=5456,
resolution_height=3632,
)
pose = {"position": [100.0, 50.0, 0.0]}
# Small 30-degree rotation around Z
angle = np.radians(30)
q = np.array([np.cos(angle / 2), 0, 0, np.sin(angle / 2)])
original_pixel = (2728.0, 1816.0) # Center
gps = transformer.pixel_to_gps(
"f1", original_pixel, pose, cam, altitude=600.0, quaternion=q
)
recovered_pixel = transformer.gps_to_pixel(
"f1", gps, pose, cam, altitude=600.0, quaternion=q
)
# Center pixel should still recover to center
assert abs(recovered_pixel[0] - 2728.0) < 10.0
assert abs(recovered_pixel[1] - 1816.0) < 10.0
class TestTransformPoints:
"""Tests for homography transform via cv2.perspectiveTransform."""
def test_transform_points_identity(self):
"""Test homography transform with identity matrix."""
transformer = CoordinateTransformer()
H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] # Identity
points = [(10.0, 20.0), (30.0, 40.0)]
result = transformer.transform_points(points, H)
assert len(result) == 2
assert abs(result[0][0] - 10.0) < 0.1
assert abs(result[0][1] - 20.0) < 0.1
assert abs(result[1][0] - 30.0) < 0.1
assert abs(result[1][1] - 40.0) < 0.1
def test_transform_points_translation(self):
"""Test homography with translation."""
transformer = CoordinateTransformer()
# Translate by (100, 50)
H = [[1, 0, 100], [0, 1, 50], [0, 0, 1]]
points = [(0.0, 0.0)]
result = transformer.transform_points(points, H)
assert len(result) == 1
assert abs(result[0][0] - 100.0) < 0.1
assert abs(result[0][1] - 50.0) < 0.1
def test_transform_points_empty(self):
"""Test transform_points with empty point list."""
transformer = CoordinateTransformer()
H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
result = transformer.transform_points([], H)
assert result == []