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

    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_TEST4864跟据函数glAlphaFunc的条件要求来决定图形透明的层度是否显示。
GL_AUTO_NORMAL3456执行后,图形能把光反射到各个方向
GL_BLEND3042启用颜色混合。例如实现半透明效果
GL_CLIP_PLANE0 ~ GL_CLIP_PLANE512288 ~ 12283根据函数glClipPlane的条件要求
启用图形切割管道。这里指六种缓存管道
GL_COLOR_LOGIC_OP3058启用每一像素的色彩为位逻辑运算
GL_COLOR_MATERIAL2930执行后,图形(材料)将根据光线的照耀进行反射。
反射要求由函数glColorMaterial进行设定。
GL_CULL_FACE2884根据函数glCullFace要求启用隐藏图形材料的面。
GL_DEPTH_TEST2929启用深度测试。
根据坐标的远近自动隐藏被遮住的图形(材料)
GL_DITHER3024启用抖动
GL_FOG2912雾化效果
例如距离越远越模糊
GL_INDEX_LOGIC_OP3057逻辑操作
GL_LIGHT0 ~ GL_LIGHT716384 ~ 16391启用0号灯到7号灯(光源)
光源要求由函数glLight函数来完成
GL_LIGHTING2896启用灯源
GL_LINE_SMOOTH2848执行后,过虑线段的锯齿
GL_LINE_STIPPLE2852执行后,画虚线
GL_LOGIC_OP3057逻辑操作
GL_MAP1_COLOR_43472根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
生成RGBA曲线
GL_MAP1_INDEX3473根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
生成颜色索引曲线
GL_MAP1_NORMAL3474根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
生成法线
GL_MAP1_TEXTURE_COORD_13475根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
生成文理坐标
GL_MAP1_TEXTURE_COORD_23476根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
生成文理坐标
GL_MAP1_TEXTURE_COORD_33477根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
生成文理坐标
GL_MAP1_TEXTURE_COORD_43478根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
生成文理坐标
GL_MAP1_VERTEX_33479根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
在三维空间里生成曲线
GL_MAP1_VERTEX_43480根据函数Map1对贝赛尔曲线的设置,
启用glEvalCoord1,glEvalMesh1,glEvalPoint1
在四维空间里生成法线
GL_MAP2_COLOR_43504根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
生成RGBA曲线
GL_MAP2_INDEX3505根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
生成颜色索引
GL_MAP2_NORMAL3506根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
生成法线
GL_MAP2_TEXTURE_COORD_13507根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
生成纹理坐标
GL_MAP2_TEXTURE_COORD_23508根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
生成纹理坐标
GL_MAP2_TEXTURE_COORD_33509根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
生成纹理坐标
GL_MAP2_TEXTURE_COORD_43510根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
生成纹理坐标
GL_MAP2_VERTEX_33511根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
在三维空间里生成曲线
GL_MAP2_VERTEX_43512根据函数Map2对贝赛尔曲线的设置,
启用glEvalCoord2,glEvalMesh2,glEvalPoint2
在三维空间里生成曲线
GL_NORMALIZE2977根据函数glNormal的设置条件,启用法向量
GL_POINT_SMOOTH2832执行后,过虑线点的锯齿
GL_POLYGON_OFFSET_FILL32823根据函数glPolygonOffset的设置,启用面的深度偏移
GL_POLYGON_OFFSET_LINE10754根据函数glPolygonOffset的设置,启用线的深度偏移
GL_POLYGON_OFFSET_POINT10753根据函数glPolygonOffset的设置,启用点的深度偏移
GL_POLYGON_SMOOTH2881过虑图形(多边形)的锯齿
GL_POLYGON_STIPPLE2882执行后,多边形为矢量画图
GL_SCISSOR_TEST3089根据函数glScissor设置,启用图形剪切
GL_STENCIL_TEST2960
GL_TEXTURE_1D3552启用一维文理
GL_TEXTURE_2D3553启用二维文理
GL_TEXTURE_GEN_Q3171根据函数glTexGen,启用纹理处理
GL_TEXTURE_GEN_R3170根据函数glTexGen,启用纹理处理
GL_TEXTURE_GEN_S3168根据函数glTexGen,启用纹理处理
GL_TEXTURE_GEN_T3169根据函数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 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;
}

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

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

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

  • 10
    点赞
  • 55
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

几度春风里

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值