diff --git a/src/gps_denied/testing/coord.py b/src/gps_denied/testing/coord.py new file mode 100644 index 0000000..e1d4306 --- /dev/null +++ b/src/gps_denied/testing/coord.py @@ -0,0 +1,76 @@ +"""Coordinate conversion helpers used by dataset adapters. + +- `ecef_to_wgs84`: closed-form Zhu/Heikkinen conversion (no iteration), accurate + to centimetres for any point above the Earth's surface. +- `euler_to_quaternion`: aerospace ZYX intrinsic convention (yaw → pitch → roll + applied to body frame in that order). Inputs in radians. +""" + +from __future__ import annotations + +import math + + +# WGS84 constants +_A = 6_378_137.0 # semi-major axis, metres +_F = 1.0 / 298.257223563 # flattening +_B = _A * (1.0 - _F) # semi-minor axis +_E2 = 1.0 - (_B * _B) / (_A * _A) # first eccentricity squared +_EP2 = (_A * _A) / (_B * _B) - 1.0 # second eccentricity squared + + +def ecef_to_wgs84(x: float, y: float, z: float) -> tuple[float, float, float]: + """ECEF (metres) → WGS84 (lat° N, lon° E, altitude metres above ellipsoid). + + Uses Heikkinen's closed-form method — no iteration, one pass, centimetre + accuracy for any realistic aerial vehicle altitude. + """ + r = math.sqrt(x * x + y * y) + if r == 0.0: + # Pole: lon is conventionally 0, lat is ±90 depending on sign of z + lat = 90.0 if z >= 0 else -90.0 + alt = abs(z) - _B + return lat, 0.0, alt + + ee = _A * _A - _B * _B + f = 54.0 * _B * _B * z * z + g = r * r + (1.0 - _E2) * z * z - _E2 * ee + c = (_E2 * _E2 * f * r * r) / (g * g * g) + s = (1.0 + c + math.sqrt(c * c + 2.0 * c)) ** (1.0 / 3.0) + p = f / (3.0 * (s + 1.0 / s + 1.0) ** 2 * g * g) + q = math.sqrt(1.0 + 2.0 * _E2 * _E2 * p) + r0 = -(p * _E2 * r) / (1.0 + q) + math.sqrt( + 0.5 * _A * _A * (1.0 + 1.0 / q) + - (p * (1.0 - _E2) * z * z) / (q * (1.0 + q)) + - 0.5 * p * r * r + ) + u = math.sqrt((r - _E2 * r0) ** 2 + z * z) + v = math.sqrt((r - _E2 * r0) ** 2 + (1.0 - _E2) * z * z) + z0 = (_B * _B * z) / (_A * v) + + lat = math.degrees(math.atan((z + _EP2 * z0) / r)) + lon = math.degrees(math.atan2(y, x)) + alt = u * (1.0 - (_B * _B) / (_A * v)) + return lat, lon, alt + + +def euler_to_quaternion( + roll: float, pitch: float, yaw: float +) -> tuple[float, float, float, float]: + """Aerospace ZYX intrinsic Euler → (qx, qy, qz, qw). + + Rotation order: yaw (about world z) → pitch (about intermediate y) → roll + (about body x). All inputs in radians. Output quaternion has unit norm. + """ + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + + qw = cr * cp * cy + sr * sp * sy + qx = sr * cp * cy - cr * sp * sy + qy = cr * sp * cy + sr * cp * sy + qz = cr * cp * sy - sr * sp * cy + return qx, qy, qz, qw diff --git a/tests/e2e/test_coord.py b/tests/e2e/test_coord.py new file mode 100644 index 0000000..19345fa --- /dev/null +++ b/tests/e2e/test_coord.py @@ -0,0 +1,80 @@ +"""Coordinate conversion helpers — ECEF↔WGS84 + Euler→quaternion.""" + +import numpy as np +import pytest + +from gps_denied.testing.coord import ecef_to_wgs84, euler_to_quaternion + + +# --- ECEF → WGS84 --- + +def test_ecef_origin_is_on_equator_prime_meridian(): + # Point on equator at lon=0, alt=0 is at x=6378137, y=0, z=0 (WGS84 semi-major) + lat, lon, alt = ecef_to_wgs84(6378137.0, 0.0, 0.0) + assert lat == pytest.approx(0.0, abs=1e-6) + assert lon == pytest.approx(0.0, abs=1e-6) + assert alt == pytest.approx(0.0, abs=1e-3) + + +def test_ecef_north_pole(): + # Semi-minor axis b ≈ 6356752.314 — north pole, lat=90, lon undefined but typically 0 + lat, lon, alt = ecef_to_wgs84(0.0, 0.0, 6356752.314245) + assert lat == pytest.approx(90.0, abs=1e-4) + assert alt == pytest.approx(0.0, abs=1e-2) + + +def test_ecef_known_point_munich(): + # Munich, Germany: lat≈48.1351, lon≈11.5820, alt≈520 m + # ECEF from standard converter: + # X ≈ 4177789.3, Y ≈ 855098.1, Z ≈ 4727807.9 + lat, lon, alt = ecef_to_wgs84(4177789.3, 855098.1, 4727807.9) + assert lat == pytest.approx(48.1351, abs=1e-3) + assert lon == pytest.approx(11.5820, abs=1e-3) + assert alt == pytest.approx(520.0, abs=2.0) + + +def test_ecef_vpair_sample_point(): + # From VPAIR sample poses_query.txt first row: + # 4023518.0, 510303.75, 4906569.65 — should be in Bonn/Eifel region, Germany + # (VPAIR was recorded near Bonn). Expected lat ~50.7°, lon ~7.2°, alt ~(200-400) m. + lat, lon, alt = ecef_to_wgs84(4023518.0, 510303.75, 4906569.65) + assert 50.0 < lat < 51.5, f"lat={lat}" + assert 6.5 < lon < 8.0, f"lon={lon}" + # Bonn-Eifel area ground elevation 100-500 m + assert 100.0 < alt < 700.0, f"alt={alt}" + + +# --- Euler → quaternion --- + +def test_euler_zero_is_identity_quaternion(): + qx, qy, qz, qw = euler_to_quaternion(0.0, 0.0, 0.0) + assert qx == pytest.approx(0.0, abs=1e-12) + assert qy == pytest.approx(0.0, abs=1e-12) + assert qz == pytest.approx(0.0, abs=1e-12) + assert qw == pytest.approx(1.0, abs=1e-12) + + +def test_euler_yaw_90_deg_about_z(): + # Yaw = π/2 around world z, roll=pitch=0 + # Expected quaternion: (0, 0, sin(π/4), cos(π/4)) ≈ (0, 0, 0.7071, 0.7071) + qx, qy, qz, qw = euler_to_quaternion(0.0, 0.0, np.pi / 2) + assert qx == pytest.approx(0.0, abs=1e-10) + assert qy == pytest.approx(0.0, abs=1e-10) + assert qz == pytest.approx(np.sin(np.pi / 4), abs=1e-10) + assert qw == pytest.approx(np.cos(np.pi / 4), abs=1e-10) + + +def test_euler_roll_90_deg_about_x(): + # Roll = π/2 around body x, pitch=yaw=0 + qx, qy, qz, qw = euler_to_quaternion(np.pi / 2, 0.0, 0.0) + assert qx == pytest.approx(np.sin(np.pi / 4), abs=1e-10) + assert qy == pytest.approx(0.0, abs=1e-10) + assert qz == pytest.approx(0.0, abs=1e-10) + assert qw == pytest.approx(np.cos(np.pi / 4), abs=1e-10) + + +def test_euler_unit_norm(): + # Arbitrary angles — the returned quaternion must be unit norm + qx, qy, qz, qw = euler_to_quaternion(0.3, -0.7, 1.2) + norm = (qx * qx + qy * qy + qz * qz + qw * qw) ** 0.5 + assert norm == pytest.approx(1.0, abs=1e-12)