SLAM可视化绘图库——Pangolin教程（三）

在上一讲中，我们学习了如何使用Pangolin为视窗添加各种不同的控件以及对视窗进行分割以显示不同的图片，今天我们将继续学习如果使用pangolin绘制数据曲线以及带位姿变化的相机模型。

• 教程代码：https://github.com/yuntianli91/pangolin_tutorial
• ROS Pangolin手册：pangolin namespace

Task5：pangolin绘制数据曲线

首先我们来学习如何使用pangolin绘制函数曲线，在本例中，我们将在一个视图中分别绘制$\sin (x)$、$\cos (x)$以及$\sin (x)+\cos (x)$的曲线，代码如下：

#include <iostream>
#include <pangolin/pangolin.h>

int main(/*int argc, char* argv[]*/)
{
  // Create OpenGL window in single line
  pangolin::CreateWindowAndBind("Main",640,480);

  // Data logger object
  pangolin::DataLog log;

  // Optionally add named labels
  std::vector<std::string> labels;
  labels.push_back(std::string("sin(t)"));
  labels.push_back(std::string("cos(t)"));
  labels.push_back(std::string("sin(t)+cos(t)"));
  log.SetLabels(labels);

  const float tinc = 0.01f;

  // OpenGL 'view' of data. We might have many views of the same data.
  pangolin::Plotter plotter(&log,0.0f,4.0f*(float)M_PI/tinc,-4.0f,4.0f,(float)M_PI/(4.0f*tinc),0.5f);
  plotter.SetBounds(0.0, 1.0, 0.0, 1.0);
  plotter.Track("$i");//坐标轴自动滚动

  // Add some sample annotations to the plot（为区域着色）
  plotter.AddMarker(pangolin::Marker::Vertical,   50*M_PI, pangolin::Marker::LessThan, pangolin::Colour::Blue().WithAlpha(0.2f) );
  plotter.AddMarker(pangolin::Marker::Horizontal,   3, pangolin::Marker::GreaterThan, pangolin::Colour::Red().WithAlpha(0.2f) );
  plotter.AddMarker(pangolin::Marker::Horizontal,    3, pangolin::Marker::Equal, pangolin::Colour::Green().WithAlpha(0.2f) );

  pangolin::DisplayBase().AddDisplay(plotter);

  float t = 0;

  // Default hooks for exiting (Esc) and fullscreen (tab).
  while( !pangolin::ShouldQuit() )
  {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    log.Log(sin(t),cos(t),sin(t)+cos(t));
    t += tinc;

    // Render graph, Swap frames and Process Events
    pangolin::FinishFrame();
  }

  return 0;
}

程序运行结果如下：

代码解析

 // Data logger object
  pangolin::DataLog log;

  // Optionally add named labels
  std::vector<std::string> labels;
  labels.push_back(std::string("sin(t)"));
  labels.push_back(std::string("cos(t)"));
  labels.push_back(std::string("sin(t)+cos(t)"));
  log.SetLabels(labels);

在pangolin中，待可视化的数据全部存储在pangolin::DataLog对象中，因此我们首先创建了一个pangolin::DataLog对象，并使用对应的成员函数SetLabels()设置对应数据的名称（即图例）。

 // OpenGL 'view' of data. We might have many views of the same data.
  pangolin::Plotter plotter(&log,0.0f,4.0f*(float)M_PI/tinc,-4.0f,4.0f,(float)M_PI/(4.0f*tinc),0.5f);
  plotter.SetBounds(0.0, 1.0, 0.0, 1.0);
  plotter.Track("$i");//坐标轴自动滚动

而数据的可视化则是通过pangolin::Plotter对象来实现的，该对象的构造参数的第一个参数为需要绘制的pangolin::DataLog对象；随后4个参数依次Plotter的左边界、右边界、下边界、上边界，即Plotter中$x$轴$y$轴的范围；最后两个参数依次为$x$轴和$y$轴的坐标轴刻度大小。

  // Add some sample annotations to the plot（为区域着色）
  plotter.AddMarker(pangolin::Marker::Vertical,   50*M_PI, pangolin::Marker::LessThan, pangolin::Colour::Blue().WithAlpha(0.2f) );
  plotter.AddMarker(pangolin::Marker::Horizontal,   3, pangolin::Marker::GreaterThan, pangolin::Colour::Red().WithAlpha(0.2f) );
  plotter.AddMarker(pangolin::Marker::Horizontal,    3, pangolin::Marker::Equal, pangolin::Colour::Green().WithAlpha(0.2f) );

随后我们演示了在Plotter中使用plotter的成员函数AddMarker添加一些标志块的功能，该函数入口参数依次为标志块的方向，标志块的数值，标志块的判别方式以及标志块的颜色。例如第一个标志块的方向为垂直方向，数值为$50\pi$，判断方式为小于，颜色为带透明度的蓝色，因此我们在程序的运行结果中会发现$x$轴坐标小于$50\pi$的范围都被标记为了透明的蓝色。同理第二个Marker将$y$轴大于3的区域标记为了红色，第三个Marker由于是等于，因此其只将$y=3$这一条线标记为了绿色。

  pangolin::DisplayBase().AddDisplay(plotter);

随后，我们将构建好的plotter添加到Display中。

while( !pangolin::ShouldQuit() )
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    log.Log(sin(t),cos(t),sin(t)+cos(t));
    t += tinc;

    // Render graph, Swap frames and Process Events
    pangolin::FinishFrame();
}

在帧循环中，我们只需要使用DataLog::Log()函数不断更新DataLog中的数据，pangolin就会根据我们之前创建的plotter自动在视窗中绘制数据。

Task6：pangolin绘制相机位姿

SLAM可视化中最重要的一个内容就是实时显示相机的轨迹和当前的位姿，本次任务的代码较多，这里只贴一些关键部分的代码，完整的代码参见github。

首先让我们来绘制一个简单的相机模型，这里我们借鉴ORB-SLAM的做法，通过一些简单的线条来表示相机模型：

	const float w = 0.2;
	const float h = w * 0.75;
	const float z = w * 0.6;

	glLineWidth(2); 
	glBegin(GL_LINES);
	glColor3f(0.0f,1.0f,1.0f);
	glVertex3f(0,0,0);		glVertex3f(w,h,z);
	glVertex3f(0,0,0);		glVertex3f(w,-h,z);
	glVertex3f(0,0,0);		glVertex3f(-w,-h,z);
	glVertex3f(0,0,0);		glVertex3f(-w,h,z);
	glVertex3f(w,h,z);		glVertex3f(w,-h,z);
	glVertex3f(-w,h,z);		glVertex3f(-w,-h,z);
	glVertex3f(-w,h,z);		glVertex3f(w,h,z);
	glVertex3f(-w,-h,z);    glVertex3f(w,-h,z);
	glEnd();

上述代码使用8条线段绘制了一个位于原点位置的相机轮廓，结果如下：
下面我们让这个相机模型动起来，显然最简单的想法是在每次获取相机的位姿后，对上述八点线段的坐标进行相应的变换，进而绘制出当前时刻的相机模型。但如果每次都需要我们去计算变换后的位姿，这无疑是非常麻烦且容易出错的。幸运的是，OpenGL提供了glMultMatrix()函数自动帮我们处理图像点的位姿转换，代码如下：

	glPushMatrix();
	std::vector<GLdouble> Twc = {R(0, 0), R(1,0), R(2, 0), 0.,
                            R(0, 1), R(1, 1), R(2, 1), 0.,
                            R(0, 2), R(1, 2), R(2, 2), 0.,
                            pos.x(), pos.y(), pos.z(), 1.};
    glMultMatrixd(Twc.data());
    // 绘制相机轮廓线
    const float w = 0.2;
    const float h = w * 0.75;
    const float z = w * 0.6;

    glLineWidth(2); 
    glBegin(GL_LINES);
    glColor3f(0.0f,1.0f,1.0f);
	glVertex3f(0,0,0);		glVertex3f(w,h,z);
	glVertex3f(0,0,0);		glVertex3f(w,-h,z);
	glVertex3f(0,0,0);		glVertex3f(-w,-h,z);
	glVertex3f(0,0,0);		glVertex3f(-w,h,z);
	glVertex3f(w,h,z);		glVertex3f(w,-h,z);
	glVertex3f(-w,h,z);		glVertex3f(-w,-h,z);
	glVertex3f(-w,h,z);		glVertex3f(w,h,z);
	glVertex3f(-w,-h,z);    glVertex3f(w,-h,z);
	glEnd();
    glPopMatrix();

首先我们需要使用glPushMatrix()告诉pangolin我们需要使用一个矩阵；随后我们使用glMultMatrixd()告诉pangolin后续绘制中的所有坐标均需要乘以这个矩阵；最后再glPopMatrix()弹出矩阵，便于下一次循环填入新的矩阵数值。需要注意的是，不同于Eigen等矩阵库，pangolin里的矩阵是按照列主序存储的。

 // -------- 绘制相机轨迹 --------//
	glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(0.f, 1.f, 0.f);
    for(size_t i=0; i<traj.size() - 1; i++){
        glVertex3d(traj[i].x(), traj[i].y(), traj[i].z());
        glVertex3d(traj[i+1].x(), traj[i+1].y(), traj[i+1].z());
    }
    glEnd();

最后，我们将所有的相机位置存储起来，并依次画线，即可得到相机的轨迹，程序最终的运行效果如下：

Task 7：使用pangolin完成简单的SLAM可视化

至此，我们基本学完了pangolin在SLAM可视化应用中的基本操作，最后一讲我们将进行一个简单的实践，使用pangolin编写一个简单的程序，对EuRoC数据集进行可视化。代码参见github主页，关键部分代码在之前的task1～task6中都有过讲解，这里不再赘述，程序运行结果如下：