目录
3.4 pangolin::OpenGlRenderState构建观察相机对象
3.6 pangolin::ShouldQuit()检测是否关闭OpenGL窗口
3.9 pangolin::FinishFrame()视窗刷新
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_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 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", ×tamp, 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;
}