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
|