c++ pcl绘制SLAM轨迹(3D点)

 
这段代码主要功能:
  - 读取保存有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;