SLAM可视化绘图库——Pangolin教程(一)

Pangolin教程(一)

  • Task1:创建一个简单的Pangolin
    • 代码解析
    • 运行结果
  • Task2: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命令刷新视窗。

运行结果

SLAM可视化绘图库——Pangolin教程(一)_第1张图片

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对视窗进行划分,并添加一些简单的显示和控件。

你可能感兴趣的:(SLAM开源工具包)