c++ ECEF与GPS坐标互相转换

ECEF与GPS坐标互相转换

#define PI 3.1415926
#define RAD2DEG(x) (x)*180.0/PI
#define DEG2RAD(x) (x)*PI/180.0
static const double WGS84_A = 6378137.0;      // major axis
static const double WGS84_E = 0.0818191908;   // first eccentricity

ECEF转GPS

cv::Point3d ECEF2LLA(cv::Point3d cur_ecef) {
    double x = cur_ecef.x;
    double y = cur_ecef.y;
    double z = cur_ecef.z;
    const double b = sqrt(WGS84_A * WGS84_A * (1 - WGS84_E * WGS84_E));
    const double ep = sqrt((WGS84_A * WGS84_A - b * b) / (b * b));
    const double p = hypot(x, y);
    const double th = atan2(WGS84_A * z, b * p);
    const double lon = atan2(y, x);
    const double lat = atan2((z + ep * ep * b * pow(sin(th), 3)), (p - WGS84_E * WGS84_E * WGS84_A * pow(cos(th), 3)));
    const double N = WGS84_A / sqrt(1 - WGS84_E * WGS84_E * sin(lat) * sin(lat));
    const double alt = p / cos(lat) - N;

    return cv::Point3d(RAD2DEG(lat), RAD2DEG(lon), alt);
}

GPS转ECEF

cv::Point3d LLA2ECEF(cv::Point3d cur_gps) {
    double lat = cur_gps.x;
    double lon = cur_gps.y;
    double alt = cur_gps.z;

    double WGS84_f = 1 / 298.257223565;
    double WGS84_E2 = WGS84_f * (2 - WGS84_f);
    double deg2rad = M_PI / 180.0;
    double rad2deg = 180.0 / M_PI;
    lat *= deg2rad;
    lon *= deg2rad;
    double N = WGS84_A / (sqrt(1 - WGS84_E2 * sin(lat) * sin(lat)));
    double x = (N + alt) * cos(lat) * cos(lon);
    double y = (N + alt) * cos(lat) * sin(lon);
    double z = (N * (1 - WGS84_f) * (1 - WGS84_f) + alt) * sin(lat);
    return cv::Point3d(x, y, z);
}