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 prepare_gps_target(self) -> dict:
target = self.target_position
lat, lon, alt = pymap3d.ned2geodetic(
target.x_val, target.y_val, target.z_val,
self.gps_home.latitude, self.gps_home.longitude, 0
)
gps_target = {
'latitude': lat, 'longitude': lon, 'altitude': alt
}
return gps_target
def ned2geodetic2String(csvSet, line):
lla_str = ''
for iii in range(0, 15):
northCoor = csvSet.iloc[line]['posX_' + str(iii)]
eastCoor = csvSet.iloc[line]['posY_' + str(iii)]
downCoor = -csvSet.iloc[line]['posZ_' + str(iii)]
if np.isnan(northCoor):
continue
lla = pm.ned2geodetic(northCoor, eastCoor, downCoor, nedOrigin[0],
nedOrigin[1], nedOrigin[2])
lla_str = lla_str + str(lla[1]) + "," + str(lla[0]) + "," + str(
lla[2]) + " "
return lla_str
def __make_log(self, rocket):
self.pos_onlauncer_LLH_log = np.array([
pm.ned2geodetic(pos_NED[0], pos_NED[1], pos_NED[2],
rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2])
for pos_NED in self.pos_onlauncher_NED_log
])
self.downrange_log = np.array([
pm.vincenty.vdist(rocket.pos0_LLH[0], rocket.pos0_LLH[1], pos[0],
pos[1]) for pos in self.pos_LLH_log
])[:, 0]
self.vel_AIR_BODYframe_abs_log = np.array(
[np.linalg.norm(vel) for vel in self.vel_AIR_BODYframe_log])
DCM_NED2BODY_log = np.array(
[coord.DCM_NED2BODY_quat(quat) for quat in self.quat_log])
self.attitude_log = np.array(
[coord.quat2euler(DCM) for DCM in DCM_NED2BODY_log])
self.pos_NED_log = np.array([
pm.ecef2ned(pos_ECEF[0], pos_ECEF[1], pos_ECEF[2],
rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2]) for pos_ECEF in self.pos_ECEF_log
])
self.pos_decent_ECEF_log = np.array([
coord.DCM_ECI2ECEF(t).dot(pos_ECI) for pos_ECI, t in zip(
self.pos_decent_ECI_log, self.time_decent_log)
])
self.pos_decent_LLH_log = np.array([
pm.ecef2geodetic(pos[0], pos[1], pos[2])
for pos in self.pos_decent_ECEF_log
])
self.vel_decent_NED_log = np.array([
coord.DCM_ECEF2NED(rocket.pos0_LLH).dot(
coord.vel_ECI2ECEF(vel_eci, coord.DCM_ECI2ECEF(t), pos_eci))
for vel_eci, t, pos_eci in
zip(self.vel_decent_ECI_log, self.time_decent_log,
self.pos_decent_ECI_log)
])
self.downrange_decent_log = np.array([
pm.vincenty.vdist(rocket.pos0_LLH[0], rocket.pos0_LLH[1], pos[0],
pos[1]) for pos in self.pos_decent_LLH_log
])[:, 0]
self.pos_hard_LLH_log = np.r_[self.pos_onlauncer_LLH_log,
self.pos_LLH_log]
index = np.argmax(self.time_log > self.time_decent_log[0])
self.pos_soft_LLH_log = np.r_[self.pos_onlauncer_LLH_log,
self.pos_LLH_log[:index, :],
self.pos_decent_LLH_log]
def test_ned_geodetic():
lla = pm.aer2geodetic(*aer0, *lla0)
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
lat, lon, alt = pm.enu2geodetic(*enu3, *lla0)
assert lat == approx(lla[0])
assert lon == approx(lla[1])
assert alt == approx(lla[2])
lat, lon, alt = pm.ned2geodetic(*ned3, *lla0)
assert lat == approx(lla[0])
assert lon == approx(lla[1])
assert alt == approx(lla[2])
def _ned_to_geodetic(self, point_ned, latdrone, longdrone, altdrone):
"""Find the GPS coordinates of the point on the ground in (NED)
Given the vector to the point on in the ground in earth reference frame (NED), this finds and returns the
GPS coordinates (geodetic coordinate system). I added the ability to either use the built in method
from pymap3d or to use the formula and the method given on slide 28 and the formula from the GPS
website.
Attributes:
point_ned (numpy array object): the vector to the point in the earth reference frame
latdrone (float): the latitude of the drone in degrees
longdrone (float): the longditude of the drone in degrees
altdrone (float): the altitude of the drone in meters
usepymap (bool): whether to calculate using the pymap3d library or with the formula
Returns:
3-tuple of GPS coords in the form (lat, long, alt). NOTE: alt should always be 0
if the function works correctly since the point is projected onto the ground
"""
return pymap.ned2geodetic(point_ned[0], point_ned[1], point_ned[2],
latdrone, longdrone, altdrone)
def test_ned_geodetic():
lat1, lon1, alt1 = pm.aer2geodetic(*aer0, *lla0)
enu3 = pm.geodetic2enu(lat1, lon1, alt1, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(lat1, lon1, alt1, *lla0) == approx(ned3)
lat, lon, alt = pm.enu2geodetic(*enu3, *lla0)
assert lat == approx(lat1)
assert lon == approx(lon1)
assert alt == approx(alt1)
assert isinstance(lat, float)
assert isinstance(lon, float)
assert isinstance(alt, float)
lat, lon, alt = pm.ned2geodetic(*ned3, *lla0)
assert lat == approx(lat1)
assert lon == approx(lon1)
assert alt == approx(alt1)
assert isinstance(lat, float)
assert isinstance(lon, float)
assert isinstance(alt, float)
def main(
pixelsize, gridlen,
latitude, longitude, elevation,
product_d,
producttype_l,
peakonly_boo,
):
'''
geolocates the products provided to a map like grid. It augments the data in
product_d with the geolocated data.
the product mask has to be of shape
(time, no. of layers, 3(mask bottom, mask peak , mask top))
geolocation assumes that horizontal translation from one pixel to the next is
relatively flat, i.e. a flat ground.
Parameters
pixelsize (float): [km] pixel size for data sampling
gridlen (int): length of map grid in consideration for geolocation
latitude (float): [deg] coords of lidar
longitude (float): [deg] coords of lidar
elevation (float): [m] height of lidar from MSL
product_d (dict): dictionary containing all relevant information
producttype_l (list): list of keys of the product types which we would to
perform pixel averaging and geolocation
peakonly_boo (boolean): decides whether or not to return the
average for each layer or just the
layer with the most number of points
Return
product_d (dict): with the following keys added
PIXELLATITUDEKEY: (np.ndarray): latitude coordinates of pixel
PIXELLONGITUDEKEY: (np.ndarray): longitude coordinates of pixel
for each producttype in producttype_l, it will add the following keys:
PIXELBOTTOMKEY: (list): mask bottom height for pixels, same order as
LATITUDE/LONGITUDEKEY
PIXELPEAKKEY: (list): mask bottom height for pixels, same order as
LATITUDE/LONGITUDEKEY
PIXELTOPKEY: (list): mask bottom height for pixels, same order as
LATITUDE/LONGITUDEKEY
'''
# etc initialisation
pixelsize *= 1000 # converting to [m]
elevation /= 1000 # converting to [km]
emptygrid_ggAl = [[None for _ in range(gridlen)] for _ in range(gridlen)]
# reading product and relevant arrays
array_d = product_d[NRBKEY]
theta_ta = array_d['theta_ta']
phi_ta = array_d['phi_ta']
# geolocating pixels
## finding coordinate limits; [[left_lim, center, right_lim], ...]
## shape (gridlen, gridlen, 2(east, north), 3(left_lim, center, right_lim))
if gridlen%2:
gridrange = np.arange(
-(gridlen//2)*pixelsize, (gridlen//2 + 1)*pixelsize, pixelsize
)
else:
gridrange = np.arange(
-(gridlen/2 - 0.5)*pixelsize, (gridlen/2 + 0.5)*pixelsize, pixelsize
)
coordlim_gg2a = np.stack(np.meshgrid(gridrange, gridrange)[::-1], axis=-1)
coordlim_gg23a = np.stack(
[
coordlim_gg2a - pixelsize/2,
coordlim_gg2a,
coordlim_gg2a + pixelsize/2
], axis=-1
)
coordlim_km_gg23a = coordlim_gg23a / 1000
## adding coordinates to dictionary
coord_p23a = list(chain(*coordlim_gg23a))
e_pa = np.array(list(map(lambda coord_23a: coord_23a[0][1], coord_p23a)))
n_pa = np.array(list(map(lambda coord_23a: coord_23a[1][1], coord_p23a)))
lat_pa, long_pa, _ = ned2geodetic(n_pa, e_pa, 0, latitude, longitude, 0)
product_d[PIXELLATITUDEKEY] = lat_pa
product_d[PIXELLONGITUDEKEY] = long_pa
# handling pixel averaging
for key in producttype_l:
prodmask_tl3a = product_d[key][MASKKEY]
# convert product to cartesian grid coordinates
# (time*max no. of layers., 3(mask bottom, mask peak, mask top))
# take note of the negative sign when computing y, because by convention
# y points to the west, we put a negative sign here to make y point to the
# east, it is better that y is changed to point to the east since this is an
# isolated case. So that the grid indexing convention can follow NE
xprodmask_tl3a = prodmask_tl3a \
* np.tan(theta_ta)[:, None, None] * np.cos(phi_ta)[:, None, None]
yprodmask_tl3a = - prodmask_tl3a \
* np.tan(theta_ta)[:, None, None] * np.sin(phi_ta)[:, None, None]
## flattening and removing all completemly invalid layers
prodmask_a3a = prodmask_tl3a.reshape((-1, 3))
xprodmask_a3a = xprodmask_tl3a.reshape((-1, 3))
yprodmask_a3a = yprodmask_tl3a.reshape((-1, 3))
invalidlayer_am = ~(np.isnan(xprodmask_a3a).all(axis=-1))
prodmask_a3a = prodmask_a3a[invalidlayer_am]
xprodmask_a3a = xprodmask_a3a[invalidlayer_am]
yprodmask_a3a = yprodmask_a3a[invalidlayer_am]
xyprodmask_a23a = np.stack([yprodmask_a3a, xprodmask_a3a], axis=1)
# locating product into it's respective pixel
## using an array of masks of shape a3a, each element in the array is a pixel
prodmask_gga23m = \
(
coordlim_km_gg23a[:, :, None, :, [0]]
<= xyprodmask_a23a[None, None, :, :]
)\
* (
coordlim_km_gg23a[:, :, None, :, [2]]
>= xyprodmask_a23a[None, None, :, :]
)
prodmask_gga3m = prodmask_gga23m.prod(axis=3).astype(np.bool)
## boolean slicing arrays, one array for mask bottom, peak, and top
prodbot_ggam = prodmask_gga3m[..., 0]
prodpeak_ggam = prodmask_gga3m[..., 1]
prodtop_ggam = prodmask_gga3m[..., 2]
### initialising empty grid
prodbot_ggAl = deepcopy(emptygrid_ggAl)
prodpeak_ggAl = deepcopy(emptygrid_ggAl)
prodtop_ggAl = deepcopy(emptygrid_ggAl)
### captical 'A' here represents variable length arrays in the list
for i in range(gridlen):
for j in range(gridlen):
# placing product masks into their respective pixels
prodbot_ggAl[i][j] = prodmask_a3a[:, 0][prodbot_ggam[i][j]]
prodpeak_ggAl[i][j] = prodmask_a3a[:, 1][prodpeak_ggam[i][j]]
prodtop_ggAl[i][j] = prodmask_a3a[:, 2][prodtop_ggam[i][j]]
# averaging within the pixel
# averaging of layers within each pixel is sorted by their altitudes
prodbot_ggAl[i][j] = intelligent_averaging(prodbot_ggAl[i][j],
peakonly_boo)
prodpeak_ggAl[i][j] = intelligent_averaging(prodpeak_ggAl[i][j],
peakonly_boo)
prodtop_ggAl[i][j] = intelligent_averaging(prodtop_ggAl[i][j],
peakonly_boo)
# interpolating across pixels if there is an empty pixel
# for a given empty pixel, we shall take the average of lowest layer
# from the neighbouring pixels
prodbot_ggAl = nearestneighbour_average(prodbot_ggAl, gridlen)
prodpeak_ggAl = nearestneighbour_average(prodpeak_ggAl, gridlen)
prodtop_ggAl = nearestneighbour_average(prodtop_ggAl, gridlen)
# correcting product height for elevation of lidar
for i in range(gridlen):
for j in range(gridlen):
prodbot_ggAl[i][j] += elevation
prodpeak_ggAl[i][j] += elevation
prodtop_ggAl[i][j] += elevation
# reshaping and adding to the dictionary
# new shape (no.of pixels, variable length based on number of layers)
product_d[key][PIXELBOTTOMKEY] = list(chain(*prodbot_ggAl))
product_d[key][PIXELPEAKKEY] = list(chain(*prodpeak_ggAl))
product_d[key][PIXELTOPKEY] = list(chain(*prodtop_ggAl))
return product_d
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 get_geodetic_path(self):
self.geodetic_path = [pymap3d.ned2geodetic(p[0], p[1], 3, self.lat_0, self.lon_0, 0)
for p in self.local_path]
return self.geodetic_path
self.xml += self.waypoint_template.substitute(lat=robot_path[0][0],
lon=robot_path[0][1])
for i, p_1 in enumerate(robot_path):
if i != 0:
self.xml += self.section_template.substitute(lat_0=p_0[0],
lon_0=p_0[1],
lat_1=p_1[0],
lon_1=p_1[1],
speed=local_path[i][3])
p_0 = p_1
self.xml += self.waypoint_template.substitute(lat=robot_path[-1][0],
lon=robot_path[-1][1])
self.xml += '</mission>\n'
def main(lat_0, lon_0, file_path):
auvpath = AuvPath(lat_0,lon_0)
auvpath.load_pickled_path(file_path + '.pkl')
auvpath.get_geodetic_path()
auvpath.save_csv(auvpath.geodetic_path, file_path + '.csv')
auvpath.create_xml(file_path + '.xml')
if __name__ == '__main__':
path_file = 'path'
distance_obstacle = 45
lat_0, lon_0, _ =pymap3d.ned2geodetic(-distance_obstacle, 0, 0, 32.841242, 34.973986, 0)
main(lat_0, lon_0,path_file)
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 to_llh(lat, lon, height, ned):
""" Convert all the ned_points to an llh list of points. """
n, e, d = ned[0] / 100, ned[1] / 100, ned[
2] / 100 # Division is for CM -> meters
return ned2geodetic(n=n, e=e, d=d, lat0=lat, lon0=lon, h0=height)
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')
def main(
lidar_lat, lidar_lg, lidar_h,
delphib,
cadastraldir,
*nrbds
):
'''
Follow these steps to perform azimuthal calibration of the lidar
1. Use .geojson_trimmer to create a suitable sized geojson file for plotting
2. specify the data set used for calibration check
Plots out the horizontal sweep of the nrb against the cadastral map
the geojson can be created and trimmed in /.cadastral_trimmer.
Note that the coordinates provided by nrb_calc are in spherical coordinates,
which map x->N, y->W.
But here we plot on a 2D-plane, which maps y->N(latitude), x->E(longitude).
This is taken care in the coordinate transform of the azimuthal array from
nrb_calc. which converts phi to psi, which is the angle starting from the E axis,
aka the longitude axis
Parameters
lidar_lat (float): [deg] lidar latitude
lidar_lg (float): [deg] lidar longitude
lidar_h (float): [m] lidar heigh above geodetic ellipsoid
delphib (float): [rad] uncert of phib
cadastraldir (str): directory of cadastral geojson
nrbds (list): list of output from product_calc.nrb_calc
'''
# reading files
sg_df = gpd.read_file(cadastraldir)
# plot creation
fig, ax = plt.subplots()
## plotting map
sg_df.plot(ax=ax)
# computing coordinates
for i, nrbd in enumerate(nrbds):
color = f'C{i+1}'
if i == 0:
markeralpha = _reference_markeralpha
markercolor = _reference_markercolor
else:
markeralpha = _markeralpha
markercolor = color
NRB_tra = nrbd['NRB_tra']
r_tra = nrbd['r_tra'] * 1000
r_trm = nrbd['r_trm']
phi_ta = nrbd['phi_ta']
psi_ta = nrbd['phi_ta'] + np.pi/2 # psi is the angle inthe 2D projection
# anti clockwise from E(longitude) axis
# scan lines; coverting local NE to lat long, referenced from the lidar
# coordinates
print('computing lidar point')
point1lat_ta = lidar_lat*np.ones_like(psi_ta)
point1lg_ta = lidar_lg*np.ones_like(psi_ta)
point1n_ta, point1e_ta, point1d_ta = geodetic2ned(
point1lat_ta, point1lg_ta, lidar_h,
lidar_lat, lidar_lg, lidar_h
)
print('computing scan points')
point2n_ta = point1n_ta + _D*np.sin(psi_ta)
point2e_ta = point1e_ta + _D*np.cos(psi_ta)
point2lat_ta, point2lg_ta, _ = ned2geodetic(
point2n_ta, point2e_ta, point1d_ta,
lidar_lat, lidar_lg, lidar_h,
)
# threshold for nrb
r_trm *= NRB_tra > _nrbthres
# range (time, bins)
print('computing range points')
rn_tra = point1n_ta[:, None] + r_tra * np.sin(psi_ta)[:, None]
re_tra = point1e_ta[:, None] + r_tra * np.cos(psi_ta)[:, None]
rlat_tra, rlg_tra, _ = ned2geodetic(
rn_tra, re_tra, point1d_ta[:, None],
lidar_lat, lidar_lg, lidar_h,
)
# error bars
## errorbar for range; (time, bins, 2(lower limit, upper limit))
print('computing range error bar')
deln_ta = _delr/2 * np.sin(psi_ta)
dele_ta = _delr/2 * np.cos(psi_ta)
delrn_tr2a = np.stack([
rn_tra - deln_ta[:, None], rn_tra + deln_ta[:, None]
], axis=2)
delre_tr2a = np.stack([
re_tra - dele_ta[:, None], re_tra + dele_ta[:, None]
], axis=2)
delrlat_tr2a, delrlg_tr2a, _ = ned2geodetic(
delrn_tr2a, delre_tr2a, point1d_ta[:, None, None],
lidar_lat, lidar_lg, lidar_h,
)
## error bar for azimuth; (time, bins, _psinum), 'n' represents _psinum
print('computing azimuthal error bar')
delpsi_na = np.linspace(-delphib, delphib, _psinum)
delpsi_tna = psi_ta[:, None] + delpsi_na
delpsin_trna = point1n_ta[:, None, None] + r_tra[:, :, None] * \
np.sin(delpsi_tna)[:, None, :]
delpsie_trna = point1e_ta[:, None, None] + r_tra[:, :, None] * \
np.cos(delpsi_tna)[:, None, :]
delpsilat_trna, delpsilg_trna, _ = ned2geodetic(
delpsin_trna, delpsie_trna, point1d_ta[:, None, None],
lidar_lat, lidar_lg, lidar_h
)
# plotting
## lidar
ax.plot(lidar_lg, lidar_lat, 'ko')
## plotting threshold nrb
for j in range(len(phi_ta)): # indexing time
# scan lines
ax.plot(
[point1lg_ta[j], point2lg_ta[j]],
[point1lat_ta[j], point2lat_ta[j]],
'-', alpha=0.3, color=color
)
# nrb threshold points
r_rm = r_trm[j] # (bins)
ax.plot(
rlg_tra[j][r_rm], rlat_tra[j][r_rm],
'o', color=markercolor, alpha=markeralpha
)
# plotting error bars
for k in range(np.sum(r_rm)):
# plotting range error bars
ax.plot(
delrlg_tr2a[j][r_rm][k],
delrlat_tr2a[j][r_rm][k],
'-', color='k'
)
# plotting azimuth error bars
ax.plot(
delpsilg_trna[j][r_rm][k],
delpsilat_trna[j][r_rm][k],
'-', color='k'
)
# showing
plt.xlabel('Long [deg]')
plt.ylabel('Lat [deg]')
plt.show()
