经纬度与平面坐标互转，经纬度与空间直角坐标互转（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\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; }
