【SLAM】pangolin的学习

Task1 创建一个简单的pangolin界面

// https://blog.csdn.net/weixin_43991178/article/details/105119610

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

#include <pangolin/pangolin.h> // 引入头文件，Pangolin几乎所有的功能都在该头文件
#include <unistd.h> // 延时函数的头文件 usleep()

int main(int argc, char *argv[])
{
    // 创建名称为“Main”的GUI窗口，尺寸为 640*480
    // 通过 CreateWindowAndBind 命令创建一个视图对象，函数的入口的参数依次为视图的名称、宽度、高度
    // 该命令类似于opencv的namewindow，即创建一个用于显示的窗体
    pangolin::CreateWindowAndBind("Main", 640, 480);
    // 启动深度测试功能
    // 该功能会使得pangolin只会绘制朝向镜头的那一面像素点，避免容易混淆的透视关系出现，
    // 因此在任何3D透视可视化中都应该开启该功能
    glEnable(GL_DEPTH_TEST);

    // 创建一个观察相机视图
    // 放置一个观察的假想相机（也就是我们从这个相机可以看到的画面），
    // ProjectionMatrix表示假想相机的内参，在我们对视图进行交互操作时，Pangolin会自动根据内参矩阵完成对应的透视变换
    // 另外还需要给出相机初始时刻所处的位置，以及相机视点的位置（相机的光轴朝向哪一个点）以及相机的本身哪一轴朝上
    // 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, -2, 0, 0, 0, -2, pangolin::AxisZ));

    // 创建交互视图
    // 用于显示上一步相机所“拍摄”到的内容，setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右）
    // 可以采用相对坐标 以及 绝对坐标（使用 Attach 对象）
    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);
        // X 轴 red
        glColor3f(0.8f, 0.0f, 0.0f); // 颜色，红色
        glVertex3f(-1, -1, -1);      // 原点 那个顶点
        glVertex3f(0, -1, -1);       // 往x方向伸出 一个单位的顶点
        // Y 轴 green
        glColor3f(0.0f, 0.8f, 0.0f);
        glVertex3f(-1, -1, -1);
        glVertex3f(-1, 0, -1);
        // Z 轴 blue
        glColor3f(0.0f, 0.0f, 0.8f);
        glVertex3f(-1, -1, -1);
        glVertex3f(-1, -1, 0);
        glEnd();

        // 运行帧循环以推进窗口事件
        // 使用 FinishFrame命令刷新视窗
        pangolin::FinishFrame();

        // 稍微加个延时
        usleep(5000); // sleep 5ms
    }

    return 0;
}

Task2 pangolin多线程

// https://blog.csdn.net/weixin_43991178/article/details/105119610
// Task2 pangolin与多线程

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

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


// 在多线程版本的pangolin中，首先利用setup() 函数创建一个视窗用于后续的显示，
// 但这个视窗是在主线程中创建的，因此在主线程调用后，需要使用GetBoundWindow()->RemoveCurrent()将其解绑
void setup()
{
    // create a window and bind its context to the main thread
    pangolin::CreateWindowAndBind(window_name, 640, 480);

    // enabel depth
    glEnable(GL_DEPTH_TEST);

    // unset the current context from the main thread
    // 从主线程取消设置当前上下文
    // GetBoundWindow()  返回指向当前pangolin window 上下文的指针，如果没有绑定则返回nullptr
    pangolin::GetBoundWindow()->RemoveCurrent();
}

// 新开一个线程，运行run()函数，在run函数中首先将之前解绑的视窗绑定到当前线程，
// 随后需要重新设置视窗的属性(启动深度测试),同样,在线程结束时,需要解绑视窗
void run()
{
    // 获取上下文并将它绑定到这个线程
    pangolin::BindToContext(window_name);

    // 我们需要手动恢复上下文的属性
    // 启动深度测试
    glEnable(GL_DEPTH_TEST);

    // 定义投影和初始模型视图矩阵
    // 创建观察相机
    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));

    // 创建交互视图
    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 info
        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 coontext in a separate rendering thread
    std::thread render_loop;
    render_loop = std::thread(run);
    render_loop.join();


    return 0;
}

Task3 pangolin添加控件

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

void SampleMethod()
{
    std::cout << "You typed ctrl-r or pushed reset " << std::endl;
}

int main(int argc, char *argv[])
{
    std::cout << " ======== Task3 ========" << std::endl;

    // 创建视窗
    pangolin::CreateWindowAndBind("Main", 640, 480);
    // 启动深度测试
    glEnable(GL_DEPTH_TEST);
    // 创建一个相机
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000),
        pangolin::ModelViewLookAt(-0, 0.5, -3, 0, 0, 0, pangolin::AxisY));

    // 分割视窗
    // 这边的150也就是宽度的意思，和上面创建界面 宽度为 640是一个概念
    const int UI_WIDTH = 150;

    // 右侧用于显示视窗
    // 这边的 pangolin::Attach::Pix(UI_WIDTH) 也就是计算UI_WIDTH占总宽度的多少
    // 因为从 task1 知道 SetBounds()函数前四个入口参数是 下 上 左 右，也就是从下到上占多少，从左到右占多少，这个范围是[0,1]
    // 所以这边通过 pangolin::Attach::Pix(UI_WIDTH) 计算一下实际的比例是多少
    pangolin::View &d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -640.0f / 480.0f)
                                .SetHandler(new pangolin::Handler3D(s_cam));

    // 左侧用于创建控制面板
    // 在坐标180像素宽度位置使用CreatePanel()命令创建一个面板,并给这个面板命名为"ui",这里"ui"是面板的tag名称
    // 后续所有控制的操作都通过这个tag绑定到对应的面板上
    // 视窗的其余部分则为用于显示 viewport
    pangolin::CreatePanel("ui")
        .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

    // 创建控制面板的控件对象，pangolin
    // 创建一系列控件
    // 将所有"控件"视为一个 pangolin::var 对象,该对象是一个模板类,我们可以向其中传递自定义的类型模板
    // 常用的 pangolin::Var 对象整理如下:
    /*
        pangolin::Var<bool> : bool型 Var对象,创建参数依次为控件的tag(名字),初始值,以及是否可以toggle.
        - 当 toggle 设置为 true 时,该对象表示一个选框(Checkbox);设置为 false 时则表示一个按钮(Button)
        - 初始值对于设置为 true 或 false 会影响选框是否被选中,对于按钮来说没有影响,但是习惯性一般都会设置为false,
        - 控件的tag是唯一的,命名格式为 panel_tag.controller_tag, 
          例如,我们所有控件需要板顶的面板为"ui",因此所有的控件tag都命名为 ui.xxx 的形式


        pangolin::Var<int/double/float> : 这一类 Var 对象为常见的滑条对象,创建参数依次为 tag, 初始值, 最小值, 最大值 和 logsacle
        - logsacle 表示是否以对数坐标形式显示,
        - 最大最小值控制滑动条的范围,如果不设置的话默认最小值为0,最大值为1
        - 初始值是滑条上初始显示的数字,因此其不需要在滑条的范围内,只不过在用户移动滑条后,显示的数字会更新为滑条当前位置对应的数字


        pangolin::Var<std::function<void()>> 这一类控件同样实现按钮控件的功能,只是其在创建时传入一个std::function函数对象,
        因此不需要在后续的循环中进行回调函数的书写.
        不过如果回调函数中如果需要进行参数的传入和传出,使用std::function会比较麻烦,因此其常用来编写一些void(void)类型的简单功能函数,即没有输入输出的函数.
    

        上面所有控件的必要参数只有控件tag和初始值,其他参数不存在时 pangolin 会自动调用默认参数进行处理.
    */

    pangolin::Var<bool> A_Button("ui.a_button", false, false);    // 按钮
    pangolin::Var<bool> A_Checkbox("ui.a_checkbox", false, true); // 选框
    pangolin::Var<double> Double_Slider("ui.a_slider", 3, 0, 5);  // double滑条
    pangolin::Var<int> Int_Slider("ui.b_slider", 2, 0, 5);        // int 滑条
    pangolin::Var<std::string> A_string("ui.a_string", "hello pangolin");

    pangolin::Var<bool> SAVE_IMG("ui.save_img", false, false);     // 按钮
    pangolin::Var<bool> SAVE_WIN("ui.save_win", false, false);     // 按钮
    pangolin::Var<bool> RECORD_WIN("ui.record_win", false, false); // 按钮

    pangolin::Var<std::function<void()>> reset("ui.Reset", SampleMethod); // 通过案件调用函数

    // 绑定键盘快捷键
    // 演示我们如何使用一个键盘快捷方式来改变一个var
    // 这条函数的意思是通过 ctrl+b 将 a_slider 的滑动条的值变成 3.5
    pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'b', pangolin::SetVarFunctor<double>("ui.a_slider", 3.5));

    // 使用键盘快捷方式来调用函数
    pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'r', SampleMethod);

    // 几个默认的快捷方式是：esc 表示退出，tab 表示全屏

    while (!pangolin::ShouldQuit())
    {

        // clear entire screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // 各控件的回调函数
        // button对象则需要使用  pangolin::Pushed(string tag) 函数判断其是否按下
        if (pangolin::Pushed(A_Button))
            std::cout << "Push button A." << std::endl;

        // checkbox 判断其本身的状态为 true 和 false
        if (A_Checkbox)
            Int_Slider = Double_Slider;

        // 保存整个win
        if (pangolin::Pushed(SAVE_WIN))
            pangolin::SaveWindowOnRender("window");

        // 保存view
        if (pangolin::Pushed(SAVE_IMG))
            d_cam.SaveOnRender("cube");

        // 录像
        // if( pangolin::Pushed(RECORD_WIN))
        // pangolin::DisplayBase().RecordOnRender("ffmpeg:[fps=50,bps=8388608,unique_filename]//screencap.avi");

        d_cam.Activate(s_cam);

        pangolin::glDrawColouredCube();

        pangolin::FinishFrame();
    }

    return 0;
}

Task 4：多视图与图片显示

#include <opencv2/opencv.hpp>
#include <pangolin/pangolin.h>
#include <iostream>

// 多视图图片显示
// pangolin中提供了SimpleMultiDisplay 例子用于演示多视图分割
// 我们首先创建在视窗中创建了三个视图，其中一个是我们很熟悉的相机视图，
// 在本例中我们特意让相机视图充满了整个视窗，以演示我们前面说明的这里的多视图其实是通过视图“叠加”实现的。
// 紧接着我们创建了另外两个视图用于显示图片，其中一个视图位于左上角，一个视图位于右下角

int main(int argc, char *argv[])
{
    std::cout << "OpenCV Version" << CV_VERSION << std::endl;

    // 创建视窗
    pangolin::CreateWindowAndBind("MultiImage", 640, 480);

    // 启动深度测试
    glEnable(GL_DEPTH_TEST);

    // 设置摄像机
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(640, 480, 420, 420, 320, 320, 0.1, 1000),
        pangolin::ModelViewLookAt(-2, 0, -2, 0, 0, 0, pangolin::AxisY));

    // --------------- 创建三个视图 ---------------
    // SetHandler 是设置交互视图用的，是设置视图句柄
    pangolin::View &d_cam = pangolin::Display("cam")
                                .SetBounds(0.0, 1.0, 0.0, 1.0, -752.0 / 480.0)
                                .SetHandler(new pangolin::Handler3D(s_cam));

    // 第五个参数，创建图片的是正值，创建三维图的是负值，这个参数实际上表征的是视图的 分辨率
    // 当该参数取正值时，pangolin会将由前四个参数设置的视图大小进行裁减，以满足所设置的分辨率
    // 当该参数取负值时，pangolin会将图片拉伸以充满由前四个参数设置的视图范围
    // 使用SetLock()函数设置了视图锁定的位置，该函数会在我们缩放整个视窗后，按照设定的lock选项自动锁定对齐位置
    // 将左上角的视图设置为left和top，右下角的视图设置为right和buttom锁定
    pangolin::View &cv_img_1 = pangolin::Display("image_1")
                                   .SetBounds(2.0 / 3.0f, 1.0f, 0.0f, 1 / 3.0f, 752.0 / 480.0f)
                                   .SetLock(pangolin::LockLeft, pangolin::LockTop);

    pangolin::View &cv_img_2 = pangolin::Display("image_2")
                                   .SetBounds(0.f, 1 / 3.f, 2 / 3.f, 1.f, 752 / 480.f)
                                   .SetLock(pangolin::LockRight, pangolin::LockBottom);

    // 创建glTexture容器用于读取图像
    // 需要创建两个图像纹理容器 pangolin::GlTexture 用于向上面创建的视图装载图像
    // 入口参数依次为：图像宽度，图像高度，pangolin的内部图像存储格式，是否开启现行采样，边界大小（像素），gl图像存储格式，gl数据存储格式
    // 因为是使用Opencv从文件中读取并存储图像，cv::Mat的图像存储顺序为BGR,而数据存储格式为uint型
    // 因此最后两个参数分别设置为 GL_BGR 和 GL_UNSIGNED_BYTE
    // 至于pangolin的内部存储格式，对图片的显示影响不大，因此一般设置为GL_RGB
    // 这边的图像的宽度和高度要设置为和原图像一致，否则会导致图像无法正常显示
    // 另外两个参数默认设置为 false和0
    pangolin::GlTexture imgTexture1(640, 480, GL_RGB, false, 0, GL_BGR, GL_UNSIGNED_BYTE);
    pangolin::GlTexture imgTexture2(640, 480, GL_RGB, false, 0, GL_BGR, GL_UNSIGNED_BYTE);

    while (!pangolin::ShouldQuit())
    {
        // 清空颜色和深度缓存
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        // 启动相机
        d_cam.Activate(s_cam);
        glColor3f(1.0f, 1.0f, 1.0f);
        pangolin::glDrawColouredCube();
        // 从文件读取图像
        cv::Mat img1 = cv::imread("../../examples/right01.jpg");
        cv::Mat img2 = cv::imread("../../examples/right01.jpg");
        // 向GPU装载图像
        // 因为该对象只接受 uchar* 对象，所以需要传递 cv::Mat的data成员，而不能传递cv::Mat本身
        // 另外两个参数 则是在创建 pangolin::GlTexture 对象时使用的最后两个参数一致。
        imgTexture1.Upload(img1.data, GL_BGR, GL_UNSIGNED_BYTE);
        imgTexture2.Upload(img2.data, GL_BGR, GL_UNSIGNED_BYTE);

        // 显示图像
        // 依次激活视窗、设置默认背景色、最后渲染显示图像
        // 这里原始渲染出的图像是倒着的，因此我们反转了 Y 轴
        cv_img_1.Activate();
        glColor3f(1.0f, 1.0f, 1.0f);         // 设置默认背景色，对于显示图片来说，不设置也没关系
        imgTexture1.RenderToViewportFlipY(); // 需要反转Y轴，否则输出是倒着的

        cv_img_2.Activate();
        glColor3f(1.0f, 1.0f, 1.0f); // 设置默认背景色，对于显示图片来说，不设置也没关系
        imgTexture2.RenderToViewportFlipY();

        pangolin::FinishFrame();
    }

    return 0;
}

Task5 绘制数据曲线

// Task5 pangolin 绘制数据曲线

#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 对象中，所以我们首先创建一个pangolin::DataLog对象
    // 并使用对应的成员函数 SetLabels()设置对应数据的名称  即图例
    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)"));
    labels.push_back(std::string("cos(2t)"));
    labels.push_back(std::string("tan(t)"));
    log.SetLabels(labels);

    const float tinc = 0.02f;

    // OpenGL 'view' of data, We might have many views of the same data
    // 数据的OpenGL“视图”，我们可能有许多相同数据的视图
    // 数据的可视化通过 pangolin::Plotter 对象来实现
    // 该对象的构造参数的第一个参数为需要绘制的 pangolin::DataLog 对象
    // 随后4个参数依次 Plotter 的左边界、右边界、下边界、上边界(即 Plotter中 X轴 Y轴的范围)
    // 最后两个参数依次为 x轴，y轴的坐标轴刻度大小
    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 添加一些标志块的功能
    // 函数的入口参数依次为 标志块的方向，标志块的数值， 标志块的判别方式以及标志块的颜色
    // eg. 第一个Marker  标志块的方向为垂直方向  数值为50pi  判断方式为小于  颜色为带透明度的蓝色  
    // eg. 第二个Marker  将y轴大于3的区域标记为红色
    // eg. 第三个Marker  由于是等于，因此只将 y = 3 这一条线标记为绿色
    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添加到Display中
    pangolin::DisplayBase().AddDisplay(plotter);

    float t = 0;

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

        // 帧循环中，只需要使用 DataLog::Log() 函数不断更新 DataLog 中的数据
        // pangolin就会根据之前创建的 plotter 自动在视窗中绘制数据
        log.Log(sin(t), cos(t), sin(t) + cos(t), cos(2*t), tan(t));
        t += tinc;

        pangolin::FinishFrame();
    }

    return 0;
}