WGS84经纬度坐标转化成UTM坐标

  在无人驾驶或者机器人领域，经常会涉及到WGS 84 经纬度坐标与UTM坐标之间的转换，本文总结了两种方式来实现两者的转化。

1 利用geodesy功能包实现转化

  头文件：

#ifndef WGS2UTM_H
#define WGS2UTM_H

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

class WGS2UTM
{
    public:

    /**
     * @brief 构造函数
     */
    WGS2UTM();

    /**
     * @brief 获取选取原点的经纬度lla坐标
     * @return  所选取原点的经纬度lla坐标
     */
    inline Eigen::Vector3d get_origin_lla() 
    {
        return origin_LLA_;
    }

    /**
     * @brief 获取选取原点的经纬度utm坐标
     * @return  所选取原点的经纬度utm坐标
     */
    inline Eigen::Vector3d get_origin_tum() 
    {
        return origin_UTM_;
    }

    /***********************************************************/
    /***********************************************************/
    /*****利用geodesy功能包实现WGS84经纬度坐标转化成UTM坐标*****/
    /***********************************************************/
    /***********************************************************/

    /**
     * @brief 把选取的原点的经纬度lla坐标转化为东北天utm坐标
     * @param origin_LLA    输入--原点经纬度lla坐标
     */
    void set_origin_1(Eigen::Vector3d origin_LLA);

    /**
     * @brief 把经纬度lla坐标转化为东北天utm坐标
     * @param LLA    输入--lla坐标
     * @param UTM    输出--utm坐标
     */
    void LLA2UTM_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM);

    /**
     * @brief 把经纬度lla坐标转化为相对于所选取原点的东北天utm坐标
     * @param LLA    输入--lla坐标
     * @param UTM    输出--utm坐标
     */
    void LLA2UTM_origin_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM_origin);

private:

    Eigen::Vector3d origin_LLA_;
    Eigen::Vector3d origin_UTM_;

    double torad = M_PI / 180;
};
#endif //WGS2UTM_H

  源文件：

#include "convert_wgs2utm/wgs2utm.h"

WGS2UTM::WGS2UTM() {}

void WGS2UTM::set_origin_1(Eigen::Vector3d origin_LLA) 
{
    origin_LLA_ = origin_LLA;
    LLA2UTM_1(origin_LLA_, origin_UTM_);
}

void WGS2UTM::LLA2UTM_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM)
{
    geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
    gps_msg->position.latitude  = LLA[0];
    gps_msg->position.longitude = LLA[1];
    gps_msg->position.altitude  = LLA[2];
    geodesy::UTMPoint utm_point;
    geodesy::fromMsg(gps_msg->position, utm_point);
    UTM[0] = utm_point.easting;
    UTM[1] = utm_point.northing;
    UTM[2] = utm_point.altitude;
}

void WGS2UTM::LLA2UTM_origin_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM_origin)
{
    //经纬度转换为UTM投影
    Eigen::Vector3d UTM;
    LLA2UTM_1(LLA, UTM);

    //UTM2ENU, UTM投影平移至东北天坐标系, 东北天坐标系原点为第一个采样点
    UTM_origin = UTM - origin_UTM_;
}

  测试案例：

#include 
#include "convert_wgs2utm/wgs2utm.h"

using namespace std;

int main()
{
    WGS2UTM wgs2utm;

    Eigen::Vector3d origin_LLA;
    Eigen::Vector3d origin_UTM;
    origin_LLA[0] = 30.9543508;
    origin_LLA[1] = 121.4795984;
    origin_LLA[2] = 17.0970000;

    wgs2utm.set_origin_1(origin_LLA);
    origin_UTM = wgs2utm.get_origin_tum();
    cout << "origin_UTM: (" <<  origin_UTM[0] << ", " << origin_UTM[1] << ", " << origin_UTM[2] << ")" << endl;
    cout << "******************************" << endl;

    Eigen::Vector3d P_LLA;  // 任意一点的经纬度坐标
    Eigen::Vector3d P_UTM;  // 该点相对于所选取原点的utm坐标

    P_LLA[0] = 30.9543508;
    P_LLA[1] = 121.4755984;
    P_LLA[2] = 17.0970000;
    wgs2utm.LLA2UTM_origin_1(P_LLA, P_UTM);
    cout << "P_UTM: (" <<  P_UTM[0] << ", " << P_UTM[1] << ", " << P_UTM[2] << ")" << endl;
    cout << "******************************" << endl;

}

  输出结果如下：

2 利用GeographicLib功能包实现转化

  头文件：

#ifndef WGS2UTM_H
#define WGS2UTM_H

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

class WGS2UTM
{
    public:

    /**
     * @brief 构造函数
     */
    WGS2UTM();

    /**
     * @brief 获取选取原点的经纬度lla坐标
     * @return  所选取原点的经纬度lla坐标
     */
    inline Eigen::Vector3d get_origin_lla() 
    {
        return origin_LLA_;
    }

    /**
     * @brief 获取选取原点的经纬度utm坐标
     * @return  所选取原点的经纬度utm坐标
     */
    inline Eigen::Vector3d get_origin_tum() 
    {
        return origin_UTM_;
    }

    /***********************************************************/
    /***********************************************************/
    /**利用GeographicLib功能包实现WGS84经纬度坐标转化成UTM坐标**/
    /***********************************************************/
    /***********************************************************/
    
    /**
     * @brief 把选取的原点的经纬度lla坐标转化为东北天utm坐标
     * @param origin_LLA    输入--原点经纬度lla坐标
     */
    void set_origin_2(Eigen::Vector3d origin_LLA);

    /**
     * @brief 把经纬度lla坐标转化为东北天utm坐标
     * @param LLA    输入--lla坐标
     * @param UTM    输出--utm坐标
     */
    void LLA2UTM_2(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM);

    /**
     * @brief 把经纬度lla坐标转化为相对于所选取原点的东北天utm坐标
     * @param LLA    输入--lla坐标
     * @param UTM    输出--utm坐标
     */
    void LLA2UTM_origin_2(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM_origin);

private:

    Eigen::Vector3d origin_LLA_;
    Eigen::Vector3d origin_UTM_;

    double torad = M_PI / 180;
};
#endif //WGS2UTM_H

  源文件：

void WGS2UTM::set_origin_2(Eigen::Vector3d origin_LLA) 
{
    origin_LLA_ = origin_LLA;
    LLA2UTM_2(origin_LLA_, origin_UTM_);
}

void WGS2UTM::LLA2UTM_2(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM)
{
    GeographicLib::Math::real lat = LLA[0];
    GeographicLib::Math::real lon = LLA[1];
    GeographicLib::Math::real alt = LLA[2];
    int UTMZone;
    bool isNorth;   
    GeographicLib::UTMUPS::Forward(lat, lon, UTMZone, isNorth, UTM[0], UTM[1]);
    UTM[2] = LLA[2];
}

void WGS2UTM::LLA2UTM_origin_2(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM_origin)
{
    //经纬度转换为UTM投影
    Eigen::Vector3d UTM;
    LLA2UTM_2(LLA, UTM);

    //UTM2ENU, UTM投影平移至东北天坐标系, 东北天坐标系原点为第一个采样点
    UTM_origin = UTM - origin_UTM_;
}

  测试案例：

#include 
#include "convert_wgs2utm/wgs2utm.h"

using namespace std;

int main()
{
    WGS2UTM wgs2utm;

    Eigen::Vector3d origin_LLA;
    Eigen::Vector3d origin_UTM;
    origin_LLA[0] = 30.9543508;
    origin_LLA[1] = 121.4795984;
    origin_LLA[2] = 17.0970000;

    wgs2utm.set_origin_2(origin_LLA);
    origin_UTM = wgs2utm.get_origin_tum();
    cout << "origin_UTM: (" <<  origin_UTM[0] << ", " << origin_UTM[1] << ", " << origin_UTM[2] << ")" << endl;
    cout << "******************************" << endl;

    Eigen::Vector3d P_LLA;  // 任意一点的经纬度坐标
    Eigen::Vector3d P_UTM;  // 该点相对于所选取原点的utm坐标

    P_LLA[0] = 30.9543508;
    P_LLA[1] = 121.4755984;
    P_LLA[2] = 17.0970000;
    wgs2utm.LLA2UTM_origin_2(P_LLA, P_UTM);
    cout << "P_UTM: (" <<  P_UTM[0] << ", " << P_UTM[1] << ", " << P_UTM[2] << ")" << endl;
    cout << "******************************" << endl;

}