def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_aer_enu(aer, enu):
assert pm.aer2enu(*aer) == approx(enu)
raer = (radians(aer[0]), radians(aer[1]), aer[2])
assert pm.aer2enu(*raer, deg=False) == approx(enu)
with pytest.raises(ValueError):
pm.aer2enu(aer[0], aer[1], -1)
assert pm.enu2aer(*enu) == approx(aer)
assert pm.enu2aer(*enu, deg=False) == approx(raer)
def mean_z_enu(sigmas, Wm):
"""
desc:transforms points to east, north, up; takes cartesian mean of enu; transforms enu back to aer
:param sigmas:
:param Wm:
:return:
"""
enu_mean = np.average(np.array([pm.aer2enu(*sigma, deg=False) for sigma in sigmas]), axis=0, weights=Wm)
return np.array(pm.enu2aer(*enu_mean, deg=False))
def mergefov(w0: xarray.Dataset, w1: xarray.Dataset, projalt: float = 110e3, method: str = None):
"""
inputs:
-------
w0: wide FOV data, particularly az/el
w1: other camera FOV data contained in w0
projalt: projection altitude METERS
ofn: plot filename
fovrow: to vastly speedup intiial overlap exploration, pick row(s) of pixels to examing--it could take half an hour + otherwise.
find the ECEF x,y,z, at 110km altitude for narrow camera outer pixel
boundary, then find the closest pixels in the wide FOV to those points.
Remember, it can be (much) faster to brute force this calculation than to use
k-d tree.
"""
if projalt < 1e3:
logging.warning(f"this function expects meters, you picked projection altitude {projalt/1e3} km")
# %% print distance from wide camera to narrow camera (just for information)
print(f"intercamera distance with {w0.site}: {vdist(w0.lat,w0.lon, w1.lat,w1.lon)[0]/1e3:.1f} kilometers")
# %% ENU projection from cam0 to cam1
e1, n1, u1 = pm.geodetic2enu(w1.lat, w1.lon, w1.alt_m, w0.lat, w0.lon, w0.alt_m)
# %% find the ENU of narrow FOV pixels at 110km from narrow FOV
w1 = pixelmask(w1, method)
if method is not None and method.lower() == "mzslice":
w0 = pixelmask(w0, method)
azSlice0, elSlice0, rSlice0 = pm.ecef2aer(w1.x2mz, w1.y2mz, w1.z2mz, w0.lat, w0.lon, w0.alt_m)
azSlice1, elSlice1, rSlice1 = pm.ecef2aer(w0.x2mz, w0.y2mz, w0.z2mz, w1.lat, w1.lon, w1.alt_m)
# find image indices (mask) corresponding to slice az,el
w0["rows"], w0["cols"] = fnd.findClosestAzel(
w0["az"].where(w0["fovmask"]), w0["el"].where(w0["fovmask"]), azSlice0, elSlice0
)
w0.attrs["Brow"], w0.attrs["Bcol"] = fnd.findClosestAzel(w0["az"], w0["el"], w0.Baz, w0.Bel)
w1["rows"], w1["cols"] = fnd.findClosestAzel(
w1["az"].where(w1["fovmask"]), w1["el"].where(w1["fovmask"]), azSlice1, elSlice1
)
w1.attrs["Brow"], w1.attrs["Bcol"] = fnd.findClosestAzel(w1["az"], w1["el"], w1.Baz, w1.Bel)
else:
# csc(x) = 1/sin(x)
slantrange = projalt / np.sin(np.radians(np.ma.masked_invalid(w1["el"].where(w1["fovmask"]))))
assert (slantrange >= projalt).all(), "slantrange must be >= projection altitude"
e0, n0, u0 = pm.aer2enu(w1["az"], w1["el"], slantrange)
# %% find az,el to narrow FOV from ASI FOV
az0, el0, _ = pm.enu2aer(e0 - e1, n0 - n1, u0 - u1)
assert (el0 >= 0).all(), "FOVs may not overlap, negative elevation from cam0 to cam1"
# %% nearest neighbor brute force
w0["rows"], w0["cols"] = fnd.findClosestAzel(w0["az"], w0["el"], az0, el0)
return w0, w1
def test_aer_enu(aer, enu):
e, n, u = pm.aer2enu(*aer)
assert e == approx(enu[0])
assert n == approx(enu[1])
assert u == approx(enu[2])
assert isinstance(e, float)
assert isinstance(n, float)
assert isinstance(u, float)
raer = (radians(aer[0]), radians(aer[1]), aer[2])
assert pm.aer2enu(*raer, deg=False) == approx(enu)
with pytest.raises(ValueError):
pm.aer2enu(aer[0], aer[1], -1)
a, e, r = pm.enu2aer(*enu)
assert a == approx(aer[0])
assert e == approx(aer[1])
assert r == approx(aer[2])
assert isinstance(a, float)
assert isinstance(e, float)
assert isinstance(r, float)
assert pm.enu2aer(*enu, deg=False) == approx(raer)
def test_ecefenu():
assert_allclose(pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt),
(a2x, a2y, a2z),
rtol=0.01,
err_msg='aer2ecef: {}'.format(
pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.aer2enu(taz, tel, tsrange), (a2e, a2n, a2u),
rtol=0.01,
err_msg='aer2enu: ' + str(pm.aer2enu(taz, tel, tsrange)))
assert_allclose(pm.aer2ned(taz, tel, tsrange), (a2n, a2e, -a2u),
rtol=0.01,
err_msg='aer2ned: ' + str(pm.aer2ned(taz, tel, tsrange)))
assert_allclose(pm.ecef2enu(tx, ty, tz, tlat, tlon, talt), (e2e, e2n, e2u),
rtol=0.01,
err_msg='ecef2enu: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2ned(tx, ty, tz, tlat, tlon, talt),
(e2n, e2e, -e2u),
rtol=0.01,
err_msg='ecef2ned: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
assert_allclose(pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt),
(a2la, a2lo, a2a),
rtol=0.01,
err_msg='aer2geodetic {}'.format(
pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.ecef2aer(tx, ty, tz, tlat, tlon, talt),
(ec2az, ec2el, ec2rn),
rtol=0.01,
err_msg='ecef2aer {}'.format(
pm.ecef2aer(a2x, a2y, a2z, tlat, tlon, talt)))
#%%
assert_allclose(pm.enu2aer(te, tn, tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.ned2aer(tn, te, -tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.enu2geodetic(te, tn, tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.ned2geodetic(tn, te, -tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.enu2ecef(te, tn, tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='enu2ecef: ' +
str(pm.enu2ecef(te, tn, tu, tlat, tlon, talt)))
assert_allclose(
pm.ned2ecef(tn, te, -tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='ned2ecef: ' + str(pm.ned2ecef(tn, te, -tu, tlat, tlon, talt)))
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
xyz1 = pm.geodetic2ecef(*lla0)
assert_allclose(pm.geodetic2ecef(*rlla0, deg=False),
xyz1,
err_msg='geodetic2ecef: rad')
assert_allclose(xyz1, xyz0, err_msg='geodetic2ecef: deg')
assert_allclose(pm.ecef2geodetic(*xyz1),
lla0,
err_msg='ecef2geodetic: deg')
assert_allclose(pm.ecef2geodetic(*xyz1, deg=False),
rlla0,
err_msg='ecef2geodetic: rad')
if pyproj:
assert_allclose(
pyproj.transform(lla, ecef, lla0[1], lla0[0], lla0[2]), xyz1)
assert_allclose(pyproj.transform(ecef, lla, *xyz1),
(lla0[1], lla0[0], lla0[2]))
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
assert_allclose(lla2, lla1, err_msg='aer2geodetic: deg')
assert_allclose(rlla2, rlla1, err_msg='aer2geodetic:rad')
assert_allclose(pm.geodetic2aer(*lla2, *lla0),
aer0,
err_msg='geodetic2aer: deg')
assert_allclose(pm.geodetic2aer(*rlla2, *rlla0, deg=False),
raer0,
err_msg='geodetic2aer: rad')
# %% aer-ecef
xyz2 = pm.aer2ecef(*aer0, *lla0)
assert_allclose(pm.aer2ecef(*raer0, *rlla0, deg=False),
axyz0,
err_msg='aer2ecef:rad')
assert_allclose(xyz2, axyz0, err_msg='aer2ecef: deg')
assert_allclose(pm.ecef2aer(*xyz2, *lla0),
aer0,
err_msg='ecef2aer:deg')
assert_allclose(pm.ecef2aer(*xyz2, *rlla0, deg=False),
raer0,
err_msg='ecef2aer:rad')
# %% aer-enu
enu1 = pm.aer2enu(*aer0)
ned1 = (enu1[1], enu1[0], -enu1[2])
assert_allclose(enu1, enu0, err_msg='aer2enu: deg')
assert_allclose(pm.aer2enu(*raer0, deg=False),
enu0,
err_msg='aer2enu: rad')
assert_allclose(pm.aer2ned(*aer0), ned0, err_msg='aer2ned')
assert_allclose(pm.enu2aer(*enu1), aer0, err_msg='enu2aer: deg')
assert_allclose(pm.enu2aer(*enu1, deg=False),
raer0,
err_msg='enu2aer: rad')
assert_allclose(pm.ned2aer(*ned1), aer0, err_msg='ned2aer')
# %% enu-ecef
assert_allclose(pm.enu2ecef(*enu1, *lla0),
xyz2,
err_msg='enu2ecef: deg')
assert_allclose(pm.enu2ecef(*enu1, *rlla0, deg=False),
xyz2,
err_msg='enu2ecef: rad')
assert_allclose(pm.ecef2enu(*xyz2, *lla0),
enu1,
err_msg='ecef2enu:deg')
assert_allclose(pm.ecef2enu(*xyz2, *rlla0, deg=False),
enu1,
err_msg='ecef2enu:rad')
assert_allclose(pm.ecef2ned(*xyz2, *lla0), ned1, err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(*ned1, *lla0), xyz2, err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, *lla0[:2]), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, *lla0[:2]), (vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla2, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert_allclose(pm.geodetic2ned(*lla2, *lla0),
ned3,
err_msg='geodetic2ned: deg')
assert_allclose(pm.enu2geodetic(*enu3, *lla0),
lla2,
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(*ned3, *lla0),
lla2,
err_msg='ned2geodetic')
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
x1, y1, z1 = pm.geodetic2ecef(tlat, tlon, talt)
assert_allclose(
pm.geodetic2ecef(radians(tlat), radians(tlon), talt, deg=False),
(x1, y1, z1))
assert_allclose((x1, y1, z1), (x0, y0, z0), err_msg='geodetic2ecef')
assert_allclose(pm.ecef2geodetic(x1, y1, z1), (tlat, tlon, talt),
err_msg='ecef2geodetic')
if pyproj:
assert_allclose(pyproj.transform(lla, ecef, tlon, tlat, talt),
(x1, y1, z1))
assert_allclose(pyproj.transform(ecef, lla, x1, y1, z1),
(tlon, tlat, talt))
lat2, lon2, alt2 = pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose((lat2, lon2, alt2), (lat1, lon1, alt1),
err_msg='aer2geodetic')
assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='geodetic2aer')
x2, y2, z2 = pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose(
pm.aer2ecef(radians(taz),
radians(tel),
tsrange,
radians(tlat),
radians(tlon),
talt,
deg=False), (a2x, a2y, a2z))
assert_allclose((x2, y2, z2), (a2x, a2y, a2z), err_msg='aer2ecef')
assert_allclose(pm.ecef2aer(x2, y2, z2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='ecef2aer')
e1, n1, u1 = pm.aer2enu(taz, tel, tsrange)
assert_allclose((e1, n1, u1), (e0, n0, u0), err_msg='aer2enu')
assert_allclose(pm.aer2ned(taz, tel, tsrange), (n0, e0, -u0),
err_msg='aer2ned')
assert_allclose(pm.enu2aer(e1, n1, u1), (taz, tel, tsrange),
err_msg='enu2aer')
assert_allclose(pm.ned2aer(n1, e1, -u1), (taz, tel, tsrange),
err_msg='ned2aer')
assert_allclose(pm.enu2ecef(e1, n1, u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='enu2ecef')
assert_allclose(pm.ecef2enu(x2, y2, z2, tlat, tlon, talt),
(e1, n1, u1),
err_msg='ecef2enu')
assert_allclose(pm.ecef2ned(x2, y2, z2, tlat, tlon, talt),
(n1, e1, -u1),
err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(n1, e1, -u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
#%%
e3, n3, u3 = pm.geodetic2enu(lat2, lon2, alt2, tlat, tlon, talt)
assert_allclose(pm.geodetic2ned(lat2, lon2, alt2, tlat, tlon, talt),
(n3, e3, -u3))
assert_allclose(pm.enu2geodetic(e3, n3, u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(n3, e3, -u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='ned2geodetic')
