参考文章1
参考文章2

源码+逐行注释

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

//trajectory文件的路径（可以使用相对路径，也可以使用绝对路径）
string trajectory_file = "../trajectory.txt";  

//画轨迹函数的声明
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {

    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;//容器的方式存储
    //因为类型是Eigen的Isometry3d和cpp的类内存分配不一样，所以要指定内存的分配方式
    //即:Eigen::aligned_allocator< Isometry3d > 
    
    ifstream fin(trajectory_file); //读文件
    //未成功读取
    if (!fin) {  
        cout << "cannot find trajectory file at " << trajectory_file << endl;
        return 1;
    }
    //一行一行读取，直到文件尾标志（efo标志）
    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        
        Isometry3d Twr(Quaterniond(qw, qx, qy, qz)); //四元数转换为旋转矩阵
        Twr.pretranslate(Vector3d(tx, ty, tz));//旋转矩阵加上平移变为转换矩阵
        poses.push_back(Twr);//添加到容器中
    }
    cout << "read total " << poses.size() << " pose entries" << endl;

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);//新建窗口，参数分别为：窗口名称、窗口的长和宽
    glEnable(GL_DEPTH_TEST);//启用深度渲染，当需要显示3D模型时需要打开，根据目标的远近自动隐藏被遮挡的模型
    glEnable(GL_BLEND);//表示窗口使用颜色混合模式，让物体显示半透明效
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	/***
	设置颜色RGBA混合因子,前面参数表示源因子，后面参数表示目标因子
	GL_ZERO：表示使用0.0作为权重因子
	GL_ONE ： 表示使用1.0作为权重因子
	GL_SRC_ALPHA ：表示使用源颜色的alpha值来作为权重因子
	GL_DST_ALPHA ：表示使用目标颜色的alpha值来作为权重因子
	GL_ONE_MINUS_SRC_ALPHA： 表示用1.0-源颜色的alpha的值来作为权重因子
	GL_ONE_MINUS_DST_ALPHA： 表示用1.0-目标颜色的alpha的值来作为权重因子
	GL_SRC_COLOR>：表示用源颜色的四个分量作为权重因子
	在画图的过程中如果程序glClearColor()；glColor3f()则后者为源颜色，前者的颜色为目标颜色以上的GL_SRC_ALPHA和GL_ONE_MINUS_SRC_ALPHA是最常用的混合模式 ***/

	//创建相机视图
    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

	//ProjectionMatrix()设置相机内参,参数分别表示相机分辨率(2)，焦距(1)，相机光心(3)，最远最大距离(2)
	//ModelViewLookAt()设置观看视角,上文对应的意思是在世界坐标(0，-0.1，-1.8)处观看坐标原点（0,0,0）并设置Y轴向上
	/**  另一种解释
	 定义观测方位向量：观测点位置：(mViewpointX mViewpointY mViewpointZ)
                     观测目标位置：(0, 0, 0)
		             观测的方位向量：(0.0,-1.0, 0.0)**/

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));
	/**创建交互视图view,用于显示上一步摄像机所“拍摄”到的内容，,setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右）,显示界面长宽比*/

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空颜色和深度缓存
        d_cam.Activate(s_cam);//激活之前设定好的视窗对象
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);//为颜色缓存区指定确定的值
        glLineWidth(2);
        for (size_t i = 0; i < poses.size(); i++) {
            // 画每个位姿的三个坐标轴
            Vector3d Ow = poses[i].translation();//无参数，返回当前变换平移部分的向量表示(可修改)，可以索引[]获取各分量
            //对三个坐标轴分别旋转
            Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));//0.1是为了让图看起来舒服，不会太大
            Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
            Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
            glBegin(GL_LINES);//开始画线
            glColor3f(1.0, 0.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Xw[0], Xw[1], Xw[2]);
            glColor3f(0.0, 1.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Yw[0], Yw[1], Yw[2]);
            glColor3f(0.0, 0.0, 1.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Zw[0], Zw[1], Zw[2]);
            glEnd();//结束画线
        }
        // 画出连线
        for (size_t i = 0; i < poses.size(); i++) {
            glColor3f(0.0, 0.0, 0.0);
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}

运行效果截图

欢迎SLAMers加QQ：258404070 一起学习SLAM
有问题请留言