一文搞定经纬度与高斯坐标相互转换，经纬度转ECEF坐标，ECEF转东北高，经纬高与东北高相互转换c++算法

总结了导航与自动驾驶中常用坐标的相互转换的c++代码，ECEF坐标转回经纬度的比较简单，网上其他大神也有写

经纬高坐标，ECEF坐标，高斯坐标，东北高坐标的概念就不介绍了，其他博客介绍的非常好

这里做个C++版本的总结，上面错误的原因是搞混了东北高与高斯坐标的区别，还是不一样的，高斯坐标类似于把球面铺平，而东北高具有一定相对性，选取一点作为原点，而这个点在哪里应该都可以

另外高斯坐标分3度带6度带，有博客说并不适用于全球，这个没有深入研究，这个函数在咱们祖国国境内是够用了～～

#include <iostream>
#include <math.h>

#include <opencv2/opencv.hpp>

class CooTrans
{
public:
    double iPI = 0.0174532925199433; //3.1415926535898/180.0
    double PI = 3.1415926535898;
    // 54年北京坐标系参数
    double a = 6378245.0;   // 长轴
    double f = 1.0 / 298.3; // 扁率   (a-b)/a

    // 80年西安坐标系参数
    // double a=6378140.0;
    // double f=1/298.257;

    // WGS84坐标系参数
    // double a = 6378137.0;
    // double f = 1 / 298.257223563;

    double ZoneWide = 6.0;     // 带宽
    double e2 = 2 * f - f * f; // e为第一偏心率，可以算可以直接提供，e2 = e * e
    cv::Mat R;
    
    //正算 输入为经纬度，并非是分秒格式，输出为高斯坐标
    std::vector<double> LLtoGaussXY(double longitude, double latitude)
    {
        int ProjNo = (int)(longitude / ZoneWide);
        double longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
        longitude0 = longitude0 * iPI;

        // 经度转换为弧度
        longitude = longitude * iPI;
        // 纬度转换为弧度
        latitude = latitude * iPI;

        double ee = e2 * (1.0 - e2);
        double N = a / sqrt(1.0 - e2 * sin(latitude) * sin(latitude)); // 该点的卯酉圈曲率半径
        double T = tan(latitude) * tan(latitude);
        double C = ee * cos(latitude) * cos(latitude);
        double A = (longitude - longitude0) * cos(latitude);
        double M = a * ((1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256) * latitude - (3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) * sin(2 * latitude) + (15 * e2 * e2 / 256 + 45 * e2 * e2 * e2 / 1024) * sin(4 * latitude) - (35 * e2 * e2 * e2 / 3072) * sin(6 * latitude));
        double xval = N * (A + (1 - T + C) * A * A * A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee) * A * A * A * A * A / 120);
        double yval = M + N * tan(latitude) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + (61 - 58 * T + T * T + 600 * C - 330 * ee) * A * A * A * A * A * A / 720);
        double X0 = 1000000 * (ProjNo + 1) + 500000;
        double Y0 = 0;
        xval = xval + X0;
        yval = yval + Y0;
        return {xval, yval};
    }
    //反算 输入为高斯坐标，输出为经纬度
    std::vector<double> GaussXYtoLL(double X, double Y)
    {
        int ProjNo = (int)(X / 1000000); // 查找带号
        double longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
        longitude0 = longitude0 * iPI; // 中央经线

        double X0 = ProjNo * 1000000 + 500000;
        double Y0 = 0;
        double xval = X - X0;
        double yval = Y - Y0; // 带内大地坐标

        double e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
        double ee = e2 / (1 - e2);
        double M = yval;
        double u = M / (a * (1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256));
        double fai = u + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * u) + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * u) + (151 * e1 * e1 * e1 / 96) * sin(6 * u) + (1097 * e1 * e1 * e1 * e1 / 512) * sin(8 * u);
        double C = ee * cos(fai) * cos(fai);
        double T = tan(fai) * tan(fai);
        double N = a / sqrt(1.0 - e2 * sin(fai) * sin(fai)); // 该点的卯酉圈曲率半径
        double R = a * (1 - e2) / sqrt((1 - e2 * sin(fai) * sin(fai)) * (1 - e2 * sin(fai) * sin(fai)) * (1 - e2 * sin(fai) * sin(fai)));
        double D = xval / N;
        // 计算经度(Longitude) 纬度(Latitude)
        double longitude = longitude0 + (D - (1 + 2 * T + C) * D * D * D / 6 + (5 - 2 * C + 28 * T - 3 * C * C + 8 * ee + 24 * T * T) * D * D * D * D * D / 120) / cos(fai);
        double latitude = fai - (N * tan(fai) / R) * (D * D / 2 - (5 + 3 * T + 10 * C - 4 * C * C - 9 * ee) * D * D * D * D / 24 + (61 + 90 * T + 298 * C + 45 * T * T - 256 * ee - 3 * C * C) * D * D * D * D * D * D / 720);
        // 转换为度 DD
        return {longitude / iPI, latitude / iPI};
    }

    // 输入为经纬高，返回ECEF坐标
    cv::Mat LLA2ECEF(double L, double B, double H)
    {
        L = L * iPI;
        B = B * iPI;
        double N = a / sqrt(1 - e2 * sin(B) * sin(B));

        double x = (N + H) * cos(B) * cos(L);
        double y = (N + H) * cos(B) * sin(L);
        double z = (N * (1 - e2) + H) * sin(B);

        cv::Mat twi(3, 1, CV_64F);
        twi.at<double>(0, 0) = x;
        twi.at<double>(1, 0) = y;
        twi.at<double>(2, 0) = z;

        // 这个R计算东北高坐标时使用，以当前函数输入的经纬度为原点，这个角度为当前原点在ecef下的角度
        R = (cv::Mat_<double>(3, 3) <<          -sin(L),           cos(L),      0,
                                       -sin(B) * cos(L), -sin(B) * sin(L), cos(B),
                                        cos(B) * cos(L),  cos(B) * sin(L), sin(B));
        return twi;
    }
    // Pw为ECEF坐标，建立以Pw为原点的东北高坐标系，计算LBH在这个坐标系下的东北高坐标，注意不要跟getrtk函数搞混，输入的坐标代表的坐标系不同
    cv::Mat getENH(cv::Mat Pw, double L, double B, double H)
    {
        L = L * iPI;
        B = B * iPI;
        
        double N = a / sqrt(1 - e2 * sin(B) * sin(B));
        cv::Mat_<double> t = (cv::Mat_<double>(3, 1) << (N + H) * cos(B) * cos(L), (N + H) * cos(B) * sin(L), (N * (1 - e2) + H) * sin(B));
        cv::Mat P = R * (t - Pw );
        return P;
    }
};

最后可以做下经纬度与高斯坐标之间变化的测试

----------------------------------------------------------------------------------------------------------------------------

更新

经纬高与东北天的相互转换

#include <iostream>
#include <math.h>

#include <eigen3/Eigen/Core>

class CooTrans
{
public:
    CooTrans() {}
    // 输入为纬经高初始化东北高坐标原点
    CooTrans(double lat, double lon, double h)
    {
        // lon = lon * iPI;
        // lat = lat * iPI;
        double N = a / sqrt(1 - e2 * sin(lat) * sin(lat));

        double x = (N + h) * cos(lat) * cos(lon);
        double y = (N + h) * cos(lat) * sin(lon);
        double z = (N * (1 - e2) + h) * sin(lat);

        r << -sin(lon), cos(lon), 0,
            -sin(lat) * cos(lon), -sin(lat) * sin(lon), cos(lat),
            cos(lat) * cos(lon), cos(lat) * sin(lon), sin(lat);

        ecef_Ow[0] = x;
        ecef_Ow[1] = y;
        ecef_Ow[2] = z;
    }
    // 重新设置原点
    void SetECEFOw(double lat, double lon, double h)
    {
        // lon = lon * iPI;
        // lat = lat * iPI;
        double N = a / sqrt(1 - e2 * sin(lat) * sin(lat));

        double x = (N + h) * cos(lat) * cos(lon);
        double y = (N + h) * cos(lat) * sin(lon);
        double z = (N * (1 - e2) + h) * sin(lat);

        r << -sin(lon), cos(lon), 0,
            -sin(lat) * cos(lon), -sin(lat) * sin(lon), cos(lat),
            cos(lat) * cos(lon), cos(lat) * sin(lon), sin(lat);

        ecef_Ow[0] = x;
        ecef_Ow[1] = y;
        ecef_Ow[2] = z;
    }
    // 经纬高转东北高， 输入为纬经高，弧度
    void getENH(double lat, double lon, double h, double &x, double &y, double &z)
    {
        double N_ = a / sqrt(1 - e2 * sin(lat) * sin(lat));
        Eigen::Vector3d t((N_ + h) * cos(lat) * cos(lon), (N_ + h) * cos(lat) * sin(lon), (N_ * (1 - e2) + h) * sin(lat));
        t = r * (t - ecef_Ow);
        x = t[0];
        y = t[1];
        z = t[2];
    }
    // 输入为一个东北高坐标，返回其经纬高
    Eigen::Vector3d ECEF2LLA(Eigen::Vector3d t)
    {
        double X, Y, Z;
        t = r.transpose() * t + ecef_Ow;
        X = t[0];
        Y = t[1];
        Z = t[2];

        double B0, R, N;
        double B_, L_;
        R = sqrt(X * X + Y * Y);
        B0 = atan2(Z, R);
        while (1)
        {
            N = a / sqrt(1.0 - e2 * sin(B0) * sin(B0));
            B_ = atan2(Z + N * e2 * sin(B0), R);
            if (fabs(B_ - B0) < 1.0e-10)
                break;

            B0 = B_;
        }
        L_ = atan2(Y, X);

        Eigen::Vector3d LLA;
        LLA(2) = R / cos(B_) - N;
        //弧度转换成经纬度
        LLA(0) = B_ * 180 / M_PI;
        LLA(1) = L_ * 180 / M_PI;

        return LLA;

    }

    double iPI = 0.0174532925199433; //3.1415926535898/180.0
    double PI = 3.1415926535898;
    // 54年北京坐标系参数
    // double a = 6378245.0;   // 长轴
    // double f = 1.0 / 298.3; // 扁率   (a-b)/a

    // 80年西安坐标系参数
    // double a = 6378140.0;
    // double f = 1 / 298.25722101;

    // WGS84坐标系参数
    double a = 6378137.0;
    double f = 1 / 298.257223563;

    double e2 = 2 * f - f * f; // e为第一偏心率，可以算可以直接提供，e2 = e * e

    Eigen::Vector3d ecef_Ow;
    Eigen::Matrix3d r;
};