ORB-SLAM2源码解读（2.2）：单目初始化、匀速运动模型跟踪、跟踪参考关键帧、跟踪局部地图

这里是Tracking部分的第二部分，详细讲述各分支的代码及其实现原理。

---

单目初始化

MonocularInitialization()

目标：从初始的两帧单目图像中，对SLAM系统进行初始化（得到初始两帧的匹配，相机初始位姿，初始MapPoints），以便之后进行跟踪。

方式：并行得计算基础矩阵E和单应矩阵H，恢复出最开始两帧相机位姿；三角化得到MapPoints的深度，获得点云地图。

寻找匹配特征点

单目的初始化有专门的初始化器，只有连续的两帧特征点均>100个才能够成功构建初始化器。

完成前两帧特征点的匹配

ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点。

这里的ORBmatcher类后面有机会再详细介绍。功能就是ORB特征点的匹配。

从匹配点中恢复初始相机位姿

在得到超过100个匹配点对后，运用RANSAC 8点法同时计算单应矩阵H和基础矩阵E，并选择最合适的结果作为相机的初始位姿。

mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)

Initializer类是初始化类。下面要详细讲一下类成员函数 intilize（）,能够根据当前帧和两帧匹配点计算出相机位姿R|t ，以及三角化得到 3D特征点。

Initializer-> intilize（）

（1）调用多线程分别用于计算fundamental matrix和homography

    // 计算homograpy并打分
    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    // 计算fundamental matrix并打分
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

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

（2）计算得分比例，选取某个模型进行恢复R|t

    float RH = SH/(SH+SF);

    // 步骤5：从H矩阵或F矩阵中恢复R,t
    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);

具体的：

FindHomography或FindFundamental

（1） 计算单应矩阵cv::Mat H