mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 21:56:38 +00:00
Yuzviak
dd9835c0cd
- ruff --fix: removed trailing whitespace (W293), sorted imports (I001) - Manual: broke long lines (E501) in eskf, rotation, vo, gpr, metric, pipeline, rotation tests - Removed unused imports (F401) in models.py, schemas/__init__.py - pyproject.toml: line-length 100→120, E501 ignore for abstract interfaces ruff check: 0 errors. pytest: 195 passed / 8 skipped. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
390 lines
13 KiB
Python
390 lines
13 KiB
Python
"""Tests for CoordinateTransformer (F13) — Real coordinate chain (ESKF-06)."""
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
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
|
|
|
|
|
|
@pytest.fixture
|
|
def transformer():
|
|
return CoordinateTransformer()
|
|
|
|
|
|
def test_enu_origin_management(transformer):
|
|
fid = "flight_123"
|
|
origin = GPSPoint(lat=48.0, lon=37.0)
|
|
|
|
# Before setting
|
|
with pytest.raises(OriginNotSetError):
|
|
transformer.get_enu_origin(fid)
|
|
|
|
# After setting
|
|
transformer.set_enu_origin(fid, origin)
|
|
assert transformer.get_enu_origin(fid).lat == 48.0
|
|
|
|
|
|
def test_gps_to_enu(transformer):
|
|
fid = "flight_123"
|
|
origin = GPSPoint(lat=48.0, lon=37.0)
|
|
transformer.set_enu_origin(fid, origin)
|
|
|
|
# Same point -> 0, 0, 0
|
|
enu = transformer.gps_to_enu(fid, origin)
|
|
assert enu == (0.0, 0.0, 0.0)
|
|
|
|
# Point north
|
|
target = GPSPoint(lat=48.01, lon=37.0)
|
|
enu_n = transformer.gps_to_enu(fid, target)
|
|
assert enu_n[0] == 0.0
|
|
assert enu_n[1] > 1000.0 # 0.01 deg lat is > 1km
|
|
assert enu_n[2] == 0.0
|
|
|
|
|
|
def test_enu_roundtrip(transformer):
|
|
fid = "flight_123"
|
|
origin = GPSPoint(lat=48.0, lon=37.0)
|
|
transformer.set_enu_origin(fid, origin)
|
|
|
|
test_gps = GPSPoint(lat=48.056, lon=37.123)
|
|
enu = transformer.gps_to_enu(fid, test_gps)
|
|
|
|
recovered = transformer.enu_to_gps(fid, enu)
|
|
assert pytest.approx(recovered.lat, abs=1e-6) == test_gps.lat
|
|
assert pytest.approx(recovered.lon, abs=1e-6) == test_gps.lon
|
|
|
|
|
|
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,
|
|
sensor_height=15.6,
|
|
resolution_width=4000,
|
|
resolution_height=3000,
|
|
)
|
|
|
|
# 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]}
|
|
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, 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 == []
|