https://github.com/Dongvdong/gnss_ecef_enu_txt_yaml
1 从txt读取数据按行拆解
测试文件
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 ${CMAKE_SOURCE_DIR}/src/libyaml-cpp.a)
target_link_libraries(node ${YAML_CPP_LIBRARIES} Geographic)
main.cpp
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include<iomanip>
#include "src/API_GNSS_ENU_TXT_YAML.h"
using namespace std;
int main() {
//string path_="/home/r9000k/v2_project/data/NWPU/";
string path_="../config/";
string path_config=path_+"FHY_config.yaml";// 初始点
string path_GNSS=path_+"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;
}
API_GNSS_ENU_TXT_YAML.h
#ifndef API_GNSS_ENU_TXT_YAML
#define API_GNSS_ENU_TXT_YAML
#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_="../config/";
// string path_config=path_+"FHY_config.yaml";// 初始点
// string path_GNSS=path_+"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;
// }
#endif
