(01)ORB-SLAM2源码无死角解析-(23) 单目SFM地图初始化→CreateInitialMapMonocular()-总体流程

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

通过前面的博客，已经知道如何从两帧图像提取特征点，进行特征值匹配(匹配的过程暂时还未进行详细的讲解)，然后使用八点法求解单应性矩阵Homography，基本矩阵Fundamental，接着使用 SVD奇异值分解求得相机位姿 R t \mathbf R\mathbf t Rt 。这是前面博客中主要讲解的内容。

然后我们来回顾一下，目前为止我们已经分析到了 src/Tracking.cc 文件中的 Tracking::MonocularInitialization() 函数，并且对于其中调用的 Frame 类对象，以及 mpInitializer->Initialize() 函数进行了详细的介绍。当然，包揽在 Tracking::MonocularInitialization() 函数 函数中的很多内容，目前还没有讲解，比如：

        ORBmatcher matcher(
            0.9,        //最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7
            true);      //检查特征点的方向
   
        int nmatches = matcher.SearchForInitialization(
         mInitialFrame,mCurrentFrame,    //初始化时的参考帧和当前帧
         mvbPrevMatched,                 //在初始化参考帧中提取得到的特征点
         mvIniMatches,                   //保存匹配关系
         100);                           //搜索窗口大小         
            

等等，不过没有关系，在后面的博客中，会进行一个详细的了解。暂时先把这些内容放一下，我们来讲解 Tracking::MonocularInitialization() 函数中调用的 CreateInitialMapMonocular() 函数。

 

二、问题思考

根据前面的博客，已经知道，通过特征点三角化,能够计算出特征点在世界坐标系的3D坐标。后续需要把这些3D坐标绘画出来的，进行可视化。那么问题就来了，应该以一个怎么样的比例去绘画呢? 简单的来说，就是计算出来的3D坐标坐标，普遍情况下，是不能进行直接绘图，如果坐标数值太大，则需要很大的图像才能够包容所有的特征点，如果坐标数值太小，则都集中在了一个点上，不方便视觉上的观察。

所以这个时候 ，需要对计算出来的3D点进行一个归一化，归一化之后的3D坐标坐标分布会比较均匀，可视化之后看起来更加舒适。但是这里需要注意一个点，其归一化针对的，并不是当前帧，而是针对所有帧特征点的3D坐标进行归一化，因为在可视化绘画的时候，是绘画所有特征点对应的3D坐标。

那么，所有特征点中最中心的特征点，展现在可视化空间的中心，然后其余特征点按照等比方式进行分布。这样的方式理论上来说是最好的。具体的细节，在下面的源码中进行分析。  

三、总体流程

单目SFM地图 Tracking::CreateInitialMapMonocular() 函数，主要的目的是根据三角化得到的3D点生成地图点(3D点与地图点存在一定比例关系)。其主要流程如下：

1 将初始关键帧,当前关键帧的描述子转为BoW
2 将关键帧插入到地图
3 用初始化得到的3D点来生成地图点MapPoints
4 全局BA优化，同时优化所有位姿和三维点
5 取场景的中值深度，用于尺度归一化 
6 将两帧之间的变换归一化到平均深度1的尺度下
7 把3D点的尺度也归一化到1
8 将关键帧插入局部地图，更新归一化后的位姿、局部地图点

从上面看起来还是挺复杂的，这里呢，先把 Tracking::CreateInitialMapMonocular() 的整体架构注释代码贴一下(大致看一下即可，后续有各个函数细节的分析)：

/**
 * @brief 单目相机成功初始化后用三角化得到的点生成MapPoints
 * e  
 */
void Tracking::CreateInitialMapMonocular()
{
    // Create KeyFrames 认为单目初始化时候的参考帧和当前帧都是关键帧
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);  // 第一帧
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);  // 第二帧

    // Step 1 将初始关键帧,当前关键帧的描述子转为BoW
    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

    // Insert KFs in the map
    // Step 2 将关键帧插入到地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

    // Create MapPoints and asscoiate to keyframes
    // Step 3 用初始化得到的3D点来生成地图点MapPoints
    //  mvIniMatches[i] 表示初始化两帧特征点匹配关系。
    //  具体解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值,没有匹配关系的话，vMatches12[i]值为 -1
    for(size_t i=0; iAddMapPoint(pMP,mvIniMatches[i]);

        // a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

        // b.从众多观测到该MapPoint的特征点中挑选最有代表性的描述子
        pMP->ComputeDistinctiveDescriptors();
        // c.更新该MapPoint平均观测方向以及观测距离的范围
        pMP->UpdateNormalAndDepth();

        //Fill Current Frame structure
        //mvIniMatches下标i表示在初始化参考帧中的特征点的序号
        //mvIniMatches[i]是初始化当前帧中的特征点的序号
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        //Add to Map
        mpMap->AddMapPoint(pMP);
    }

    // Update Connections
    // Step 3.3 更新关键帧间的连接关系
    // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();

    // Bundle Adjustment
    cout GetPose());

    mpMap->mvpKeyFrameOrigins.push_back(pKFini);

    mState=OK;// 初始化成功，至此，初始化过程完成
}

 

四、步骤分析

根据上面总体架构，结合代码分析，可以直观的看到其上有如下函数是比较重要的：

pKFini->ComputeBoW(); //将初始关键帧,当前关键帧的描述子转为BoW

mpMap->AddKeyFrame(pKFini); //将关键帧插入到地图

MapPoint* pMP = new MapPoint( worldPos,pKFcur, mpMap); //用3D点构造MapPoint

pMP->ComputeDistinctiveDescriptors(); //从众多观测到该MapPoint的特征点中挑选最有代表性的描述子

pMP->ComputeDistinctiveDescriptors(); //更新该MapPoint平均观测方向以及观测距离的范围

pKFini->UpdateConnections(); //在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数

Optimizer::GlobalBundleAdjustemnt(mpMap,20); //全局BA优化，同时优化所有位姿和三维点

mvpLocalMapPoints=mpMap->GetAllMapPoints(); // 单目初始化之后，得到的初始地图中的所有点都是局部地图点

可以看到其上调用的函数还是很多的。比较复杂的部分，在下个章节进行讲解。接下来，我们讲解几个比较简单的函数。

 

五、简要函数分析

ComputeBoW()： 该函数主要是把描述子转换为BoW，该作用会再后续的章节进行详细的讲解。

mpMap->AddKeyFrame(): 添加关键帧到地图之中， 把帧插入到成员变量 mspKeyFrames 之中。

 

六、结语

该篇博客主要对 CreateInitialMapMonocular() 函数进行整体讲解， 在下一篇博客中，会对 CreateInitialMapMonocular() 调用的重要函数进行细节分析。

    本文内容来自计算机视觉life ORB-SLAM2 课程课件