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

本文介绍了无人驾驶和机器人领域中常见的WGS84经纬度坐标与UTM坐标的转换,提供了使用geodesy功能包和GeographicLib库的详细实现。通过设置原点坐标,实现了从经纬度到UTM的转换,并给出了测试案例展示转换过程。
摘要由CSDN通过智能技术生成


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

1 利用geodesy功能包实现转化

  头文件:

#ifndef WGS2UTM_H
#define WGS2UTM_H

#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Geometry>
#include <GeographicLib/UTMUPS.hpp>
#include <geographic_msgs/GeoPointStamped.h>
#include <geodesy/utm.h>
#include <ros/ros.h>

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 <iostream>
#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 <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Geometry>
#include <GeographicLib/UTMUPS.hpp>
#include <geographic_msgs/GeoPointStamped.h>
#include <geodesy/utm.h>
#include <ros/ros.h>

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 <iostream>
#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;

}
  • 6
    点赞
  • 26
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Roar冷颜

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值