【pangolin】【pangolin实践】【pangolin的学习使用记录】
0 前言
1 pangolin的使用说明
1.1 CMakeLists.txt的使用说明
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(xxx src/xxx.cpp)
target_link_libraries(xxx ${Pangolin_LIBRARIES})
1.2 头文件的使用
#include <pangolin/pangolin.h>
1.3 代码的使用
1.3.1 简单例子
-
参考:【slam十四讲第二版】【课本例题代码向】【第三~四讲刚体运动、李群和李代数】【eigen3.3.4和pangolin安装,Sophus及fim的安装使用】【绘制轨迹】【计算以及介绍轨迹误差】的3 显示运动轨迹
-
由于这里把使用pangolin模块写成了一个子函数,所以这里main函数就不介绍了,可以自己去看
- 函数声明
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
- 输入参数
vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>
:是许多点的位姿集合
- 函数实现
/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
// create pangolin window and plot the trajectory
/*
* 接下来,我们使用CreateWindowAndBind命令创建了一个视窗对象,
* 函数的入口的参数依次为视窗的名称、宽度和高度,
* 该命令类似于OpenCV中的namedWindow,即创建一个用于显示的窗体。
*/
// 创建名称为“Trajectory Viewer”的GUI窗口,尺寸为640×640
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
//启动深度测试。同时,我们启动了深度测试功能,
// 该功能会使得pangolin只会绘制朝向镜头的那一面像素点,避免容易混淆的透视关系出现,因此在任何3D可视化中都应该开启该功能。
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// 创建一个观察相机视图
pangolin::OpenGlRenderState s_cam(
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)
);
// 创建一个观察相机视图
// ProjectMatrix(int w, int h, 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)
// 参数依次为相机所在的位置,以及相机所看的视点位置(一般会设置在原点)
//在完成视窗的创建后,我们需要在视窗中“放置”一个摄像机(注意这里的摄像机是用于观察的摄像机而非SLAM中的相机),
// 我们需要给出摄像机的内参矩阵ProjectionMatrix从而在我们对摄像机进行交互操作时,
// Pangolin会自动根据内参矩阵完成对应的透视变换。
// 此外,我们还需要给出摄像机初始时刻所处的位置,摄像机的视点位置(即摄像机的光轴朝向哪一个点)以及摄像机的本身哪一轴朝上。
// 创建交互视图
//pangolin::Handler3D handler(s_cam); //交互相机视图句柄
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));//.SetHandler(&handler);
//接下来我们需要创建一个交互式视图(view)用于显示上一步摄像机所“拍摄”到的内容,这一步类似于OpenGL中的viewport处理。
// setBounds()函数前四个参数依次表示视图在视窗中的范围(下、上、左、右),可以采用相对坐标(0~1)以及绝对坐标(使用Attach对象)。
while (pangolin::ShouldQuit() == false) {
// 清空颜色和深度缓存
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);//激活
//在完成了上述所有准备工作之后,我们就可以开始绘制我们需要的图形了,
// 首先我们使用glclear命令分别清空色彩缓存和深度缓存
// 并激活之前设定好的视窗对象(否则视窗内会保留上一帧的图形,这种“多重曝光”效果通常并不是我们需要的)
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glLineWidth(2);//定义其中线条的宽度
for (size_t i = 0; i < poses.size(); i++) {
// 画每个位姿的三个坐标轴
Vector3d Ow = poses[i].translation();
Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
glBegin(GL_LINES);
glColor3f(1.0, 0.0, 0.0);
glVertex3d(Ow[0], Ow[1], Ow[2]);
glVertex3d(Xw[0], Xw[1], Xw[2]);
glColor3f(0.0, 1.0, 0.0);
glVertex3d(Ow[0], Ow[1], Ow[2]);
glVertex3d(Yw[0], Yw[1], Yw[2]);
glColor3f(0.0, 0.0, 1.0);
glVertex3d(Ow[0], Ow[1], Ow[2]);
glVertex3d(Zw[0], Zw[1], Zw[2]);
glEnd();
}
// 画出连线
for (size_t i = 0; i < poses.size(); i++) {
glColor3f(0.0, 0.0, 0.0);
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[i + 1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
//在绘制完成后,需要使用FinishFrame命令刷新视窗。
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
}
1.3.1.1 代码剖析
1.3.1.1.1 初始化pangolin
/*
* 接下来,我们使用CreateWindowAndBind命令创建了一个视窗对象,
* 函数的入口的参数依次为视窗的名称、宽度和高度,
* 该命令类似于OpenCV中的namedWindow,即创建一个用于显示的窗体。
*/
// 创建名称为“Trajectory Viewer”的GUI窗口,尺寸为640×640
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
//启动深度测试。同时,我们启动了深度测试功能,
// 该功能会使得pangolin只会绘制朝向镜头的那一面像素点,避免容易混淆的透视关系出现,因此在任何3D可视化中都应该开启该功能。
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
:创建名称为“Trajectory Viewer”的GUI窗口,尺寸为640×640;函数的入口的参数依次为视窗的名称、宽度和高度, 该命令类似于OpenCV中的namedWindow,即创建一个用于显示的窗体。glEnable(GL_DEPTH_TEST);
:启动了深度测试功能,该功能会使得pangolin只会绘制朝向镜头的那一面像素点,避免容易混淆的透视关系出现,因此在任何3D可视化中都应该开启该功能。
1.3.1.1.2 创建一个观察相机视图
// 创建一个观察相机视图
pangolin::OpenGlRenderState s_cam(
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)
);
// 创建一个观察相机视图
// ProjectMatrix(int w, int h, 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)
// 参数依次为相机所在的位置,以及相机所看的视点位置(一般会设置在原点)
//在完成视窗的创建后,我们需要在视窗中“放置”一个摄像机(注意这里的摄像机是用于观察的摄像机而非SLAM中的相机),
// 我们需要给出摄像机的内参矩阵ProjectionMatrix从而在我们对摄像机进行交互操作时,
// Pangolin会自动根据内参矩阵完成对应的透视变换。
// 此外,我们还需要给出摄像机初始时刻所处的位置,摄像机的视点位置(即摄像机的光轴朝向哪一个点)以及摄像机的本身哪一轴朝上。
ProjectMatrix(int w, int h, 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)
:参数依次为相机所在的位置,以及相机所看的视点位置(一般会设置在原点)- 需要在视窗中“放置”一个摄像机(注意这里的摄像机是用于观察的摄像机而非SLAM中的相机),给出摄像机的内参矩阵ProjectionMatrix从而在我们对摄像机进行交互操作时,Pangolin会自动根据内参矩阵完成对应的透视变换。
- 我们还需要给出摄像机初始时刻所处的位置,摄像机的视点位置(即摄像机的光轴朝向哪一个点)以及摄像机的本身哪一轴朝上。
1.3.1.1.3 创建交互视图
// 创建交互视图
//pangolin::Handler3D handler(s_cam); //交互相机视图句柄
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));//.SetHandler(&handler);
//接下来我们需要创建一个交互式视图(view)用于显示上一步摄像机所“拍摄”到的内容,这一步类似于OpenGL中的viewport处理。
// setBounds()函数前四个参数依次表示视图在视窗中的范围(下、上、左、右),可以采用相对坐标(0~1)以及绝对坐标(使用Attach对象)。
- 创建一个交互式视图(view)用于显示上一步摄像机所“拍摄”到的内容
setBounds()
:前四个参数依次表示视图在视窗中的范围(下、上、左、右),可以采用相对坐标(0~1)以及绝对坐标(使用Attach对象)
1.3.1.1.4 绘图循环
绘制图形的操作是在while()循环里面完成的
while (pangolin::ShouldQuit() == false) {
// 清空颜色和深度缓存
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);//激活
//在完成了上述所有准备工作之后,我们就可以开始绘制我们需要的图形了,
// 首先我们使用glclear命令分别清空色彩缓存和深度缓存
// 并激活之前设定好的视窗对象(否则视窗内会保留上一帧的图形,这种“多重曝光”效果通常并不是我们需要的)
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);//设定背景的颜色,此处为白色
//glLineWidth(2);//定义其中线条的宽度
//这里省略画图的代码,会在下面展示
//在绘制完成后,需要使用FinishFrame命令刷新视窗。
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
是清空颜色和深度缓存,否则视窗内会保留上一帧的图形,这种“多重曝光”效果通常并不是我们需要的d_cam.Activate(s_cam);
是激活之前设定好的视窗对象glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
:设定背景的颜色,此处为白色pangolin::FinishFrame();
:在绘制完成后,需要使用FinishFrame命令刷新视窗。
1.3.1.1.5 绘图操作
1.3.1.1.5.1 根据点的位姿绘制器坐标轴
glLineWidth(2);//定义其中线条的宽度
for (size_t i = 0; i < poses.size(); i++) {
// 画每个位姿的三个坐标轴
Vector3d Ow = poses[i].translation();
Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
glBegin(GL_LINES);
glColor3f(1.0, 0.0, 0.0);
glVertex3d(Ow[0], Ow[1], Ow[2]);
glVertex3d(Xw[0], Xw[1], Xw[2]);
glColor3f(0.0, 1.0, 0.0);
glVertex3d(Ow[0], Ow[1], Ow[2]);
glVertex3d(Yw[0], Yw[1], Yw[2]);
glColor3f(0.0, 0.0, 1.0);
glVertex3d(Ow[0], Ow[1], Ow[2]);
glVertex3d(Zw[0], Zw[1], Zw[2]);
glEnd();
}
1.3.1.1.5.2 绘制两个位姿之间的连线(位姿)
// 画出连线
for (size_t i = 0; i < poses.size(); i++) {
glColor3f(0.0, 0.0, 0.0);
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[i + 1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
1.3.1.1.5.3 画点
glPointSize(2);//设定点的尺寸
glBegin(GL_POINTS);//设定为点的绘制
for (auto& landmark : active_landmarks_) {//获取各当前活跃路标点
Eigen::Matrix<double, 3, 1> pos = landmark.second->Pos();//获取该路标点3D坐标
glColor3f(red[0], red[1], red[2]);
glVertex3d(pos[0], pos[1], pos[2]);
}
glEnd();
1.3.1.1.6 进阶操作
1.3.1.1.6.1 设定暂时原点
- 我自己起的名是暂时原点,因为他不改变真正的原点,可以恢复原样,如果理解不对请多多指出
- 主要功能就是:实现了之后的位置都是在这个暂时原点为中心去绘制的,会自动转换到原来的中心下
- 用途:知道不断变幻的相机位姿之后,绘制东西用这种方式就比较简单
- 精简版必要代码
glPushMatrix();//必须,类似于标志
Sophus::Matrix4f m = Twc.matrix().template cast<float>();//定义所能接受的位姿格式为 4*4矩阵
glMultMatrixf((GLfloat*)m.data());//设定原点
//,,,,,此处省略,类似于1.3.1.1.5.1和1.3.1.1.5.2
glPopMatrix();//删除这个暂时的原点
}
- 完整版代码
void Viewer::DrawFrame(Frame::Ptr frame, const float* color) {
SE3 Twc = frame->Pose().inverse();//此处获取的是相机在世界坐标系下的位姿
const float sz = 1.0;//深度
const int line_width = 2.0;//线宽
//相机内参
const float fx = 400;
const float fy = 400;
const float cx = 512;
const float cy = 384;
const float width = 1080;
const float height = 768;
glPushMatrix();//必须,类似于标志
Sophus::Matrix4f m = Twc.matrix().template cast<float>();//定义所能接受的位姿格式为 4*4矩阵
glMultMatrixf((GLfloat*)m.data());//设定原点
if (color == nullptr) {
glColor3f(1, 0, 0);
} else
glColor3f(color[0], color[1], color[2]);
glLineWidth(line_width);
glBegin(GL_LINES);
glVertex3f(0, 0, 0);
glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
glVertex3f(0, 0, 0);
glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
glVertex3f(0, 0, 0);
glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
glVertex3f(0, 0, 0);
glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
glEnd();
glPopMatrix();//删除这个暂时的原点
}
1.3.1.1.6.2 重新设定新的视角
SE3 Twc = current_frame_->Pose().inverse();// SE3格式的位姿
pangolin::OpenGlMatrix m(Twc.matrix());//定义还是需要4*4的matrix矩阵形式
vis_camera.Follow(m, true);
1.3.2 pangolin在多线程中的使用
- 这里用到再说,先知道有这种用法,参考SLAM可视化绘图库——Pangolin教程(一)的Task2:Pangolin与多线程