ENU与ecef与WGS84相互转换（c++和python）

野性酷女 2023-01-09 15:59 493阅读 0赞

（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);//此处经纬度是站点经纬度 //六个参数，偏移每个坐标