这段代码主要功能:
- 读取保存有pose的文件,文件格式为:timestamp tx ty tz qw qx qy qz;
- 第3个参数dataset_i表示测试的第i个数据集的轨迹,代码中,直接将第i个数据集的groundtruth写死了.
- PCL绘制轨迹,用红色表示第一个文件中的轨迹,绿色表示第二个文件中的轨迹.
**局部效果图: **
代码如下:
#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <string>
using namespace std;
int main( int argc, char** argv )
{
if(argc != 4){
cerr << "Error! ./joinMap poses1.txt poses2.txt index_of_dataset" << endl;
return 0;
}
string path1 = argv[1] , path2 = argv[2];
int dataset_i = atoi(argv[3]);
ifstream fin1, fin2;
fin1.open(path1.c_str());
fin2.open(path2.c_str());
if (!fin1 || !fin2)
{
cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
return 1;
}
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;