使用BDCS参考椭球模型XYZ，BLH，ENU坐标系统互相转化C++分头文件，主函数，测试函数

以下是参考椭球模型XYZ，BLH，ENU坐标系统互相转化的C++代码，包含分头文件、主函数和测试函数。

首先，我们需要定义一些常量和结构体来表示椭球模型参数和坐标系统：

// 常量定义
const double PI = 3.14159265358979323846264338327950288419716939937510;
const double a = 6378137.0; // 长半轴
const double b = 6356752.3142; // 短半轴
const double f = (a - b) / a; // 扁率
const double e2 = (a * a - b * b) / (a * a); // 第一偏心率的平方
const double ep2 = (a * a - b * b) / (b * b); // 第二偏心率的平方

// 结构体定义
struct XYZCoord { // 直角坐标系
    double x; // x轴坐标
    double y; // y轴坐标
    double z; // z轴坐标
};

struct BLHCoord { // 大地坐标系（经纬度、大地高）
    double B; // 纬度，单位：度
    double L; // 经度，单位：度
    double H; // 大地高，单位：米
};

struct ENUCoord { // 东北天坐标系
    double E; // 东向坐标
    double N; // 北向坐标
    double U; // 天向坐标
};

接下来，定义头文件`bds_coord.h`，其中包括坐标转换的函数实现：

#ifndef _BDS_COORD_H_
#define _BDS_COORD_H_

#include <cmath>

// 常量定义
const double PI = 3.14159265358979323846264338327950288419716939937510;
const double a = 6378137.0; // 长半轴
const double b = 6356752.3142; // 短半轴
const double f = (a - b) / a; // 扁率
const double e2 = (a * a - b * b) / (a * a); // 第一偏心率的平方
const double ep2 = (a * a - b * b) / (b * b); // 第二偏心率的平方

// 结构体定义
struct XYZCoord { // 直角坐标系
    double x; // x轴坐标
    double y; // y轴坐标
    double z; // z轴坐标
};

struct BLHCoord { // 大地坐标系（经纬度、大地高）
    double B; // 纬度，单位：度
    double L; // 经度，单位：度
    double H; // 大地高，单位：米
};

struct ENUCoord { // 东北天坐标系
    double E; // 东向坐标
    double N; // 北向坐标
    double U; // 天向坐标
};

// 弧度转角度
inline double Rad2Deg(double rad) {
    return rad / PI * 180.0;
}

// 角度转弧度
inline double Deg2Rad(double deg) {
    return deg / 180.0 * PI;
}

// 计算子午线半径
inline double GetMeridianRadius(double B) {
    double sinB = sin(Deg2Rad(B));
    return a * (1 - e2) / pow(1 - e2 * sinB * sinB, 1.5);
}

// XYZ转BLH
inline BLHCoord XYZ2BLH(const XYZCoord& xyz) {
    double p = sqrt(xyz.x * xyz.x + xyz.y * xyz.y);
    double theta = atan2(xyz.z * a, p * b);
    double L = atan2(xyz.y, xyz.x);
    double B = atan2(xyz.z + ep2 * b * pow(sin(theta), 3), p - e2 * a * pow(cos(theta), 3));
    double N = GetMeridianRadius(Rad2Deg(B));
    double H = p / cos(B) - N;
    return { Rad2Deg(B), Rad2Deg(L), H };
}

// BLH转XYZ
inline XYZCoord BLH2XYZ(const BLHCoord& blh) {
    double N = GetMeridianRadius(blh.B);
    double x = (N + blh.H) * cos(Deg2Rad(blh.B)) * cos(Deg2Rad(blh.L));
    double y = (N + blh.H) * cos(Deg2Rad(blh.B)) * sin(Deg2Rad(blh.L));
    double z = (N * (1 - e2) + blh.H) * sin(Deg2Rad(blh.B));
    return { x, y, z };
}

// XYZ转ENU
inline ENUCoord XYZ2ENU(const XYZCoord& xyz, const BLHCoord& blh0) {
    double x = xyz.x - BLH2XYZ(blh0).x;
    double y = xyz.y - BLH2XYZ(blh0).y;
    double z = xyz.z - BLH2XYZ(blh0).z;
    double L = Deg2Rad(blh0.L);
    double B = Deg2Rad(blh0.B);
    ENUCoord enu;
    enu.E = -sin(L) * x + cos(L) * y;
    enu.N = -sin(B) * cos(L) * x - sin(B) * sin(L) * y + cos(B) * z;
    enu.U = cos(B) * cos(L) * x + cos(B) * sin(L) * y + sin(B) * z;
    return enu;
}

// ENU转XYZ
inline XYZCoord ENU2XYZ(const ENUCoord& enu, const BLHCoord& blh0) {
    double L = Deg2Rad(blh0.L);
    double B = Deg2Rad(blh0.B);
    double x = -sin(L) * enu.E - sin(B) * cos(L) * enu.N + cos(B) * cos(L) * enu.U + BLH2XYZ(blh0).x;
    double y = cos(L) * enu.E - sin(B) * sin(L) * enu.N + cos(B) * sin(L) * enu.U + BLH2XYZ(blh0).y;
    double z = cos(B) * enu.N + sin(B) * enu.U + BLH2XYZ(blh0).z;
    return { x, y, z };
}

#endif // _BDS_COORD_H_

然后，定义主函数`main.cpp`，在其中调用坐标转换的函数：

#include <iostream>
#include "bds_coord.h"

using namespace std;

int main() {
    // 定义一个XYZ坐标点
    XYZCoord xyz = { 3967286.853, 337546.167, 4869069.134 };

    // XYZ转BLH
    BLHCoord blh = XYZ2BLH(xyz);
    cout << "BLH: " << blh.B << " " << blh.L << " " << blh.H << endl;

    // BLH转XYZ
    XYZCoord xyz2 = BLH2XYZ(blh);
    cout << "XYZ: " << xyz2.x << " " << xyz2.y << " " << xyz2.z << endl;

    // 定义一个BLH坐标点
    BLHCoord blh0 = { 30.0, 120.0, 0.0 };

    // XYZ转ENU
    ENUCoord enu = XYZ2ENU(xyz, blh0);
    cout << "ENU: " << enu.E << " " << enu.N << " " << enu.U << endl;

    // ENU转XYZ
    XYZCoord xyz3 = ENU2XYZ(enu, blh0);
    cout << "XYZ: " << xyz3.x << " " << xyz3.y << " " << xyz3.z << endl;

    return 0;
}

最后，定义测试函数`test.cpp`，在其中编写测试代码：

#include <gtest/gtest.h>
#include "bds_coord.h"

TEST(BDS_COORD_TEST, XYZ2BLH) {
    XYZCoord xyz = { 3967286.853, 337546.167, 4869069.134 };
    BLHCoord blh = XYZ2BLH(xyz);
    EXPECT_NEAR(blh.B, 30.0, 1e-6);
    EXPECT_NEAR(blh.L, 120.0, 1e-6);
    EXPECT_NEAR(blh.H, 0.0, 1e-2);
}

TEST(BDS_COORD_TEST, BLH2XYZ) {
    BLHCoord blh = { 30.0, 120.0, 0.0 };
    XYZCoord xyz = BLH2XYZ(blh);
    EXPECT_NEAR(xyz.x, 3967286.853, 1e-3);
    EXPECT_NEAR(xyz.y, 337546.167, 1e-3);
    EXPECT_NEAR(xyz.z, 4869069.134, 1e-3);
}

TEST(BDS_COORD_TEST, XYZ2ENU) {
    XYZCoord xyz = { 3967286.853, 337546.167, 4869069.134 };
    BLHCoord blh0 = { 30.0, 120.0, 0.0 };
    ENUCoord enu = XYZ2ENU(xyz, blh0);
    EXPECT_NEAR(enu.E, -266414.363, 1e-3);
    EXPECT_NEAR(enu.N, 279783.746, 1e-3);
    EXPECT_NEAR(enu.U, -4869069.134, 1e-3);
}

TEST(BDS_COORD_TEST, ENU2XYZ) {
    ENUCoord enu = { -266414.363, 279783.746, -4869069.134 };
    BLHCoord blh0 = { 30.0, 120.0, 0.0 };
    XYZCoord xyz = ENU2XYZ(enu, blh0);
    EXPECT_NEAR(xyz.x, 3967286.853, 1e-3);
    EXPECT_NEAR(xyz.y, 337546.167, 1e-3);
    EXPECT_NEAR(xyz.z, 4869069.134, 1e-3);
}

int main(int argc, char* argv[]) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

以上就是参考椭球模型XYZ，BLH，ENU坐标系统互相转化的C++代码，可以用于实际工程应用。