读取如下内容：
[INFO] [2023927 13:23:04.303] [estimator.cpp:1894]: XYZI: 1.70492 -1.33857 1.99379 134
[INFO] [2023927 13:23:04.303] [estimator.cpp:1894]: XYZI: 1.70944 -1.32323 1.99325 125
[INFO] [2023927 13:23:04.303] [estimator.cpp:1892]: 1695792183.950074.pcd
[INFO] [2023927 13:23:04.303] [estimator.cpp:1894]: XYZI: 1.69904 -1.3083 1.99324 142
[INFO] [2023927 13:23:04.303] [estimator.cpp:1894]: XYZI: 1.70866 -1.29287 1.99253 140
[INFO] [2023927 13:23:04.304] [estimator.cpp:1894]: XYZI: 1.71834 -1.2775 1.99181 138
[INFO] [2023927 13:23:04.304] [estimator.cpp:388]: Trailer angle estimator init success
[INFO] [2023927 13:23:04.304] [estimator.cpp:312]: last_angle: 1695792184.150050 0.000625
[INFO] [2023927 13:23:04.353] [estimator.cpp:1941]: vehicle_pose: 1695792184.158000 -1456.254047 34.195933 93.047454
[INFO] [2023927 13:23:04.353] [estimator.cpp:1969]: trailer_pose: 1695792184.150050 34.231745
[INFO] [2023927 13:23:04.408] [estimator.cpp:475]: filter_angle: 1695792184.250059 0.000680
[INFO] [2023927 13:23:04.408] [estimator.cpp:490]: predict_angle: 1695792184.250059 0.000289
[INFO] [2023927 13:23:04.408] [estimator.cpp:352]: trailer_angle: 910 0.016561
解析出角度值 “filter_angle”， “predict_angle”，“trailer_angle”，并将结果保存成txt文档。
解析出XYZI: 1.70866 -1.29287 1.99253 140，并将结果按照解析出的“1695792183.950074.pcd ”来保存。
#include <dirent.h>
#include <fstream>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <sstream> //istringstream 必须包含这个头文件
#include <stdlib.h>
#include <string>
#include <vector>
using namespace std;
struct angle_struct {
double timestamp;
double angle;
};
struct point_struct {
std::string pcd_name;
double x;
double y;
double z;
double i;
};
void Stringsplit(const std::string str, const char split,
std::vector<std::string> &res) {
std::istringstream iss(str);
std::string token;
while (getline(iss, token, split)) {
res.push_back(token);
}
}
// 1695792261.850105.pcd
void test(std::string &input_path) {
ifstream ifs(input_path); //读取文件
if (!ifs.good()) {
cerr << "ifstream open file error!\n";
return;
}
std::vector<std::vector<point_struct>> pcds;
string line;
// vector<string> lines;
std::string get_pcd_name;
point_struct point0;
std::vector<point_struct> points_init;
std::vector<angle_struct> trailer_angle;
std::vector<angle_struct> predict_angle;
std::vector<angle_struct> filter_angle;
int count = 0;
while (getline(ifs, line)) //获取每一行数据
{
//按照空格分
// lines.push_back(line); //将每一行依次存入到vector中
// cout << line << endl; //顺便打印一下这一行
std::vector<std::string> str_list;
point_struct point;
std::vector<point_struct> points;
//----
angle_struct angle_value;
//按照空格将每一行内容划分为不同字符串，将每个字符串保存到vector里，此时每个vcetor保存这一行的内容
Stringsplit(line, ' ', str_list);
if (str_list.size() > 5) {
string str_list_4 = str_list[4];
if (str_list_4 == "XYZI:") {
// std::cout << "XYZI: " << str_list[5] << " " << str_list[6] << " "
// << str_list[7] << " " << str_list[8] << std::endl;
point.x = stod(str_list[5]);
point.y = stod(str_list[6]);
point.z = stod(str_list[7]);
point.i = stod(str_list[8]);
point.pcd_name = get_pcd_name;
pcds.back().push_back(point);
}
}
if (str_list.size() == 5) {
get_pcd_name = str_list[4];
pcds.push_back(points);
count++;
}
if (str_list.size() == 7) {
string ang_str = str_list[4];
if (ang_str == "trailer_angle:") {
angle_value.timestamp = stod(str_list[5]);
angle_value.angle = stod(str_list[6]);
trailer_angle.push_back(angle_value);
}
if (ang_str == "predict_angle:") {
angle_value.timestamp = stod(str_list[5]);
angle_value.angle = stod(str_list[6]);
predict_angle.push_back(angle_value);
}
if (ang_str == "filter_angle:") {
angle_value.timestamp = stod(str_list[5]);
angle_value.angle = stod(str_list[6]);
filter_angle.push_back(angle_value);
}
}
else {
continue;
}
}
ifs.close();
std::cout << "pcds: " << pcds.size() << std::endl;
std::cout << "count: " << count << std::endl;
for (int i = 0; i < pcds.size(); i++) {
pcl::PointCloud<pcl::PointXYZINormal>::Ptr single_cloud(
new pcl::PointCloud<pcl::PointXYZINormal>);
std::string _name;
//保存成每一帧点云
single_cloud->width = 1;
single_cloud->height = pcds[i].size();
for (int j = 0; j < pcds[i].size(); j++) {
pcl::PointXYZINormal Point;
_name = pcds[i][j].pcd_name;
Point.x = pcds[i][j].x;
Point.y = pcds[i][j].y;
Point.z = pcds[i][j].z;
Point.intensity = pcds[i][j].i;
single_cloud->points.push_back(Point);
std::cout << "XYZI: " << pcds[i][j].pcd_name << " " << pcds[i][j].x << " "
<< pcds[i][j].y << " " << pcds[i][j].z << " " << pcds[i][j].i
<< std::endl;
}
//保存单帧pcd
std::string path_save = "../JTG/pcds/" + _name;
std::cout << "path_save : " << path_save << std::endl;
if (single_cloud->points.size() > 0) {
pcl::io::savePCDFileBinaryCompressed(path_save,
*single_cloud); //压缩保存
}
}
ofstream ofs1("../JTG/trailer_angle.txt"); //把内容输出到a.txt文件中
if (!ofs1.good()) {
cerr << "ofstream open file error!\n";
return;
}
for (auto &data : trailer_angle) {
ofs1 << std::fixed << data.timestamp << " " << data.angle << '\n';
}
ofs1.close();
//---------
ofstream ofs2("../JTG/predict_angle.txt"); //把内容输出到a.txt文件中
if (!ofs2.good()) {
cerr << "ofstream open file error!\n";
return;
}
for (auto &data : predict_angle) {
ofs2 << std::fixed << data.timestamp << " " << data.angle << '\n';
}
ofs2.close();
//------
ofstream ofs3("../JTG/filter_angle.txt"); //把内容输出到a.txt文件中
if (!ofs3.good()) {
cerr << "ofstream open file error!\n";
return;
}
for (auto &data : filter_angle) {
ofs3 << std::fixed << data.timestamp << " " << data.angle << '\n';
}
ofs3.close();
//-------
}
int test2() {
ofstream ofs("china_daily.txt", std::ios::app); //在文档中添加字符串
if (!ofs) {
cerr << "ofstream error!" << endl;
return -1;
}
cout << ofs.tellp() << endl; //输出整个文件的长度
ofs << "this new line" << endl; //在文件末尾追加“this new line”
ofs.close();
return 0;
}
int main(void) {
std::string input_path =
"../JTG/uto_trailer_pose_estimator_185511_1695792081229.txt";
test(input_path);
// test2();
return 0;
}
Cmakelist文件如下：
cmake_minimum_required(VERSION 2.6)
project(main)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(PCL 1.8 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable(main main.cpp)
target_link_libraries (main ${
PCL_LIBRARIES})