(01)ORB-SLAM2源码无死角解析-(15) 单目初始化MonocularInitialization()→Initializer

讲解关于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→官方认证  

一、前言

上一篇博客对 Tracking::MonocularInitialization() 进行了讲解。现在我们再来回顾一下，其中比较重要的函数有如下几个：

	// 由当前帧构造初始器 sigma:1.0 iterations:200
	mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
	
    // Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对
    ORBmatcher matcher(0.9,0.7true);
        
    // 对 mInitialFrame,mCurrentFrame 进行特征点匹配
    int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);       

    mpInitializer->Initialize(
            mCurrentFrame,      //当前帧
            mvIniMatches,       //当前帧和参考帧的特征点的匹配关系
            Rcw, tcw,           //初始化得到的相机的位姿
            mvIniP3D,           //进行三角化得到的空间点集合
            vbTriangulated)           
      
    // Step 7 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
    mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
    
    // Step 8 创建初始化地图点MapPoints
	CreateInitialMapMonocular();

对于特征匹配，三角化，内容，在后面的内容会有详细的讲解。目前我们先来看看其上的 Initializer，类对象为mpInitializer，也就是初始器的相关内容。首先来看看他的构造函数，位于sec/Initializer.cc文件中，代码内容如下：

/**
 * @brief 根据参考帧构造初始化器
 * 
 * @param[in] ReferenceFrame        参考帧
 * @param[in] sigma                 测量误差
 * @param[in] iterations            RANSAC迭代次数
 */
Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
{
	//从参考帧中获取相机的内参数矩阵
    mK = ReferenceFrame.mK.clone();
	
	// 从参考帧中获取去畸变后的特征点
    mvKeys1 = ReferenceFrame.mvKeysUn;
	
	//获取估计误差
    mSigma = sigma;
    mSigma2 = sigma*sigma;

	//最大迭代次数
    mMaxIterations = iterations;
}

可以看到其中的操作可谓是简单至极，仅仅是简单赋值而已。其还是还是在 mpInitializer->Initialize() 函数中，也就是 Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)。那么我们就来看看吧。

 

二、代码流程

关于 Initializer::Initializer 的主要代码流程如下：

	1、核心关键变量为mvMatches12,每个元素存在两个值，第一个存储的是帧一(参考帧)特征索引，
	第二个存储帧二(当前帧)特征索引，该对索引值是具备匹配关系的。

	2、从mvMatches12中随机选取(不重复)8对特征点

	3、开启两个线程，使用八点法分别计算 Homography 与 Fundamental 矩阵。

	4、对比两个矩阵的值，决定使用 Homography 还是 Fundamental 恢复位姿。

 

三、源码注释

在代码中可以看到 RH = SH/(SH+SF); if(RH>0.40)，可以得知其更倾向于使用 H 矩阵恢复位姿。

/**
 * @brief 计算基础矩阵和单应性矩阵，选取最佳的来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点
 * Step 1 重新记录特征点对的匹配关系
 * Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
 * Step 3 计算fundamental 矩阵 和homography 矩阵，为了加速分别开了线程计算 
 * Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
 * 
 * @param[in] CurrentFrame          当前帧，也就是SLAM意义上的第二帧
 * @param[in] vMatches12            当前帧（2）和参考帧（1）图像中特征点的匹配关系
 *                                  vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
 *                                  没有匹配关系的话，vMatches12[i]值为 -1
 * @param[in & out] R21                   相机从参考帧到当前帧的旋转
 * @param[in & out] t21                   相机从参考帧到当前帧的平移
 * @param[in & out] vP3D                  三角化测量之后的三维地图点
 * @param[in & out] vbTriangulated        标记三角化点是否有效，有效为true
 * @return true                     该帧可以成功初始化，返回true
 * @return false                    该帧不满足初始化条件，返回false
 */
bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21,
                             vector &vP3D, vector &vbTriangulated)
{
    // Fill structures with current keypoints and matches with reference frame
    // Reference Frame: 1, Current Frame: 2

    //获取当前帧的去畸变之后的特征点
    mvKeys2 = CurrentFrame.mvKeysUn;

    // mvMatches12记录匹配上的特征点对，记录的是帧2在帧1的匹配索引
    mvMatches12.clear();
	// 预分配空间，大小和关键点数目一致mvKeys2.size()
    mvMatches12.reserve(mvKeys2.size());

    // 记录参考帧1中的每个特征点是否有匹配的特征点
    // 这个成员变量后面没有用到，后面只关心匹配上的特征点 	
    mvbMatched1.resize(mvKeys1.size());

    // Step 1 重新记录特征点对的匹配关系存储在mvMatches12，是否有匹配存储在mvbMatched1
    // 将vMatches12（有冗余） 转化为 mvMatches12（只记录了匹配关系）
    for(size_t i=0, iend=vMatches12.size();i=0)
        {
			//mvMatches12 中只记录有匹配关系的特征点对的索引值
            //i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
            mvMatches12.push_back(make_pair(i,vMatches12[i]));
			//标记参考帧1中的这个特征点有匹配关系
            mvbMatched1[i]=true;
        }
        else
			//标记参考帧1中的这个特征点没有匹配关系
            mvbMatched1[i]=false;
    }

    // 有匹配的特征点的对数
    const int N = mvMatches12.size();
    // Indices for minimum set selection
    // 新建一个容器vAllIndices存储特征点索引，并预分配空间
    vector vAllIndices;
    vAllIndices.reserve(N);

	//在RANSAC的某次迭代中，还可以被抽取来作为数据样本的特征点对的索引，所以这里起的名字叫做可用的索引
    vector vAvailableIndices;
	//初始化所有特征点对的索引，索引值0到N-1
    for(int i=0; i