125 lines
3.4 KiB
Python
125 lines
3.4 KiB
Python
'''
|
||
通用算法
|
||
'''
|
||
import numpy as np
|
||
import math
|
||
|
||
def FindInfomationFromJson(HeaderFile_dom_json,node_path_list):
|
||
'''
|
||
在Json文件中，按照指定路径解析出制定节点
|
||
'''
|
||
result_node=HeaderFile_dom_json
|
||
for nodename in node_path_list:
|
||
result_node=result_node[nodename]
|
||
return result_node
|
||
|
||
def GetVectorNorm(Vecter):
|
||
'''
|
||
得到向量的模
|
||
'''
|
||
Vecter=Vecter.reshape(-1,1)
|
||
Vecter_Norm_pow=np.matmul(Vecter.T,Vecter)
|
||
return np.sqrt(Vecter_Norm_pow)
|
||
|
||
def appendInfomationForCsv(appendtext,FilePath="./GridBlock"):
|
||
'''在文件末尾追加信息
|
||
Args:
|
||
appendtext: 需要附加的字符串信息
|
||
FilePath: 文件路径
|
||
return:
|
||
state：文件状态
|
||
'''
|
||
with open(FilePath,'a',encoding='utf-8') as fp:
|
||
fp.write("{}\n".format(appendtext))
|
||
return True
|
||
return False
|
||
|
||
def LLA_to_XYZ(self,latitude, longitude, altitude):
|
||
''' 经纬度转大地坐标系
|
||
args:
|
||
latitude:纬度
|
||
longitude:经纬
|
||
altitude:海拔高程
|
||
refrence: https://blog.csdn.net/dulingwen/article/details/96868530
|
||
'''
|
||
# 经纬度的余弦值
|
||
cosLat = math.cos(latitude * math.pi / 180)
|
||
sinLat = math.sin(latitude * math.pi / 180)
|
||
cosLon = math.cos(longitude * math.pi / 180)
|
||
sinLon = math.sin(longitude * math.pi / 180)
|
||
|
||
# WGS84坐标系的参数
|
||
rad = 6378137.0 #地球赤道平均半径（椭球长半轴：a）
|
||
f = 1.0 / 298.257224 # WGS84椭球扁率 :f = (a-b)/a
|
||
C = 1.0 / math.sqrt(cosLat * cosLat + (1-f) * (1-f) * sinLat * sinLat)
|
||
S = (1-f) * (1-f) * C
|
||
h = altitude
|
||
|
||
# 计算XYZ坐标
|
||
X = (rad * C + h) * cosLat * cosLon
|
||
Y = (rad * C + h) * cosLat * sinLon
|
||
Z = (rad * S + h) * sinLat
|
||
|
||
return np.array([X, Y, Z]).reshape(1,3)
|
||
|
||
|
||
|
||
|
||
""" def XYZ_to_LLA(X, Y, Z):
|
||
''' 大地坐标系转经纬度
|
||
适用于WGS84坐标系
|
||
args:
|
||
x,y,z
|
||
return:
|
||
lat,long,altitude
|
||
'''
|
||
# WGS84坐标系的参数
|
||
a = 6378137.0 # 椭球长半轴
|
||
b = 6356752.314245 # 椭球短半轴
|
||
ea = np.sqrt((a ** 2 - b ** 2) / a ** 2)
|
||
eb = np.sqrt((a ** 2 - b ** 2) / b ** 2)
|
||
p = np.sqrt(X ** 2 + Y ** 2)
|
||
theta = np.arctan2(Z * a, p * b)
|
||
|
||
# 计算经纬度及海拔
|
||
longitude = np.arctan2(Y, X)
|
||
latitude = np.arctan2(Z + eb ** 2 * b * np.sin(theta) ** 3, p - ea ** 2 * a * np.cos(theta) ** 3)
|
||
N = a / np.sqrt(1 - ea ** 2 * np.sin(latitude) ** 2)
|
||
altitude = p / np.cos(latitude) - N
|
||
|
||
return np.array([np.degrees(latitude), np.degrees(longitude), altitude]) """
|
||
|
||
|
||
def XYZ_to_LLA(x, y,z):
|
||
epsilon = 0.000000000000001
|
||
pi = 3.14159265358979323846
|
||
d2r = pi / 180
|
||
r2d = 180 / pi
|
||
|
||
a = 6378137.0
|
||
f_inverse = 298.257223563
|
||
b = a - a / f_inverse
|
||
|
||
e = np.sqrt(a * a - b * b) / a
|
||
|
||
|
||
tmpX = x
|
||
temY = y
|
||
temZ = z
|
||
|
||
curB = 0
|
||
N = 0;
|
||
calB = math.atan2(temZ, math.sqrt(tmpX * tmpX + temY * temY));
|
||
|
||
counter = 0
|
||
while (abs(curB - calB) * r2d > epsilon and counter < 25):
|
||
curB = calB
|
||
N = a / math.sqrt(1 - e * e * math.sin(curB) * math.sin(curB));
|
||
calB = math.atan2(temZ + N * e * e * math.sin(curB), math.sqrt(tmpX * tmpX + temY * temY));
|
||
counter=counter+1
|
||
|
||
x = math.atan2(temY, tmpX) * r2d
|
||
y = curB * r2d
|
||
z = temZ / math.sin(curB) - N * (1 - e * e);
|
||
result= np.array([x,y, z])
|
||
return result |