SLAM——ORB-SLAM3代码分析（六）MapDrawer
发布时间
阅读量：
阅读量
2021SC@SDUSC
MapDrawer分析
今天来到带 Map 标签的最后一部分——对 MapDrawer 进行分析。在基于 Map 和 MapPoint 的基础上，采用 MapDrawer 技术来绘制地图点的方式。
首先该函数具有重要意义，并负责执行一系列关键操作。它会完成以下步骤：第一步会提取全部的地图数据；随后会提取与之相关的参考地图数据；接着会将vpRefMPs转换为集合类型（这一转换有助于快速统计集合中的元素数量）；最后会分别展示全局的地图数据及其局部区域的数据
void MapDrawer::DrawMapPoints()
{
const vector<MapPoint*> &vpMPs = mpAtlas->GetAllMapPoints();
const vector<MapPoint*> &vpRefMPs = mpAtlas->GetReferenceMapPoints();
set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());
if(vpMPs.empty())
return;
glPointSize(mPointSize);
glBegin(GL_POINTS);
glColor3f(0.0,0.0,0.0);
for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
{
if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
continue;
cv::Mat pos = vpMPs[i]->GetWorldPos();
glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));
}
glEnd();
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;
cv::Mat pos = (*sit)->GetWorldPos();
glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));
}
glEnd();
}随后介绍另一个关键函数DrawKeyFrames 。考虑到该函数拥有大量代码量较为庞大 ，为了提高可读性 以便更好地理解其运行逻辑 我会在代码中直接添加注释来进行分析 而不再将其单独列出作为外部说明
void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph)
{
//历史关键帧图标
const float &w = mKeyFrameSize;
const float h = w*0.75;
const float z = w*0.6;
// step 1：取出所有的关键帧
const vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();
// step 2：显示所有关键帧图标
//通过显示界面选择是否显示历史关键帧图标
if(bDrawKF)
{
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];
cv::Mat Twc = pKF->GetPoseInverse().t();
unsigned int index_color = pKF->mnOriginMapId;
glPushMatrix();
//因为OpenGL中的矩阵为列优先存储，因此实际为Tcw，即相机在世界坐标下的位姿
glMultMatrixf(Twc.ptr<GLfloat>(0));
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
{
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();
glEnd();
}
}
// step 3：显示所有关键帧的Essential Graph (本征图)，通过显示界面选择是否显示关键帧连接关系。
/**已知共视图中存储了所有关键帧的共视关系，本征图中对边进行了优化,
保存了所有节点,只存储了具有较多共视点的边,用于进行优化，
而生成树则进一步进行了优化,保存了所有节点,但是值保存具有最多共视地图点的关键帧的边**/
if(bDrawGraph)
{
glLineWidth(mGraphLineWidth);
glColor4f(0.0f,1.0f,0.0f,0.6f);
glBegin(GL_LINES);
for(size_t i=0; i<vpKFs.size(); i++)
{
// Covisibility Graph
// step 3.1 共视程度比较高的共视关键帧用线连接
//遍历每一个关键帧，得到它们共视程度比较高的关键帧
const vector<KeyFrame*> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100);
//遍历每一个关键帧，得到它在世界坐标系下的相机坐标
cv::Mat 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;
cv::Mat Ow2 = (*vit)->GetCameraCenter();
glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
glVertex3f(Ow2.at<float>(0),Ow2.at<float>(1),Ow2.at<float>(2));
}
}
// Spanning tree
// step 3.2 连接最小生成树
KeyFrame* pParent = vpKFs[i]->GetParent();
if(pParent)
{
cv::Mat Owp = pParent->GetCameraCenter();
glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
glVertex3f(Owp.at<float>(0),Owp.at<float>(1),Owp.at<float>(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;
cv::Mat Owl = (*sit)->GetCameraCenter();
glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
glVertex3f(Owl.at<float>(0),Owl.at<float>(1),Owl.at<float>(2));
}
}
glEnd();
}
if(bDrawInertialGraph && mpAtlas->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];
cv::Mat Ow = pKFi->GetCameraCenter();
KeyFrame* pNext = pKFi->mNextKF;
if(pNext)
{
cv::Mat Owp = pNext->GetCameraCenter();
glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
glVertex3f(Owp.at<float>(0),Owp.at<float>(1),Owp.at<float>(2));
}
}
glEnd();
}
vector<Map*> vpMaps = mpAtlas->GetAllMaps();
if(bDrawKF)
{
for(Map* pMap : vpMaps)
{
if(pMap == mpAtlas->GetCurrentMap())
continue;
vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];
cv::Mat Twc = pKF->GetPoseInverse().t();
unsigned int index_color = pKF->mnOriginMapId;
glPushMatrix();
glMultMatrixf(Twc.ptr<GLfloat>(0));
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 功能模块，在初始化阶段设置了历史关键帧图标，并根据绘图参数如宽度、颜色等具体绘制点如何连接进行了详细参数设置。与之前分析过的功能模块类似，在此从略不做详细代码展示。
- 最后实现了SetCurrentCameraPose 功能模块负责对当前相机位姿进行配置，并主要目的是为了优化多线程操作性能。
void MapDrawer::SetCurrentCameraPose(const cv::Mat &Tcw)
{
unique_lock<mutex> lock(mMutexCamera);
mCameraPose = Tcw.clone();
}该函数负责将相机姿态mCameraPose从Mat数据格式转换为OpenGl矩阵格式
这一部分主要介绍了MapDrawer类的主要内容，在功能上与Map和MapPoint相比相对较少但作用明确非常简洁明了。接下来将对带map tag的三个类进行详细分析然后介绍Converter类的相关内容
全部评论 (0)
还没有任何评论哟~