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函数来绘制相机的视锥体,以及相机的三个坐标轴,以便于我们更直观地观察相机的位置和方向。