mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 03:41:12 +00:00
[AZ-270] [AZ-272] [AZ-279] [AZ-281] [AZ-283] Compose root + FDR schema + 3 Layer-1 helpers
AZ-270: composition root with strategy registry, tier-gated lookup, topo-order construction, all-or-nothing teardown, StrategyNotLinkedError payload. AZ-272: orjson-backed FdrRecord serialise/parse with forward-compat for unknown payload + top-level fields and canonical overrun-record shape. AZ-279: pyproj-backed WGS84/ECEF/ENU + OSM slippy-map tile math with WgsConversionError for shape/range/zoom guards. AZ-281: strict EngineFilenameSchema build/parse/matches_host with anchored regex + enum validation; round-trip identity by construction. AZ-283: dtype-preserving (fp16/fp32) single + batch L2 normaliser with zero-norm safety and descriptor_metric() source-of-truth. pyproject.toml pins pyproj>=3.6 and orjson>=3.9 (named-backend deps per the AZ-272 / AZ-279 contracts). New DTOs LatLonAlt + BoundingBox and EngineCacheKey + HostCapabilities land in _types/ to back the helper contracts. 203 unit tests pass (64 new). Review verdict: PASS_WITH_WARNINGS; findings are perf-NFR deferrals + dep amendment + minor docstring polish. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -1,26 +1,170 @@
|
||||
"""WGS84 ↔ local-tangent-plane converter — STUB.
|
||||
"""WGS84 ↔ ECEF ↔ ENU ↔ slippy-map tile-xy conversions (AZ-279 / E-CC-HELPERS).
|
||||
|
||||
Concrete implementation is owned by AZ-279. Contract:
|
||||
`_docs/02_document/common-helpers/04_helper_wgs_converter.md`.
|
||||
Public surface frozen by
|
||||
``_docs/02_document/contracts/shared_helpers/wgs_converter.md`` v1.0.0.
|
||||
|
||||
Backed by ``pyproj`` for the geodesy primitives. Slippy-map tile math is hand
|
||||
rolled to match OSM's `{zoom}/{x}/{y}.jpg` convention exactly so the on-disk
|
||||
layout produced by ``satellite-provider`` round-trips byte-equal.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Final
|
||||
|
||||
def wgs84_to_ltp(
|
||||
lat_deg: float,
|
||||
lon_deg: float,
|
||||
alt_m: float,
|
||||
ref_lat_deg: float,
|
||||
ref_lon_deg: float,
|
||||
ref_alt_m: float,
|
||||
) -> tuple[float, float, float]:
|
||||
"""Convert a WGS-84 lat/lon/alt to local-tangent-plane east/north/up metres."""
|
||||
raise NotImplementedError("wgs_converter concrete impl is AZ-279")
|
||||
import numpy as np
|
||||
from pyproj import Transformer # type: ignore[import-not-found]
|
||||
|
||||
from gps_denied_onboard._types.geo import BoundingBox, LatLonAlt
|
||||
|
||||
__all__ = ["MAX_ZOOM", "WEB_MERCATOR_MAX_LAT_DEG", "WgsConversionError", "WgsConverter"]
|
||||
|
||||
|
||||
def ltp_to_wgs84(
|
||||
e_m: float, n_m: float, u_m: float, ref_lat_deg: float, ref_lon_deg: float, ref_alt_m: float
|
||||
) -> tuple[float, float, float]:
|
||||
"""Inverse of wgs84_to_ltp."""
|
||||
raise NotImplementedError("wgs_converter concrete impl is AZ-279")
|
||||
WEB_MERCATOR_MAX_LAT_DEG: Final[float] = 85.0511287798066
|
||||
MAX_ZOOM: Final[int] = 22
|
||||
|
||||
|
||||
class WgsConversionError(ValueError):
|
||||
"""Raised on shape / range violations in any ``WgsConverter`` static method."""
|
||||
|
||||
|
||||
_ECEF_FROM_LLA: Final[Transformer] = Transformer.from_crs("EPSG:4326", "EPSG:4978", always_xy=True)
|
||||
_LLA_FROM_ECEF: Final[Transformer] = Transformer.from_crs("EPSG:4978", "EPSG:4326", always_xy=True)
|
||||
|
||||
|
||||
def _validate_finite_latlonalt(p: LatLonAlt, label: str) -> None:
|
||||
if not (math.isfinite(p.lat_deg) and math.isfinite(p.lon_deg) and math.isfinite(p.alt_m)):
|
||||
raise WgsConversionError(f"{label}: non-finite component in {p!r}")
|
||||
if not (-90.0 <= p.lat_deg <= 90.0):
|
||||
raise WgsConversionError(f"{label}: latitude {p.lat_deg} outside [-90, 90]")
|
||||
if not (-180.0 <= p.lon_deg <= 180.0):
|
||||
raise WgsConversionError(f"{label}: longitude {p.lon_deg} outside [-180, 180]")
|
||||
|
||||
|
||||
def _enforce_ecef_shape(arr: np.ndarray, label: str) -> None:
|
||||
if not isinstance(arr, np.ndarray):
|
||||
raise WgsConversionError(
|
||||
f"{label}: expected np.ndarray of shape (3,); got {type(arr).__name__}"
|
||||
)
|
||||
if arr.shape != (3,):
|
||||
raise WgsConversionError(
|
||||
f"{label}: expected np.ndarray of shape (3,); got shape {arr.shape}"
|
||||
)
|
||||
if not np.all(np.isfinite(arr)):
|
||||
raise WgsConversionError(f"{label}: non-finite component in {arr!r}")
|
||||
|
||||
|
||||
class WgsConverter:
|
||||
"""Stateless WGS84 / ECEF / ENU / slippy-map-tile converter.
|
||||
|
||||
Every method is a pure function of its arguments; no module-level state
|
||||
other than the cached ``pyproj`` transformer pair.
|
||||
"""
|
||||
|
||||
@staticmethod
|
||||
def latlonalt_to_ecef(p: LatLonAlt) -> np.ndarray:
|
||||
_validate_finite_latlonalt(p, "latlonalt_to_ecef")
|
||||
x, y, z = _ECEF_FROM_LLA.transform(p.lon_deg, p.lat_deg, p.alt_m)
|
||||
return np.array([x, y, z], dtype=np.float64)
|
||||
|
||||
@staticmethod
|
||||
def ecef_to_latlonalt(p_ecef: np.ndarray) -> LatLonAlt:
|
||||
_enforce_ecef_shape(p_ecef, "ecef_to_latlonalt")
|
||||
lon, lat, alt = _LLA_FROM_ECEF.transform(
|
||||
float(p_ecef[0]), float(p_ecef[1]), float(p_ecef[2])
|
||||
)
|
||||
return LatLonAlt(lat_deg=float(lat), lon_deg=float(lon), alt_m=float(alt))
|
||||
|
||||
@staticmethod
|
||||
def latlonalt_to_local_enu(origin: LatLonAlt, p: LatLonAlt) -> np.ndarray:
|
||||
_validate_finite_latlonalt(origin, "latlonalt_to_local_enu/origin")
|
||||
_validate_finite_latlonalt(p, "latlonalt_to_local_enu/p")
|
||||
return _ecef_delta_to_enu(origin, WgsConverter.latlonalt_to_ecef(p))
|
||||
|
||||
@staticmethod
|
||||
def local_enu_to_latlonalt(origin: LatLonAlt, p_enu: np.ndarray) -> LatLonAlt:
|
||||
_validate_finite_latlonalt(origin, "local_enu_to_latlonalt/origin")
|
||||
_enforce_ecef_shape(p_enu, "local_enu_to_latlonalt/p_enu")
|
||||
origin_ecef = WgsConverter.latlonalt_to_ecef(origin)
|
||||
rotation = _enu_to_ecef_rotation(origin.lat_deg, origin.lon_deg)
|
||||
delta_ecef = rotation @ p_enu.astype(np.float64)
|
||||
return WgsConverter.ecef_to_latlonalt(origin_ecef + delta_ecef)
|
||||
|
||||
@staticmethod
|
||||
def latlon_to_tile_xy(zoom: int, lat: float, lon: float) -> tuple[int, int]:
|
||||
_validate_zoom(zoom)
|
||||
if not (math.isfinite(lat) and math.isfinite(lon)):
|
||||
raise WgsConversionError(f"latlon_to_tile_xy: non-finite input (lat={lat}, lon={lon})")
|
||||
if abs(lat) > WEB_MERCATOR_MAX_LAT_DEG:
|
||||
raise WgsConversionError(
|
||||
f"latlon_to_tile_xy: latitude {lat} outside Web-Mercator range "
|
||||
f"[-{WEB_MERCATOR_MAX_LAT_DEG}, {WEB_MERCATOR_MAX_LAT_DEG}]"
|
||||
)
|
||||
if not (-180.0 <= lon <= 180.0):
|
||||
raise WgsConversionError(f"latlon_to_tile_xy: longitude {lon} outside [-180, 180]")
|
||||
n = 1 << zoom
|
||||
lat_rad = math.radians(lat)
|
||||
x = math.floor((lon + 180.0) / 360.0 * n)
|
||||
y = math.floor(
|
||||
(1.0 - math.log(math.tan(lat_rad) + 1.0 / math.cos(lat_rad)) / math.pi) / 2.0 * n
|
||||
)
|
||||
x = max(0, min(x, n - 1))
|
||||
y = max(0, min(y, n - 1))
|
||||
return x, y
|
||||
|
||||
@staticmethod
|
||||
def tile_xy_to_latlon_bounds(zoom: int, x: int, y: int) -> BoundingBox:
|
||||
_validate_zoom(zoom)
|
||||
n = 1 << zoom
|
||||
if not (0 <= x < n and 0 <= y < n):
|
||||
raise WgsConversionError(
|
||||
f"tile_xy_to_latlon_bounds: tile (x={x}, y={y}) outside [0, {n}) at zoom {zoom}"
|
||||
)
|
||||
return BoundingBox(
|
||||
min_lat_deg=_tile_y_to_lat(y + 1, n),
|
||||
min_lon_deg=_tile_x_to_lon(x, n),
|
||||
max_lat_deg=_tile_y_to_lat(y, n),
|
||||
max_lon_deg=_tile_x_to_lon(x + 1, n),
|
||||
)
|
||||
|
||||
|
||||
def _validate_zoom(zoom: int) -> None:
|
||||
if not isinstance(zoom, int) or isinstance(zoom, bool):
|
||||
raise WgsConversionError(f"zoom must be a non-bool integer; got {zoom!r}")
|
||||
if not (0 <= zoom <= MAX_ZOOM):
|
||||
raise WgsConversionError(f"zoom {zoom} outside supported range [0, {MAX_ZOOM}]")
|
||||
|
||||
|
||||
def _tile_x_to_lon(x: int, n: int) -> float:
|
||||
return x / n * 360.0 - 180.0
|
||||
|
||||
|
||||
def _tile_y_to_lat(y: int, n: int) -> float:
|
||||
t = math.pi * (1.0 - 2.0 * y / n)
|
||||
return math.degrees(math.atan(math.sinh(t)))
|
||||
|
||||
|
||||
def _enu_to_ecef_rotation(lat_deg: float, lon_deg: float) -> np.ndarray:
|
||||
"""Rotation matrix mapping local ENU vectors to ECEF deltas at ``(lat, lon)``."""
|
||||
lat = math.radians(lat_deg)
|
||||
lon = math.radians(lon_deg)
|
||||
sin_lat = math.sin(lat)
|
||||
cos_lat = math.cos(lat)
|
||||
sin_lon = math.sin(lon)
|
||||
cos_lon = math.cos(lon)
|
||||
return np.array(
|
||||
[
|
||||
[-sin_lon, -sin_lat * cos_lon, cos_lat * cos_lon],
|
||||
[cos_lon, -sin_lat * sin_lon, cos_lat * sin_lon],
|
||||
[0.0, cos_lat, sin_lat],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
|
||||
def _ecef_delta_to_enu(origin: LatLonAlt, p_ecef: np.ndarray) -> np.ndarray:
|
||||
origin_ecef = WgsConverter.latlonalt_to_ecef(origin)
|
||||
delta = p_ecef - origin_ecef
|
||||
rotation = _enu_to_ecef_rotation(origin.lat_deg, origin.lon_deg)
|
||||
return rotation.T @ delta
|
||||
|
||||
Reference in New Issue
Block a user