SLAM——ORB-SLAM3代码分析（六）MapDrawer

最新推荐文章于 2024-01-22 09:23:22 发布

Jevies

最新推荐文章于 2024-01-22 09:23:22 发布

阅读量654

点赞数 1

文章标签： c++

本文链接：https://blog.csdn.net/qq_45794281/article/details/121193307

版权

2021SC@SDUSC

MapDrawer分析

今天来到带Map tag的最后一部分——MapDrawer的分析。MapDrawer是在Map和MapPoint的基础上进行地图点的绘制。

首先一个很重要的函数是DrawMapPoints，取出所有的地图点，再取出mvpReferenceMapPoints，接着将vpRefMPs从vector容器类型转化为set容器类型，便于使用set::count（set::count用于返回集合中为某个值的元素的个数）快速统计，最后分别显示所有地图点和局部地图点。

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();
}

GetCurrentOpenGLCameraMatrix函数是将相机位姿mCameraPose由Mat类型转化为OpenGlMatrix类型

以上就是MapDrawer类的大致内容，相比Map和MapPoint，它的函数较少，不过也作用分明，十分简明。
那么带map tag的三个类就分析结束了，下一篇博客来进行对Converter类的分析。

Jevies

关注

1
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
SLAM——ORB-SLAM3代码分析（六）MapDrawer

2021SC@SDUSCMapDrawer分析今天来到带Map tag的最后一部分——MapDrawer的分析。MapDrawer是在Map和MapPoint的基础上进行地图点的绘制。首先一个很重要的函数是DrawMapPoints，取出所有的地图点，再取出mvpReferenceMapPoints，接着将vpRefMPs从vector容器类型转化为set容器类型，便于使用set::count（set::count用于返回集合中为某个值的元素的个数）快速统计，最后分别显示所有地图点和局部地图点。
复制链接

扫一扫