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

Pangolin是一个基于OpenGL的轻量级开源绘图库，在许多开源SLAM算法（例如ORB-SLAM）中都会用来进行可视化操作。遗憾的是，Pangolin除了doxygen外并没有详细的入门教程和手册，只在其 github homepage 上给出了几个简单的examples，网络上的相关文章也并没有给出系统的教程，对于新人来说想要上手比较困难。

因此本系列文章综合官方样例和网上的各类文章信息，逐个解析官方代码的功能及算法逻辑，进而给出一些在SLAM可视化中应用的简单例子，便于新人快速上手。

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

Task1：创建一个简单的Pangolin

就像每一个编程语言的教程中都会有的Hello World一样，在Pangolin的学习中，也让我们首先来看一个简单的例子，在这个例子中，我们会创建一个交互窗口，并在窗口中显示一个立方体和对应的坐标系，代码如下：

#include 

int main( int /*argc*/, char** /*argv*/ )
{
    // 创建名称为“Main”的GUI窗口，尺寸为640×640
    pangolin::CreateWindowAndBind("Main",640,480);
    // 启动深度测试
    glEnable(GL_DEPTH_TEST);

    // 创建一个观察相机视图
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(640,480,420,420,320,320,0.2,100),
        pangolin::ModelViewLookAt(2,0,2, 0,0,0, pangolin::AxisY)
    );

    // 创建交互视图
    pangolin::Handler3D handler(s_cam); //交互相机视图句柄
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
            .SetHandler(&handler);

    while( !pangolin::ShouldQuit() )
    {
        // 清空颜色和深度缓存
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        // 在原点绘制一个立方体
        pangolin::glDrawColouredCube();
        // 绘制坐标系
       	glLineWidth(3);
        glBegin ( GL_LINES );
	    glColor3f ( 0.8f,0.f,0.f );
	    glVertex3f( -1,-1,-1 );
	    glVertex3f( 0,-1,-1 );
	    glColor3f( 0.f,0.8f,0.f);
	    glVertex3f( -1,-1,-1 );
	    glVertex3f( -1,0,-1 );
	    glColor3f( 0.2f,0.2f,1.f);
	    glVertex3f( -1,-1,-1 );
	    glVertex3f( -1,-1,0 );
	    glEnd();

        // 运行帧循环以推进窗口事件
        pangolin::FinishFrame();
    }
    
    return 0;
}

代码解析

现在让我们逐步剖析上述代码：

#include 

首先我们引入pangolin的头文件，Pangolin几乎所有的功能都在该头文件中。

// 创建名称为“Main”的GUI窗口，尺寸为640×640
pangolin::CreateWindowAndBind("Main",640,480);
// 启动深度测试
glEnable(GL_DEPTH_TEST);

接下来，我们使用CreateWindowAndBind命令创建了一个视窗对象，函数的入口的参数依次为视窗的名称、宽度和高度，该命令类似于OpenCV中的namedWindow，即创建一个用于显示的窗体。同时，我们启动了深度测试功能，该功能会使得pangolin只会绘制朝向镜头的那一面像素点，避免容易混淆的透视关系出现，因此在任何3D可视化中都应该开启该功能。

// 创建一个观察相机视图
// ProjectMatrix(int h, int w, int fu, int fv, int cu, int cv, int znear, int zfar) 
//      参数依次为观察相机的图像高度、宽度、4个内参以及最近和最远视距
// ModelViewLookAt(double x, double y, double z,double lx, double ly, double lz, AxisDirection Up)
//      参数依次为相机所在的位置，以及相机所看的视点位置(一般会设置在原点)
pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(640,480,420,420,320,320,0.2,100),
    pangolin::ModelViewLookAt(2,0,2, 0,0,0, pangolin::AxisY)
);

在完成视窗的创建后，我们需要在视窗中“放置”一个摄像机（注意这里的摄像机是用于观察的摄像机而非SLAM中的相机），我们需要给出摄像机的内参矩阵ProjectionMatrix从而在我们对摄像机进行交互操作时，Pangolin会自动根据内参矩阵完成对应的透视变换。此外，我们还需要给出摄像机初始时刻所处的位置，摄像机的视点位置（即摄像机的光轴朝向哪一个点）以及摄像机的本身哪一轴朝上。

// 创建交互视图
pangolin::Handler3D handler(s_cam); //交互相机视图句柄
pangolin::View& d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
        .SetHandler(&handler);

接下来我们需要创建一个交互式视图（view）用于显示上一步摄像机所“拍摄”到的内容，这一步类似于OpenGL中的viewport处理。setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右），可以采用相对坐标（0~1）以及绝对坐标（使用Attach对象）。

 while( !pangolin::ShouldQuit() )
    {
        // 清空颜色和深度缓存
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        // 在原点绘制一个立方体
        pangolin::glDrawColouredCube();
        // 绘制坐标系
       	glLineWidth(3);
        glBegin ( GL_LINES );
	    glColor3f ( 0.8f,0.f,0.f );
	    glVertex3f( -1,-1,-1 );
	    glVertex3f( 0,-1,-1 );
	    glColor3f( 0.f,0.8f,0.f);
	    glVertex3f( -1,-1,-1 );
	    glVertex3f( -1,0,-1 );
	    glColor3f( 0.2f,0.2f,1.f);
	    glVertex3f( -1,-1,-1 );
	    glVertex3f( -1,-1,0 );
	    glEnd();

        // 运行帧循环以推进窗口事件
        pangolin::FinishFrame();
    }

在完成了上述所有准备工作之后，我们就可以开始绘制我们需要的图形了，首先我们使用glclear命令分别清空色彩缓存和深度缓存并激活之前设定好的视窗对象（否则视窗内会保留上一帧的图形，这种“多重曝光”效果通常并不是我们需要的）。接下来我们分别绘制了一个彩色立方体和一个坐标系，在绘制完成后，需要使用FinishFrame命令刷新视窗。

运行结果

Task2：Pangolin与多线程

在Task1中，我们在主线程中创建了视窗、视图并绘制了对应的图像，但在更多的应用中，出于效率考虑，可视化部分内容通常单独运行在一个线程中，现在让我们来将Task1中的代码改写为多线程版本：

#include 
#include 

static const std::string window_name = "HelloPangolinThreads";

void setup() {
    // create a window and bind its context to the main thread
    pangolin::CreateWindowAndBind(window_name, 640, 480);

    // enable depth
    glEnable(GL_DEPTH_TEST);

    // unset the current context from the main thread
    pangolin::GetBoundWindow()->RemoveCurrent();
}

void run() {
    // fetch the context and bind it to this thread
    pangolin::BindToContext(window_name);

    // we manually need to restore the properties of the context
    glEnable(GL_DEPTH_TEST);

    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,100),
        pangolin::ModelViewLookAt(-2,2,-2, 0,0,0, pangolin::AxisY)
    );

    // Create Interactive View in window
    pangolin::Handler3D handler(s_cam);
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
            .SetHandler(&handler);

    while( !pangolin::ShouldQuit() )
    {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        // Render OpenGL Cube
        pangolin::glDrawColouredCube();

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

    // unset the current context from the main thread
    pangolin::GetBoundWindow()->RemoveCurrent();
}

int main( int /*argc*/, char** /*argv*/ )
{
    // create window and context in the main thread
    setup();

    // use the context in a separate rendering thread
    std::thread render_loop;
    render_loop = std::thread(run);
    render_loop.join();

    return 0;
}

代码解析

void setup() {
    // create a window and bind its context to the main thread
    pangolin::CreateWindowAndBind(window_name, 640, 480);

    // enable depth
    glEnable(GL_DEPTH_TEST);

    // unset the current context from the main thread
    pangolin::GetBoundWindow()->RemoveCurrent();
}

在多线程版本的HelloPangolin中，我们首先利用setup()函数创建了一个视窗用于后续的显示，但这个视窗实在主线程中创建的，因此在主线程调用玩后，需要使用GetBoundWindow()->RemoveCurrent()将其解绑。

void run() {
    // fetch the context and bind it to this thread
    pangolin::BindToContext(window_name);

    // we manually need to restore the properties of the context
    glEnable(GL_DEPTH_TEST);
	...
	...
    // unset the current context from the main thread
    pangolin::GetBoundWindow()->RemoveCurrent();
}

随后，我们新开一个线程，运行我们的run()函数，在run()函数中，我们首先使用BindToContext()函数将之前解绑的视窗绑定到当前线程，随后需要重新设置视窗的属性（即启动深度测试）；同样，在线程结束时，我们需要解绑视窗。其余部分的代码则与task1完全一致。

至此，我们学习了如果使用Pangolin创建一个视窗以显示我们所期望的内容，并在独立的线程中运行绘图程序，但这离我们的SLAM的可视化还有不小的距离。在SLAM的可视化中，我们不光期望能定性的实时观察相机的轨迹，还希望能定量的观察并记录相机的实施位姿。因此在下一讲中，我们将学习如果使用Pangolin对视窗进行划分，并添加一些简单的显示和控件。