在三维激光点云处理中，需经常用到经纬度与平面坐标、空间直角坐标互转的功能，有时只是临时写一个测试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\t%.9lf\t%.3lf\n", lat, lon, ht);
ztGCT.BLH2XYZ(lat, lon, ht, x, y, z);
printf("%.3lf\t%.3lf\t%.3lf\n\n", x, y, z);
// 真值： 39.731939769 116.300105669
x = 440000;
y = 4400000;
ztGCT.meridianLine = 117;
ztGCT.XY2BL(x, y, lat, lon);
printf("%.9lf\t%.9lf\n", lat, lon);
ztGCT.BL2XY(lat, lon, x, y);
printf("%.3lf\t%.3lf\n\n", x, y);
return 0;
}