经纬度与平面坐标互转，经纬度与空间直角坐标互转（C++代码）

在三维激光点云处理中，需经常用到经纬度与平面坐标、空间直角坐标互转的功能，有时只是临时写一个测试demo，不想调用gdal，太麻烦，希望有更简单的调用方式。

网上一通搜索，并没有找到很完整的代码，一些代码杂乱无章，正确性还需确认，于是自己动手写了这四个转换函数，在此与大家分享使用：

头文件：

/*******************************************************************
*
*    作者:    Sun Zhenxing
*    创建日期:    20190819
*
*    说明：实现经纬度与平面坐标互转，实现经纬度与空间直角坐标互转
*
******************************************************************/

#ifndef ZTGEOGRAPHYCOORDINATETRANSFORM_H
#define ZTGEOGRAPHYCOORDINATETRANSFORM_H

#include <math.h>

struct EllipsoidParameter
{
    double a, b, f;
    double e2, ep2;

    // 高斯投影参数
    double c;
    double a0, a2, a4, a6;

    EllipsoidParameter()
    {
        // Default: wgs84
        a = 6378137.0;
        e2 = 0.00669437999013;

        b = sqrt(a * a * (1 - e2));
        ep2 = (a * a - b * b) / (b * b);
        f = (a - b) / a;
        double f0 = 1 / 298.257223563;
        double f1 = 1 / f;

        c = a / (1 - f);
        double m0, m2, m4, m6, m8;
        m0 = a * (1 - e2);
        m2 = 1.5 * e2 * m0;
        m4 = 1.25 * e2 * m2;
        m6 = 7 * e2 * m4 / 6;
        m8 = 9 * e2 * m6 / 8;
        a0 = m0 + m2 / 2 + 3 * m4 / 8 + 5 * m6 / 16 + 35 * m8 / 128;
        a2 = m2 / 2 + m4 / 2 + 15 * m6 / 32 + 7 * m8 / 16;
        a4 = m4 / 8 + 3 * m6 / 16 + 7 * m8 / 32;
        a6 = m6 / 32 + m8 / 16;
    }

    EllipsoidParameter(double ia, double ib)
    {
        if (ib > 1000000)    // ib 是短半轴
        {
            a = ia;
            b = ib;

            f = (a - b) / a;
            e2 = (a * a - b * b) / (a * a);
            ep2 = (a * a - b * b) / (b * b);
        }
        else if (ib < 1)    // ib 是椭球第一偏心率的平方
        {
            a = ia;
            e2 = ib;

            b = sqrt(a * a * (1 - e2));
            ep2 = (a * a - b * b) / (b * b);
            f = (a - b) / a;
        }

        c = a / (1 - f);
        double m0, m2, m4, m6, m8;
        m0 = a * (1 - e2);
        m2 = 1.5 * e2 * m0;
        m4 = 1.25 * e2 * m2;
        m6 = 7 * e2 * m4 / 6;
        m8 = 9 * e2 * m6 / 8;
        a0 = m0 + m2 / 2 + 3 * m4 / 8 + 5 * m6 / 16 + 35 * m8 / 128;
        a2 = m2 / 2 + m4 / 2 + 15 * m6 / 32 + 7 * m8 / 16;
        a4 = m4 / 8 + 3 * m6 / 16 + 7 * m8 / 32;
        a6 = m6 / 32 + m8 / 16;
    }
};

class ZtGeographyCoordinateTransform
{
public:
    ZtGeographyCoordinateTransform();
    ~ZtGeographyCoordinateTransform();

    EllipsoidParameter ellipPmt;

    double meridianLine;
    char projType;            // 'u': utm, 'g': gauss-kruger

    /*
    *    In projection coordinate system: x: east  y: north  z: height
    */

    bool XY2BL(double x, double y, double &lat, double &lon);
    bool BL2XY(double lat, double lon, double &x, double &y);
    bool XYZ2BLH(double x, double y, double z, double &lat, double &lon, double &ht);
    bool BLH2XYZ(double lat, double lon, double ht, double &x, double &y, double &z);
};


#endif

源文件：

#include "stdafx.h"

#include "ztGeographyCoordinateTransform.h"

/*
*    此处未定义PI，直接使用PI值，防止与其他文件宏定义冲突
*/

ZtGeographyCoordinateTransform::ZtGeographyCoordinateTransform()
    : meridianLine(-360), projType('g')
{

}

ZtGeographyCoordinateTransform::~ZtGeographyCoordinateTransform()
{

}

bool ZtGeographyCoordinateTransform::XY2BL(double x, double y, double &lat, double &lon)
{
    if (projType == 'u')
    {
        y = y / 0.9996;
    }

    double bf0 = y / ellipPmt.a0, bf;
    double threshould = 1.0;
    while (threshould > 0.00000001)
    {
        double y0 = -ellipPmt.a2 * sin(2 * bf0) / 2 + ellipPmt.a4 * sin(4 * bf0) / 4 - ellipPmt.a6 * sin(6 * bf0) / 6;
        bf = (y - y0) / ellipPmt.a0;
        threshould = bf - bf0;
        bf0 = bf;
    }

    double t, j2;
    t = tan(bf);
    j2 = ellipPmt.ep2 * pow(cos(bf), 2);

    double v, n, m;
    v = sqrt(1 - ellipPmt.e2 * sin(bf) * sin(bf));
    n = ellipPmt.a / v;
    m = ellipPmt.a * (1 - ellipPmt.e2) / pow(v, 3);

    x = x - 500000;
    if (projType == 'u')
    {
        x = x / 0.9996;
    }

    double temp0, temp1, temp2;
    temp0 = t * x * x / (2 * m * n);
    temp1 = t * (5 + 3 * t * t + j2 - 9 * j2 * t * t) * pow(x, 4) / (24 * m * pow(n, 3));
    temp2 = t * (61 + 90 * t * t + 45 * pow(t, 4)) * pow(x, 6) / (720 * pow(n, 5) * m);
    lat = (bf - temp0 + temp1 - temp2) * 57.29577951308232;

    temp0 = x / (n*cos(bf));
    temp1 = (1 + 2 * t * t + j2) * pow(x, 3) / (6 * pow(n, 3) * cos(bf));
    temp2 = (5 + 28 * t * t + 6 * j2 + 24 * pow(t, 4) + 8 * t * t * j2) * pow(x, 5) / (120 * pow(n, 5) * cos(bf));
    lon = (temp0 - temp1 + temp2) * 57.29577951308232 + meridianLine;

    return true;
}

bool ZtGeographyCoordinateTransform::BL2XY(double lat, double lon, double &x, double &y)
{
    if (meridianLine < -180)
    {
        meridianLine = int((lon + 1.5) / 3) * 3;
    }

    lat = lat * 0.0174532925199432957692;
    double dL = (lon - meridianLine) * 0.0174532925199432957692;

    double X = ellipPmt.a0 * lat - ellipPmt.a2 * sin(2 * lat) / 2 + ellipPmt.a4 * sin(4 * lat) / 4 - ellipPmt.a6 * sin(6 * lat) / 6;
    double tn = tan(lat);
    double tn2 = tn * tn;
    double tn4 = tn2 * tn2;

    double j2 = (1 / pow(1 - ellipPmt.f, 2) - 1) * pow(cos(lat), 2);
    double n = ellipPmt.a / sqrt(1.0 - ellipPmt.e2 * sin(lat) * sin(lat));

    double temp[6] = { 0 };
    temp[0] = n * sin(lat) * cos(lat) * dL * dL / 2;
    temp[1] = n * sin(lat) * pow(cos(lat), 3) * (5 - tn2 + 9 * j2 + 4 * j2 * j2) * pow(dL, 4) / 24;
    temp[2] = n * sin(lat) * pow(cos(lat), 5) * (61 - 58 * tn2 + tn4) * pow(dL, 6) / 720;
    temp[3] = n * cos(lat) * dL;
    temp[4] = n * pow(cos(lat), 3) * (1 - tn2 + j2) * pow(dL, 3) / 6;
    temp[5] = n * pow(cos(lat), 5) * (5 - 18 * tn2 + tn4 + 14 * j2 - 58 * tn2 * j2) * pow(dL, 5) / 120;

    y = X + temp[0] + temp[1] + temp[2];
    x = temp[3] + temp[4] + temp[5];

    if (projType == 'g')
    {
        x = x + 500000;
    }
    else if (projType == 'u')
    {
        x = x * 0.9996 + 500000;
        y = y * 0.9996;
    }

    return true;
}

bool ZtGeographyCoordinateTransform::XYZ2BLH(double x, double y, double z, double &lat, double &lon, double &ht)
{
    double preB, preN;
    double nowB = 0, nowN = 0;
    double threshould = 1.0;

    preB = atan(z / sqrt(x * x + y * y));
    preN = ellipPmt.a / sqrt(1 - ellipPmt.e2 * sin(preB) * sin(preB));
    while (threshould > 0.0000000001)
    {
        nowN = ellipPmt.a / sqrt(1 - ellipPmt.e2 * sin(preB) * sin(preB));
        nowB = atan((z + preN * ellipPmt.e2 * sin(preB)) / sqrt(x * x + y * y));

        threshould = fabs(nowB - preB);
        preB = nowB;
        preN = nowN;
    }
    ht = sqrt(x * x + y * y) / cos(nowB) - nowN;
    lon = atan2(y, x) * 57.29577951308232;    // 180 / pi
    lat = nowB * 57.29577951308232;

    return true;
}

bool ZtGeographyCoordinateTransform::BLH2XYZ(double lat, double lon, double ht, double &x, double &y, double &z)
{
    double sinB = sin(lat / 57.29577951308232);
    double cosB = cos(lat / 57.29577951308232);
    double sinL = sin(lon / 57.29577951308232);
    double cosL = cos(lon / 57.29577951308232);
    
    double N = ellipPmt.a / sqrt(1.0 - ellipPmt.e2 * sinB * sinB);
    x = (N + ht) * cosB * cosL;
    y = (N + ht) * cosB * sinL;
    z = (N * ellipPmt.b * ellipPmt.b / (ellipPmt.a * ellipPmt.a) + ht) * sinB;

    return true;
}

测试代码：

int _tmain(int argc, _TCHAR* argv[])
{
    ZtGeographyCoordinateTransform ztGCT;

    // 真值： 21.863450812    108.799096876    -4.103
    double x = -1908410.6124, y = 5606209.0019,  z = 2360385.6084;

    double lat, lon, ht;
    ztGCT.XYZ2BLH(x, y, z, lat, lon, ht);
    printf("%.9lf	%.9lf	%.3lf
", lat, lon, ht);

    ztGCT.BLH2XYZ(lat, lon, ht, x, y, z);
    printf("%.3lf	%.3lf	%.3lf

", x, y, z);

    // 真值： 39.731939769    116.300105669
    x = 440000;
    y = 4400000;

    ztGCT.meridianLine = 117;
    ztGCT.XY2BL(x, y, lat, lon);
    printf("%.9lf	%.9lf
", lat, lon);

    ztGCT.BL2XY(lat, lon, x, y);
    printf("%.3lf	%.3lf

", x, y);

    return 0;
}