c++ 从txt读取数据gnss转化为enu数据

https://github.com/Dongvdong/gnss_ecef_enu_txt_yaml

测试文件

config.yaml

#==============#
# Camera Model #
#==============#

Camera.name: EH2022left monocular
Camera.setup: monocular
Camera.model: perspective
Camera.fx: 1220
Camera.fy: 1220
Camera.cx: 960
Camera.cy: 540
Camera.k1: -0
Camera.k2: 0
Camera.p1: 0
Camera.p2: 0
Camera.k3: -0
Camera.k4: 0
Camera.fps: 10
Camera.cols: 1920
Camera.rows: 1080
Camera.color_order: RGB
Initial.lat: 34.0342694493
Initial.lon: 108.755911045
Initial.alt: 587.800293
Feature.max_num_keypoints: 3000
Feature.scale_factor: 1.2
Feature.num_levels: 8
PangolinViewer.keyframe_size: 0.07
PangolinViewer.keyframe_line_width: 2
PangolinViewer.graph_line_width: 1
PangolinViewer.point_size: 2
PangolinViewer.camera_size: 0.08
PangolinViewer.camera_line_width: 3
PangolinViewer.viewpoint_x: 0
PangolinViewer.viewpoint_y: -0.65
PangolinViewer.viewpoint_z: -1.9
PangolinViewer.viewpoint_f: 400
Marker.Num: 0
vAcc: 1.0
hAcc: 1.0
Fixed.altitude_flag: 0
Fixed.altitude: 400.0 
Save.newmap: 1
Save.data: 1
op.is_Second_Optimize: 0
op.Second_Optimize_Th: 0  
op.Remove_Kayframe_Th: 6.0
op.Global_Optimize_Th: 1.0 
Loop_Th: 80.0 
Relocalize_Th: 80.0 
Relocalize_KF_Th: 3.0 
V_Instant_Th: 200.0
Tracking_CF_Th: 10.0

gps.txt

1453132356.600000 34.0342694493 108.755911045 587.800293
1453132357.960000 34.0344383177 108.755909682 587.673778
1453132358.520000 34.0345050891 108.755905295 587.568409
1453132359.160000 34.0345823584 108.755898876 587.587611
1453132359.560000 34.0346306268 108.755897244 587.612678
1453132360.200000 34.0347072511 108.755895617 587.583863
1453132360.840000 34.0347838932 108.755894644 587.438686
1453132361.400000 34.034850341 108.755894968 587.336082
1453132362.040000 34.0349259665 108.755895105 587.321752
1453132362.600000 34.0349939502 108.755895669 587.300844
1453132363.160000 34.0350612885 108.755896463 587.323285
1453132363.720000 34.0351295704 108.755895922 587.301819
1453132364.280000 34.0351972108 108.75589547 587.372351
1453132364.840000 34.0352654922 108.755893457 587.384214
1453132365.480000 34.0353438263 108.755889651 587.441598
1453132366.360000 34.0354513461 108.755882747 587.483984
1453132367.160000 34.0355485419 108.755871215 587.549042
1453132367.880000 34.0356326538 108.755860899 587.705846
1453132368.520000 34.0357092702 108.755849687 587.778205
1453132369.400000 34.0358125899 108.755839936 587.83467
1453132370.280000 34.0359174818 108.755836712 588.006536
1453132371.000000 34.0360013516 108.755839913 588.162609
1453132371.880000 34.0361035224 108.755849286 588.3736

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
 
project(ReadTextFile)
 
# 设置 C++ 标准为 C++11
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED True)


# 包含GeographicLib的头文件路径
include_directories("/usr/include/GeographicLib")


# 添加可执行文件，并链接主程序文件和自定义类的头文件
add_executable(node main.cpp)

set(YAML_CPP_LIBRARIES "/home/r9000k/v2_project/v3_SLAM/dll/GNSS/dll/libyaml-cpp.a")
target_link_libraries(node ${YAML_CPP_LIBRARIES} Geographic)

　　main.cpp

cmake_minimum_required(VERSION 3.10)
 
project(ReadTextFile)
 
# 设置 C++ 标准为 C++11
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED True)


# 包含GeographicLib的头文件路径
include_directories("/usr/include/GeographicLib")


# 添加可执行文件，并链接主程序文件和自定义类的头文件
add_executable(node main.cpp)

set(YAML_CPP_LIBRARIES "/home/r9000k/v2_project/v3_SLAM/dll/GNSS/dll/libyaml-cpp.a")
target_link_libraries(node ${YAML_CPP_LIBRARIES} Geographic)#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
 
#include<iomanip>


#include <yaml-cpp/yaml.h>
#include <GeographicLib/LocalCartesian.hpp>
 
using namespace std;
 
 
class gnss_data
{
 
public:
   // 原始数据
   double time;
   double lat;
   double lon;
   double high;
     
   // ENU坐标GNSS的原点
   double lat0;
   double lon0;
   double high0;
 
   // 在ENU坐标下
   double x;
   double y;
   double z;
 
   double ecef_x;
   double ecef_y;
   double ecef_z;

public:
   // 初始化 赋予原始数据
   gnss_data(double time_,double lat_,double lon_,double high_):time(time_), lat(lat_),lon(lon_),high(high_) {}
   bool Set_Gnss(double time_,double lat_,double lon_,double high_){
        time=time_;
        lat=lat_;
        lon=lon_;
        high=high_;
   };
 
   bool Set_orinGnss(double lat0_,double lon0_,double high0_){
     lat0=lat0_;
     lon0=lon0_;
     high0=high0_;
   }
 
   bool Set_ENU(double x_,double y_,double z_){
     x=x_;
     y=y_;
     z=z_;
   }
 
};
 
 
class GNSS_TextFileReader {
public:
    std::string filename;
    char delimiter;
    std::vector<std::vector<std::string>> data; // 二维向量，存储每一行拆分后的数据
    std::vector<gnss_data> gnss_List;
 
 
  
public:
    GNSS_TextFileReader(const std::string &filename, char delimiter)
        : filename(filename), delimiter(delimiter) {}
 
    bool readFile() {
        std::ifstream file(filename);
        if (!file.is_open()) {
            std::cerr << "Error opening file " << filename << std::endl;
            return false;
        }
 
        std::string line;
        while (std::getline(file, line)) {
            std::stringstream ss(line);
            std::string token;
            std::vector<std::string> row;
 
            while (std::getline(ss, token, delimiter)) { // delimiter 分割
                row.push_back(token);
            }
            data.push_back(row);//  保存string数据double
 
            gnss_data gnss_data_i( stod(row[0]),stod(row[1]),stod(row[2]),stod(row[3])); //保存double数据
            gnss_List.push_back(gnss_data_i);
        }
 
        file.close();
 
 
 
        return true;
    }
 
    void printData() {
        // for (const auto &row : data) {
        //     for (const auto &col : row) {
        //         std::cout << col << " ";
        //     }
        //     std::cout << std::endl;
        // }
 
       //for (const auto &gnss_List_i : gnss_List) { 
       for(int i=0;i<gnss_List.size();i++){
        gnss_data gnss_List_i=gnss_List[i];
        cout<< "编号 " << i << " 时间戳 "<< gnss_List_i.time<< " 纬度 " << gnss_List_i.lat <<  "  经度 " << gnss_List_i.lon<< "  高度 "<<  gnss_List_i.high << fixed << setprecision(10)<< endl;
       }
 
    }
 
    const std::vector<std::vector<std::string>>& getData() const {
        return data;
    }
 
    const std::vector<gnss_data>& get_gnss_List() const {
        return gnss_List;
    }
};


int Get_GNSS_INTI_YAML(std::string read_path,gnss_data &gnss_data_int0) {
    try {
        // 读取YAML文件
        YAML::Node config = YAML::LoadFile(read_path);
 
        // 访问YAML中的数据
        std::string lat0 = config["Initial.lat"].as<std::string>();
        std::string lon0 = config["Initial.lon"].as<std::string>();
        std::string alt0 = config["Initial.alt"].as<std::string>();
        double time0=0.0;
        
        gnss_data_int0.Set_Gnss(time0,stod(lat0),stod(lon0),stod(alt0));

        // // 打印读取的数据
        std::cout << "函数内部" << std::endl;
        //std::cout  << fixed << setprecision(10)<< endl;
        std::cout << "原点 纬度: " << lat0 << std::endl;
        std::cout << "原点 经度: " << lon0 << std::endl;
        std::cout << "原点 高度: " << alt0 << std::endl;
    } catch (const YAML::Exception& e) {
        std::cerr << "YAML Exception: " << e.what() << std::endl;
        return 1;
    }
 
    return 0;
}
 


 
int main() {
 
    //string path_="/home/r9000k/v2_project/data/NWPU/";
    string path_="/home/r9000k/v2_project/data/NWPU/";
    string path_config=path_+"FHY_config/FHY_config.yaml";// 初始点
    string path_GNSS=path_+"FHY_config/FHY_gps.txt";// 原始数据
    

    // 1 获取参考点
    gnss_data gnss_data_int0(-1,-1,-1,-1);
    Get_GNSS_INTI_YAML(path_config,gnss_data_int0);
    std::cout  << fixed << setprecision(10)<< endl;
    std::cout << "原点 纬度: " << gnss_data_int0.lat << endl;
    std::cout << "原点 经度: " << gnss_data_int0.lon << endl;
    std::cout << "原点 高度: " << gnss_data_int0.high << endl;
    
    
    // 2 当前点的经纬度和高度，作为局部坐标系的原点
    double origin_latitude =  gnss_data_int0.lat;   // 纬度
    double origin_longitude = gnss_data_int0.lon; // 经度
    double origin_height = gnss_data_int0.high;   // 高度
 
    // 转化为enu，并设置原点
    GeographicLib::LocalCartesian geoConverter;
    geoConverter.Reset(origin_latitude, origin_longitude, origin_height);

    // 转化为ecef，使用WGS84椭球模型
    GeographicLib::Geocentric wgs84(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());//  6378137  298.257223563LL
    //GeographicLib::Geocentric cgcs2000(6378137.0, 1.0 / 298.257222101);


    //3 获取数据点
    GNSS_TextFileReader reader(path_GNSS, ' '); // 读取路径 分隔符号
    
    std::vector<gnss_data> gnss_Lists ;
    if (reader.readFile()) {
        //reader.printData();
        gnss_Lists =  reader.get_gnss_List();
        for(int i=0;i<gnss_Lists.size();i++){
            
            gnss_Lists[i].Set_orinGnss(gnss_data_int0.lat,gnss_data_int0.lon,gnss_data_int0.high);// 设置原始GNSS点

            double target_latitude = gnss_Lists[i].lat;
            double target_longitude = gnss_Lists[i].lon;
            double target_height = gnss_Lists[i].high;


            // gnss转化为enu
            double x, y, z;
            geoConverter.Forward(target_latitude, target_longitude, target_height, x, y, z);

            gnss_Lists[i].x=x;
            gnss_Lists[i].y=y;
            gnss_Lists[i].z=z;


            //  WGS84 gnss转化为ecef
            wgs84.Forward(target_latitude, target_longitude, target_height, x, y, z);
            gnss_Lists[i].ecef_x=x;
            gnss_Lists[i].ecef_y=y;
            gnss_Lists[i].ecef_z=z;      

            gnss_data gnss_List_i=gnss_Lists[i];
            
            cout << fixed << setprecision(10)<< endl;
            cout<< "编号 " << i 
            << " 时间戳 "<< gnss_List_i.time
            << " \n纬度 " << gnss_List_i.lat 
            << " 经度 " << gnss_List_i.lon
            << " 高度 "<<  gnss_List_i.high 
            << " \nenu-x " << gnss_List_i.x 
            << " enu-y " << gnss_List_i.y
            << " enu-z "<<  gnss_List_i.z 
            << " \necef_x " << gnss_List_i.ecef_x
            << " ecef_y " << gnss_List_i.ecef_y
            << " ecef_z "<<  gnss_List_i.ecef_z
            << endl;
        }
    }
 
 
    return 0;
}