Pangolin库学习大全

1 Pangolin介绍

2.安装

3 常见方法

3.1 创建窗口

3.2 glEnable()启用各种功能。

3.3 glBlendFunc()颜色混合

3.4 pangolin::OpenGlRenderState构建观察相机对象

3.5 交互视图（View）

3.6 pangolin::ShouldQuit()检测是否关闭OpenGL窗口

3.7 glClear()清理缓存区

3.8 d_cam.Activate(s_cam)相机激活

3.9 pangolin::FinishFrame()视窗刷新

4 图形绘制

4.1 绘制点和直线

4.2 绘制轨迹

5 GUI交互按钮

6 其他官方案例

6.1 pangolin多线程

6.2 多视图图片显示

6.3 绘制位姿曲线

6.4 slam数据集可视化

1 Pangolin介绍

常见的3D绘图的程序库有很多，MATLAB、Python的Matplotlib、OpenGL。在Linux上常用的一个3D绘图库是Pangolin，它是基于OpenGL完成的，它不但支持OpenGL的基本操作，还提供了一些GUI的功能。对于在SLAM的学习中，它是必不可少的3D显示工具。

2.安装

链接文章：Pangolin安装-1.2节

3 常见方法

先通过一个官方案例来逐步了解，代码中出现的方法在后面逐步讲解。

/* CMakeLists.txt
find_package(Pangolin 0.8 REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})add_executable(HelloPangolin main.cpp)
target_link_libraries(HelloPangolin pango_display pango_python)
*/#include <pangolin/display/display.h>
#include <pangolin/display/view.h>
#include <pangolin/handler/handler.h>
#include <pangolin/gl/gldraw.h>int main( int /*argc*/, char** /*argv*/ )
{pangolin::CreateWindowAndBind("Main",640,480);glEnable(GL_DEPTH_TEST);// Define Projection and initial ModelView matrixpangolin::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 windowpangolin::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 intoglClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);// Render OpenGL Cubepangolin::glDrawColouredCube();// Swap frames and Process Eventspangolin::FinishFrame();}return 0;
}

3.1 创建窗口

一个名叫"Main"的GUI窗口用于显示，窗口的大小是640x480像素。

pangolin::CreateWindowAndBind("Main",640,480);

3.2 glEnable()启用各种功能。

glEnable()启用各种功能，功能由参数决定。与glDisable相对应，glDisable是用来关闭的。两个函数参数取值是一致的。

void glEnable(GLenum cap)

GLenum：是unsigned int 类型
cap：是一个取值。由值决定启用的功能键。各种功能请看参数cap的取值表

说明：glEnable不能写在glBegin和glEnd两个函数中间。

参数cap的取值
类型	值	说明
GL_ALPHA_TEST	4864	跟据函数glAlphaFunc的条件要求来决定图形透明的层度是否显示。
GL_AUTO_NORMAL	3456	执行后，图形能把光反射到各个方向
GL_BLEND	3042	启用颜色混合。例如实现半透明效果
GL_CLIP_PLANE0 ~ GL_CLIP_PLANE5	12288 ~ 12283	根据函数glClipPlane的条件要求启用图形切割管道。这里指六种缓存管道
GL_COLOR_LOGIC_OP	3058	启用每一像素的色彩为位逻辑运算
GL_COLOR_MATERIAL	2930	执行后，图形（材料）将根据光线的照耀进行反射。反射要求由函数glColorMaterial进行设定。
GL_CULL_FACE	2884	根据函数glCullFace要求启用隐藏图形材料的面。
GL_DEPTH_TEST	2929	启用深度测试。根据坐标的远近自动隐藏被遮住的图形（材料）
GL_DITHER	3024	启用抖动
GL_FOG	2912	雾化效果例如距离越远越模糊
GL_INDEX_LOGIC_OP	3057	逻辑操作
GL_LIGHT0 ~ GL_LIGHT7	16384 ~ 16391	启用0号灯到7号灯(光源) 光源要求由函数glLight函数来完成
GL_LIGHTING	2896	启用灯源
GL_LINE_SMOOTH	2848	执行后，过虑线段的锯齿
GL_LINE_STIPPLE	2852	执行后，画虚线
GL_LOGIC_OP	3057	逻辑操作
GL_MAP1_COLOR_4	3472	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 生成RGBA曲线
GL_MAP1_INDEX	3473	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 生成颜色索引曲线
GL_MAP1_NORMAL	3474	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 生成法线
GL_MAP1_TEXTURE_COORD_1	3475	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 生成文理坐标
GL_MAP1_TEXTURE_COORD_2	3476	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 生成文理坐标
GL_MAP1_TEXTURE_COORD_3	3477	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 生成文理坐标
GL_MAP1_TEXTURE_COORD_4	3478	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 生成文理坐标
GL_MAP1_VERTEX_3	3479	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 在三维空间里生成曲线
GL_MAP1_VERTEX_4	3480	根据函数Map1对贝赛尔曲线的设置，启用glEvalCoord1,glEvalMesh1,glEvalPoint1 在四维空间里生成法线
GL_MAP2_COLOR_4	3504	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 生成RGBA曲线
GL_MAP2_INDEX	3505	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 生成颜色索引
GL_MAP2_NORMAL	3506	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 生成法线
GL_MAP2_TEXTURE_COORD_1	3507	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 生成纹理坐标
GL_MAP2_TEXTURE_COORD_2	3508	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 生成纹理坐标
GL_MAP2_TEXTURE_COORD_3	3509	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 生成纹理坐标
GL_MAP2_TEXTURE_COORD_4	3510	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 生成纹理坐标
GL_MAP2_VERTEX_3	3511	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 在三维空间里生成曲线
GL_MAP2_VERTEX_4	3512	根据函数Map2对贝赛尔曲线的设置，启用glEvalCoord2,glEvalMesh2,glEvalPoint2 在三维空间里生成曲线
GL_NORMALIZE	2977	根据函数glNormal的设置条件，启用法向量
GL_POINT_SMOOTH	2832	执行后，过虑线点的锯齿
GL_POLYGON_OFFSET_FILL	32823	根据函数glPolygonOffset的设置，启用面的深度偏移
GL_POLYGON_OFFSET_LINE	10754	根据函数glPolygonOffset的设置，启用线的深度偏移
GL_POLYGON_OFFSET_POINT	10753	根据函数glPolygonOffset的设置，启用点的深度偏移
GL_POLYGON_SMOOTH	2881	过虑图形（多边形）的锯齿
GL_POLYGON_STIPPLE	2882	执行后，多边形为矢量画图
GL_SCISSOR_TEST	3089	根据函数glScissor设置，启用图形剪切
GL_STENCIL_TEST	2960
GL_TEXTURE_1D	3552	启用一维文理
GL_TEXTURE_2D	3553	启用二维文理
GL_TEXTURE_GEN_Q	3171	根据函数glTexGen，启用纹理处理
GL_TEXTURE_GEN_R	3170	根据函数glTexGen，启用纹理处理
GL_TEXTURE_GEN_S	3168	根据函数glTexGen，启用纹理处理
GL_TEXTURE_GEN_T	3169	根据函数glTexGen，启用纹理处理

3.3 glBlendFunc()颜色混合

参考文章：颜色混合opengl--glBlendFunc函数

混合是什么呢？混合就是把两种颜色混在一起。具体一点，就是把某一像素位置原来的颜色和将要画上去的颜色，通过某种方式混在一起，从而实现特殊的效果。 注意：只有在RGBA模式下，才可以使用混合功能，颜色索引模式下是无法使用混合功能的。

要使用OpenGL的混合功能，只需要调用：glEnable(GL_BLEND);即可。
要关闭OpenGL的混合功能，只需要调用：glDisable(GL_BLEND);即可。

void glBlendFunc(GLenum sfactor, GLenum dfactor)

sfactor：指定如何计算红色，绿色，蓝色和alpha源混合因子。

下列符号常量被接受：GL_ZERO，GL_ONE，GL_SRC_COLOR，GL_ONE_MINUS_SRC_COLOR，GL_DST_COLOR，GL_ONE_MINUS_DST_COLOR，GL_SRC_ALPHA，GL_ONE_MINUS_SRC_ALPHA，GL_DST_ALPHA，GL_ONE_MINUS_DST_ALPHA，GL_CONSTANT_COLOR，GL_ONE_MINUS_CONSTANT_COLOR，GL_CONSTANT_ALPHA，GL_ONE_MINUS_CONSTANT_ALPHA和GL_SRC_ALPHA_SATURATE。初始值为GL_ONE。

dfactor：指定如何计算红色，绿色，蓝色和alpha目标混合因子。

接受以下符号常量：GL_ZERO，GL_ONE，GL_SRC_COLOR，GL_ONE_MINUS_SRC_COLOR，GL_DST_COLOR，GL_ONE_MINUS_DST_COLOR，GL_SRC_ALPHA，GL_ONE_MINUS_SRC_ALPHA，GL_DST_ALPHA，GL_ONE_MINUS_DST_ALPHA。 GL_CONSTANT_COLOR，GL_ONE_MINUS_CONSTANT_COLOR，GL_CONSTANT_ALPHA和GL_ONE_MINUS_CONSTANT_ALPHA。初始值为GL_ZERO。

这两个参数可以是多种值，下面介绍比较常用的几种。

        GL_ZERO：表示使用0.0作为因子，实际上相当于不使用这种颜色参与混合运算。
        GL_ONE：表示使用1.0作为因子，实际上相当于完全的使用了这种颜色参与混合运算。
        GL_SRC_ALPHA：表示使用源颜色的alpha值来作为因子。
        GL_DST_ALPHA：表示使用目标颜色的alpha值来作为因子。
        GL_ONE_MINUS_SRC_ALPHA：表示用1.0减去源颜色的alpha值来作为因子。
        GL_ONE_MINUS_DST_ALPHA：表示用1.0减去目标颜色的alpha值来作为因子。
        GL_SRC_COLOR：把源颜色的四个分量分别作为因子的四个分量。

（1）源因子和目标因子

前面我们已经提到，混合需要把原来的颜色和将要画上去的颜色找出来，经过某种方式处理后得到一种新的颜色。这里把将要画上去的颜色称为“源颜色”，把原来的颜色称为“目标颜色”。
OpenGL 会把源颜色和目标颜色各自取出，并乘以一个系数（源颜色乘以的系数称为“源因子”，目标颜色乘以的系数称为“目标因子”），然后相加，这样就得到了新的颜色。（也可以不是相加，新版本的OpenGL可以设置运算方式，包括加、减、取两者中较大的、取两者中较小的、逻辑运算等。
下面用数学公式来表达一下这个运算方式。假设源颜色的四个分量（指红色，绿色，蓝色，alpha值）是(Rs, Gs, Bs, As)，目标颜色的四个分量是(Rd, Gd, Bd, Ad)，又设源因子为(Sr, Sg, Sb, Sa)，目标因子为(Dr, Dg, Db, Da)。则混合产生的新颜色可以表示为：

(Rs*Sr+Rd*Dr, Gs*Sg+Gd*Dg, Bs*Sb+Bd*Db, As*Sa+Ad*Da)

当然了，如果颜色的某一分量超过了1.0，则它会被自动截取为1.0，不需要考虑越界的问题。

源因子和目标因子是可以通过glBlendFunc函数来进行设置的。glBlendFunc有两个参数，前者表示源因子，后者表示目标因子。

举例来说：

如果设置了glBlendFunc(GL_ONE, GL_ZERO);，则表示完全使用源颜色，完全不使用目标颜色，因此画面效果和不使用混合的时候一致（当然效率可能会低一点点）。如果没有设置源因子和目标因子，则默认情况就是这样的设置。

如果设置了glBlendFunc(GL_ZERO, GL_ONE);，则表示完全不使用源颜色，因此无论你想画什么，最后都不会被画上去了。

如果设置了glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);，则表示源颜色乘以自身的alpha 值，目标颜色乘以1.0减去源颜色的alpha值，这样一来，源颜色的alpha值越大，则产生的新颜色中源颜色所占比例就越大，而目标颜色所占比例则减小。这种情况下，我们可以简单的将源颜色的alpha值理解为“不透明度”。这也是混合时最常用的方式。

如果设置了glBlendFunc(GL_ONE, GL_ONE);，则表示完全使用源颜色和目标颜色，最终的颜色实际上就是两种颜色的简单相加。例如红色(1, 0, 0)和绿色(0, 1, 0)相加得到(1, 1, 0)，结果为黄色。

注意：所谓源颜色和目标颜色，是跟绘制的顺序有关的。假如先绘制了一个红色的物体，再在其上绘制绿色的物体。则绿色是源颜色，红色是目标颜色。如果顺序反过来，则红色就是源颜色，绿色才是目标颜色。在绘制时，应该注意顺序，使得绘制的源颜色与设置的源因子对应，目标颜色与设置的目标因子对应。

（2）实现三维混合

在进行三维场景的混合时必须注意的一点是深度缓冲。深度缓冲是这样一段数据，它记录了每一个像素距离观察者有多近。在启用深度缓冲测试的情况下，如果将要绘制的像素比原来的像素更近，则像素将被绘制。否则，像素就会被忽略掉，不进行绘制。这在绘制不透明的物体时非常有用——不管是先绘制近的物体再绘制远的物体，还是先绘制远的物体再绘制近的物体，或者干脆以混乱的顺序进行绘制，最后的显示结果总是近的物体遮住远的物体。

然而在你需要实现半透明效果时，发现一切都不是那么美好了。如果你绘制了一个近距离的半透明物体，则它在深度缓冲区内保留了一些信息，使得远处的物体将无法再被绘制出来。虽然半透明的物体仍然半透明，但透过它看到的却不是正确的内容了。

要解决以上问题，需要在绘制半透明物体时将深度缓冲区设置为只读，这样一来，虽然半透明物体被绘制上去了，深度缓冲区还保持在原来的状态。如果再有一个物体出现在半透明物体之后，在不透明物体之前，则它也可以被绘制（因为此时深度缓冲区中记录的是那个不透明物体的深度）。以后再要绘制不透明物体时，只需要再将深度缓冲区设置为可读可写的形式即可。如何绘制一个一部分半透明一部分不透明的物体？这个好办，只需要把物体分为两个部分，一部分全是半透明的，一部分全是不透明的，分别绘制就可以了。

即使使用了以上技巧，我们仍然不能随心所欲的按照混乱顺序来进行绘制。必须是先绘制不透明的物体，然后绘制透明的物体。否则，假设背景为蓝色，近处一块红色玻璃，中间一个绿色物体。如果先绘制红色半透明玻璃的话，它先和蓝色背景进行混合，则以后绘制中间的绿色物体时，想单独与红色玻璃混合已经不能实现了。

总结起来，绘制顺序就是：

（1）首先绘制所有不透明的物体。

（2）如果两个物体都是不透明的，则谁先谁后都没有关系。然后，将深度缓冲区设置为只读。

（3）接下来，绘制所有半透明的物体。如果两个物体都是半透明的，则谁先谁后只需要根据自己的意愿（注意了，先绘制的将成为“目标颜色”，后绘制的将成为“源颜色”，所以绘制的顺序将会对结果造成一些影响）。

（4）最后，将深度缓冲区设置为可读可写形式。调用glDepthMask(GL_FALSE);可将深度缓冲区设置为只读形式。调用glDepthMask(GL_TRUE)；可将深度缓冲区设置为可读可写形式。

3.4 pangolin::OpenGlRenderState构建观察相机对象

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

该行代码表示创建一个相机的观察视图，相当于是模拟一个真实的相机去观测虚拟的三维世界。既然是模拟相机观测，那就得有相机的一些配置参数：

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)：前三个参数依次为相机所在的位置，第四到第六个参数相机所看的视点位置(一般会设置在原点)，最后是相机轴的方向，最终在GUI中呈现的图像就是通过这个设置的相机内外参得到的。

3.5 交互视图（View）

用于显示相机观察到的信息内容，首先需要pangolin::Handler3D创建相机视图句柄。

pangolin::Handler3D handler(s_cam)；

括号内参数填写相机对象，随后使用pangolin::View构建交互视图对象构建交互视图对象。

pangolin::View& d_cam=pangolin::CreateDisplay().SetBounds(Attach bottom, Attach top, Attach left, Attach right, double aspect).SetHandler(Handler* handler);

SetBounds()：用于确定视图属性，其内参数如下：
bottom、top：视图在视窗内的上下范围，依次为下、上，采用相对坐标表示（0：最下侧，1：最上侧）
left、right：视图在视窗内的左右范围，依次为左、右，采用相对左边表示（0：最左侧，1：最右侧）
aspect：视图的分辨率，也即分辨率。参数aspect取正值，将由前四个参数设置的视图大小来裁剪达到设置的分辨率，参数aspect取负值，将拉伸图像以充满由前四个参数设置的视图范围。
SetHandler()：用于确定视图的相机句柄

3.6 pangolin::ShouldQuit()检测是否关闭OpenGL窗口

pangolin::ShouldQuit()

通过pangolin::ShouldQuit()检测你是否关闭OpenGL窗口。

3.7 glClear()清理缓存区

清空颜色和深度缓存。这样每次都会刷新显示，不至于前后帧的颜信息相互干扰。

glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

3.8 d_cam.Activate(s_cam)相机激活

激活显示并设置状态矩阵。

d_cam.Activate(s_cam);

3.9 pangolin::FinishFrame()视窗刷新

执行后期渲染，事件处理和帧交换，相当于前面设置了那么多现在可以进行最终的显示了。

pangolin::FinishFrame();

4 图形绘制

4.1 绘制点和直线

下面我要向你实际介绍一些用法，包括怎么绘制点、直线等等。

/* CMakeLists.txt
find_package(Pangolin 0.8 REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})add_executable(HelloPangolin main.cpp)
target_link_libraries(HelloPangolin pango_display pango_python)
*/#include <pangolin/display/display.h>
#include <pangolin/plot/plotter.h>int main(/*int argc, char* argv[]*/)
{pangolin::CreateWindowAndBind("Main",640,480);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);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);//绘制点glBegin( GL_POINTS );//点设置的开始glColor3f(1.0,1.0,1.0);glVertex3f(0.0f,0.0f,0.0f);glVertex3f(1,0,0);glVertex3f(0,2,0);glEnd();//点设置的结束//绘制直线glBegin(GL_LINES);glLineWidth(2.0);glColor3f(1.0, 1.0, 1.0);glVertex3f(0,0,0);glVertex3f(1,1,1);glVertex3f(0,0,0);glVertex3f(0,1,1);glVertex3f(0,0,0);glVertex3f(1,0,1);glEnd();//绘制三角形glBegin(GL_TRIANGLES);glVertex3f(0,1,1);glVertex3f(0,0,0);glVertex3f(1,0,1);glEnd();pangolin::FinishFrame();}return 0;
}

可设置的形状还有

GL_POINTS：点
GL_LINES：线
GL_LINE_STRIP：折线
GL_LINE_LOOP：封闭折线
GL_TRIANGLES：三角形
GL_POLYGON：多边形

总之你绘制的图形必须，以及图像的相关设置必须放在glBegin()和glEnd()之间。这之间还有一些可用的配置如下：

glVertex(); 设置顶点坐标
glColor(); 设置当前颜色
glIndex(); 设置当前颜色表
glNormal(); 设置法向坐标
glEvalCoord(); 产生坐标
glCallList(),glCallLists(); 执行显示列表
glTexCoord(); 设置纹理坐标
glEdgeFlag(); 控制边界绘制
glMaterial(); 设置材质

4.2 绘制轨迹

#include <pangolin/display/display.h>
#include <pangolin/plot/plotter.h>int main(/*int argc, char* argv[]*/)
{pangolin::CreateWindowAndBind("Main",640,480);pangolin::DataLog log;//常规数据格式// 添加label字符串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,-2.0f,2.0f,(float)M_PI/(4.0f*tinc),0.1);//左边界、右边界、下边界、上边界,  每个小格x,y的边界值plotter.SetBounds(0, 1.0, 0, 1.0); //设置画图在显示界面边界plotter.Track("$i",""); //设置图像动态跟踪当前值//plotter.AddMarker 修改一片区域背景颜色，方向，值，范围，颜色//pangolin::Marker::Horizontal：水平方向//pangolin::Marker::Equal 等于//angolin::Marker::LessThan：小于 //pangolin::Marker::GreaterThan：大于//pangolin::Colour::Blue().WithAlpha(0.2f) 蓝色plotter.AddMarker(pangolin::Marker::Vertical,   -10, pangolin::Marker::LessThan, pangolin::Colour::Blue().WithAlpha(0.2f) );plotter.AddMarker(pangolin::Marker::Horizontal,   20, pangolin::Marker::GreaterThan, pangolin::Colour::Red().WithAlpha(0.2f) );plotter.AddMarker(pangolin::Marker::Horizontal,    10, 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;pangolin::FinishFrame();}return 0;
}

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

数据的可视化是通过对象 pangolin::Plotter 来实现的。

// pangolin::Plotterz构造函数
Plotter(DataLog* default_log,float left=0, float right=600, float bottom=-1, float top=1,float tickx=30, float ticky=0.5,Plotter* linked_plotter_x = 0,Plotter* linked_plotter_y = 0);

DataLog*：为需要绘制的 pangolin::DataLog 对象。
left、right、bottom、top：依次为 Plotter 的左边界、右边界、下边界、上边界，即 Plotter 中 x轴 y 轴的范围。
float tickx、float ticky：是x 和 y 轴的刻度间隔

随后该程序在 Plotter 中使用 plotter 的成员函数 AddMarker 添加一些标志块的功能。

Marker& AddMarker(Marker::Direction d, float value,Marker::Equality leg = Marker::Equal, Colour c = Colour()
);

Marker::Direction：标志块的方向。
value：标志块的数值。
Marker::Equality：标志块的判别方式。
Colour：标志块的颜色。

后面将构建好的 plotter 添加到 Display 中：

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

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

5 GUI交互按钮

#include <pangolin/display/display.h>
#include <pangolin/plot/plotter.h>
#include <pangolin/var/varextra.h>
#include <pangolin/display/widgets.h>
#include <pangolin/gl/gldraw.h>
#include <string>
#include <iostream>
// ------------------------------------- //void SampleMethod()
{std::cout << "You typed ctrl-r or pushed reset " << std::endl;
}void function(){std::cout << "Hello pangolin" << std::endl;
}int main() {pangolin::CreateWindowAndBind("Main",640,480);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);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);pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(180));//创建pangolin::Var<bool> a_button("ui.A_Button", false, false);//设置一个按钮，默认值为false，最后一个false表示按钮形式pangolin::Var<double> a_double("ui.A_Double", 3, 0, 5);//设置一个double的、可拖动变换值的玩意(不知道咋形容)！pangolin::Var<int> a_int("ui.A_Int", 2, 0, 5);//设置一个int的、可拖动变换值的玩意pangolin::Var<std::function<void(void)>> reset("ui.Reset", function);//设置一个按钮，用于调用function函数while( !pangolin::ShouldQuit() ){glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);if (pangolin::Pushed(a_button)){//如果a_button按钮被点，就运行if里面的语句std::cout << "You tough a_buttom" << std::endl;a_double = 0;a_int = 0;}glColor3f(1.0, 1.0, 1.0);pangolin::glDrawColouredCube();pangolin::FinishFrame();}return 0;
}

Pangolin是一个很简单好用的3D显示库，加上GUI的一些交互操作简直飞起。如果你不会使用ROS或者闲ROS运行麻烦，使用Pangolin显示一些3D效果十分方便。

可以参考Pangolin官方例程进行学习。
https://github.com/stevenlovegrove/Pangolin/tree/master/examples

6 其他案例

文章来源：https://www.cnblogs.com/Balcher/p/16821096.html#_label2

6.1 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 threadpangolin::CreateWindowAndBind(window_name, 640, 480);// enabel depthglEnable(GL_DEPTH_TEST);// unset the current context from the main thread// 从主线程取消设置当前上下文// GetBoundWindow()  返回指向当前pangolin window 上下文的指针，如果没有绑定则返回nullptrpangolin::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 infoglClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);// Render OpenGL Cubepangolin::glDrawColouredCube();// Swap frames and Process Eventspangolin::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 threadsetup();// use the coontext in a separate rendering threadstd::thread render_loop;render_loop = std::thread(run);render_loop.join();return 0;
}

6.2 多视图图片显示

#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和0pangolin::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;
}

6.3 绘制位姿曲线

#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.858680.165471   0.939666    0.299410.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.0139790.00932699    0.996232  -0.0862205  -0.0130823-0.0646754   0.0866447    0.994138  -0.01086960           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;
}

6.4 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 stuffglColor3f(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;
}