(01)ORB-SLAM2源码无死角解析-(14) 地图初始化→单目初始化MonocularInitialization():尺度不确定性

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

一、前言

上一篇博客我们讲解了 src/Tracking.cc 文件中: void Tracking::Track() 函数，我相信大家暂时都是蒙蔽了，不过没有关系，我们对其进行拆分，一个一个细节的来进行讲解。那么今天我们就是讲解其中的地图初始化部分，也就是 Track() 函数中调用的如下代码：

    // Step 1：地图初始化
    if(mState==NOT_INITIALIZED)
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            //双目RGBD相机的初始化共用一个函数
            StereoInitialization();
        else
            //单目初始化
            MonocularInitialization();

        //更新帧绘制器中存储的最新状态
        mpFrameDrawer->Update(this);

        //这个状态量在上面的初始化函数中被更新
        if(mState!=OK)
            return;
    }

如果地图没有初始化，则进行初始化处理。接下来我们对其中的单目初始化进行讲解(双目RGBD相机的初始化相对简单)。了解了单目初始化之后再去了解双目RGBD相机的初始
就会比较容易。
 

二、代码流程

在ORB-SLAM2中初始化和使用的传感器类型有关，其中单目相机模式初始化相对复杂，需要运行一段时间才能成功初始化。而双目相机、RGB-D相机模式下比较简单，一般从第一帧开始就可以完成初始化。

为什么不同传感器类型初始化差别这么大呢？

我们从最简单的RGB-D相机初始化来说，因为该相机可以直接输出RGB图像和对应的深度图像，所以每个像素点对应的深度值是确定的，也就是说，我在第一帧提取了特征点后，特征点对应的三维点在空间的绝对坐标是可以计算出来的（需要用到内参）。

对于双目相机来说，也可以通过第一帧左右目图像立体匹配来得到特征点对应的三维点在空间的绝对坐标。因为第一帧的三维点是作为地图来实现跟踪的，所以这些三维点我们也称为地图点。所以理论来说，双目相机、RGB-D相机在第一帧就可以完成初始化。

而对于单目相机来说，仅仅有第一帧无法得到三维点，想要初始化，需要像双目相机那样去进行立体。这些内容我们会在后面进行详细的讲解。

Tracking::MonocularInitialization() 函数实现流程如下：

	1、如果当前没有创建单目初始器，则创建单目初始器，创建单目初始器的条件
	当前帧的特征点数目大于100个。否则函数返回
	
	2、如果已经创建了初始器，那么判断当前帧的特征点是否超过100，如果没有
	超过，则删除初始化器，函数返回
	
	3、创建了初始化器，且当前帧特征数超过100，则当前帧与初始帧进行特征匹配。
	如果匹配的特征数太少(低于100)，则又删除初始器，返回函数。

	4、如果当前帧与初始帧匹配特征数足够(大于100)，则把初始帧作为世界坐标系，
	因此第一帧变换矩阵为单位矩阵。然后求解当前帧的姿态。
	
	5、对匹配点进行三角化

	6.求解当前帧的姿态、创建初始化地图点MapPoints，

上面的流程中，大家要注意一个点，就是双目摄像头初始化，是需要连续的两帧，且连续两帧的特征点个数都大于100时，才能完成初始化过程。所以我们在运行单目摄像头示例的时候，他需要运行一段时间才显示三维空间点，这就是因为之前再做地图初始化。然而双目和深度相机是需要等待的，因为其第一帧即可完成初始化。
 

三、单目尺度不确定性

使用双目摄像头，以及深度摄像头，我们是可以获得绝对尺度(简单理解世界坐标的真实距离)，但是单目摄像头 我们只能获得相对尺度。这是为什么呢?

《视觉 SLAM 十四讲:从理论到实践》中是这样描述这个问题的：由于单目相机拍摄的图像只是三维空间的二维投影，所以，如果真想恢复三维结构，必须改变相机的视角。在单目 SLAM 中也是同样的原理。我们必须移动相机，才能估计它的运动 (Motion ), 同时估计场景中物体的远近和大小，不妨称之为结构( Structure )。那么，怎么估计这些运动和结构呢?想象你坐在…辆运动的列车中。一方面，如果列车往右边移动，那么我们看到的东西就会往左边移动-一这就给我们推测运动带来了信息。另一方面，我们还知道:近处的物体移动快，远处的物体移动慢，极远处(无穷远处)的物体(如太阳、月亮)看上去是不动的。于是，当相机移动时，这些物体在图像上的运动就形成了视差( Disparity )。通过视差，我们就能定量地判断哪些物体离得远，哪些物体离得近。
        然而，即使我们知道了物体远近，它们仍然只是一个相对的值。比如我们在看电影时，虽然能够知道电影场景中哪些物体比另一些大，但无法确定电影里那些物体的"真实尺度"那些大楼是真实的高楼大厦，还是放在桌上的模型?而榷毁大厦的是真实怪兽，还是穿着特摄服装的演员?如果把相机的运动和场景大小同时放大两倍，单吕相机所看到的像是一样的。同样地，把这个大小乘以任意倍数，我们都将看到一样的景象。这说明，单目 SLAM 估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是所谓的尺度( Scale )①。由于单目 SLAM 无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性( Scale Ambiguity )。
        平移之后才能计算深度，以及无法确定真实尺度，这两件事情给单目 SLAM 的应用造成了很大的麻烦。其根本原因是通过单张图像元法确定深度。所以，为了得到这个深度，人们开始使用双目相机和深度相机。

通过上面的描述，我们不知道大家看明白没有，大概的意思就是说，单目相机缺少深度信息，所以无法知道物体的真实尺度，在估算位置的时候，当前帧所见的物体大小都是先对于上一帧(或者之前帧)而言。比如，单目初始化完成之后，已经确定了第一帧关键帧，如果相机远离物体，相对于第一帧关键帧来说，当前帧的物体就变小了，如果靠近，那么相对来说，就是物体变大了。也就是说，根据物体的变化，可以判断相机的运动，但是我们却无法根据相机的运动姿态估算出物体的真实尺度，因为其是相对的。我们来看下面的图示：

如果上面的图示表示一个双目相机，一般情况下相机 $O_1$ 与 $O_2$ 的距离是已知的，通常称为基线，知道该参数，我们是可以计算 $P$ 点相对于相机 $O_1$ 的绝对物理坐标。但是如果我们只有相机一个相机，初始其位于 $O_1$，然后其移动到了 $O_2$，如果不知道 $O_1$ $O_2$的距离，那么是没有办法计算出 $P$ 点的真实坐标的的(在后面的学习中，会了解到) 。因为其缺少平移变量 $\mathbf{t}$。后面的博客中，会对这个问题进行详细的讲解。
 

四、代码注释

Tracking::MonocularInitialization() 函数的注释如下：

/*
 * @brief 单目的地图初始化
 *
 * 并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
 * 得到初始两帧的匹配、相对运动、初始MapPoints
 * 
 * Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
 * Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
 * Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
 * Step 4：如果初始化的两帧之间的匹配点太少，重新初始化
 * Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
 * Step 6：删除那些无法进行三角化的匹配点
 * Step 7：将三角化得到的3D点包装成MapPoints
 */
void Tracking::MonocularInitialization()
{
    // Step 1 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个
    if(!mpInitializer)
    {
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if(mCurrentFrame.mvKeys.size()>100)
        {
            // 初始化需要两帧，分别是mInitialFrame，mCurrentFrame
            mInitialFrame = Frame(mCurrentFrame);
            // 用当前帧更新上一帧
            mLastFrame = Frame(mCurrentFrame);
            // mvbPrevMatched  记录"上一帧"所有特征点
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
                mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;

            // 删除前判断一下，来避免出现段错误。不过在这里是多余的判断
            // 不过在这里是多余的判断，因为前面已经判断过了
            if(mpInitializer)
                delete mpInitializer;

            // 由当前帧构造初始器 sigma:1.0 iterations:200
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);

            // 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的id
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

            return;
        }
    }
    else    //如果单目初始化器已经被创建
    {
        // Try to initialize
        // Step 2 如果当前帧特征点数太少（不超过100），则重新构造初始器
        // NOTICE 只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }

        // Find correspondences
        // Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对
        ORBmatcher matcher(
            0.9,        //最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7
            true);      //检查特征点的方向

        // 对 mInitialFrame,mCurrentFrame 进行特征点匹配
        // mvbPrevMatched为参考帧的特征点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标
        // mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
        int nmatches = matcher.SearchForInitialization(
            mInitialFrame,mCurrentFrame,    //初始化时的参考帧和当前帧
            mvbPrevMatched,                 //在初始化参考帧中提取得到的特征点
            mvIniMatches,                   //保存匹配关系
            100);                           //搜索窗口大小

        // Check if there are enough correspondences
        // Step 4 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
        if(nmatches<100)
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            return;
        }

        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)

        // Step 5 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
        if(mpInitializer->Initialize(
            mCurrentFrame,      //当前帧
            mvIniMatches,       //当前帧和参考帧的特征点的匹配关系
            Rcw, tcw,           //初始化得到的相机的位姿
            mvIniP3D,           //进行三角化得到的空间点集合
            vbTriangulated))    //以及对应于mvIniMatches来讲,其中哪些点被三角化了
        {
            // Step 6 初始化成功后，删除那些无法进行三角化的匹配点
            for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
            {
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }

            // Set Frame Poses
            // Step 7 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            // 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到相机坐标系的变换矩阵
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);

            // Step 8 创建初始化地图点MapPoints
            // Initialize函数会得到mvIniP3D，
            // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
            // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
            CreateInitialMapMonocular();
        }//当初始化成功的时候进行
    }//如果单目初始化器已经被创建
}

 

五、结语

该篇博客，我们了解了单目初始化的大概流程，并且额外提及深度以及双目相机。下面我们主要是对 单目初始化MonocularInitialization() 做一个细致的了解，也就是详细了解其中的每个函数实现。

$注意：$ MonocularInitialization()中最后调用的CreateInitialMapMonocular()函数，有一个比较重要的地方，那就是相对平移 $\mathbf{t}$ 的设定。

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