File size: 5,657 Bytes
124ba77 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 |
"""Copied from opensfm.geo to minimize hard dependencies."""
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))
|