https://github.com/Dongvdong/v1_1_slam_tool
''' gnss 和 enu 坐标系相互转化 ''' import numpy as np from pyproj import Proj, Transformer import pyproj import math from API_1GetGpsFromIMG import * use_cgcs2000Towgs84=0 # 大疆采集的rtk默认坐标系是cgcs2000Towgs84 是否需要转化 貌似转化没啥区别 # # WGS-84定义的常数,用于CGCS2000系统(与WGS-84非常接近) # 1-1 def Api_cgcs2000Towgs84(Gnss_in): # 定义CGCS2000和WGS-84坐标系 cgcs2000 = Proj('epsg:4490') # CGCS2000的EPSG代码 wgs84 = Proj('epsg:4326') # WGS-84的EPSG代码 # 使用Transformer进行转换 transformer = Transformer.from_proj(cgcs2000, wgs84, always_xy=True) # 示例坐标(经度, 纬度, 高度) #lon, lat, h = 116.391, 39.907, 50.0 # 高度为50米 lon, lat, h = Gnss_in[1], Gnss_in[0], Gnss_in[2] # 高度为50米 Gnss_out=[-1,-1,-1] # 进行坐标转换 x, y, z = transformer.transform(lon, lat, h) Gnss_out=[y,x,z] #print(f"输入 CGCS2000坐标: 经度={lon}, 纬度={lat}, 高度={h}") #print(f"输出 WGS-84坐标: 经度={x}, 纬度={y}, 高度={z}") return Gnss_out # 1-2 def Api_wgs84Tocgcs2000(Gnss_in): # 定义CGCS2000和WGS-84坐标系 cgcs2000 = Proj('epsg:4490') # CGCS2000的EPSG代码 wgs84 = Proj('epsg:4326') # WGS-84的EPSG代码 # 使用Transformer进行转换 transformer = Transformer.from_proj(wgs84,cgcs2000 , always_xy=True) # 示例坐标(经度, 纬度, 高度) #lon, lat, h = 116.391, 39.907, 50.0 # 高度为50米 lon, lat, h = Gnss_in[1], Gnss_in[0], Gnss_in[2] # 高度为50米 # 进行坐标转换 x, y, z = transformer.transform(lon, lat, h) Gnss_out=[y,x,z] #print(f"输入 WGS-84坐标: 经度={x}, 纬度={y}, 高度={z}") #print(f"输出 CGCS2000坐标: 经度={lon}, 纬度={lat}, 高度={h}") return Gnss_out #============================================================= # WGS-84定义的常数,用于CGCS2000系统(与WGS-84非常接近) a = 6378137.0 # 长半轴(单位:米) b = 6356752.3142 #f = (a - b) / a f = 1 / 298.257223563 # 扁率 CGCS2000系统 #f = 1 / 298.257223565 # 扁率 WGS-84 e2 = 2*f - f**2 # 第一偏心率的平方 pi = 3.14159265359 # 2-1-1 gps转换到ecef def gnss_to_ecef(lat, lon, h): """将地理坐标(经度、纬度、高程)转换为ECEF坐标系""" lat = np.radians(lat) lon = np.radians(lon) N = a / np.sqrt(1 - e2 * np.sin(lat)**2) X = (N + h) * np.cos(lat) * np.cos(lon) Y = (N + h) * np.cos(lat) * np.sin(lon) Z = (N * (1 - e2) + h) * np.sin(lat) return X, Y, Z # 2-1-2 gps转换到ecef def gnss_to_ecef1(lat_ref,lon_ref,h_ref): transformer = pyproj.Transformer.from_crs( {"proj":'latlong', "ellps":'WGS84', "datum":'WGS84'}, {"proj":'geocent', "ellps":'WGS84', "datum":'WGS84'}, ) x_ref, y_ref, z_ref = transformer.transform(lon_ref, lat_ref, h_ref ,radians=False) to_ecef=[x_ref,y_ref,z_ref] return to_ecef #2-2 ''' 功能: # 大地坐标系ECEF转化到gps 输入: 等待转换的ecef 坐标 x, y, z 输出: GPS 坐标 lat, lon, h ''' def ecef_to_gnss(x, y, z): x=float(x) y=float(y) z=float(z) # Convert from ECEF cartesian coordinates to # latitude, longitude and height. WGS-84 x2 = x ** 2 y2 = y ** 2 z2 = z ** 2 #a = 6378137.0000 # earth radius in meters #b = 6356752.3142 # earth semiminor in meters e = math.sqrt (1-(b/a)**2) b2 = b*b e2 = e ** 2 ep = e*(a/b) r = math.sqrt(x2+y2) r2 = r*r E2 = a ** 2 - b ** 2 F = 54*b2*z2 G = r2 + (1-e2)*z2 - e2*E2 c = (e2*e2*F*r2)/(G*G*G) s = ( 1 + c + math.sqrt(c*c + 2*c) )**(1/3) P = F / (3 * (s+1/s+1)**2 * G*G) Q = math.sqrt(1+2*e2*e2*P) ro = -(P*e2*r)/(1+Q) + math.sqrt((a*a/2)*(1+1/Q) - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2) tmp = (r - e2*ro) ** 2 U = math.sqrt( tmp + z2 ) V = math.sqrt( tmp + (1-e2)*z2 ) zo = (b2*z)/(a*V) height = U*( 1 - b2/(a*V) ) lat = math.atan( (z + ep*ep*zo)/r ) temp = math.atan(y/x) if x >=0 : long = temp elif (x < 0) & (y >= 0): long = pi + temp else : long = temp - pi lat0 = lat/(pi/180) lon0 = long/(pi/180) h0 = height return lat0, lon0, h0 def ecef_to_gnss_1(x,y,z): transformer = pyproj.Transformer.from_crs( {"proj":'geocent', "ellps":'WGS84', "datum":'WGS84'}, {"proj":'latlong', "ellps":'WGS84', "datum":'WGS84'}, ) lon, lat, h= transformer.transform(x, y, z ) to_gnss=[lat,lon, h] #print(f"从 ENU坐标: 东={east}, 北={north}, 高={up}") #print(f"转换后的CGCS2000坐标: 纬度={lat}, 经度={lon}, 高度={h}") return to_gnss # 3-1 ecef转换到enu ''' 功能: # 大地坐标系 转化到GPS第一帧为原点的本地ENU坐标系 输入: 等待转换的ecef 坐标 x, y, z 作为原点的GPS第一帧 坐标lat0, lon0, h0 输出: 本地第一帧GPS为原点的 ENU 坐标系 xEast, yNorth, zUp ''' def ecef_to_enu(X, Y, Z, lat_ref, lon_ref, h_ref): """将ECEF坐标转换为ENU坐标""" # 参考点的ECEF坐标 Xr, Yr, Zr = gnss_to_ecef(lat_ref, lon_ref, h_ref) # ECEF到ENU的旋转矩阵 lat_ref = np.radians(lat_ref) lon_ref = np.radians(lon_ref) R = np.array([ [-np.sin(lon_ref), np.cos(lon_ref), 0], [-np.sin(lat_ref)*np.cos(lon_ref), -np.sin(lat_ref)*np.sin(lon_ref), np.cos(lat_ref)], [np.cos(lat_ref)*np.cos(lon_ref), np.cos(lat_ref)*np.sin(lon_ref), np.sin(lat_ref)] ]) # ECEF坐标差 dX = X - Xr dY = Y - Yr dZ = Z - Zr # 计算ENU坐标 enu = R @ np.array([dX, dY, dZ]) return enu # 3-2 enu转换到ecef ''' 功能: enu坐标转化到ecef坐标 输入: 等待转换的ENU坐标 坐标 xEast, yNorth, zUp GPS第一帧原点 坐标 lat0, lon0, h0 输出: ecef 坐标 x, y, z ''' def enu_to_ecef(east, north, up, lat_ref, lon_ref, h_ref): # 1 参考GNSS点 转化到ecef # 定义参考点的CGCS2000坐标(经度, 纬度, 高度) #lon_ref, lat_ref, h_ref = 116.391, 39.907, 50.0 # 示例参考点 ref_ecef=gnss_to_ecef(lat_ref,lon_ref,h_ref) ecef_x_ref=ref_ecef[0] ecef_y_ref=ref_ecef[1] ecef_z_ref=ref_ecef[2] # 2 等待转换的enu点变换到到ecef坐标系下相对位移 # 将参考点的地理坐标转换为弧度 lat_ref = np.radians(lat_ref) lon_ref = np.radians(lon_ref) # ENU到ECEF的旋转矩阵 R = np.array([ [-np.sin(lon_ref), np.cos(lon_ref), 0], [-np.sin(lat_ref)*np.cos(lon_ref), -np.sin(lat_ref)*np.sin(lon_ref), np.cos(lat_ref)], [np.cos(lat_ref)*np.cos(lon_ref), np.cos(lat_ref)*np.sin(lon_ref), np.sin(lat_ref)] ]) # 将ENU坐标转换为ECEF坐标 # 定义ENU坐标(East, North, Up) #east, north, up = 100, 200, 30 # 示例ENU坐标 enu_vector = np.array([east, north, up]) ecef_vector = R.T @ enu_vector # 使用矩阵转置进行旋转 # 将ECEF坐标添加到参考点的ECEF坐标 x = ecef_x_ref + ecef_vector[0] y = ecef_y_ref + ecef_vector[1] z = ecef_z_ref + ecef_vector[2] return x,y,z # 4-1 将一个gps转换到enu ''' 功能: # gps直接转化到enu坐标系 相对于指定GPS_ref为原点(一般都是第一帧)的enu坐标系 输入: gnss_in 等待转换的GPS 坐标 lat, lon, h gnss_ref 参考原点GPS 坐标 lat_ref, lon_ref, h_ref 输出: enu坐标 x, y, z ''' def API_gnss_to_enu(gnss_in, gnss_ref): lat=gnss_in[0] lon=gnss_in[1] alt=gnss_in[2] x, y, z = gnss_to_ecef(lat, lon, alt) #x1, y2, z3 = gnss_to_ecef1(lat, lon, alt) lat0=gnss_ref[0] lon0=gnss_ref[1] alt0=gnss_ref[2] e,n,u=ecef_to_enu(x, y, z, lat0, lon0, alt0) #print(f"ENU coordinates: E={e}, N={n}, U={u}") return e,n,u ''' # 原始gnss输入 名字 纬度 经度 高度 DJI_0002.JPG 34.032505638888885 108.76779925 514.638 DJI_0005.JPG 34.03267641666667 108.76781155555555 514.464 DJI_0011.JPG 34.03394725 108.76789833333333 514.635 转化为 纬度 经度 高度 34.032505638888885 108.76779925 514.638 34.03267641666667 108.76781155555555 514.464 34.03394725 108.76789833333333 514.635 ''' def API_data0123_to_data123(data0123): data123=[] for data_i in data0123: data_0=float(data_i[1]) data_1=float(data_i[2]) data_2=float(data_i[3]) data_ii=[data_0,data_1,data_2] data123.append(data_ii) return data123 ''' # 将gnss列表集中转换过去enu 输入: 纬度 经度 高度 列表 34.032505638888885 108.76779925 514.638 34.03267641666667 108.76781155555555 514.464 34.03394725 108.76789833333333 514.635 ''' def API_gnss3_to_enu3_List(gnss0Lat1Lon2H_List): # 4 将gps转滑到enu坐标系 # 4-1 第一帧为参考点 lat0=float(gnss0Lat1Lon2H_List[0][0]) lon0=float(gnss0Lat1Lon2H_List[0][1]) alt0=float(gnss0Lat1Lon2H_List[0][2]) gnss_ref=[lat0,lon0,alt0] if use_cgcs2000Towgs84:gnss_ref=Api_cgcs2000Towgs84(gnss_ref) print("参考GNSS位置",gnss_ref) ENU_List=[] for gps_i in gnss0Lat1Lon2H_List: lat=float(gps_i[0]) lon=float(gps_i[1]) alt=float(gps_i[2]) gnss_in=[lat,lon,alt] if use_cgcs2000Towgs84:gnss_in=Api_cgcs2000Towgs84(gnss_in) # 4-2 转化坐标系 e, n, u = API_gnss_to_enu(gnss_in,gnss_ref) # e=round(e, 3) # n=round(n, 3) # u=round(u, 3) ENU_List.append([e,n,u]) #print("gnss-enu 单位m",name_,"输入经纬度",lat,lon,alt,"转化后的enu",e, n, u ) return ENU_List # 测试 # Gnss_list_Read = API_read2txt(GPS_txt_name) # 将txt数据去掉第一列 # Gnss0Lat1Lon2H=API_data0123_to_data123(Gnss_list_Read) # ENU_List=API_gnss_to_enu_List(Gnss0Lat1Lon2H) # 4-2 将一个enu在给定gnss参考原点下转换到gnss ''' 功能: # enu直接转化到gnss坐标系 相对于指定GPS_ref为原点(一般都是第一帧)的enu坐标系 输入: from_enu 等待转换的GPS 坐标 lat, lon, h gnss_ref 参考原点GPS 坐标 lat_ref, lon_ref, h_ref 输出: gnss坐标 lat, lon, h ''' def API_enu_to_gnss(from_enu,gnss_ref): e=from_enu[0] n=from_enu[1] u=from_enu[2] lat0=gnss_ref[0] lon0=gnss_ref[1] alt0=gnss_ref[2] # enu转换到ecef 在指定gnss_ref参考点下 x, y, z = enu_to_ecef(e,n,u,lat0, lon0, alt0) # 从ecef转换到gnss gnss_=ecef_to_gnss(x,y,z) return gnss_ ''' # 将enu列表集中转换过去gnss 输入: 参数1 enu_list_Read e n u 列表 0 0 0 1 0 0 1 1 0 参数2 gnss_ref 参考gnss点 输出 gps 位置 ''' def API_enu3_to_gnss3_list(enu_list_Read,gnss_ref): #gnss_ref=[lat0,lon0,alt0] print("参考GNSS位置",gnss_ref) GNSS_List=[] for enu_i in enu_list_Read: name_=enu_i[0] e=float(enu_i[1]) n=float(enu_i[2]) u=float(enu_i[3]) from_enu_=[e,n,u] gnss_out=API_enu_to_gnss(from_enu_,gnss_ref) GNSS_List.append([gnss_out[0],gnss_out[1],gnss_out[2]]) return GNSS_List #5-1 多个txt数据 gnss转化到enu # 第一帧为参考帧 def API_gnss4_to_enu4_List(Gnss_list_Read): #GPS_txt_name="d1_100mRTKColmap.txt" # 3读取txt #Gnss_list_Read = API_read2txt(GPS_txt_name) # 4 将gps转滑到enu坐标系 # 4-1 第一帧为参考点 lat0=float(Gnss_list_Read[0][1]) lon0=float(Gnss_list_Read[0][2]) alt0=float(Gnss_list_Read[0][3]) gnss_ref=[lat0,lon0,alt0] if use_cgcs2000Towgs84:gnss_ref=Api_cgcs2000Towgs84(gnss_ref) print("参考GNSS位置",gnss_ref) ENU_List=[] for gps_i in Gnss_list_Read: lat=float(gps_i[1]) lon=float(gps_i[2]) alt=float(gps_i[3]) gnss_in=[lat,lon,alt] if use_cgcs2000Towgs84:gnss_in=Api_cgcs2000Towgs84(gnss_in) name_=gps_i[0] # 4-2 转化坐标系 e, n, u = API_gnss_to_enu(gnss_in,gnss_ref) # e=round(e, 3) # n=round(n, 3) # u=round(u, 3) ENU_List.append([name_,e,n,u]) #print("gnss-enu 单位m",name_,"输入经纬度",lat,lon,alt,"转化后的enu",e, n, u ) return ENU_List #5-2 多个txt数据 enu转化到gnss # 第一帧为参考帧 def API_enu4_to_gnss4_list(enu_list_Read,gnss_ref): #enu_list_Read = API_read2txt(ENU_txt_name) #gnss_ref=[lat0,lon0,alt0] print("参考GNSS位置",gnss_ref) GNSS_List=[] for enu_i in enu_list_Read: name_=enu_i[0] e=float(enu_i[1]) n=float(enu_i[2]) u=float(enu_i[3]) from_enu_=[e,n,u] gnss_out=API_enu_to_gnss(from_enu_,gnss_ref) GNSS_List.append([name_,gnss_out[0],gnss_out[1],gnss_out[2]]) return GNSS_List #def waitUse(): #import numpy as np #from scipy.spatial.transform import Rotation as R # 将四元数转换为旋转矩阵 # rotation = R.from_quat([qx, qy, qz, qw]) # rotation_matrix = rotation.as_matrix() # # 将旋转矩阵转换为欧拉角 (Omega, Phi, Kappa) # # 摄影测量中通常使用 ZYX 旋转顺序 # omega, phi, kappa = rotation.as_euler('ZYX', degrees=True) # print("旋转矩阵:\n", rotation_matrix) # print("Omega:", omega, "Phi:", phi, "Kappa:", kappa) #=========================================================== # if __name__ == "__main__": # # 参数 # # 0-1 gps照片路径 # img_path_dir="0测试数据/d1_100mRTKColmap/images/gps_images/" # # 0-2 txt保存的名字 # # 1-1从照片读取gnss数据 # Gnss_list=API_read_directory(img_path_dir) # # 1-2保存gps txt # GPS_txt_name="data/1GNSS_from_img.txt" # API_Save2txt(GPS_txt_name,Gnss_list) # # 3 gps转化到enu 第一帧参考位置 # # 3-1 读取GNSS数据 -名字 lat lon h # enu_list_Read = API_read2txt(GPS_txt_name) # # 3-2 gnss数据转换为enu # ENU_List=API_gnss4_to_enu4_List(enu_list_Read) # # 3-2 保存enu结果 -名字 e n u # ENU_txt_name="data/2ENU_from_GNSS.txt" # API_Save2txt(ENU_txt_name,ENU_List) # # 4 读取enu数据 转化到 gnss # # 4-1 获取gnss参考点 - 名字 纬 经 高 # Gnss_list_Read = API_read2txt(GPS_txt_name) # img_name=Gnss_list_Read[0][0] # lat0=float(Gnss_list_Read[0][1]) # lon0=float(Gnss_list_Read[0][2]) # alt0=float(Gnss_list_Read[0][3]) # gnss_ref=[lat0,lon0,alt0] # if use_cgcs2000Towgs84:gnss_ref=Api_cgcs2000Towgs84(gnss_ref) # print("参考GNSS位置",gnss_ref) # # 4-2 获取enu数据集 -名字 e n u # enu_list_Read=API_read2txt(ENU_txt_name) # # 4-3 ENU数据转化为gnss数据 # GNSS_list_from_enu=API_enu4_to_gnss4_list(enu_list_Read,gnss_ref) # # 4-2 保存gnss结果 名字 纬 经 高 # GNSS_From_ENU_txt_name="data/3GNSS_From_ENU.txt" # API_Save2txt(GNSS_From_ENU_txt_name,GNSS_list_from_enu) # # 5 数据转化 为3D-3D计算相似变换准备 # #ENU_List :名字 e n u 转化为: e n u # ENU_List_3=API_data0123_to_data123(ENU_List) # 去掉第一列名字 # GNSS_list_from_enu_3=API_data0123_to_data123(GNSS_list_from_enu)
数据格式
1GNSS_from_img.txt
DJI_0002.JPG 34.032505638888885 108.76779925 514.638 DJI_0005.JPG 34.03267641666667 108.76781155555555 514.464 DJI_0011.JPG 34.03394725 108.76789833333333 514.635 DJI_0015.JPG 34.03487661111111 108.76796561111111 514.642 DJI_0018.JPG 34.03509530555555 108.76797844444444 514.615 DJI_0022.JPG 34.03506447222222 108.76773913888889 514.582 DJI_0025.JPG 34.03463080555555 108.76770336111112 514.66 DJI_0028.JPG 34.03403180555556 108.76765755555556 514.578
2ENU_from_GNSS.txt
DJI_0002.JPG 0.0 0.0 0.0 DJI_0005.JPG 1.136502194718024 18.94471263835052 -0.1740283354476304 DJI_0011.JPG 9.150887116507509 159.92076256807346 -0.005018428985010814 DJI_0015.JPG 15.364189902744634 263.01664813277375 -0.0014604668066482418 DJI_0018.JPG 16.54936288252069 287.2768610609836 -0.029513647144568722 DJI_0022.JPG -5.551516671578071 283.8564433571913 -0.062340937273717145 DJI_0025.JPG -8.855791726698286 235.74893956148208 0.01762175801103183 DJI_0028.JPG -13.086242906526223 169.3006810553214 -0.06226821051640741 DJI_0031.JPG -17.473255978463847 97.30275378560809 -0.09176870888806832
3GNSS_From_ENU.txt
DJI_0002.JPG 34.03250563926396 108.7677992500047 514.6380141189607 DJI_0005.JPG 34.03267641704175 108.76781155556024 514.4640141184373 DJI_0011.JPG 34.03394725037507 108.76789833333802 514.635014120011 DJI_0015.JPG 34.03487661148619 108.76796561111581 514.6420141205874 DJI_0018.JPG 34.03509530593064 108.76797844444913 514.6150141188659 DJI_0022.JPG 34.035064472597305 108.76773913889359 514.58201412093 DJI_0025.JPG 34.03463080593064 108.76770336111582 514.6600141209601 DJI_0028.JPG 34.03403180593064 108.76765755556025 514.5780141181838 DJI_0031.JPG 34.033382778152856 108.76761005556025 514.5470141202469 DJI_0035.JPG 34.032533167041734 108.7675511111158 514.6610141190716 DJI_0041.JPG 34.03248758370841 108.7671833611158 514.8310141174146 DJI_0042.JPG 34.03248605593062 108.76717719444913 514.781014119351
标签:ECEF,GNSS,lon,gnss,slam,enu,lat,np,ref From: https://www.cnblogs.com/gooutlook/p/18253341