Spaces:
Running
Running
# Copied from OrienterNet | |
# Copyright (c) Meta Platforms, Inc. and affiliates. | |
import numpy as np | |
from numpy import ndarray | |
from typing import Tuple | |
WGS84_a = 6378137.0 | |
WGS84_b = 6356752.314245 | |
def ecef_from_lla(lat, lon, alt: float) -> Tuple[float, ...]: | |
""" | |
Compute ECEF XYZ from latitude, longitude and altitude. | |
All using the WGS84 model. | |
Altitude is the distance to the WGS84 ellipsoid. | |
Check results here http://www.oc.nps.edu/oc2902w/coord/llhxyz.htm | |
>>> lat, lon, alt = 10, 20, 30 | |
>>> x, y, z = ecef_from_lla(lat, lon, alt) | |
>>> np.allclose(lla_from_ecef(x,y,z), [lat, lon, alt]) | |
True | |
""" | |
a2 = WGS84_a**2 | |
b2 = WGS84_b**2 | |
lat = np.radians(lat) | |
lon = np.radians(lon) | |
L = 1.0 / np.sqrt(a2 * np.cos(lat) ** 2 + b2 * np.sin(lat) ** 2) | |
x = (a2 * L + alt) * np.cos(lat) * np.cos(lon) | |
y = (a2 * L + alt) * np.cos(lat) * np.sin(lon) | |
z = (b2 * L + alt) * np.sin(lat) | |
return x, y, z | |
def lla_from_ecef(x, y, z): | |
""" | |
Compute latitude, longitude and altitude from ECEF XYZ. | |
All using the WGS84 model. | |
Altitude is the distance to the WGS84 ellipsoid. | |
""" | |
a = WGS84_a | |
b = WGS84_b | |
ea = np.sqrt((a**2 - b**2) / a**2) | |
eb = np.sqrt((a**2 - b**2) / b**2) | |
p = np.sqrt(x**2 + y**2) | |
theta = np.arctan2(z * a, p * b) | |
lon = np.arctan2(y, x) | |
lat = np.arctan2( | |
z + eb**2 * b * np.sin(theta) ** 3, p - ea**2 * a * np.cos(theta) ** 3 | |
) | |
N = a / np.sqrt(1 - ea**2 * np.sin(lat) ** 2) | |
alt = p / np.cos(lat) - N | |
return np.degrees(lat), np.degrees(lon), alt | |
def ecef_from_topocentric_transform(lat, lon, alt: float) -> ndarray: | |
""" | |
Transformation from a topocentric frame at reference position to ECEF. | |
The topocentric reference frame is a metric one with the origin | |
at the given (lat, lon, alt) position, with the X axis heading east, | |
the Y axis heading north and the Z axis vertical to the ellipsoid. | |
>>> a = ecef_from_topocentric_transform(30, 20, 10) | |
>>> b = ecef_from_topocentric_transform_finite_diff(30, 20, 10) | |
>>> np.allclose(a, b) | |
True | |
""" | |
x, y, z = ecef_from_lla(lat, lon, alt) | |
sa = np.sin(np.radians(lat)) | |
ca = np.cos(np.radians(lat)) | |
so = np.sin(np.radians(lon)) | |
co = np.cos(np.radians(lon)) | |
return np.array( | |
[ | |
[-so, -sa * co, ca * co, x], | |
[co, -sa * so, ca * so, y], | |
[0, ca, sa, z], | |
[0, 0, 0, 1], | |
] | |
) | |
def ecef_from_topocentric_transform_finite_diff(lat, lon, alt: float) -> ndarray: | |
""" | |
Transformation from a topocentric frame at reference position to ECEF. | |
The topocentric reference frame is a metric one with the origin | |
at the given (lat, lon, alt) position, with the X axis heading east, | |
the Y axis heading north and the Z axis vertical to the ellipsoid. | |
""" | |
eps = 1e-2 | |
x, y, z = ecef_from_lla(lat, lon, alt) | |
v1 = ( | |
( | |
np.array(ecef_from_lla(lat, lon + eps, alt)) | |
- np.array(ecef_from_lla(lat, lon - eps, alt)) | |
) | |
/ 2 | |
/ eps | |
) | |
v2 = ( | |
( | |
np.array(ecef_from_lla(lat + eps, lon, alt)) | |
- np.array(ecef_from_lla(lat - eps, lon, alt)) | |
) | |
/ 2 | |
/ eps | |
) | |
v3 = ( | |
( | |
np.array(ecef_from_lla(lat, lon, alt + eps)) | |
- np.array(ecef_from_lla(lat, lon, alt - eps)) | |
) | |
/ 2 | |
/ eps | |
) | |
v1 /= np.linalg.norm(v1) | |
v2 /= np.linalg.norm(v2) | |
v3 /= np.linalg.norm(v3) | |
return np.array( | |
[ | |
[v1[0], v2[0], v3[0], x], | |
[v1[1], v2[1], v3[1], y], | |
[v1[2], v2[2], v3[2], z], | |
[0, 0, 0, 1], | |
] | |
) | |
def topocentric_from_lla(lat, lon, alt: float, reflat, reflon, refalt: float): | |
""" | |
Transform from lat, lon, alt to topocentric XYZ. | |
>>> lat, lon, alt = -10, 20, 100 | |
>>> np.allclose(topocentric_from_lla(lat, lon, alt, lat, lon, alt), | |
... [0,0,0]) | |
True | |
>>> x, y, z = topocentric_from_lla(lat, lon, alt, 0, 0, 0) | |
>>> np.allclose(lla_from_topocentric(x, y, z, 0, 0, 0), | |
... [lat, lon, alt]) | |
True | |
""" | |
T = np.linalg.inv(ecef_from_topocentric_transform(reflat, reflon, refalt)) | |
x, y, z = ecef_from_lla(lat, lon, alt) | |
tx = T[0, 0] * x + T[0, 1] * y + T[0, 2] * z + T[0, 3] | |
ty = T[1, 0] * x + T[1, 1] * y + T[1, 2] * z + T[1, 3] | |
tz = T[2, 0] * x + T[2, 1] * y + T[2, 2] * z + T[2, 3] | |
return tx, ty, tz | |
def lla_from_topocentric(x, y, z, reflat, reflon, refalt: float): | |
""" | |
Transform from topocentric XYZ to lat, lon, alt. | |
""" | |
T = ecef_from_topocentric_transform(reflat, reflon, refalt) | |
ex = T[0, 0] * x + T[0, 1] * y + T[0, 2] * z + T[0, 3] | |
ey = T[1, 0] * x + T[1, 1] * y + T[1, 2] * z + T[1, 3] | |
ez = T[2, 0] * x + T[2, 1] * y + T[2, 2] * z + T[2, 3] | |
return lla_from_ecef(ex, ey, ez) | |
class TopocentricConverter(object): | |
"""Convert to and from a topocentric reference frame.""" | |
def __init__(self, reflat, reflon, refalt): | |
"""Init the converter given the reference origin.""" | |
self.lat = reflat | |
self.lon = reflon | |
self.alt = refalt | |
def to_topocentric(self, lat, lon, alt): | |
"""Convert lat, lon, alt to topocentric x, y, z.""" | |
return topocentric_from_lla(lat, lon, alt, self.lat, self.lon, self.alt) | |
def to_lla(self, x, y, z): | |
"""Convert topocentric x, y, z to lat, lon, alt.""" | |
return lla_from_topocentric(x, y, z, self.lat, self.lon, self.alt) | |
def __eq__(self, o): | |
return np.allclose([self.lat, self.lon, self.alt], (o.lat, o.lon, o.alt)) |