ENU与ecef与WGS84相互转换（c++和python）
Posted 乌托邦2号
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了ENU与ecef与WGS84相互转换（c++和python）相关的知识，希望对你有一定的参考价值。
（1）enu转wgs84
c++版本：
void enu_to_wgs84(double x_east, double y_north, double z_up, double lati, double longti,double height, double*result)
double pi = std::acos(-1.0);
double a = 6378137.0; //earth radius in meters
double b = 6356752.3142; //earth semiminor in meters
double f = (a - b) / a;
double e_sq = f * (2-f);
double lamb = lati * pi / 180.0;
double phi = longti * pi / 180.0;
double sl = std::sin(lamb);
double N = a / std::sqrt(1 - e_sq * sl * sl);
double sin_lambda = std::sin(lamb);
double cos_lambda = std::cos(lamb);
double sin_phi = std::sin(phi);
double cos_phi = std::cos(phi);
double x0 = (height + N) * cos_lambda * cos_phi;
double y0 = (height + N) * cos_lambda * sin_phi;
double z0 = (height + (1 - e_sq) * N) * sin_lambda;
double t = cos_lambda * z_up - sin_lambda * y_north;
double zd = sin_lambda * z_up + cos_lambda * y_north;
double xd = cos_phi * t - sin_phi * x_east;
double yd = sin_phi * t + cos_phi * x_east;
//Convert from ECEF cartesian coordinates to
//latitude, longitude and height. WGS-8
double x = xd + x0;
double y = yd + y0;
double z = zd + z0;
double x2 = std::pow(x, 2);
double y2 = std::pow(y, 2);
double z2 = std::pow(z, 2);
double e = std::sqrt (1-std::pow((b/a) , 2));
double b2 = b*b;
double e2 = e*e;
double ep = e*(a/b);
double r = std::sqrt(x2+y2);
double r2 = r*r;
double E2 = a*a - b*b;
double F = 54*b2*z2;
double G = r2 + (1-e2)*z2 - e2*E2;
double c = (e2*e2*F*r2)/(G*G*G);
double s = std::pow(( 1 + c + std::sqrt(c*c + 2*c) ) ,(1/3));
double P = F / (3 * std::pow((s+1/s+1), 2) * G*G);
double Q = std::sqrt(1+2*e2*e2*P);
double ro = -(P*e2*r)/(1+Q) + std::sqrt((a*a/2)*(1+1/Q) - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2);
double tmp = std::pow((r - e2*ro), 2);
double U = std::sqrt( tmp + z2 );
double V = std::sqrt( tmp + (1-e2)*z2 );
double zo = (b2*z)/(a*V);
double high = U*( 1 - b2/(a*V) );
double lat = std::atan( (z + ep*ep*zo)/r );
double temp = std::atan(y/x);
double lon = temp - pi;
if (x >= 0)
lon = temp;
else if (x < 0 && y >= 0)
lon = pi + temp;
*result = lat/(pi/180);
*(result + 1) = lon/(pi/180);
*(result + 2) = high;
调用
char result[3] = 0;
enu_to_wgs84(x-190.000000, y-170.000000, z-20.000000, 30.37,114.88, result);
python版本：
# -*- coding: utf-8 -*-
# @Author: wuzida
# @Date: 2018-12-28 15:20:54
# @Last Modified by: wuzida
# @Last Modified time: 2018-12-29 16:49:16
import math
a = 6378137
b = 6356752.3142
f = (a - b) / a
e_sq = f * (2-f)
pi = 3.14159265359
def geodetic_to_ecef(lat, lon, h):
# (lat, lon) in degrees
# h in meters
lamb = math.radians(lat)
phi = math.radians(lon)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x = (h + N) * cos_lambda * cos_phi
y = (h + N) * cos_lambda * sin_phi
z = (h + (1 - e_sq) * N) * sin_lambda
return x, y, z
def ecef_to_enu(x, y, z, lat0, lon0, h0):
lamb = math.radians(lat0)
phi = math.radians(lon0)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x0 = (h0 + N) * cos_lambda * cos_phi
y0 = (h0 + N) * cos_lambda * sin_phi
z0 = (h0 + (1 - e_sq) * N) * sin_lambda
xd = x - x0
yd = y - y0
zd = z - z0
t = -cos_phi * xd - sin_phi * yd
xEast = -sin_phi * xd + cos_phi * yd
yNorth = t * sin_lambda + cos_lambda * zd
zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd
return xEast, yNorth, zUp
def enu_to_ecef(xEast, yNorth, zUp, lat0, lon0, h0):
lamb = math.radians(lat0)
phi = math.radians(lon0)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x0 = (h0 + N) * cos_lambda * cos_phi
y0 = (h0 + N) * cos_lambda * sin_phi
z0 = (h0 + (1 - e_sq) * N) * sin_lambda
t = cos_lambda * zUp - sin_lambda * yNorth
zd = sin_lambda * zUp + cos_lambda * yNorth
xd = cos_phi * t - sin_phi * xEast
yd = sin_phi * t + cos_phi * xEast
x = xd + x0
y = yd + y0
z = zd + z0
return x, y, z
def ecef_to_geodetic(x, y, 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 geodetic_to_enu(lat, lon, h, lat_ref, lon_ref, h_ref):
x, y, z = geodetic_to_ecef(lat, lon, h)
return ecef_to_enu(x, y, z, lat_ref, lon_ref, h_ref)
def enu_to_geodetic(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref):
x,y,z = enu_to_ecef(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref)
return ecef_to_geodetic(x,y,z)（2）WGS84坐标系转为地心坐标系
void double[] wgs84ToEcef(double lat, double lon, double h)
double a = 6378137;
double b = 6356752.3142;
double f = (a - b) / a;
double e_sq = f * (2 - f);
double lamb = Math.toRadians(lat);
double phi = Math.toRadians(lon);
double s = Math.sin(lamb);
double N = a / Math.sqrt(1 - e_sq * s * s);
double sin_lambda = Math.sin(lamb);
double cos_lambda = Math.cos(lamb);
double sin_phi = Math.sin(phi);
double cos_phi = Math.cos(phi);
double x = (h + N) * cos_lambda * cos_phi;
double y = (h + N) * cos_lambda * sin_phi;
double z = (h + (1 - e_sq) * N) * sin_lambda;
return new double[]x,y,z;
（3）站点经纬度转Enu
void double[] ecefToEnu(double x, double y, double z, double lat, double lng, double height)
double a = 6378137;
double b = 6356752.3142;
double f = (a - b) / a;
double e_sq = f * (2 - f);
double lamb = Math.toRadians(lat);
double phi = Math.toRadians(lng);
double s = Math.sin(lamb);
double N = a / Math.sqrt(1 - e_sq * s * s);
double sin_lambda = Math.sin(lamb);
double cos_lambda = Math.cos(lamb);
double sin_phi = Math.sin(phi);
double cos_phi = Math.cos(phi);
double x0 = (height + N) * cos_lambda * cos_phi;
double y0 = (height + N) * cos_lambda * sin_phi;
double z0 = (height + (1 - e_sq) * N) * sin_lambda;
double xd = x - x0;
double yd = y - y0;
double zd = z - z0;
double t = -cos_phi * xd - sin_phi * yd;
double xEast = -sin_phi * xd + cos_phi * yd;
double yNorth = t * sin_lambda + cos_lambda * zd;
double zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;
return new double[] xEast, yNorth, zUp ;
double[] arr1 = wgs84ToEcef(30.7335905, 103.968191, 493.080);//此处经纬度是需要比对的偏移经纬度 //对应<SRS>ENU:30.37479991,114.8956375</SRS>和 <SRSOrigin>-196.000000,-177.000000,-22.000000</SRSOrigin>
ecefToEnu(arr1[0],arr1[1], arr1[2],30.7335905, 103.968191, 493.080);//此处经纬度是站点经纬度 //六个参数，偏移每个坐标
以上是关于ENU与ecef与WGS84相互转换（c++和python）的主要内容，如果未能解决你的问题，请参考以下文章