Skip to content

Instantly share code, notes, and snippets.

@SkybuckFlying
Forked from mpkuse/XYZ_2_latlong.py
Created August 6, 2022 10:53
Show Gist options
  • Save SkybuckFlying/ee035810dbfdce2841008c6cab8ca229 to your computer and use it in GitHub Desktop.
Save SkybuckFlying/ee035810dbfdce2841008c6cab8ca229 to your computer and use it in GitHub Desktop.
Convert GPS (Latitude and Longitude) to XYZ
""" Convert GPS (Latitude and Longitude) to XYZ wrt a set Lat Long as origin """
def geodedic_to_ecef( lati, longi, alti ):
""" lati in degrees, longi in degrees. alti in meters (mean sea level) """
# Adopted from https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
phi = lati / 180. * np.pi
lambada = longi / 180. * np.pi
h = alti
#N = 6371000 #in meters
e = 0.081819191 #earth ecentricity
q = np.sin( phi )
N = 6378137.0 / np.sqrt( 1 - e*e * q*q )
X = (N + h) * np.cos( phi ) * np.cos( lambada )
Y = (N + h) * np.cos( phi ) * np.sin( lambada )
Z = (N*(1-e*e) + h) * np.sin( phi )
return X,Y,Z
def compute_ecef_to_enu_transform( lati_r, longi_r ):
""" Computes a matrix_3x3 which transforms a ecef diff-point to ENU (East-North-Up)
Needs as input the latitude and longitude (in degrees) of the reference point
"""
# Adopted from https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU
phi = lati_r / 180. * np.pi
lambada = longi_r / 180. * np.pi
cp = np.cos( phi ) #cos(phi)
sp = np.sin( phi ) #cos(phi)
cl = np.cos( lambada )
sl = np.sin( lambada )
T = np.zeros( (3,3), dtype='float64' )
T[0,0] = -sl
T[0,1] = cl
T[0,2] = 0
T[1,0] = -sp * cl
T[1,1] = -sp * sl
T[1,2] = cp
T[2,0] = cp * cl
T[2,1] = cp * sl
T[2,2] = sp
T_enu_ecef = T
return T_enu_ecef
# GPS (geodedic to Earth-center cords, ie. ecef )
Xr, Yr, Zr = geodedic_to_ecef( 22.3349060891, 114.263170818, 173.073608398 ) #of radar
T_enu_ecef = compute_ecef_to_enu_transform( 22.3349060891, 114.263170818 )
Xp, Yp, Zp = geodedic_to_ecef( data.latitude, data.longitude, data.altitude ) #curr pos of drone
#
# ECEF to ENU (East-North-Up)
delta = np.array( [Xp-Xr, Yp-Yr, Zp-Zr] )
p = np.dot( T_enu_ecef, delta )
""" Give a) points in localcoordinate system b) a gps lat long of the origin of the local coordinate system,
this script helps to convert XYZ to latlong.
Beware that the transformation will be off by an yaw angle. This is because the local cordinate frame is may/or may not align with the East-North-Up frame.
The way it works is XYZ --> ECEF --> geodedic (latlong)
main reference is still the wiki https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU.
However the procedure to convert from ECEF to geodedic in wikip does not give accurate results. Instead the algorithm
from https://hal.archives-ouvertes.fr/hal-01704943v2/document gives a better result.
"""
#### Mandatory inputs
# 46.7183003,6.9636191,741.498
radar_lat = 46.7183003
radar_lon = 6.9636191
radar_msl = 741.498
def ecef_to_geodedic_2( ecef_X ):
""" ECEF --> lat (PHI), long (LAMBDA)
algorithm2 https://hal.archives-ouvertes.fr/hal-01704943v2/document
"""
a = 6378137 #(earth's semimarjor axis in meters)
e = 0.081819191 #earth ecentricity
b = a * np.sqrt( 1.0 - e*e )
_X = ecef_X[0]
_Y = ecef_X[1]
_Z = ecef_X[2]
w_2 = _X**2 + _Y**2
l = e**2 / 2.0
m = w_2 / a**2
n = _Z**2 * (1.0 - e*e) / (a*a)
p = (m+n - 4*l*l)/6.0
G = m*n*l*l
H = 2*p**3 + G
C = np.cbrt( H+G+2*np.sqrt(H*G) ) / np.cbrt(2)
i = -(2.*l*l + m + n ) / 2.0
P = p*p
beta = i/3.0 - C -P/C
k = l*l * ( l*l - m - n )
t = np.sqrt( np.sqrt( beta**2 - k ) - (beta+i)/2.0 ) - np.sign( m-n ) * np.sqrt( np.abs(beta-i) / 2.0 )
F = t**4 + 2*i*t*t + 2.*l*(m-n)*t + k
dF_dt = 4*t**3 + 4*i*t + 2*l*(m-n)
delta_t = -F / dF_dt
u = t + delta_t + l
v = t + delta_t - l
w = np.sqrt( w_2 )
__phi = np.arctan2( _Z*u, w*v )
delta_w = w* (1.0-1.0/u )
delta_z = _Z*( 1- (1-e*e) / v )
h = np.sign( u-1.0 ) * np.sqrt( delta_w**2 + delta_z**2 )
__lambda = np.arctan2( _Y, _X )
return (__phi, __lambda)
# Main:
# ENU --> ECEF
T_enu_ecef = np.linalg.inv( compute_ecef_to_enu_transform( radar_lat, radar_lon ) )
ecef_radar = geodedic_to_ecef( radar_lat, radar_lon, radar_msl )
enu_X = np.array( [ data.pose.position.x, data.pose.position.y, data.pose.position.z ] )
# print( "enu_X: " , enu_X )
ecef_X = np.dot( T_enu_ecef, enu_X ) + ecef_radar
# print( "ecef_X: ", ecef_X )
# ECEF --> lat (PHI), long (LAMBDA)
(__phi, __lambda) = ecef_to_geodedic_2( ecef_X )
lambda_long_indegs = __lambda /np.pi * 180.
phi_lat_indegs = __phi / np.pi * 180.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment