【SLAM】pangolin的学习

 


官方Examples:https://github.com/stevenlovegrove/Pangolin/tree/master/examples

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;
}

Task4:多视图与图片显示

#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;
}

Task6 绘制位姿曲线

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <stdio.h>

// 法一  法二 说明:
//  - 法一是通过设置旋转矩阵一步一步求解
//  - 法二是直接都转换为欧氏变换矩阵 进行处理
// 相比较而言,转换为欧氏变换矩阵之后好处理一些


using namespace std;

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

    FILE *fp_gt;
    // EuRoc数据集
    // 数据集的解读:https://blog.csdn.net/shyjhyp11/article/details/115334614
    /* groundtruth 输出格式
        - p  代表position,指的是MAV的空间3D坐标
        - RS 代表这个坐标是在R坐标系的值,也就是LEICA位姿跟踪坐标系下测到的值
        - S  指的是从Sensor坐标系下得到的,后来又变换到R坐标系
        - R  可能代表LEICA坐标系,
        - x代表3D位置的x轴方向上的真值,单位为 m
            - p_RS_R_x [m]
            - p_RS_R_y [m]
            - p_RS_R_z [m]


        - q  代表 quaternion 四元数,表达了 MAV 的朝向信息,
        - RS 代表这个坐标是在R坐标系的值,也就是LEICA位姿跟踪坐标系下测到的值
        - w  四元数的实部
        - xyz 四元数的虚部
            - q_RS_w []
            - q_RS_x []
            - q_RS_y []
            - q_RS_z []


        - v  表示这是MAV的速度信息,而且是在R坐标系下的速度信息,单位 m/s
            - v_RS_R_x  [m s^-1]
            - v_RS_R_y  [m s^-1]
            - v_RS_R_z  [m s^-1]


        - w  表示这是 MAV 在R坐标系下的角速度信息,单位 rad/s
            - b_w_RS_S_x [rad s^-1]
            - b_w_RS_S_y [rad s^-1]
            - b_w_RS_S_z [rad s^-1]


        - a  表示这是 MAV 在R坐标系下的线加速度信息,单位 m/s^2
            - b_a_RS_S_x [m s^-2]
            - b_a_RS_S_y [m s^-2]
            - b_a_RS_S_z [m s^-2]
    */
    fp_gt = fopen("/home/bck20/DataSet/EuRoc/MH_01_easy/mav0/state_groundtruth_estimate0/data.csv", "r");
    if (fp_gt == nullptr)
    {
        cout << "failed to open file !\n";
        return EXIT_FAILURE;
    }

    // 跳过第一行
    char fl_buf[1024];
    fgets(fl_buf, sizeof(fl_buf), fp_gt);
    // 创建数据寄存器
    ulong time_stamp(0);
    double px(0.0f), py(0.0f), pz(0.0f);                                     // position 3D坐标
    double qw(0.0f), qx(0.0f), qy(0.0f), qz(0.0f);                           // 四元数
    double vx(0.0f), vy(0.0f), vz(0.0f);                                     // 速度
    double bwx(0.0f), bwy(0.0f), bwz(0.0f), bax(0.0f), bay(0.0f), baz(0.0f); // 角加速度,线加速度

    // 法一:
    vector<Eigen::Vector3d> traj;

    // 法二:
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;

    // 初始化视窗
    pangolin::CreateWindowAndBind("camera_pose", 752 * 2, 480 * 2);
    glEnable(GL_DEPTH_TEST);

    pangolin::OpenGlRenderState s_cam = pangolin::OpenGlRenderState(
        pangolin::ProjectionMatrix(752 * 2, 480 * 2, 420, 420, 320, 240, 0.1, 1000),
        pangolin::ModelViewLookAt(5, -3, 5, 0, 0, 0, pangolin::AxisZ));

    pangolin::View &d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0f, 1.0f, 0.0f, 1.0f, -752 / 480.0f)
                                .SetHandler(new pangolin::Handler3D(s_cam));


    while (!feof(fp_gt))
    {
        // ============== 常规操作 ===============
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        // ======================================

        fscanf(fp_gt, "%lu,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
               &time_stamp, &px, &py, &pz,
               &qw, &qx, &qy, &qz,
               &vx, &vy, &vz,
               &bwx, &bwy, &bwz,
               &bax, &bay, &baz);

        // 法一:
        Eigen::Quaterniond quat(qw, qx, qy, qz);
        Eigen::Vector3d pos(px, py, pz);
        traj.push_back(pos);

        // 法二:
        Eigen::Isometry3d Twr(Eigen::Quaterniond(qw, qx, qy, qz));
        Twr.pretranslate(Eigen::Vector3d(px, py, pz));
        poses.push_back(Twr);

        // ==============  绘制坐标系  =============
        glLineWidth(3);
        glBegin(GL_LINES);
        glColor3f(1.0f, 0.f, 0.f);
        glVertex3f(0, 0, 0);
        glVertex3f(1, 0, 0);
        glColor3f(0.f, 1.0f, 0.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 1, 0);
        glColor3f(0.f, 0.f, 1.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 0, 1);
        glEnd();

        // -------- 绘制随位姿变化的相机模型 -------- //
        // 构建位姿变换矩阵,pangolin中为列主序
        Eigen::Matrix3d R = quat.toRotationMatrix();

        /*  形式如下:  第一个 R =
            -0.382623   0.340983   -0.85868
            0.165471   0.939666    0.29941
            0.908967 -0.0275257  -0.415961

            相当于是 Twr.rotation()
        */

        // glPushMatrix和glPopMatrix的作用: https://blog.csdn.net/passtome/article/details/7768379
        // glPushMatrix, glPopMatrix 操作其实就相当于栈里的入栈和出栈

        // 首先需要使用 glPushMatrix() 告诉pangolin我们需要使用一个矩阵,
        // 然后我们使用 glMulmatrixd() 告诉pangolin后续绘制中的所有坐标均需要乘以这个矩阵,
        // 最后再glPopMatrix() 弹出矩阵,便于下一次循环填入新的矩阵数值,
        // 不同于Eigen等矩阵库, pangolin里的矩阵是按照列主序存储的
        // - 行主序:在数组中按照a[0][0]、a[0][1]、a[0][2]…a[1][0]、a[1][1]、a[1][2]…依次存储数据
        // - 列主序:在数组中按照a[0][0]、a[1][0]、a[2][0]…a[0][1]、a[1][1]、a[2][1]…依次存储数据
        glPushMatrix();

        // 这个是vector容器,是按照每列每列排的
        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.};
        // 让相机模型动起来,最简单的想法是在每次获取相机的位姿后,对上述八点线段的坐标进行相应的变换,
        // 进而绘制出当前时刻的相机模型,但是如果每次都需要去计算变换后的位姿,这无疑是非常麻烦且容易出错的.
        // opengl 提供了 glMultMatrix() 函数自动处理图像点的位姿转换

        // 法一:
        glMultMatrixd(Twc.data());

        // 法二:
        // glMultMatrixd(Twr.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();

        // -------- 绘制相机轨迹 --------//
        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());
        }

        // 法二:
        // for (size_t i = 0; i < poses.size() - 1; i++)
        // {
        //     glVertex3d(poses[i].translation()[0], poses[i].translation()[1], poses[i].translation()[2]);
        //     glVertex3d(poses[i + 1].translation()[0], poses[i + 1].translation()[1], poses[i + 1].translation()[2]);
        // }

        glEnd();

        pangolin::FinishFrame();

        if (pangolin::ShouldQuit())
            break;
    }

    return 0;
}

这个是通过法二绘制slam14讲中的数据集的代码:

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

using namespace std;

int main(int argc, char const *argv[])
{

    string trajectory_file = "../../examples/trajectory.txt";

    ifstream fin(trajectory_file);
    if (!fin)
    {
        cout << "cannot find trajectory file at " << trajectory_file << endl;
        return EXIT_FAILURE;
    }

    // 初始化视窗
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);

    pangolin::OpenGlRenderState s_cam = pangolin::OpenGlRenderState(
        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));

    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));

    double time, tx, ty, tz, qx, qy, qz, qw;
    vector<Eigen::Vector3d> traj;
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;

    int i = 0;
    while (!fin.eof())
    {
        // ============== 常规操作 ===============
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;

        Eigen::Isometry3d Twr(Eigen::Quaterniond(qw, qx, qy, qz));
        Twr.pretranslate(Eigen::Vector3d(tx, ty, tz));
        poses.push_back(Twr);

        // 绘制坐标系
        glLineWidth(3);
        glBegin(GL_LINES);
        glColor3f(1.0f, 0.f, 0.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0.3, 0, 0);
        glColor3f(0.f, 1.0f, 0.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 0.3, 0);
        glColor3f(0.f, 0.f, 1.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 0, 0.3);
        glEnd();

        // ------------------- 绘制随位姿变换的相机模型 ----------
        glPushMatrix();

        glMultMatrixd(Twr.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();

        // -------- 绘制相机轨迹 --------//
        glLineWidth(2);
        glBegin(GL_LINES);

        glColor3f(1.0f, 0.0f, 0.0f);
        for (size_t i = 0; i < poses.size() - 1; i++)
        {
            auto p1 = poses[i], p2 = poses[i + 1];
            /* 这边说明一下:
              p1 是 4x4的齐次矩阵,比如这个是  poses[2].matrix() 打印出来的
                0.997863 -0.00369598   0.0652398    0.013979
                0.00932699    0.996232  -0.0862205  -0.0130823
                -0.0646754   0.0866447    0.994138  -0.0108696
                0           0           0           1

                而下面用到的poses[i].translation() 表示从4x4的欧式变换矩阵中提取出 平移向量
                同理,poses[i].rotation() 表示从4x4的欧式变换矩阵中提取出  旋转向量

            */
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
        }

        if (i++ == 2)
        {
            std::cout << poses[2].matrix() << std::endl;
            std::cout << std::endl
                      << poses[2].translation().transpose() << std::endl;
            std::cout << std::endl
                      << poses[2].rotation() << std::endl;
        }

        glEnd();

        pangolin::FinishFrame();

        if (pangolin::ShouldQuit())
            break;

        usleep(30000);
    }

    return 0;
}

Task7 slam数据集可视化

// https://github.com/yuntianli91/pangolin_tutorial.git

#include <iostream>
#include <iomanip>
#include <Eigen/Dense>
#include <pangolin/pangolin.h>
#include <opencv2/opencv.hpp>
#include <queue>
#include <unistd.h>
#include <thread>
#include <string>

using namespace std;

// ================== custom_struct.h ===============

// 创建一个VectorXd用于pangolin::Var 输出数据
// 根据pangolin文档,自定义类型需要重载输入输出流操作符
struct VecXd
{
    Eigen::VectorXd vec_ = Eigen::Vector3d::Zero();
};

// 使用 inline 休息避免头文件中的非模板、非成员重复包含
inline ostream &operator<<(ostream &out, const VecXd &r)
{
    int N = r.vec_.size();
    out.setf(ios::fixed);
    out << "="
        << " [";
    for (int i = 0; i < N - 1; i++)
    {
        out << setprecision(2) << r.vec_(i) << ", ";
    }
    out << r.vec_(N - 1) << "]";
    return out;
}

inline istream &operator>>(istream &in, VecXd &r)
{
    return in;
}

// ==================== slam_visualizer.h =============
class SlamVisualizer
{
public:
    SlamVisualizer(int width = 752, int height = 480) : WIN_WIDTH_(width), WIN_HEIGHT_(height) {}
    ~SlamVisualizer() {}

    void initDraw();

    void activeAllView();

    void drawCubeTest();

    void drawCamWithPose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat);

    void drawTraj(vector<Eigen::Vector3d> &traj);
    /**
     * @brief 画一个简单的相机模型
     * @param scale:缩放尺寸,默认为1
     */
    void drawCam(const float scale = 1.);

    void drawCoordinate();

    void displayImg(cv::Mat &originImg, cv::Mat &trackImg);

    void displayData(Eigen::Vector3d &pos, Eigen::Quaterniond &quat);

    void registerUICallback();

private:
    pangolin::OpenGlRenderState s_cam_;
    pangolin::View d_cam_, d_img_, d_track_;
    pangolin::GlTexture imageTexture_, trackTexture_;
    pangolin::DataLog pose_log_;

    // 存储ui面板的控件对象
    std::vector<pangolin::Var<bool>> ui_set_;
    // 存储data面板的控件对象
    std::vector<pangolin::Var<VecXd>> data_set_;
    // 是否显示相机
    bool camera_visible_ = true;
    // 是否显示轨迹
    bool traj_visible_ = true;
    // 是否显示参考坐标系
    bool coordinate_visible_ = true;
    // 是否显示图像
    bool img_visible_ = true;
    // 窗口尺寸
    int WIN_WIDTH_;
    int WIN_HEIGHT_;
};

void SlamVisualizer::initDraw()
{
    pangolin::CreateWindowAndBind("camera_pose", WIN_WIDTH_, WIN_HEIGHT_);
    glEnable(GL_DEPTH_TEST);

    s_cam_ = pangolin::OpenGlRenderState(
        pangolin::ProjectionMatrix(WIN_WIDTH_, WIN_HEIGHT_, 420, 420, 320, 240, 0.1, 1000),
        pangolin::ModelViewLookAt(5, -3, 5, 0, 0, 0, pangolin::AxisX));

    int PANEL_WIDTH = WIN_WIDTH_ / 4;
    int PANEL_HEIGHT = WIN_HEIGHT_ / 4;

    // 轨迹显示窗口
    d_cam_ = pangolin::CreateDisplay()
                 .SetBounds(0.0f, 1.0f, pangolin::Attach::Pix(PANEL_WIDTH), 1.0f, -(float)WIN_WIDTH_ / (float)WIN_HEIGHT_)
                 .SetHandler(new pangolin::Handler3D(s_cam_));

    // 控制面板
    pangolin::CreatePanel("ui")
        .SetBounds(pangolin::Attach::Pix(3.0f * PANEL_HEIGHT), 1.0f, 0.0f, pangolin::Attach::Pix(PANEL_WIDTH), (float)WIN_WIDTH_ / (float)WIN_HEIGHT_);

    ui_set_.clear();
    pangolin::Var<bool> show_cam("ui.show_cam", true, true);
    pangolin::Var<bool> show_traj("ui.show_traj", true, true);
    pangolin::Var<bool> show_img("ui.show_img", true, true);
    pangolin::Var<bool> show_coordinate("ui.show_coordinate", true, true);
    pangolin::Var<bool> save_map("ui.save_map", false, false);
    pangolin::Var<bool> save_win("ui.save_win", false, false);

    ui_set_.push_back(show_cam);
    ui_set_.push_back(show_traj);
    ui_set_.push_back(show_img);
    ui_set_.push_back(show_coordinate);
    ui_set_.push_back(save_map);
    ui_set_.push_back(save_win);

    // 数据显示
    pangolin::CreatePanel("data")
        .SetBounds(pangolin::Attach::Pix(2.0f * PANEL_HEIGHT), pangolin::Attach::Pix(3.0f * PANEL_HEIGHT), 0, pangolin::Attach::Pix(PANEL_WIDTH), (float)WIN_WIDTH_ / (float)WIN_HEIGHT_);

    data_set_.clear();
    pangolin::Var<VecXd> curr_pos("data.pos", VecXd());
    pangolin::Var<VecXd> curr_att("data.euler_angle", VecXd());
    data_set_.push_back(curr_pos);
    data_set_.push_back(curr_att);

    // 原图片显示
    d_img_ = pangolin::CreateDisplay()
                 .SetBounds(pangolin::Attach::Pix(1.0f * PANEL_HEIGHT), pangolin::Attach::Pix(2.0f * PANEL_HEIGHT),
                            0.0f, pangolin::Attach::Pix(PANEL_WIDTH), (float)WIN_WIDTH_ / (float)WIN_HEIGHT_)
                 .SetLock(pangolin::LockLeft, pangolin::LockBottom);

    imageTexture_ = pangolin::GlTexture(752, 480, GL_RGB, false, 0, GL_BGR, GL_UNSIGNED_BYTE);

    // 跟踪图片显示
    d_track_ = pangolin::CreateDisplay()
                   .SetBounds(0., pangolin::Attach::Pix(1.0f * PANEL_HEIGHT),
                              0., pangolin::Attach::Pix(PANEL_WIDTH), (float)WIN_WIDTH_ / (float)WIN_HEIGHT_)
                   .SetLock(pangolin::LockLeft, pangolin::LockBottom);

    trackTexture_ = pangolin::GlTexture(752, 480, GL_RGB, false, 0, GL_BGR, GL_UNSIGNED_BYTE);
}

void SlamVisualizer::activeAllView()
{
    d_cam_.Activate(s_cam_);
}

void SlamVisualizer::drawCubeTest()
{
    // Render some stuff
    glColor3f(1.0, 0.0, 1.0);
    pangolin::glDrawColouredCube();
}

void SlamVisualizer::drawCam(const float scale)
{
    if (scale < 0)
    {
        cerr << "scale should be positive !\n";
        return;
    }

    const float w = 0.2 * scale;
    const float h = w * 0.75;
    const float z = w * 0.8;

    glLineWidth(2 * scale);
    // 绘制相机轮廓线
    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();

    return;
}

void SlamVisualizer::drawCamWithPose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat)
{
    if (!camera_visible_)
        return;

    Eigen::Matrix3d R = quat.toRotationMatrix();

    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());
    drawCam();
    glPopMatrix();
}

void SlamVisualizer::drawTraj(vector<Eigen::Vector3d> &traj)
{
    if (!traj_visible_)
        return;
    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();
}

void SlamVisualizer::drawCoordinate()
{
    if (!coordinate_visible_)
        return;
    // 绘制坐标系
    glLineWidth(3);
    glBegin(GL_LINES);
    glColor3f(1.0f, 0.f, 0.f);
    glVertex3f(0, 0, 0);
    glVertex3f(1, 0, 0);
    glColor3f(0.f, 1.0f, 0.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 1, 0);
    glColor3f(0.f, 0.f, 1.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 1);
    glEnd();
}

void SlamVisualizer::displayImg(cv::Mat &originImg, cv::Mat &trackImg)
{
    if (!img_visible_)
        return;

    imageTexture_.Upload(originImg.data, GL_BGR, GL_UNSIGNED_BYTE);

    // 显示图像
    d_img_.Activate();
    glColor3f(1.0f, 1.0f, 1.0f);           // 设置默认背景色,对于显示图片来说,不设置也没关系
    imageTexture_.RenderToViewportFlipY(); // 需要反转Y轴,否则输出是倒着的

    trackTexture_.Upload(trackImg.data, GL_BGR, GL_UNSIGNED_BYTE);
    // 显示图像
    d_track_.Activate();
    glColor3f(1.0f, 1.0f, 1.0f);           // 设置默认背景色,对于显示图片来说,不设置也没关系
    trackTexture_.RenderToViewportFlipY(); // 需要反转Y轴,否则输出是倒着的
}

void SlamVisualizer::displayData(Eigen::Vector3d &pos, Eigen::Quaterniond &quat)
{
    VecXd tmp_pose, tmp_euler;
    tmp_pose.vec_ = pos;
    tmp_euler.vec_ = quat.matrix().eulerAngles(2, 1, 0); // YPR, quat是否需要转置?

    tmp_euler.vec_ *= (180 / M_PI);
    data_set_[0] = tmp_pose;
    data_set_[1] = tmp_euler;
}

void SlamVisualizer::registerUICallback()
{
    camera_visible_ = ui_set_[0] ? true : false;
    traj_visible_ = ui_set_[1] ? true : false;
    img_visible_ = ui_set_[2] ? true : false;
    coordinate_visible_ = ui_set_[3] ? true : false;

    if (pangolin::Pushed(ui_set_[4]))
        d_cam_.SaveOnRender("map");

    if (pangolin::Pushed(ui_set_[5]))
        pangolin::SaveWindowOnRender("win");
}

// ================= main.cpp ================

SlamVisualizer visualizer(1504, 960);
queue<string> imgFileNames;
queue<ulong> imgTimeStamps;

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

    FILE *fp_gt, *fp_img;

    fp_gt = fopen("/home/bck20/DataSet/EuRoc/MH_01_easy/mav0/state_groundtruth_estimate0/data.csv", "r");
    fp_img = fopen("/home/bck20/DataSet/EuRoc/MH_01_easy/mav0/cam0/data.csv", "r");

    if (fp_gt == nullptr || fp_img == nullptr)
    {
        cout << "failed to open file \n";
        return EXIT_FAILURE;
    }

    // ============= 读取图片路径 ===============
    // 跳过第一行
    char fl_buf[1024];
    fgets(fl_buf, sizeof(fl_buf), fp_img);

    while (!feof(fp_img))
    {
        char filename[23];
        ulong timestamp;
        fscanf(fp_img, "%lu,%s", &timestamp, filename);

        imgTimeStamps.push(timestamp);
        imgFileNames.push(string(filename));
    }

    // ======================= 读取 groundtruth ===================
    // 跳过第一行
    fgets(fl_buf, sizeof(fl_buf), fp_gt);

    // 初始化视窗
    visualizer.initDraw();
    vector<Eigen::Vector3d> traj;
    while (!feof(fp_gt))
    {
        // 常规操作
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        visualizer.activeAllView();
        // 注册ui回调函数
        visualizer.registerUICallback();
        // 从数据集中读取数据
        // 创建数据寄存器
        ulong time_stamp(0);
        double px(0.), py(0.), pz(0.);
        double qw(0.), qx(0.), qy(0.), qz(0.);
        double vx(0.), vy(0.), vz(0.);
        double bwx(0.), bwy(0.), bwz(0.), bax(0.), bay(0.), baz(0.);
        fscanf(fp_gt, "%lu,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
               &time_stamp, &px, &py, &pz,
               &qw, &qx, &qy, &qz,
               &vx, &vy, &vz,
               &bwx, &bwy, &bwz,
               &bax, &bay, &baz);

        Eigen::Quaterniond quat(qw, qx, qy, qz); // quat是否要转置?
        Eigen::Vector3d pos(px, py, pz);
        traj.push_back(pos);

        // 显示数据
        visualizer.displayData(pos, quat);
        // 绘制轨迹可视化部分
        visualizer.drawCoordinate();
        visualizer.drawCamWithPose(pos, quat);
        visualizer.drawTraj(traj);
        // 弹出当前时刻之前的图像
        double imu_time, img_time;
        imu_time = (double)time_stamp / 1e9;
        img_time = (double)imgTimeStamps.front() / 1e9;
        if (imu_time > img_time)
        {
            // cout << imgFileNames.front() << endl;
            imgTimeStamps.pop();
            imgFileNames.pop();
        }

        // 显示图像(由于数据集没有跟踪图像,这里两幅图像显示一样)
        cv::Mat img;
        string img_file = "/home/bck20/DataSet/EuRoc/MH_01_easy/mav0/cam0/data/" + imgFileNames.front();
        img = cv::imread(img_file, cv::IMREAD_COLOR);
        visualizer.displayImg(img, img);
        // 循环与退出判断
        pangolin::FinishFrame();

        if (pangolin::ShouldQuit())
            break;
    }

    return 0;
}

posted @   乞力马扎罗山的雪  阅读(1444)  评论(0编辑  收藏  举报
相关博文:
阅读排行:
· 震惊!C++程序真的从main开始吗?99%的程序员都答错了
· 【硬核科普】Trae如何「偷看」你的代码?零基础破解AI编程运行原理
· 单元测试从入门到精通
· 上周热点回顾(3.3-3.9)
· winform 绘制太阳,地球,月球 运作规律
点击右上角即可分享
微信分享提示

目录导航