(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
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过前面的博客，已经知道如何从两帧图像提取特征点，进行特征值匹配(匹配的过程暂时还未进行详细的讲解)，然后使用八点法求解单应性矩阵Homography，基本矩阵Fundamental，接着使用 SVD奇异值分解求得相机位姿 $\mathbf{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; i<mvIniMatches.size();i++)
    {
        // 没有匹配，跳过
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
        // 用三角化点初始化为空间点的世界坐标
        cv::Mat worldPos(mvIniP3D[i]);

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

        // Step 3.2 为该MapPoint添加属性：
        // a.观测到该MapPoint的关键帧
        // b.该MapPoint的描述子
        // c.该MapPoint的平均观测方向和深度范围

        // 表示该KeyFrame的2D特征点和对应的3D地图点
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(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 << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;

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

    // Set median depth to 1
    // Step 5 取场景的中值深度，用于尺度归一化 
    // 为什么是 pKFini 而不是 pKCur ? 答：都可以的，内部做了位姿变换了
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;
    
    //两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于100
    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    // Step 6 将两帧之间的变换归一化到平均深度1的尺度下
    // Scale initial baseline
    cv::Mat Tc2w = pKFcur->GetPose();
    // x/z y/z 将z归一化到1 
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

    // Scale points
    // Step 7 把3D点的尺度也归一化到1
    // 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? 答：是的，因为是同样的三维点
    vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
    for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
    {
        if(vpAllMapPoints[iMP])
        {
            MapPoint* pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
        }
    }

    //  Step 8 将关键帧插入局部地图，更新归一化后的位姿、局部地图点
    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);

    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId=mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;

    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    // 单目初始化之后，得到的初始地图中的所有点都是局部地图点
    mvpLocalMapPoints=mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    //也只能这样子设置了,毕竟是最近的关键帧
    mCurrentFrame.mpReferenceKF = pKFcur;

    mLastFrame = Frame(mCurrentFrame);

    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

    mpMapDrawer->SetCurrentCameraPose(pKFcur->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 课程课件