帧间位姿估计和地图初始化总体过程

SLAM系统要运行一般都会进行一个初始化的操作，在这个过程中确立坐标系，估计机器人的初始位姿，建立初始的地图。对于单目相机而言由于尺度的缺失，仅通过单幅图像是不能进行初始化的，需要两帧具有一定视差的图像来完成这一任务。而双目和深度相机，由于可以直接获取深度信息，所以不存在尺度不确定性的问题，只要一帧的特征点足够多就可以直接完成初始化。

本文中，我们重点分析单目初始化的基本原理及其实现方式，关于双目和深度相机的初始化过程，我们放到针对双目和深度相机的调整(TRACKING)中介绍。实际上单目相机的初始化问题，可以归结为计算前后两帧图像的帧间位姿估计。ORB的作者提出了一种自动化的初始化策略，不需要像它的前身PTAM那样人为的选择两个具有较大视差的帧，使用起来十分方便。针对平面场景(planar scene)和非平面场景(non-planar scene)，ORB-SLAM2提供了单应(homography)和基础矩阵(fundamental matrix)两种几何模型。它会在两个线程中同时计算这两个模型，然后根据一些启发式信息选取一个较合适的模型。

本文中，我们先介绍一下基础矩阵和单应矩阵的基础理论知识，简单讨论一下帧间位姿估计的方法。再结合代码分析 ORB-SLAM2 中单目相机初始化的总体过程。我们将专门用两篇文章详细分析单应矩阵和基础矩阵的求解过程。最后，简单查看一下初始地图的构造过程。

1. 单目相机的帧间位姿估计方法

虽然从单幅图像中无法得到距离信息，但是我们可以拿着相机运动，研究运动前后两帧图像中某些点的映射关系，还是可以估计出相机运动的。我们把前后帧相机的相对运动关系称为帧间位姿，用符号$\langle \boldsymbol{R}, \boldsymbol{t} \rangle$来表示。其中$\boldsymbol{R}$是相机的旋转矩阵，$\boldsymbol{t}$则是平移向量。

基于对极约束，ORB-SLAM2 计算出了相机的基础矩阵 $\boldsymbol{F}$，进而求得帧间的相对位姿变换关系。这种对级几何关系描述的是两个视角内在的映射关系，具有一般性。但在一些情况下，比如说相机只有旋转运动，图像中存在大面积的平面，基础矩阵会出现退化，容易受到测量噪声的干扰。针对基础矩阵的一些缺陷，有时我们还会基于摄影变换关系，估计帧间的单应矩阵 $\boldsymbol{H}$，通过该矩阵也可以给出一个帧间的位姿变换关系。

针对单目相机只要我们估计出前两帧的帧间位姿关系，就可以确立一个参考尺度，进而完成地图的初始化。在研究代码之前，让我们先准备一点理论知识，搞明白基础矩阵和单应矩阵是如何计算的，如何估计出帧间位姿的吧。

1.1 对级约束与基础矩阵

如右图所示，我们拿着相机分别在点$\boldsymbol{C}$和$\boldsymbol{C'}$观测空间中一点$\boldsymbol{P}$，该点在两个视图平面上分别被投影到了点$\boldsymbol{x}$和$\boldsymbol{x'}$。显然，由于两次测量都是对同一个点$\boldsymbol{P}$的观测，所以$\boldsymbol{Cx}$和$\boldsymbol{C'x'}$的延长线相交与点$\boldsymbol{P}$，即点$\boldsymbol{P}, \boldsymbol{C}, \boldsymbol{C'}, \boldsymbol{x}, \boldsymbol{x'}$都在一个平面上，这个平面被称为极面(epipolar plane)，用符号$\boldsymbol{\pi}$来表示，连线$\boldsymbol{CC'}$被称为基线(baseline)，基线分别与两个像平面相交与点$\boldsymbol{e}, \boldsymbol{e'}$，这两个点被称为极点(epipole)，它们分别与成像点$\boldsymbol{x}, \boldsymbol{x'}$的连线$\boldsymbol{xe}, \boldsymbol{x'e'}$所在的直线被称为极线(epipolar line)。

假设 $\boldsymbol{P}$ 相对于第一帧相机坐标系的坐标为$\begin{bmatrix} x & y & z \end{bmatrix}^T$，成像点$\boldsymbol{x}$的齐次坐标为$\begin{bmatrix} u & v & 1 \end{bmatrix}^T$，根据针孔相机模型，我们能够写出如下的投影关系：

$$ \begin{equation}\label{f1} \boldsymbol{x} = \cfrac{1}{z} \boldsymbol{KP} \Longleftrightarrow \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \cfrac{1}{z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} \end{equation} $$

其中矩阵$K$为相机的内参矩阵，记录了相机的xy轴上的焦距$f_x, f_y$和光心坐标$c_x, c_y$。相机经过位姿变换$\langle \boldsymbol{R}, \boldsymbol{t} \rangle$后，观测到$\boldsymbol{P}$点坐标为：

$$ \begin{bmatrix} x_2 \\ y_2 \\ z_2 \end{bmatrix} = \boldsymbol{R}\begin{bmatrix} x \\ y \\ z \end{bmatrix} + \boldsymbol{t} $$

记此时成像点$\boldsymbol{x'}$的齐次坐标为$\begin{bmatrix} u' & v' & 1 \end{bmatrix}^T$，我们有投影关系:

$$ \begin{equation}\label{f2} \boldsymbol{x'} = \cfrac{1}{z_2} \boldsymbol{K}\left(\boldsymbol{RP} + \boldsymbol{t}\right) \Longleftrightarrow \begin{bmatrix} u' \\ v' \\ 1 \end{bmatrix} = \cfrac{1}{z_2} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_2 \\ y_2 \\ z_2 \end{bmatrix} \end{equation} $$

将式($\ref{f1}$)改写成 $\boldsymbol{P} = z\boldsymbol{K}^{-1}\boldsymbol{x}$ 之后代入式($\ref{f2}$)整理后有：

$$ \begin{array}{cl} & z_2 \boldsymbol{K}^{-1} \boldsymbol{x'} = z \boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} + \boldsymbol{t} \\ \Leftrightarrow & z_2 [\boldsymbol{t}]_{\times}\boldsymbol{K}^{-1}\boldsymbol{x'} = z [\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} \\ \Leftrightarrow & z_2 \left(\boldsymbol{K}^{-1}\boldsymbol{x'}\right)^T[\boldsymbol{t}]_{\times}\boldsymbol{K}^{-1}\boldsymbol{x'} = z \left(\boldsymbol{K}^{-1}\boldsymbol{x'}\right)^T [\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} \\ \end{array} $$

上式中， $[\boldsymbol{t}]_{\times}$是相机平移向量$\boldsymbol{t}$构成的斜对称矩阵，相当于向量的叉积运算。我们关注式子的左侧，$[\boldsymbol{t}]_{\times}\boldsymbol{K}^{-1}\boldsymbol{x'}$的含义就是向量 $\boldsymbol{t}$ 与 $\boldsymbol{K}^{-1}\boldsymbol{x'}$ 的叉乘，将得到一个同时与这两个向量垂直的向量。该向量与 $\boldsymbol{K}^{-1}\boldsymbol{x'}$ 的点积为零。所以上式可以写成如下的形式：

$$ z \boldsymbol{x'}^T\boldsymbol{K}^{-T}[\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} = 0 $$

上式就是一个等式为零的约束，所以其中$z$的取值实际上没有什么作用，只要非零就行。我们把矩阵$\boldsymbol{K}^{-T}[\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}$称为基础矩阵，用符号$\boldsymbol{F}$表示。$\boldsymbol{K}$是相机的内参矩阵，可以通过标定得到。 $[\boldsymbol{t}]_{\times}\boldsymbol{R}$被称为本征矩阵(Essential Matrix)，记为$\boldsymbol{E}$。上式可以简记如下，被称为对极约束。

$$ \begin{equation}\label{f3} \begin{array}{c} \boldsymbol{x'}^T \boldsymbol{Fx} = 0 \\ \boldsymbol{x'}^T \boldsymbol{K}^{-T}\boldsymbol{E}\boldsymbol{K}^{-1}\boldsymbol{x} = 0 \\ \end{array} \end{equation} $$

我们主要关注这里的本征矩阵$\boldsymbol{E}$，它是一个$3\times 3$的矩阵，包含了相机的所有变换信息。虽然它有9个未知参数，但由于对极约束是一个等零的约束，所以该矩阵乘上任何一个非零标量都不会改变约束。因此，这个本征矩阵是缺少尺度信息的。我们只需要提供8个匹配的特征点代入上式，解零空间就可以求得$\boldsymbol{E}$。再对其进行奇异值分解后就可以获得平移向量$\boldsymbol{t}$和旋转矩阵$\boldsymbol{R}$。只要图像的纹理丰富一些，我们就可以得到很多匹配的特征点对，这样就可以通过 RANSAC 的方式估计帧间位姿，在一定程度上提高估计结果的可靠性。

基础矩阵或者说是本征矩阵的几何意义就是，在两个不同的位置上观测空间中同一个点，其成像一定在极线上。 也就说对极约束只能把一个点约束到一条直线上，并不能建立起两个成像点的一一对应关系。这也正是基础矩阵和本矩阵缺少尺度信息的一个结果。但是在一些特定的场景下，我们还是可以得到匹配点的一一对应关系的。比如下面要介绍的平面图像和单应矩阵。

1.2 摄影变换和单应矩阵

很多时候为了计算方便，我们会假设相机采集的图像就是一个平面，比如说安装在无人机上对地观测的相机、安装在机器人上对着天花板的相机等等。从不同的视角观测同一个平面的过程，可以通过摄影变换的形式建立起两幅图像之间各点的一一对应关系。这个映射关系可以用一个$3 \times 3$的齐次矩阵$\boldsymbol{H}$来表示，我们称之为单应矩阵。

假设观测的平面$\alpha$在第一帧相机坐标系的法向量为$\boldsymbol{N}$，到相机的距离为$d$，记$\alpha$上一点$P$在第一帧相机坐标系下的坐标记为$\boldsymbol{X_1} = \begin{bmatrix} x_1 & y_1 & z_1 \end{bmatrix}^T$，按照平面的点法式方程，可以得到如下的关系：

$$ \boldsymbol{N}^T\boldsymbol{X_1} = d \Longrightarrow \cfrac{1}{d}\boldsymbol{N}^T\boldsymbol{X_1} = 1 $$

若相机发生了$\langle \boldsymbol{R}, \boldsymbol{t} \rangle$的变换后，$P$点在新的相机坐标系下的坐标为$\boldsymbol{X_2} = \begin{bmatrix} x_2 & y_2 & z_2 \end{bmatrix}^T$，那么有运动关系：

$$ \begin{equation}\label{f_frame} \begin{array}{rl} & \boldsymbol{X_2} = \boldsymbol{RX_1} + \boldsymbol{t} \\ \Rightarrow & \boldsymbol{X_2} = \boldsymbol{RX_1} + \boldsymbol{t}\left( \cfrac{1}{d}\boldsymbol{N}^T\boldsymbol{X_1} \right) \\ \Rightarrow & \boldsymbol{X_2} = \left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{X_1} \end{array} \end{equation} $$

考虑相机的内参，根据式$(\ref{f1})$，将$\boldsymbol{X_1}, \boldsymbol{X_2}$投影到成像平面上有：

$$ \begin{equation}\label{f_homo_project} \begin{cases} \boldsymbol{x} = \cfrac{1}{z_1} \boldsymbol{K} \boldsymbol{X_1} \\ \boldsymbol{x'} = \cfrac{1}{z_2} \boldsymbol{K} \boldsymbol{X_2} \end{cases} \Rightarrow \begin{cases} \boldsymbol{X_1} = z_1\boldsymbol{K}^{-1} \boldsymbol{x} \\ \boldsymbol{X_2} = z_2\boldsymbol{K}^{-1} \boldsymbol{x'} \end{cases} \end{equation} $$

其中，$\boldsymbol{x}, \boldsymbol{x'}$ 分别是 $\boldsymbol{X_1}, \boldsymbol{X_2}$ 在图像中的像素齐次坐标，将上式$(\ref{f_homo_project})$代入式$(\ref{f_frame})$，有：

$$ \begin{equation}\label{f_homography_dlt} \begin{array}{rl} & z_2\boldsymbol{K}^{-1}\boldsymbol{x'} = z_1\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\boldsymbol{x} \\ \Rightarrow & \boldsymbol{x'} = \cfrac{z_1}{z_2}\boldsymbol{K}\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\boldsymbol{x} \end{array} \end{equation} $$

令$\boldsymbol{H} = \boldsymbol{K}\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}$，即是两幅图像的单应矩阵。这个单应矩阵建立了两个图像之间的映射关系 $\boldsymbol{x'} = s\boldsymbol{H}\boldsymbol{x}$。其中，$s = z_1 / z_2 $是两幅图像之间的尺度因子，并没有体现在单应矩阵中。所以这种映射模型同样存在尺度缺失的问题，由于相机内参可以通过标定获得，所以只需要计算$\boldsymbol{H_{\alpha}} = \boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T$即可通过奇异值分解的方法进一步的计算出相机的位姿。与基础矩阵和本征矩阵类似，我们可以通过匹配的特征点来计算该矩阵。下面我们结合代码，深入了解一下单目相机的初始化过程。

2. 单目相机初始化的总体过程

        void Tracking::Track() {
            if(mState==NO_IMAGES_YET)
                mState = NOT_INITIALIZED;
            mLastProcessedState=mState;

            unique_lock lock(mpMap->mMutexMapUpdate);
            if(mState==NOT_INITIALIZED) {
                if(mSensor==System::STEREO || mSensor==System::RGBD)
                    StereoInitialization();
                else
                    MonocularInitialization();
                mpFrameDrawer->Update(this);
                if(mState!=OK)
                   return;
            }

初始化的最基本任务就是建立坐标系，估计机器人的初始位姿，创建初始的地图。对于单目相机而言，由于尺度的缺失，还需要建立一个基础的尺度，以后的地图重建都是在这个基础尺度上进行的。

在分析轨迹跟踪的总体过程时，我们看到 ORB-SLAM2 在类 Tracking 的成员函数 Track() 完成了地图的初始化工作。如右侧的代码片段所示，当检查状态 mState 发现尚未进行初始化的时候，针对单目相机，将通过函数 MonocularInitialization() 完成初始化操作。

接下来我们将详细分析函数 MonocularInitialization()，将会看到 ORB-SLAM 会选择两帧具有足够多的匹配特征点和视差的图像，把相对较早的那帧图像当作参考帧进行初始化。解算出两帧图像下相机的相对位姿变换，以参考帧的相机坐标系为基础建立世界坐标。在该坐标系下相机的初始位姿为单位矩阵，即没有旋转和位移。它还会对匹配的特征点三角化，估计相对的空间点坐标，并以参考帧下特征点深度的中位数为标准，建立基础尺度。进而构建地图点，建立初始地图。

2.1 初始化的总体业务流程

        void Tracking::MonocularInitialization() {
            if (!mpInitializer) {
                if(mCurrentFrame.mvKeys.size() > 100) {
                    mInitialFrame = Frame(mCurrentFrame);
                    mLastFrame = Frame(mCurrentFrame);
                    mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
                    for(size_t i=0; i < mCurrentFrame.mvKeysUn.size(); i++)
                        mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
                    mpInitializer = new Initializer(mCurrentFrame,1.0,200);
                    fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
                }
                return;
            }

右侧是函数 MonocularInitialzation() 的代码片段。指针 mpInitializer 是类 Tracking 中为了支持单目相机初始化而定义的对象。在函数的一开始先检查该指针，如果它是个空指针就以当前帧作为参考帧实例化对象 mpInitializer。为了保证初始化的质量，只有当图像的特征点数量超过了100个，才进行初始化操作。

如片段中第4、5行所示，在实例化 mpInitializer 之前，先将暂时将当前帧标记为初始帧，并用指针 mInitialFrame 记录，同时用mLastFrame记录当前帧。此外，还要把当前帧中提取的矫畸变(undistorted)后的特征点保存到容器 mvbPrevMatched 中，如6到8行所示。然后在第9行中以当前帧为参数构造初始化器，最后将mvIniMatches中所有的特征点匹配值都置为-1，表示它们都还没有匹配。

如果初始化对象已经被具例化了，说明我们已经有了一个参考帧。如下面左侧代码片段所示，此时我们再次检查当前帧的特征点数量。只有特征点足够多(> 100)，才认为找到了用于初始化的第二帧图像。否则将销毁 mpInitializer 重新记录初始帧。为排版紧凑一些，我们增加了下面右侧的函数 FreeInitializer。

            if((int)mCurrentFrame.mvKeys.size() <= 100) {
                FreeInitializer();
                return;
            }

        void Tracking::FreeInitializer() {
            delete mpInitializer;
            mpInitializer = nullptr;
        }

然后，在下面的第 18 行中构建了一个 ORB 特征点匹配器，通过接口 SearchForInitialization 针对初始化进行特征匹配。如果匹配点数量少于 100 个，认为当前帧和初始帧不适合初始化，将销毁 mpInitializer 重新记录初始帧。类 Tracking 的成员变量 mvIniMatches 中记录了初始帧中各个特征点与当前帧匹配的特征点索引关系。

            ORBmatcher matcher(0.9,true);
            int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
            if(nmatches < 100) {
                FreeInitializer();
                return;
            }

通过了层层考验之后，我们通过对象 mpInitializer 的接口 Initialize() 完成初始化操作，估计相机的旋转量和平移量，并对特征点进行三角化形成初始的地图点。这些结果将被保存在Rcw, tcw, mvIniP3D, vbTriangulated中。其中 vbTriangulated 是与 mvIniMatches 一一对应的布尔型数组，用于记录对应的匹配特征点对是否成功三角化了。下面这个 for 循环用来剔除匹配点对中没有成功三角化的点对。这个剔除的过程完全可以放到 mpInitializer->Initialize 中进行。

            cv::Mat Rcw; // Current Camera Rotation
            cv::Mat tcw; // Current Camera Translation
            vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
            if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) {
                for(size_t i=0, iend=mvIniMatches.size(); i < iend;i++) {
                    if(mvIniMatches[i] >=0 && !vbTriangulated[i]) {
                        mvIniMatches[i]=-1;
                        nmatches--;
                    }
                }

成功的完成了初始化之后，我们就得到了当前帧相对于参考帧的相机变换关系。此时我们可以把参考帧的相机位姿当作原点构建世界坐标系，以后的相机位姿估计都是相对于该坐标系的。所以这里设定初始帧的齐次矩阵为单位阵，并用刚刚估计的旋转矩阵和平移向量构建当前帧的齐次矩阵。最后，调用函数CreateInitialMapMonocular来完成地图的初始化工作。

                mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
                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);
        
                CreateInitialMapMonocular();
        }   }

2.2 类 Initializer

我们已经看到，单目相机的初始化专门有一个类 Initializer(.h, .cc)来完成。下面折叠的代码中列举了它的所有成员变量，主要是记录了相机内参、参考帧与当前帧的特征点和匹配关系、RANSAC 算法的迭代参数和样本组。

展开类 Initializer 的成员变量 >>>

        cv::Mat mK;                              // 相机内参矩阵 3x3
        std::vector<cv::KeyPoint> mvKeys1;       // 参考帧的矫校畸变后的特征点 (Frame 1)
        std::vector<cv::KeyPoint> mvKeys2;       // 当前帧的矫畸变后的特征点 (Frame 2)

        typedef std::pair<int,int> Match;        // 匹配点对, first 参考帧，second 当前帧
        std::vector<Match> mvMatches12;          // 匹配点对
        std::vector<bool> mvbMatched1;           // 对应参考帧中每个特征点，在当前帧中是否有匹配点

        float mSigma;                            // 标准差
        float mSigma2;                           // 方差
        int mMaxIterations;                      // RANSAC 最大迭代次数
        std::vector<std::vector<size_t>> mvSets; // RANSAC 样本组集合

如下面的代码所示，它的构造函数有三个参数，其中 ReferenceFrame 是用于初始化的参考帧，sigma 和 iterations 是两个运行参数，分别定义了标准差和最大迭代次数。在函数体中还完成了mK和mvKeys1的赋值，它们分别是相机的内参矩阵和参考帧中提取的特征点。

展开类 Initializer 的构造函数 >>>

        Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations) {
            mK = ReferenceFrame.mK.clone();
            mvKeys1 = ReferenceFrame.mvKeysUn;
            mSigma = sigma;
            mSigma2 = sigma*sigma;
            mMaxIterations = iterations;
        }

        bool Initializer::Initialize(const Frame & CurrentFrame,
            const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,    
            std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated)
        {
            mvKeys2 = CurrentFrame.mvKeysUn;
            mvMatches12.clear();
            mvMatches12.reserve(mvKeys2.size());
            mvbMatched1.resize(mvKeys1.size());
            for(size_t i=0, iend=vMatches12.size(); i < iend; i++) {
                if(vMatches12[i] >= 0) {
                    mvMatches12.push_back(make_pair(i,vMatches12[i]));
                    mvbMatched1[i]=true;
                } else
                    mvbMatched1[i]=false;
            }

实际的初始化过程都是由成员函数 Initialize 完成的，如右侧的代码片段所示，它有6个参数，其中 CurrentFrame 是当前帧，vMatches12 中记录了两帧特征点匹配关系。 vMatches12 与参考帧的特征点一一对应，记录了当前帧中匹配的特征点索引。若成功完成了初始化，将通过参数 R21 和 t21 输出当前帧相对于参考帧的姿态矩阵和平移向量。输出参数 vP3D 中记录了所有三角化的地图点，vbTriangulated 记录匹配的特征点是否被三角化了。

在函数的一开始，使用成员变量 mvKeys2 记录下当前帧中的特征点。然后遍历输入参数 vMatches12，将匹配点对应关系记录到成员容器 mvMatches12 中，同时用 mvbMatched1 标记参考帧中的特征点是否有匹配对象。

特征点是很容易误匹配的，但我们并没有太好的手段把这些误匹配的特征点剔除掉。错误的匹配点对势必会对相机的位姿估计产生负面的影响。 RANSAC 算法能够有效的降低负面影响。它通过随机采样的方法，每次从样本集中只取出能够解决问题的最少样本，求解位姿。求解单应矩阵只需要4个匹配点对，基础矩阵则需要8个。对样本组输出的相机位姿计算一个损失函数。多次随机采样之后，选取损失函数最小的那组样本的解作为最后的相机位姿输出。

一般情况下，误匹配的特征点还是很少的。RANSAC 随机采样最少的样本求解方程，很大概率上能够找到一组匹配点对集合，不存在误匹配，相比于那些有误匹配的样本组应当更准确，损失函数值也就越低。在 ORB-SLAM2 中，这一 RANSAC 过程被分为两个部分来执行了。首先对所有的匹配点对进行随机采样，准备样本组集合。由 mvSets 记录了所有 RANSAC 样本组，每个样本组都有8个特征点，如下面的代码片段所示。这一段代码有着比较明确的计算目的，感觉封装到一个函数里会比较好一些。

展开 RANSAC 采样过程 >>>

            const int N = mvMatches12.size();
        	// Indices for minimum set selection
        	std::vector<size_t> vAllIndices;
        	vAllIndices.reserve(N);
        	std::vector<size_t> vAvailableIndices;
        	for(int i = 0; i < N; i++)
        	    vAllIndices.push_back(i);
        	// Generate sets of 8 points for each RANSAC iteration
        	mvSets = std::vector<std::vector<size_t>>(mMaxIterations,vector<size_t>(8,0));
        	DUtils::Random::SeedRandOnce(0);
        	for(int it = 0; it < mMaxIterations; it++) {
        	    vAvailableIndices = vAllIndices;
        	    // Select a minimum set
        	    for(size_t j = 0; j < 8; j++) {
        	        int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
        	        int idx = vAvailableIndices[randi];
        	        mvSets[it][j] = idx;
        	        vAvailableIndices[randi] = vAvailableIndices.back();
        	        vAvailableIndices.pop_back();
        	    }
        	}

然后在两个线程中，完成 RANSAC 的求解过程，计算单应矩阵和基础矩阵，记录到局部变量 H, F 中。局部变量 SH 和 SF 分别记录了两种计算模型解的损失函数值。具体的求解过程，我们将在后文分析函数 FindHomography 和 FindFundamental 时详细介绍。

            vector<bool> vbMatchesInliersH, vbMatchesInliersF;
            float SH, SF;
            cv::Mat H, F;
            thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
            thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
            threadH.join();
            threadF.join();

最后按照ORB-SLAM论文的说法，根据启发式规则参数 $R_H = \cfrac{S_H}{S_H + S_F}$ 来选择进行初始化的模型，并调用 ReconstructH 和 ReconstructF 从单应矩阵或者基础矩阵中恢复出相机的位姿。

            float RH = SH/(SH+SF);
            if(RH > 0.40)
                return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
            else //if(pF_HF > 0.6)
                return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
            return false;
        }

通过分析单目相机的初始化总体过程可以看到，实际的帧间位姿估计是由 FindHomography, FindFundamental, ReconstructH, ReconstructF 四个函数具体完成的。我们将专门用两篇文章详细分析单应矩阵和基础矩阵的求解过程。

3. 地图初始化

现在，我们已经成功的估计了相机的位姿，并对匹配的特征点进行了三角化得到了它们的世界坐标。接下来就需要调用函数 CreateInitialMapMonocular 来完成世界地图的初始化工作。下面是这个函数代码片段，一开始先根据参考帧和当前帧创建两个关键帧(KeyFrame)。

        void Tracking::CreateInitialMapMonocular() {
            KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
            KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

然后更新词袋，并将这两个关键帧插入地图中。

            pKFini->ComputeBoW();
            pKFcur->ComputeBoW();
            mpMap->AddKeyFrame(pKFini);
            mpMap->AddKeyFrame(pKFcur);

接着在一个for循环中根据成功三角化的特征点创建地图点(MapPoint)，并建立地图点与关键帧之间的可视关系。

            for (size_t i=0; i < mvIniMatches.size(); i++) {
                if(mvIniMatches[i] < 0)
                    continue;
                cv::Mat worldPos(mvIniP3D[i]);
                MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
                // 省略建立各种关联关系的语句
                mpMap->AddMapPoint(pMP);
            }

此时，我们就可以根据地图点和关键帧之间的关系提取出共视图(covisibility graph)和基图(essential graph)了。下面两句话用于更新关键帧的连接关系。

            pKFini->UpdateConnections();
            pKFcur->UpdateConnections();

最后进行一次全局的BA优化，就完成了初始地图的构建。论文中的初始化介绍也就到此为止了。

            Optimizer::GlobalBundleAdjustemnt(mpMap,20);

看起来相机的位姿也估计了，地图也建立了，故事应该结束了。但是对于单目而言尺度仍然是缺失的，所以函数CreateInitialMapMonocular还进一步的以当前地图点深度的中位数为基础建立基础尺度，以后的地图重建工作都是在这个基础的尺度上进行的。下面的代码片段中，先获取了初始关键帧下的深度中位数，并进一步的检查该深度以及当前关键帧跟踪上的地图点数量。

            float medianDepth = pKFini->ComputeSceneMedianDepth(2);
            float invMedianDepth = 1.0f/medianDepth;
            if(medianDepth < 0 || pKFcur->TrackedMapPoints(1) < 100) {
                cout >> "Wrong initialization, reseting..." >> endl;
                Reset();
                return;
            }

最后初始化基线和地图点的尺度。至此，我们才算完成了初始化的操作，建立了基础尺度和初始地图。

            // Scale initial baseline
            cv::Mat Tc2w = pKFcur->GetPose();
            Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
            pKFcur->SetPose(Tc2w);
            // Scale points
            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);
                }
            }
            // 省略Tracking的各种成员的更新
        }

5. 完

对于单目相机整个初始化过程可以总结为六个步骤：

计算两帧图像的ORB特征点，并进行匹配；
在两个线程中以RANSAC策略并行的计算单应矩阵和基础矩阵；
根据评判标准在单应矩阵和基础矩阵之间选择一个模型；
根据选定的模型分解相机的旋转矩阵和平移向量，并对匹配的特征点进行三角化；
建立关键帧和地图点，进行完全BA(full BA)优化；
以参考帧下深度的中位数为基准建立基础尺度；