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()