一步步带你看懂orbslam2源码--单目初始化(五)

---

ORB-SLAM2目录：

   一步步带你看懂orbslam2源码–总体框架(一)
   一步步带你看懂orbslam2源码–orb特征点提取(二)
   一步步带你看懂orbslam2源码–单目初始化(三)
   一步步带你看懂orbslam2源码–单应矩阵/基础矩阵,求解R,t(四)
   一步步带你看懂orbslam2源码–单目初始化(五)

---

回顾:
  前面我们已经讲解了orbslam2中的理论环节,包括了RANSAC随机采样一致性算法,阈值的选择缘由,对极约束的原理,单应矩阵和基础矩阵的计算,如何从单应矩阵或基础矩阵中分解出R,t,单目初始化单应矩阵或基础矩阵选择策略等.本章节我们将主要进行单目初始化的理论环节,话不多说,接下来就直接进入正题.

---

理论环节

  先贴上Track()函数的整体框架代码,首先第一次执行时,mState将会默认等于NO_IMAGES_YET,也就是说系统将会进行一次初始化,然后初始化成功之后的每次运行,系统将不会再次进入if(mState==NOT_INITIALIZED)处的内容 (除非跟踪丢失,重定位?还没看完,暂时不清楚) ,即每次将会进入else{…}部分进行前端跟踪.

  由于本系列教程,我们是针对单目SLAM进行讲解的,所以接下来我们将进入MonocularInitialization();进行单目初始化.

  同样的,我们先来看来看一下该函数的主要框架.首先,系统将会选择一张图像特征点数量大于100的图像作为参考帧,并将该帧中的特征点作为预匹配特征点存入mvbPrevMatched向量中.什么意思呢?什么叫做预匹配点呢?其实由于单目初始化时,系统假设在两帧之间的位移很少(即对于参考帧中的特征点对应帧2中的匹配点,该匹配点与帧1特征点位置很接近),所以在寻找帧2匹配点时,将以帧1中特征点的位置的周边一小块区域作为搜索对象.不知道同学们听懂了没有…emm…假设大家都已经听懂了…下面给出总体框架的流程图

  也就是说,系统将会选择当前帧作为参考帧(如果该帧特征点数量>100),然后与下帧进行匹配,如果匹配失败,则将更新参考帧,并再与下帧进行匹配,以此类推,直至匹配成功.
接下来我们将讲解如下函数:

// Find correspondences
ORBmatcher matcher(0.9,true);
//输入参数:mInitialFrame,mCurrentFrame这是待匹配的两帧图片
//mvbPrevMatched为预匹配点坐标,函数运算后将会更新为真正的匹配点坐标
//mvIniMatches为mInitialFrame帧中的特征点在mCurrentFrame帧中的匹配点的索引
//100为搜索的区域边长,源码中是正方形的边长
//return 成功匹配的数量
int nmatches=matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

让我们来看看SearchForInitialization()函数的总体流程图吧

  该函数篇幅相对较长,但是基本上都不会怎么难,这里只是给出了整体的操作流程,相信读者们可以自行阅读懂该函数.好了,当匹配成功之后,系统将会执行以下程序计算相机的POSE,并且创建初始地图.

        //匹配成功,计算POSE
        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--;
                }
            }

            // Set Frame Poses
            //初始帧pose设置为单位矩阵
            //更新当前帧pose
            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();
        }

以上程序的难点主要在于以下函数

//mvIniMatches:帧1特征点在帧2中的匹配
//Rcw,tcw:待求相机位姿
//mvIniP3D:成功匹配点的空间3D坐标
//vbTriangulated:成功三角测量--ture,三角测量失败--false
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)

同样地,我们来看下这个函数的流程图.

  如下为创建组随机8点对的程序,如果大家没忘记的话,这不就是RANSAC随机采样一致性算法中的选取若干个最小子集?以下程序实现随机选取的思路大致如下:首先利用随机种子选择vAvailableIndices.size()范围内的一个索引值(vAvailableIndices.size()这玩意不就是成功匹配点的数量?),然后利用vAvailableIndices[randi] = vAvailableIndices.back()和 vAvailableIndices.pop_back();分别实现:将vAvailableIndices向量中的最后一位放置于已被抽选的索引值处;将vAvailableIndices向量中的最后一位删除.这样子就实现了vAvailableIndices向量一直保存为未抽取到的索引值,且向量大小逐渐减小.

    // Generate sets of 8 points for each RANSAC iteration
    //创建具有 mMaxIterations 个八点对向量的向量
    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));

    DUtils::Random::SeedRandOnce(0);

    //随机选取 mMaxIterations 组,每组包含8个点,存入 mvSets 中
    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();
        }
    }

  选取好mMaxIterations组8点对之后,将创建两个临时线程,并行计算单应矩阵和基础矩阵,其中vbMatchesInliersH,SH,H为传入参数引用,目的是为了更新这两个值.

    // Launch threads to compute in parallel a fundamental matrix and a homography
    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));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();

这里就稍微补充点C++知识吧,这是创建线程并启动线程的方式:

std::thread m_thrProcess(&类名::函数名, &类对象, 函数参数);

  至于为什么函数参数前面要加上ref()呢?std::ref只是尝试模拟引用传递，并不能真正变成引用，在非模板情况下，std::ref根本没法实现引用传递，只有模板自动推导类型时，ref能用包装类型reference_wrapper来代替原本会被识别的值类型，而reference_wrapper能隐式转换为被引用的值的引用类型。比如thread的方法传递引用的时候，必须外层用ref来进行引用传递，否则就是浅拷贝。而在下方添加threadH.join()时,所在线程将会被阻塞,直至线程完成,也就是说只有计算完H和F矩阵,程序才会运行后面的内容.

  言归正传,接下来让我们来看看FindHomography这个函数吧,至于FindFundamental这个函数,在这里笔者就不讲了,非常类似,在有了前面所讲的理论基础之后,相信同学们应该可以没什么障碍的读懂.

  进入FindHomography()函数,我们可以发现其中先将特征点进行归一化,再利用归一化后的坐标进行计算单应矩阵,而后再换算成原坐标的单应矩阵,至于为什么使用归一化的坐标呢?其实笔者也不是特别理解,希望路过的读者们谁知道的麻烦传授下笔者,感激不尽~~
归一化公式如下所示:
$$u^{{}^{\prime }}=\frac{u-u_{mean}}{{\sum }_{i=1}^N\left|u-u_{mean}\right|/N}\text{\hspace{1em}(1)}$$
$$v^{{}^{\prime }}=\frac{v-v_{mean}}{{\sum }_{i=1}^N\left|v-v_{mean}\right|/N}\text{\hspace{1em}(2)}$$
经过公式(1)和公式(2)的换算之后,就可以将原特征点的坐标归一化为均值为0,一阶绝对矩为1.令
$$sX=\sum _{i=1}^N\left|u-u_{mean}\right|/N$$
$$sY=\sum _{i=1}^N|v-$$