Skip to content

Instantly share code, notes, and snippets.

@komasaru komasaru/blh2enu.py
Created Feb 23, 2019

Embed
What would you like to do?
Python script to convert WGS84(BLH) to ENU coordinate.
#! /usr/local/bin/python3
"""
BLH -> ENU 変換
: WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height)を
ENU (East/North/Up; 地平) 座標に変換する。
* 途中、 ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標
への変換を経由。
Date Author Version
2019.02.18 mk-mode.com 1.00 新規作成
Copyright(C) 2019 mk-mode.com All Rights Reserved.
---
引数: LATITUDE(BETA) LONGITUDE(LAMBDA) HEIGHT
"""
import math
import sys
import traceback
class BlhToEnu:
USAGE = (
"[USAGE] ./blh2enu.py BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT\n"
" (BETA: LATITUDE, LAMBDA: LONGITUDE)"
)
PI_180 = math.pi / 180.0
# WGS84 座標パラメータ
A = 6378137.0 # a(地球楕円体長半径(赤道面平均半径))
ONE_F = 298.257223563 # 1 / f(地球楕円体扁平率=(a - b) / a)
B = A * (1.0 - 1.0 / ONE_F) # b(地球楕円体短半径)
E2 = (1.0 / ONE_F) * (2 - (1.0 / ONE_F))
# e^2 = 2 * f - f * f
# = (a^2 - b^2) / a^2
ED2 = E2 * A * A / (B * B) # e'^2= (a^2 - b^2) / b^2
def __init__(self):
""" Initialization
: コマンドライン引数の取得
"""
try:
if len(sys.argv) < 7:
print(self.USAGE)
sys.exit(0)
self.b_o, self.l_o, self.h_o, self.b, self.l, self.h = \
map(lambda x: float(x), sys.argv[1:7])
except Exception as e:
raise
def exec(self):
""" Execution """
try:
print((
"BLH_O: BETA = {:12.8f}°\n"
" LAMBDA = {:12.8f}°\n"
" HEIGHT = {:7.3f}m"
).format(self.b_o, self.l_o, self.h_o))
print((
"BLH: BETA = {:12.8f}°\n"
" LAMBDA = {:12.8f}°\n"
" HEIGHT = {:7.3f}m"
).format(self.b, self.l, self.h))
enu = self.__blh2enu(
self.b_o, self.l_o, self.h_o, self.b, self.l, self.h
)
az = math.atan2(enu[0], enu[1]) * 180.0 / math.pi
if az < 0.0:
az += 360.0
el = math.atan2(
enu[2],
math.sqrt(enu[0] * enu[0] + enu[1] * enu[1])
) * 180.0 / math.pi
dst = math.sqrt(sum([a * a for a in enu]))
print("--->")
print((
"ENU: E = {:12.3f}m\n"
" N = {:12.3f}m\n"
" U = {:12.3f}m\n"
"方位角 = {:12.3f}°\n"
" 仰角 = {:12.3f}°\n"
" 距離 = {:12.3f}m"
).format(*enu, az, el, dst))
except Exception as e:
raise
def __blh2enu(self, b_o, l_o, h_o, b, l, h):
""" BLH -> ENU 変換(Earth, North, Up)
:param float b_o: 原点 Beta(Latitude)
:param float l_o: 原点 Lambda(Longitude)
:param float h_o: 原点 Height
:param float b: 対象 Beta(Latitude)
:param float l: 対象 Lambda(Longitude)
:param float h: 対象 Height
:return list enu: ENU 座標 [e, n, u]
"""
try:
x_o, y_o, z_o = self.__blh2ecef(b_o, l_o, h_o)
x, y, z = self.__blh2ecef(b, l, h)
mat_0 = self.__mat_z(90.0)
mat_1 = self.__mat_y(90.0 - b_o)
mat_2 = self.__mat_z(l_o)
mat = self.__mul_mat(self.__mul_mat(mat_0, mat_1), mat_2)
return self.__rotate(mat, [x - x_o, y - y_o, z - z_o])
except Exception as e:
raise
def __blh2ecef(self, lat, lon, ht):
""" BLH -> ECEF 変換
:param float lat: Latitude
:param float lon: Longitude
:param float ht: Height
:return list : ECEF 座標 [x, y, z]
"""
try:
n = lambda x: self.A / \
math.sqrt(1.0 - self.E2 * math.sin(x * self.PI_180)**2)
x = (n(lat) + ht) \
* math.cos(lat * self.PI_180) \
* math.cos(lon * self.PI_180)
y = (n(lat) + ht) \
* math.cos(lat * self.PI_180) \
* math.sin(lon * self.PI_180)
z = (n(lat) * (1.0 - self.E2) + ht) \
* math.sin(lat * self.PI_180)
return [x, y, z]
except Exception as e:
raise
def __mat_x(self, ang):
""" x 軸を軸とした回転行列
:param float ang: 回転量(°)
:return list : 回転行列(3x3)
"""
try:
a = ang * self.PI_180
c = math.cos(a)
s = math.sin(a)
return [
[1.0, 0.0, 0.0],
[0.0, c, s],
[0.0, -s, c]
]
except Exception as e:
raise
def __mat_y(self, ang):
""" y 軸を軸とした回転行列
:param float ang: 回転量(°)
:return list : 回転行列(3x3)
"""
try:
a = ang * self.PI_180
c = math.cos(a)
s = math.sin(a)
return [
[ c, 0.0, -s],
[ 0.0, 1.0, 0.0],
[ s, 0.0, c]
]
except Exception as e:
raise
def __mat_z(self, ang):
""" z 軸を軸とした回転行列
:param float ang: 回転量(°)
:return list : 回転行列(3x3)
"""
try:
a = ang * self.PI_180
c = math.cos(a)
s = math.sin(a)
return [
[ c, s, 0.0],
[ -s, c, 0.0],
[0.0, 0.0, 1.0]
]
except Exception as e:
raise
def __mul_mat(self, mat_a, mat_b):
""" 2行列(3x3)の積
:param list mat_a: 3x3 行列
:param list mat_b: 3x3 行列
:return list : 3x3 行列
"""
try:
return [
[sum([mat_a[k][i] * mat_b[i][j] for i in range(3)
]) for j in range(3)] for k in range(3)
]
except Exception as e:
raise
def __rotate(self, mat, pt):
""" 点の回転
:param list mat: 3x3 回転行列
:param list pt: 回転前座標 [x, y, z]
:return list : 回転後座標 [x, y, z]
"""
try:
return [
sum([mat[j][i] * pt[i] for i in range(3)])
for j in range(3)
]
except Exception as e:
raise
if __name__ == '__main__':
try:
obj = BlhToEnu()
obj.exec()
except Exception as e:
traceback.print_exc()
sys.exit(1)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.