"""Initialize an ellipsoid"""
from math import *
import numpy as np
importsys
class Geoid:
def __init__(self, a, f):
"""
Parameters
----------
a : float, radius in meters
f: float, flattening
"""
self.a = a
self.f = f
self.e2 = f * (2 - f)
self.e2m = (1 - f)**2
self.e2a = abs(self.e2)
self.e4a = self.e2**2
self.max_rad = 2. * self.a/sys.float_info.epsilon
wgs84_a = 6378137
wgs84_f = 1 / (np.uint64(298257223563) / 1000000000)
wgs84_geoid = Geoid(wgs84_a, wgs84_f)
def geocentric_forward(lat, lon, h, geoid=wgs84_geoid):
"""
Parameters
----------
lat : float, latitude in degrees [-90, 90]
lon: float, longitude in degrees [-180, 180]
h : float, height in meters
geoid : the ellipsoid model
Returns
-------
x : float, meters
y : float, meters
z: float, meters
"""
sphi = sin(radians(lat))
cphi = cos(radians(lat))
slam = sin(radians(lon))
clam = cos(radians(lon))
n = geoid.a / sqrt(1. - geoid.e2 * sphi**2)
Z = (geoid.e2m * n + h) * sphi
X = (n + h) * cphi
Y = X * slam
X *= clam
return (X, Y, Z)
def geocentric_reverse(x, y, z, geoid=wgs84_geoid):
"""
Parameters
----------
x : float, meters
y: float, meters
z: float, meters
geoid : the ellipsoid model
Returns
-------
latitude : float, latitude in degrees [-90, 90]
longitude : float, longitude in degrees [-180, 180]
height : float, height in meters
"""
R = hypot(x, y)
slam = y / R if R else 0.
clam = x / R if R else 1.
#Distance to center of earth
h = hypot(R, z)
if h > geoid.max_rad:
# We really far away (> 12 million light years); treat the earth as a
# point and h, above, is an acceptable approximation to the height.
# This avoids overflow, e.g., in the computation of disc below. It's
# possible that h has overflowed to inf; but that's OK.
#
# Treat the case x, y finite, but R overflows to + inf by scaling by 2.
R = hypot(x / 2., y / 2.)
slam = (y / 2.) / R if R else 0.
clam = (x / 2.) / R if R else 1.
H = hypot(z / 2., R)
sphi = (z / 2.) / H
cphi=R/H
elif geoid.e4a == 0.:
# Treat the spherical case. Dealing with underflow in the general case
# with _e2 = 0 is difficult. Origin maps to N pole same as with
#ellipsoid.
H = hypot(1. if h == 0. else z, R)
sphi = (1. if h == 0. else z) / H
cphi=R/H
h -= geoid.a
else:
# Treat prolate spheroids by swapping R and z here and by switching
# the arguments to phi = atan2(...) at the end.
p = (R / geoid.a)**2
q = geoid.e2m * (z / geoid.a)**2
r = (p + q - geoid.e4a) / 6.
if geoid.f < 0:
p, q = q, p
if not (geoid.e4a * q == 0 and r <= 0):
# Avoid possible division by zero when r = 0 by multiplying
# equations for s and t by r^3 and r, resp.
S = geoid.e4a * p * q / 4.
r2 = r**2
r3 = r * r2
disc = S * (2. * r3 + S)
u = r
if disc >= 0:
T3 = S + r3
# Pick the sign on the sqrt to maximize abs(T3). This minimizes
# loss of precision due to cancellation. The result is unchanged
# because of the way the T is used in definition of u.
T3 + = -sqrt(disc) if T3 < 0 else sqrt(disc)
# N.B. cbrt always returns the real root. cbrt(-8) = -2.
T = T3**(1. / 3.)
# T can be zero; but then r2 / T -> 0.
u + = T + (r2 / T if T else 0.)
else:
# T is complex, but the way u is defined the result is real.
ang = atan2(sqrt(-disc), -(S + r3))
# There are three possible cube roots. We choose the root which
# avoids cancellation. Note that disc < 0 implies that r < 0.
u + = 2. * r * cos(ang / 3.)
#guaranteed positive
v = sqrt(u**2 + geoid.e4a * q)
# Avoid loss of accuracy when u < 0. Underflow doesn't occur in
# e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
# u + v, guaranteed positive
uv = geoid.e4a * q / (v - u) if u < 0 else u + v
# Need to guard against w going negative due to roundoff in uv - q.
w = max(0., geoid.e2a * (uv - q) / (2. * v))
# Rearrange expression for k to avoid loss of accuracy due to
# subtraction. Division by 0 not possible because uv > 0, w >= 0.
k = uv / (sqrt(uv + w**2) + w)
k1 = k if geoid.f >= 0 else k - geoid.e2
k2 = k + geoid.e2 if geoid.f >= 0 else k
d=k1*R/k2
H = hypot(z / k1, R / k2)
sphi = (z / k1) / H
cphi = (R / k2) / H
h = (1. - geoid.e2m / k1) * hypot(d, z)
else:
# This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
# (prolate, rotation axis) and the generation of 0/0 in the general
# formulas for phi and h. using the general formula and division by 0
# in formula for h. So handle this case by taking the limits:
# f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p)
# f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
zz = sqrt((geoid.e4a - p if geoid.f >= 0 else p) / geoid.e2m)
xx = sqrt(geoid.e4a - p if geoid.f < 0 else p)
H = hypot(zz, xx)
sphi=zz/H
cphi=xx/H
# for tiny negative z (not for prolate)
if z < 0:
sphi = -sphi
h = -geoid.a * (geoid.e2m if geoid.f >= 0 else 1) * H / geoid.e2a
lat = atan2(sphi, cphi)
lon = atan2(slam, clam)
return (degrees(lat), degrees(lon), h)
def get_lla_to_enu_R_matrix(lat, lng):
"""
Get a East, North, Up rotation matrix
Parameters
----------
lat : float, latitude in degrees [-90, 90]
lng : float, longitude in degrees [-180, 180]
h : float, height in meters
geoid : the ellipsoid model
Returns
-------
rotation_matrix : np.array, 3x3
"""
sphi = sin(radians(lat))
cphi = cos(radians(lat))
slam = sin(radians(lng))
clam = cos(radians(lng))
R_c_to_o = np.array(
[[-slam, clam, 0.], [-clam * sphi, -slam * sphi, cphi], [clam * cphi, slam * cphi, sphi]])
return R_c_to_o
def local_cartesian_forward(origin_lla, ref_lla, geoid=wgs84_geoid):
"""
Get the East, North, Up coordinates relative to an origin
Parameters
----------
origin_lla : (float, float, float), (latitude, longitude, height) angles are in degrees. Height is in meters
ref_lla : (float, float, float), (latitude, longitude, height) angles are in degrees. Height is in meters
geoid : the ellipsoid model
Returns
-------
x : float, Easting
y : float, Northing
z : float, height
"""
if len(origin_lla) == 2:
origin_lla = (origin_lla[0], origin_lla[1], 0.0)
if len(ref_lla) == 2:
ref_lla = (ref_lla[0], ref_lla[1], 0.0)
origin_xyz = geocentric_forward(
origin_lla[0], origin_lla[1], origin_lla[2], wgs84_geoid)
geocentric_ref_xyz = geocentric_forward(
ref_lla[0], ref_lla[1], ref_lla[2], wgs84_geoid)
R_c_to_o = get_lla_to_enu_R_matrix(origin_lla[0], origin_lla[1])
pt_at_center = np.array(geocentric_ref_xyz) - np.array(origin_xyz)
t = np.matmul(R_c_to_o, pt_at_center)
return t
def local_cartesian_reverse(origin_lla, ref_xyz, geoid=wgs84_geoid):
"""
Get the Latitude, Longitude, Height coordinates given an origin and cartesian offset
Parameters
----------
origin_lla : (float, float, float), (latitude, longitude, height) angles are in degrees. Height is in meters
xyz : (float, float, float), (Easting, Northing, Height)
geoid : the ellipsoid model
Returns
-------
latitude : float, latitude in degrees
longitude : float, longitude in degrees
height : float, height in meters
"""
if len(origin_lla) == 2:
origin_lla = (origin_lla[0], origin_lla[1], 0.0)
if len(ref_xyz) == 2:
ref_xyz = (ref_xyz[0], ref_xyz[1], 0.0)
origin_xyz = geocentric_forward(
origin_lla[0], origin_lla[1], origin_lla[2], wgs84_geoid)
R_c_to_o = get_lla_to_enu_R_matrix(origin_lla[0], origin_lla[1])
R_o_to_c = R_c_to_o.T
geocentric_ref_xyz = np.array(
np.matmul(R_o_to_c, ref_xyz)) + np.array(origin_xyz)
return geocentric_reverse(geocentric_ref_xyz[0], geocentric_ref_xyz[1], geocentric_ref_xyz[2], wgs84_geoid)
Already verified
c++ output results
I20231101 17:34:25.019203 364861 gps_align_node.cpp:63] enu: 0 0 0
I20231101 17:34:25.019237 364861 gps_align_node.cpp:63] enu: 0.7988347796 0.9201198964 0.7749998838
I20231101 17:34:25.019268 364861 gps_align_node.cpp:63] enu: 1.366680585 0.7427471075 -2.14000019
I20231101 17:34:25.019294 364861 gps_align_node.cpp:63] enu: 1.4244277 0.5321173694 -2.045000181
I20231101 17:34:25.019321 364861 gps_align_node.cpp:63] enu: 1.568795427 0.7981760758 -1.669000243
I20231101 17:34:25.019349 364861 gps_align_node.cpp:63] enu: 1.742036085 0.6318892301 -3.803000269
I20231101 17:34:25.019376 364861 gps_align_node.cpp:63] enu: 1.491798942 0.1219436117 -3.736000175
I20231101 17:34:25.019407 364861 gps_align_node.cpp:63] enu: 1.588043851 0.03325743219 -4.524000199
I20231101 17:34:25.019433 364861 gps_align_node.cpp:63] enu: 2.204011265 0.221715559 -7.631000386
I20231101 17:34:25.019461 364861 gps_align_node.cpp:63] enu: 2.694861492 0.8646903775 -5.934000628
I20231101 17:34:25.019490 364861 gps_align_node.cpp:63] enu: 3.176086533 1.540922452 -5.995000977
I20231101 17:34:25.019516 364861 gps_align_node.cpp:63] enu: 3.1664636 3.081846417 -1.926001533
I20231101 17:34:25.019543 364861 gps_align_node.cpp:63] enu: 2.983598012 3.857851137 -1.283001869
I20231101 17:34:25.019572 364861 gps_align_node.cpp:63] enu: 3.060594228 4.744714002 -0.4760025066
I20231101 17:34:25.019600 364861 gps_align_node.cpp:63] enu: 3.79205621 5.520718027 -1.352003525
I20231101 17:34:25.019629 364861 gps_align_node.cpp:63] enu: 3.946047147 5.65374577 -3.203003736
I20231101 17:34:25.019656 364861 gps_align_node.cpp:63] enu: 3.484070677 5.908718196 -3.455003699
I20231101 17:34:25.019683 364861 gps_align_node.cpp:63] enu: 3.272331955 7.150326495 -1.990004864
I20231101 17:34:25.019709 364861 gps_align_node.cpp:63] enu: 3.638062519 8.003930602 -2.643006081
I20231101 17:34:25.019738 364861 gps_align_node.cpp:63] enu: 4.533141515 8.613649316 -2.114007451
I20231101 17:34:25.019765 364861 gps_align_node.cpp:63] enu: 5.351223744 9.422910957 -2.398009233
I20231101 17:34:25.019793 364861 gps_align_node.cpp:63] enu: 5.976815662 9.844169853 -2.992010426
I20231101 17:34:25.019819 364861 gps_align_node.cpp:63] enu: 6.97776384 10.78646224 -2.458012973
I20231101 17:34:25.019846 364861 gps_align_node.cpp:63] enu: 7.61297689 11.67331942 -5.354015266
I20231101 17:34:25.019876 364861 gps_align_node.cpp:63] enu: 8.103825561 12.78189713 -5.229018005
I20231101 17:34:25.019903 364861 gps_align_node.cpp:63] enu: 8.469553814 13.60224132 -6.704020183
I20231101 17:34:25.019930 364861 gps_align_node.cpp:63] enu: 8.75828913 14.21196048 -5.964021909
I20231101 17:34:25.019959 364861 gps_align_node.cpp:63] enu: 8.700543579 14.61105107 -4.697022735
I20231101 17:34:25.019986 364861 gps_align_node.cpp:63] enu: 8.344436257 14.57779167 -5.483022183
I20231101 17:34:25.020015 364861 gps_align_node.cpp:63] enu: 8.623546479 14.56670592 -5.566022528
I20231101 17:34:25.020045 364861 gps_align_node.cpp:63] enu: 8.729418913 14.53345367 -3.390022595
I20231101 17:34:25.020071 364861 gps_align_node.cpp:63] enu: 8.652424841 14.311741 -2.114021988
I20231101 17:34:25.020102 364861 gps_align_node.cpp:63] enu: 9.047024657 14.11218978 -5.514022088
I20231101 17:34:25.020128 364861 gps_align_node.cpp:63] enu: 9.15289249 13.91264329 -6.7490218
I20231101 17:34:25.020156 364861 gps_align_node.cpp:63] enu: 9.210639146 13.30292489 -7.313020575
I20231101 17:34:25.020291 364861 gps_align_node.cpp:63] enu: 9.268386007 12.83732202 -7.648019702
I20231101 17:34:25.020319 364861 gps_align_node.cpp:63] enu: 9.35500345 12.47148736 -9.900019099
I20231101 17:34:25.020346 364861 gps_align_node.cpp:63] enu: 9.316503656 12.09456857 -11.37801831
I20231101 17:34:25.020375 364861 gps_align_node.cpp:63] enu: 9.345374859 11.7841641 -13.10301777
I20231101 17:34:25.020402 364861 gps_align_node.cpp:63] enu: 9.056641502 11.29639151 -12.54801647
I20231101 17:34:25.020429 364861 gps_align_node.cpp:63] enu: 8.469549278 10.66450401 -11.83601457
I20231101 17:34:25.020458 364861 gps_align_node.cpp:63] enu: 8.036447907 10.0437013 -11.737013
I20231101 17:34:25.020485 364861 gps_align_node.cpp:63] enu: 7.641843796 9.533755511 -12.10301173
I20231101 17:34:25.020514 364861 gps_align_node.cpp:63] enu: 7.44935445 9.145754003 -11.96701093
I20231101 17:34:25.020541 364861 gps_align_node.cpp:63] enu: 7.102872224 8.813179944 -12.72101007
I20231101 17:34:25.020568 364861 gps_align_node.cpp:63] enu: 6.698643123 8.48060592 -13.48900918
I20231101 17:34:25.020596 364861 gps_align_node.cpp:63] enu: 6.21741894 8.170204453 -13.48400828
I20231101 17:34:25.020623 364861 gps_align_node.cpp:63] enu: 5.716946592 7.416373515 -12.80400689
I20231101 17:34:25.020650 364861 gps_align_node.cpp:63] enu: 5.091354492 6.817741498 -13.53100569
I20231101 17:34:25.020678 364861 gps_align_node.cpp:63] enu: 4.619755286 6.329968728 -12.79700483
I20231101 17:34:25.020705 364861 gps_align_node.cpp:63] enu: 4.639003527 6.130423999 -13.93000464
I20231101 17:34:25.020735 364861 gps_align_node.cpp:63] enu: 4.542757 6.174764768 -16.20900462
I20231101 17:34:25.021235 364861 gps_align_node.cpp:63] enu: 4.263646932 5.964135255 -16.32900422
I20231101 17:34:25.021281 364861 gps_align_node.cpp:63] enu: 3.936414351 5.498533203 -16.73900359
I20231101 17:34:25.021312 364861 gps_align_node.cpp:63] enu: 3.570683797 5.143788695 -17.17000308
I20231101 17:34:25.021339 364861 gps_align_node.cpp:63] enu: 3.022087981 5.409846323 -17.42100302
I20231101 17:34:25.021366 364861 gps_align_node.cpp:63] enu: 2.58898585 5.565046033 -18.19400296
I20231101 17:34:25.021395 364861 gps_align_node.cpp:63] enu: 2.338749737 5.143788301 -17.24600251
I20231101 17:34:25.021423 364861 gps_align_node.cpp:63] enu: 1.742031997 4.545158218 -16.49700186
I20231101 17:34:25.021451 364861 gps_align_node.cpp:63] enu: 1.337804313 4.112815965 -13.23900147
I20231101 17:34:25.021481 364861 gps_align_node.cpp:63] enu: 1.799780345 3.536357345 -12.01000124
I20231101 17:34:25.021507 364861 gps_align_node.cpp:63] enu: 1.953773704 3.015328802 -7.553001016
I20231101 17:34:25.021534 364861 gps_align_node.cpp:63] enu: 1.357054494 2.35018229 -8.634000579
I20231101 17:34:25.021564 364861 gps_align_node.cpp:63] enu: 1.501421889 2.095209516 -9.224000522
I20231101 17:34:25.021590 364861 gps_align_node.cpp:63] enu: 2.338753621 2.361268356 -8.270000866
I20231101 17:34:25.021617 364861 gps_align_node.cpp:63] enu: 2.993219175 3.292472592 -8.748001555
I20231101 17:34:25.021646 364861 gps_align_node.cpp:63] enu: 3.965293402 4.02413349 -8.589002506
I20231101 17:34:25.021673 364861 gps_align_node.cpp:63] enu: 4.446519107 3.813504732 -7.596002693
I20231101 17:34:25.021700 364861 gps_align_node.cpp:63] enu: 4.542766508 4.212594773 -3.993003013
I20231101 17:34:25.021730 364861 gps_align_node.cpp:63] enu: 4.79300389 5.310087238 -3.029004019
I20231101 17:34:25.021757 364861 gps_align_node.cpp:63] enu: 5.081739195 5.775690315 -2.593004648
I20231101 17:34:25.021785 364861 gps_align_node.cpp:63] enu: 5.380100774 6.341067589 -0.02000543277
I20231101 17:34:25.021816 364861 gps_align_node.cpp:63] enu: 5.44747 6.507351618 -2.663005658
I20231101 17:34:25.021843 364861 gps_align_node.cpp:63] enu: 5.611086862 6.496266232 -2.361005788
I20231101 17:34:25.021871 364861 gps_align_node.cpp:63] enu: 5.832452011 6.784498482 -0.5320062877
I20231101 17:34:25.021899 364861 gps_align_node.cpp:63] enu: 6.178934678 7.139244273 0.05599299606
I20231101 17:34:25.021930 364861 gps_align_node.cpp:63] enu: 6.785277246 7.006213636 -1.42300747
I20231101 17:34:25.022048 364861 gps_align_node.cpp:63] enu: 7.362748722 7.139244475 -0.4160082596
I20231101 17:34:25.022078 364861 gps_align_node.cpp:63] enu: 7.584105441 7.372039347 -6.145008783
I20231101 17:34:25.022110 364861 gps_align_node.cpp:63] enu: 8.074951347 7.40529375 -8.938009424
I20231101 17:34:25.022137 364861 gps_align_node.cpp:63] enu: 8.382930852 7.704605834 -12.06601018
I20231101 17:34:25.022167 364861 gps_align_node.cpp:63] enu: 8.440672319 8.303231923 -15.85601101
I20231101 17:34:25.022197 364861 gps_align_node.cpp:63] enu: 8.354047273 9.167916113 -18.93801208
I20231101 17:34:25.022226 364861 gps_align_node.cpp:63] enu: 8.325168671 9.810883895 -22.52301301
I20231101 17:34:25.022378 364861 gps_align_node.cpp:63] enu: 8.344409875 10.50927647 -28.03801415
I20231101 17:34:25.022420 364861 gps_align_node.cpp:63] enu: 8.729379438 11.08572393 -34.26801564
I20231101 17:34:25.022738 364861 gps_align_node.cpp:63] enu: 9.162471991 11.89497251 -39.27201771
I20231101 17:34:25.022786 364861 gps_align_node.cpp:63] enu: 9.277957154 12.60444781 -44.48701925
I20231101 17:34:25.022815 364861 gps_align_node.cpp:63] enu: 9.287575022 12.82615302 -48.87301971
I20231101 17:34:25.022845 364861 gps_align_node.cpp:63] enu: 9.451185903 13.35826088 -51.55502104
I20231101 17:34:25.022872 364861 gps_align_node.cpp:63] enu: 9.50892848 13.46911231 -54.16002136
I20231101 17:34:25.022900 364861 gps_align_node.cpp:63] enu: 9.287561269 13.50236116 -57.93102111
I20231101 17:34:25.022930 364861 gps_align_node.cpp:63] enu: 8.960333404 13.85710661 -56.1060214
I20231101 17:34:25.022965 364861 gps_align_node.cpp:63] enu: 9.759157064 14.40030192 -57.78802378
I20231101 17:34:25.022994 364861 gps_align_node.cpp:63] enu: 10.01901376 14.89915465 -59.10102534
I20231101 17:34:25.023025 364861 gps_align_node.cpp:63] enu: 9.566663915 15.48669201 -60.38602605
I20231101 17:34:25.023053 364861 gps_align_node.cpp:63] enu: 9.547409612 15.87468202 -63.82502698
I20231101 17:34:25.023082 364861 gps_align_node.cpp:63] enu: 9.797642675 15.93010828 -64.7460275
I20231101 17:34:25.023110 364861 gps_align_node.cpp:63] enu: 9.518533257 15.67513447 -66.04802644
I20231101 17:34:25.023138 364861 gps_align_node.cpp:63] enu: 9.354918591 15.53102075 -66.02102584
I20231101 17:34:25.023166 364861 gps_align_node.cpp:63] enu: 9.441537525 15.43124857 -66.54102573
Python output results of the same data
enu [0. 0. 0.]
enu [0.79883478 0.9201199 0.77499988]
enu [1.36668058 0.74274711 -2.14000019]
enu [1.4244277 0.53211737 -2.04500018]
enu [1.56879543 0.79817608 -1.66900024]
enu [1.74203609 0.63188923 -3.80300027]
enu [1.49179894 0.12194361 -3.73600018]
enu [1.58804385 0.03325743 -4.5240002]
enu [2.20401127 0.22171556 -7.63100039]
enu [2.69486149 0.86469038 -5.93400063]
enu [3.17608653 1.54092245 -5.99500098]
enu [3.1664636 3.08184642 -1.92600153]
enu [2.98359801 3.85785114 -1.28300187]
enu [3.06059423 4.744714 -0.47600251]
enu [3.79205621 5.52071803 -1.35200353]
enu [3.94604715 5.65374577 -3.20300374]
enu [3.48407068 5.9087182 -3.4550037]
enu [3.27233196 7.1503265 -1.99000486]
enu [3.63806252 8.0039306 -2.64300608]
enu [4.53314151 8.61364932 -2.11400745]
enu [5.35122374 9.42291096 -2.39800923]
enu [5.97681566 9.84416985 -2.99201043]
enu [6.97776384 10.78646224 -2.45801297]
enu [7.61297689 11.67331942 -5.35401527]
enu [8.10382556 12.78189713 -5.22901801]
enu [8.46955382 13.60224132 -6.70402018]
enu [8.75828913 14.21196048 -5.96402191]
enu [8.70054358 14.61105107 -4.69702273]
enu [8.34443626 14.57779167 -5.48302218]
enu [8.62354648 14.56670592 -5.56602253]
enu [8.72941891 14.53345367 -3.3900226]
enu [8.65242484 14.311741 -2.11402199]
enu [9.04702466 14.11218978 -5.51402209]
enu [9.15289249 13.91264329 -6.7490218]
enu [9.21063915 13.30292489 -7.31302058]
enu [9.26838601 12.83732202 -7.6480197]
enu [9.35500345 12.47148736 -9.9000191]
enu [9.31650366 12.09456857 -11.37801831]
enu [9.34537486 11.7841641 -13.10301777]
enu [9.0566415 11.29639151 -12.54801647]
enu [8.46954928 10.66450401 -11.83601457]
enu [8.03644791 10.0437013 -11.737013]
enu [7.6418438 9.53375551 -12.10301173]
enu [7.44935445 9.145754 -11.96701093]
enu [7.10287222 8.81317994 -12.72101007]
enu [6.69864312 8.48060592 -13.48900918]
enu [6.21741894 8.17020445 -13.48400828]
enu [5.71694659 7.41637352 -12.80400689]
enu [5.09135449 6.8177415 -13.53100569]
enu [4.61975529 6.32996873 -12.79700483]
enu [4.63900353 6.130424 -13.93000464]
enu [4.542757 6.17476477 -16.20900462]
enu [4.26364693 5.96413525 -16.32900422]
enu [3.93641435 5.4985332 -16.73900359]
enu [3.5706838 5.1437887 -17.17000308]
enu [3.02208798 5.40984632 -17.42100302]
enu [2.58898585 5.56504603 -18.19400296]
enu [2.33874974 5.1437883 -17.24600251]
enu [1.742032 4.54515822 -16.49700186]
enu [1.33780431 4.11281597 -13.23900147]
enu [1.79978034 3.53635734 -12.01000124]
enu [1.9537737 3.0153288 -7.55300102]
enu [1.35705449 2.35018229 -8.63400058]
enu [1.50142189 2.09520952 -9.22400052]
enu [2.33875362 2.36126836 -8.27000087]
enu [2.99321917 3.29247259 -8.74800156]
enu [3.9652934 4.02413349 -8.58900251]
enu [4.44651911 3.81350473 -7.59600269]
enu [4.54276651 4.21259477 -3.99300301]
enu [4.79300389 5.31008724 -3.02900402]
enu [5.08173919 5.77569031 -2.59300465]
enu [5.38010077 6.34106759 -0.02000543]
enu [5.44747 6.50735162 -2.66300566]
enu [5.61108686 6.49626623 -2.36100579]
enu [5.83245201 6.78449848 -0.53200629]
enu [6.17893468 7.13924427 0.055993 ]
enu [6.78527725 7.00621364 -1.42300747]
enu [7.36274872 7.13924447 -0.41600826]
enu [7.58410544 7.37203935 -6.14500878]
enu [8.07495135 7.40529375 -8.93800942]
enu [8.38293085 7.70460583 -12.06601018]
enu [8.44067232 8.30323192 -15.85601101]
enu [8.35404727 9.16791611 -18.93801208]
enu [8.32516867 9.81088389 -22.52301301]
enu [8.34440988 10.50927647 -28.03801415]
enu [8.72937944 11.08572393 -34.26801564]
enu [9.16247199 11.89497251 -39.27201771]
enu [9.27795716 12.60444781 -44.48701925]
enu [9.28757502 12.82615302 -48.87301971]
enu [9.4511859 13.35826088 -51.55502104]
enu [9.50892848 13.46911231 -54.16002136]
enu [9.28756127 13.50236117 -57.93102111]
enu [8.9603334 13.85710661 -56.1060214]
enu [9.75915706 14.40030192 -57.78802379]
enu [10.01901376 14.89915465 -59.10102534]
enu [9.56666391 15.48669201 -60.38602605]
enu [9.54740961 15.87468202 -63.82502698]
enu [9.79764268 15.93010828 -64.7460275]
enu [9.51853326 15.67513447 -66.04802644]
enu [9.35491859 15.53102075 -66.02102584]
enu [9.44153752 15.43124857 -66.54102573]
References
- https://github.com/geoffviola/local_cartesian_python/tree/master