0. 简介
在slam当中，我们在回环的时候会使用关键帧的概念，而关键帧怎么样去可视化，并将可视化的关键帧在RVIZ中显示，对于我们理解这一概念非常重要，当然目前有很多SLAM算法中已经插入了这一点，但是这一点的操作仍然值得我们去了解学习。一般在论文中我们称这类方法为Covisibility Graph可视化，它是一个无向有权图(graph),这个概念最早来自2010的文章《Closing Loops Without Places》。
1. 从ORB-SLAM3来学习关键帧绘制
在ORB-SLAM3中我们看到的地图显示界面就是VIew线程来负责显示的。如下图所示：
这个图中有两个窗口，上边的窗口一直在显示一张一张的图片，图片中绿色方形和圆圈标注的就是该图像中提取的ORB特征点。下边的窗口用于显示相机的位姿（也就是关键帧）和地图点（每个地图点和图像中的特征点有对应关系），其中绿色的为当前相机的展示，蓝色的是历史相机位姿展示。缩小一下窗口，可以看到相机走过的轨迹，如下图所示：
主要内容是在MapDrawer.cpp这个函数当中添加构造函数以及参数读取信息
MapDrawer::MapDrawer(Atlas *pAtlas, const string &strSettingPath, Settings *settings) : mpAtlas(pAtlas) // 构造函数
{
if (settings) // 如果settings不为空
{
newParameterLoader(settings); // 调用newParameterLoader函数
}
else // 如果settings为空
{
cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); // 读取配置文件
bool is_correct = ParseViewerParamFile(fSettings); // 调用ParseViewerParamFile函数
if (!is_correct) // 如果is_correct为false
{
std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl; // 输出错误信息
try
{
throw -1; // 抛出异常
}
catch (exception &e)
{
}
}
}
}
void MapDrawer::newParameterLoader(Settings *settings)// 读取配置文件
{
mKeyFrameSize = settings->keyFrameSize();// 读取关键帧大小
mKeyFrameLineWidth = settings->keyFrameLineWidth();// 读取关键帧线宽
mGraphLineWidth = settings->graphLineWidth();// 读取图线宽
mPointSize = settings->pointSize();// 读取点大小
mCameraSize = settings->cameraSize();// 读取相机大小
mCameraLineWidth = settings->cameraLineWidth();// 读取相机线宽
}
bool MapDrawer::ParseViewerParamFile(cv::FileStorage &fSettings)// 读取可视化配置文件
{
bool b_miss_params = false;// 是否缺少参数
cv::FileNode node = fSettings["Viewer.KeyFrameSize"];// 读取关键帧大小
if (!node.empty())// 如果不为空
{
mKeyFrameSize = node.real();// 关键帧大小传入mKeyFrameSize
}
else
{
std::cerr << "*Viewer.KeyFrameSize parameter doesn't exist or is not a real number*" << std::endl;
b_miss_params = true;
}
node = fSettings["Viewer.KeyFrameLineWidth"];// 读取关键帧线宽
if (!node.empty())
{
mKeyFrameLineWidth = node.real();
}
else
{
std::cerr << "*Viewer.KeyFrameLineWidth parameter doesn't exist or is not a real number*" << std::endl;
b_miss_params = true;
}
node = fSettings["Viewer.GraphLineWidth"];// 读取图线宽
if (!node.empty())
{
mGraphLineWidth = node.real();
}
else
{
std::cerr << "*Viewer.GraphLineWidth parameter doesn't exist or is not a real number*" << std::endl;
b_miss_params = true;
}
node = fSettings["Viewer.PointSize"];// 读取点大小
if (!node.empty())
{
mPointSize = node.real();
}
else
{
std::cerr << "*Viewer.PointSize parameter doesn't exist or is not a real number*" << std::endl;
b_miss_params = true;
}
node = fSettings["Viewer.CameraSize"];// 读取相机大小
if (!node.empty())
{
mCameraSize = node.real();
}
else
{
std::cerr << "*Viewer.CameraSize parameter doesn't exist or is not a real number*" << std::endl;
b_miss_params = true;
}
node = fSettings["Viewer.CameraLineWidth"];// 读取相机线宽
if (!node.empty())
{
mCameraLineWidth = node.real();
}
else
{
std::cerr << "*Viewer.CameraLineWidth parameter doesn't exist or is not a real number*" << std::endl;
b_miss_params = true;
}
return !b_miss_params;// 返回是否缺少参数
}
然后下面就是DrawMapPoints函数，通过取出所有的地图点，再取出mvpReferenceMapPoints，接着将vpRefMPs从vector容器类型转化为set容器类型，便于使用set::count（set::count用于返回集合中为某个值的元素的个数）快速统计，最后分别显示所有地图点和局部地图点。
// 关于gl相关的函数，可直接google, 并加上msdn关键词
void MapDrawer::DrawMapPoints()
{
Map *pActiveMap = mpAtlas->GetCurrentMap(); // 获取当前地图
if (!pActiveMap) // 如果当前地图为空
return;
const vector<MapPoint *> &vpMPs = pActiveMap->GetAllMapPoints(); // 获取所有地图点
const vector<MapPoint *> &vpRefMPs = pActiveMap->GetReferenceMapPoints(); // 获取参考地图点
// 将vpRefMPs从vector容器类型转化为set容器类型，便于使用set::count快速统计
set<MapPoint *> spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); // 将参考地图点转换为set
if (vpMPs.empty()) // 如果地图点为空
return; // 返回
// 显示所有的地图点（不包括局部地图点），大小为2个像素，黑色
glPointSize(mPointSize); // 设置点大小
glBegin(GL_POINTS); // 开始绘制点
glColor3f(0.0, 0.0, 0.0); // 设置颜色
for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) // 遍历所有地图点
{
// 不包括ReferenceMapPoints（局部地图点）
if (vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i])) // 如果地图点为空或者是局部地图点
continue;
Eigen::Matrix<float, 3, 1> pos = vpMPs[i]->GetWorldPos(); // 获取地图点的世界坐标
glVertex3f(pos(0), pos(1), pos(2)); // 绘制点
}
glEnd(); // 结束绘制点
// 显示局部地图点，大小为2个像素，红色
glPointSize(mPointSize);
glBegin(GL_POINTS);
glColor3f(1.0, 0.0, 0.0);
for (set<MapPoint *>::iterator sit = spRefMPs.begin(), send = spRefMPs.end(); sit != send; sit++) // 遍历局部地图点,与上面同理
{
if ((*sit)->isBad())
continue;
Eigen::Matrix<float, 3, 1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0), pos(1), pos(2));
}
glEnd();
}
接下来是另一个非常重要的函数DrawKeyFrames。该函数主要用于绘制关键帧的状态，包括位姿、三维点云等信息，并且将它们通过连线链接起来。这些连线表示关键帧之间的关联，从而形成一个图结构。ORB-SLAM3中的这个函数是实现视觉SLAM中的图优化的关键部分之一。在SLAM过程中，我们需要根据相机的运动和三维空间中的点云，不断优化图结构中的位姿和地图信息，从而得到更加精确的相机位姿和场景重建结果。DrawKeyFrames函数就可以清晰的让读者了解关键帧的信息。
// 绘制关键帧, bDrawKF为true时绘制关键帧，bDrawGraph为true时绘制关键帧之间的连线, bDrawInertialGraph为true时绘制IMU预积分连线, bDrawOptLba为true时绘制优化后的关键帧
void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph, const bool bDrawOptLba)
{
// 历史关键帧图标
const float &w = mKeyFrameSize; // 设置关键帧图标的大小
const float h = w * 0.75; // 设置关键帧图标的高度
const float z = w * 0.6; // 设置关键帧图标的深度
// step 1：取出所有的关键帧
Map *pActiveMap = mpAtlas->GetCurrentMap(); // 获取当前地图
// DEBUG LBA
std::set<long unsigned int> sOptKFs = pActiveMap->msOptKFs; // 优化后的关键帧
std::set<long unsigned int> sFixedKFs = pActiveMap->msFixedKFs; // 固定的关键帧
if (!pActiveMap) // 如果当前地图为空
return; // 返回
const vector<KeyFrame *> vpKFs = pActiveMap->GetAllKeyFrames(); // 获取所有关键帧
// step 2：显示所有关键帧图标
if (bDrawKF) // 如果绘制关键帧开关为true
{
for (size_t i = 0; i < vpKFs.size(); i++) // 遍历所有关键帧
{
KeyFrame *pKF = vpKFs[i]; // 取出关键帧
Eigen::Matrix4f Twc = pKF->GetPoseInverse().matrix(); // 获取关键帧的位姿
glPushMatrix(); // 保存当前矩阵
// 因为OpenGL中的矩阵为列优先存储，因此实际为Tcw，即相机在世界坐标下的位姿
glMultMatrixf((GLfloat *)Twc.data()); // 设置当前矩阵为Twc,Twc为关键帧的位姿
if (!pKF->GetParent()) // It is the first KF in the map
{
// 设置绘制图形时线的宽度
glLineWidth(mKeyFrameLineWidth * 5);
glColor3f(1.0f, 0.0f, 0.0f);
// 用线将下面的顶点两两相连
glBegin(GL_LINES);
}
else
{
// cout << "Child KF: " << vpKFs[i]->mnId << endl;
glLineWidth(mKeyFrameLineWidth); // 设置绘制图形时线的宽度
if (bDrawOptLba) // 如果绘制优化后的关键帧开关为true
{
if (sOptKFs.find(pKF->mnId) != sOptKFs.end()) // 如果该关键帧在优化后的关键帧中
{
glColor3f(0.0f, 1.0f, 0.0f); // 将KF关键帧图标设置为绿色
}
else if (sFixedKFs.find(pKF->mnId) != sFixedKFs.end())
{
glColor3f(1.0f, 0.0f, 0.0f); // 将KF关键帧图标设置为红色
}
else
{
glColor3f(0.0f, 0.0f, 1.0f); // 基础颜色，将KF关键帧图标设置为蓝色
}
}
else
{
glColor3f(0.0f, 0.0f, 1.0f); // 基础颜色，将KF关键帧图标设置为蓝色
}
glBegin(GL_LINES);
}
// 下面的这些代码就是为了绘制关键帧的图标
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(); // 恢复之前的矩阵
glEnd();
}
}
// step 3：显示所有关键帧的Essential Graph (本征图)，通过显示界面选择是否显示关键帧连接关系。
/**已知共视图中存储了所有关键帧的共视关系，本征图中对边进行了优化,
保存了所有节点,只存储了具有较多共视点的边,用于进行优化，
而生成树则进一步进行了优化,保存了所有节点,但是值保存具有最多共视地图点的关键帧的边**/
if (bDrawGraph)
{
glLineWidth(mGraphLineWidth); // 设置线的宽度
glColor4f(0.0f, 1.0f, 0.0f, 0.6f);
glBegin(GL_LINES);
// cout << "-----------------Draw graph-----------------" << endl;
for (size_t i = 0; i < vpKFs.size(); i++)
{
// Covisibility Graph
// step 3.1 共视程度比较高的共视关键帧用线连接
// 遍历每一个关键帧，得到它们共视程度比较高的关键帧
const vector<KeyFrame *> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100);
// 遍历每一个关键帧，得到它在世界坐标系下的相机坐标
Eigen::Vector3f Ow = vpKFs[i]->GetCameraCenter();
if (!vCovKFs.empty()) // 如果共视的关键帧找到了
{
for (vector<KeyFrame *>::const_iterator vit = vCovKFs.begin(), vend = vCovKFs.end(); vit != vend; vit++) // 循环所有的共视信息
{
// 单向绘制
if ((*vit)->mnId < vpKFs[i]->mnId)
continue;
Eigen::Vector3f Ow2 = (*vit)->GetCameraCenter(); // 得到共视关键帧在世界坐标系下的相机坐标
glVertex3f(Ow(0), Ow(1), Ow(2)); // 设置顶点是关键帧的中心
glVertex3f(Ow2(0), Ow2(1), Ow2(2)); // 设置顶点是关键帧图标的右上角
}
}
// Spanning tree
// step 3.2 连接最小生成树
KeyFrame *pParent = vpKFs[i]->GetParent(); // 得到该关键帧的父节点
if (pParent) // 如果父节点存在
{
Eigen::Vector3f Owp = pParent->GetCameraCenter(); // 得到父节点在世界坐标系下的相机坐标
glVertex3f(Ow(0), Ow(1), Ow(2)); // 设置顶点是关键帧的中心
glVertex3f(Owp(0), Owp(1), Owp(2)); // 设置顶点是关键帧图标的右上角
}
// Loops
// step 3.3 连接闭环时形成的连接关系
set<KeyFrame *> sLoopKFs = vpKFs[i]->GetLoopEdges(); // 得到该关键帧形成的闭环关系
for (set<KeyFrame *>::iterator sit = sLoopKFs.begin(), send = sLoopKFs.end(); sit != send; sit++) // 遍历所有的闭环关系
{
if ((*sit)->mnId < vpKFs[i]->mnId) // 单向绘制
continue;
Eigen::Vector3f Owl = (*sit)->GetCameraCenter();
glVertex3f(Ow(0), Ow(1), Ow(2));
glVertex3f(Owl(0), Owl(1), Owl(2));
}
}
glEnd();
}
if (bDrawInertialGraph && pActiveMap->isImuInitialized()) // 如果显示惯性图
{
glLineWidth(mGraphLineWidth); // 设置线的宽度
glColor4f(1.0f, 0.0f, 0.0f, 0.6f);
glBegin(GL_LINES);
// Draw inertial links
for (size_t i = 0; i < vpKFs.size(); i++) // 遍历所有的关键帧
{
KeyFrame *pKFi = vpKFs[i]; // 得到关键帧
Eigen::Vector3f Ow = pKFi->GetCameraCenter(); // 得到关键帧在世界坐标系下的相机坐标
KeyFrame *pNext = pKFi->mNextKF; // 得到关键帧的下一个关键帧
if (pNext) // 如果下一个关键帧存在
{
Eigen::Vector3f Owp = pNext->GetCameraCenter(); // 得到下一个关键帧在世界坐标系下的相机坐标
glVertex3f(Ow(0), Ow(1), Ow(2));
glVertex3f(Owp(0), Owp(1), Owp(2));
}
}
glEnd();
}
vector<Map *> vpMaps = mpAtlas->GetAllMaps(); // 得到所有的地图
if (bDrawKF) // 如果显示关键帧
{
for (Map *pMap : vpMaps) // 遍历所有的地图
{
if (pMap == pActiveMap) // 如果是当前地图
continue;
vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames(); // 得到所有的关键帧
for (size_t i = 0; i < vpKFs.size(); i++) // 遍历所有的关键帧
{
KeyFrame *pKF = vpKFs[i]; // 得到关键帧
Eigen::Matrix4f Twc = pKF->GetPoseInverse().matrix(); // 得到关键帧的逆变换矩阵
unsigned int index_color = pKF->mnOriginMapId; // 得到关键帧的颜色
glPushMatrix(); // 保存当前的矩阵
glMultMatrixf((GLfloat *)Twc.data()); // 设置矩阵
if (!vpKFs[i]->GetParent()) // It is the first KF in the map// 如果是地图的第一个关键帧
{
glLineWidth(mKeyFrameLineWidth * 5); // 设置线的宽度
glColor3f(1.0f, 0.0f, 0.0f); // 设置颜色
glBegin(GL_LINES); // 开始绘制线
}
else
{
glLineWidth(mKeyFrameLineWidth); // 设置线的宽度
glColor3f(mfFrameColors[index_color][0], mfFrameColors[index_color][1], mfFrameColors[index_color][2]); // 设置颜色
glBegin(GL_LINES); // 开始绘制线
}
// 画关键帧的图标
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();
}
}
}
}
下一个函数是DrawCurrentCamera，该函数负责绘制当前相机的状态和位置，与前面的DrawKeyFrames函数相似，首先设置了历史关键帧的图标，然后进行了一系列绘图时的宽度、颜色、具体绘制点如何连接的设置。具体而言，它使用了OpenGL的矩阵堆栈功能来将相机的姿态和位置变换到观察坐标系中。然后它使用glLineWidth函数设置线条宽度，并使用glColor3f函数设置线条颜色。最后，它使用glBegin和glVertex3f函数来绘制相机的视锥体，以及相机的三个坐标轴，以便于我们更直观地观察相机的位置和方向。
// 画当前相机的位置
void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc)
{
const float &w = mCameraSize;
const float h = w * 0.75;
const float z = w * 0.6;
glPushMatrix(); // 保存当前的矩阵
#ifdef HAVE_GLES // 如果有GLES
glMultMatrixf(Twc.m); // 设置矩阵,使用的是float类型的矩阵
#else
glMultMatrixd(Twc.m); // 设置矩阵,使用的是double类型的矩阵
#endif
glLineWidth(mCameraLineWidth);
glColor3f(0.0f, 1.0f, 0.0f);
glBegin(GL_LINES);
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();
}
下一个函数是 SetCurrentCameraPose，它主要负责设置当前相机的位姿。由于 ORB-SLAM3 是多线程的，因此需要在处理线程之间进行同步，以确保在不同线程之间正确设置相机位姿。函数中还包括了其他的一些细节，例如更新当前帧的投影和位姿，以及更新观察到的特征点。这些步骤都是为了确保 ORB-SLAM3 能够高效且准确地处理图像和特征点。
// 设置当前相机的位姿
void MapDrawer::SetCurrentCameraPose(const Sophus::SE3f &Tcw)
{
unique_lock<mutex> lock(mMutexCamera); // 锁住相机的互斥量
mCameraPose = Tcw.inverse(); // 转换相机的位姿
}
GetCurrentOpenGLCameraMatrix函数是将相机位姿mCameraPose由Mat类型转化为OpenGlMatrix类型，具体实现中，该函数会将相机位姿矩阵mCameraPose分解为旋转矩阵和平移向量。
// 得到当前OpenGL的相机矩阵
void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw)
{
Eigen::Matrix4f Twc;
{
unique_lock<mutex> lock(mMutexCamera); // 锁住相机的互斥量
Twc = mCameraPose.matrix(); // 得到相机的位姿
}
for (int i = 0; i < 4; i++) // 设置矩阵，M当中存放了相机的旋转矩阵
{
M.m[4 * i] = Twc(0, i);
M.m[4 * i + 1] = Twc(1, i);
M.m[4 * i + 2] = Twc(2, i);
M.m[4 * i + 3] = Twc(3, i);
}
MOw.SetIdentity(); // 设置矩阵,MOW则是相机的平移矩阵
MOw.m[12] = Twc(0, 3);
MOw.m[13] = Twc(1, 3);
MOw.m[14] = Twc(2, 3);
}
2. 对关键帧进行 pcl 保存点云
对于ORB-SLAM而言其实在此处也可以完成关键帧点云的保存
第一步，先加入 pcl 保存点云所需的头文件：
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
第二步，找到 DrawMapPoints 函数，并将其修改为如下代码：
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_saved(new pcl::PointCloud<pcl::PointXYZ>());
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
//modified by Awei
pcl::PointXYZ p;
p.x = pos(0);
p.y = pos(1);
p.z = pos(2);
cloud_saved->points.push_back(p);
}
if (cloud_saved->points.size())
pcl::io::savePCDFileBinary("map.pcd", *cloud_saved);
然后在 CMakeLists.txt文件中添加 PCL 库即可，一共三段
#1、
find_package(PCL REQUIRED)
#2、
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${PROJECT_SOURCE_DIR}/include/CameraModels
${PROJECT_SOURCE_DIR}/Thirdparty/Sophus
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
#3、
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PCL_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
-lboost_serialization
-lcrypto
)
3. 参考链接
https://blog.csdn.net/weixin_39752599/article/details/90314096
评论（0）
您还未登录，请登录后发表或查看评论