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 Hn = ComputeH21(vPn1i,vPn2i);，并进行评分currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);，分别通过单应矩阵将第一帧的特征点投影到第二帧，以及将第二帧的特征点投影到第一帧，计算两次重投影误差，叠加作为评分SH。

（2） 计算基础矩阵cv::Mat Fn = ComputeF21(vPn1i,vPn2i);，并进行评分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);，两次重投影误差叠加作为评分SF。

创建初始地图

如果恢复相机初始位姿成功，那么我们能得到前两帧图像的位姿，以及三角测量后的3维地图点。之后进行如下操作：

            // Set Frame Poses
            // 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
            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);

            // 步骤6：将三角化得到的3D点包装成MapPoints
            // Initialize函数会得到mvIniP3D，
            // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
            // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
            CreateInitialMapMonocular();

 先将当前帧位姿Tcw设置好，之后CreateInitialMapMonocular () 创建初始地图，并进行相应的3D点数据关联操作：

CreateInitialMapMonocular ()

（1）创建关键帧

因为只有前两帧，所以将这两帧都设置为关键帧，并计算对应的BoW，并在地图中插入这两个关键帧。

（2）创建地图点+数据关联

将3D点包装成地图点，将地图点与关键帧，地图进行数据关联。

• 关键帧与地图点关联

一个地图点可被多个关键帧观测到，将观测到这个地图点的关键帧与这个地图点进行关联；关键帧能够观测到很多地图点，将这些地图点与该关键帧关联。

        //关键帧上加地图点
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
        //地图点的属性：能够观测到的关键帧
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

地图点与关键帧上的特征点关联后，计算最合适的描述子来描述该地图点，用于之后跟踪的匹配。

        // b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
        pMP->ComputeDistinctiveDescriptors();
        // c.更新该MapPoint平均观测方向以及观测距离的范围
        pMP->UpdateNormalAndDepth();

• 关键帧的特征点与地图点关联

一个关键帧上特征点由多个地图点投影而成，将关键帧与地图点关联。

        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

• 关键帧与关键帧关联

关键帧之间会共视一个地图点，如果共视的地图点个数越多，说明这两个关键帧之间的联系越紧密。对于某个关键帧，统计其与其他关键帧共视的特征点个数，如果大于某个阈值，那么将这两个关键帧进行关联。

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

（3）Global BA

​ 对上述只有两个关键帧和地图点的地图进行全局BA。

    // 步骤5：BA优化
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

（4） 地图尺寸初始化（中值深度为1）

​ 由于单目SLAM的的地图点坐标尺寸不是真实的，比如x=1可能是1m也可能是1cm。选择地图点深度的中位数作为单位尺寸1当作基准来进行地图的尺寸初始化

---

跟踪匀速运动模型

TrackWithMotionModel()

假设匀速运动模型，对上一帧的MapPoints进行跟踪，用上一帧位姿和速度估计当前帧位姿。

（1）上一帧地图点投影到当前帧，完成匹配。

int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

这是ORBmatcher类中定义的类成员函数SearchByProjection（）。

如果匹配点不够20个，扩大面积继续投影。

（2）优化相机位姿，BA算法，最小二乘法。

Optimizer::PoseOptimization(&mCurrentFrame);

（3）剔除优化后的outlier匹配点

跟踪参考关键帧模型

TrackReferenceKeyFrame()

在运动模型还未建立或者刚完成重定位时，通过BoW进行当前帧与参考关键帧特征点匹配，进而优化位姿

（1）将当前帧的描述子转化为BoW向量，与关键帧的BoW进行匹配。

mCurrentFrame.ComputeBoW();
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

SearchByBoW（）是ORBmatcher的类成员函数。

（2）将上一帧的位姿态作为当前帧位姿的初始值

mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些

（3）通过优化3D-2D的重投影误差来获得位姿

（4）剔除优化后的outlier匹配点（MapPoints）

---

重定位

Relocalization()

当跟踪失败，将当前帧转成BoW向量向所有关键帧中查找与当前帧有充足匹配点的候选帧，Ransac迭代，PnP求解位姿，再通过最小化重投影误差对位姿进行优化。

（1）计算当前帧的BoW映射

（2）找到与当前帧相似的候选关键帧

vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

（3）匹配当前帧与找到的候选关键帧，计算相机位姿Tcw（RANSAC+PNP算法）

int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);

 通过BoW向量匹配上，之后初始化PnPSolver，使用RANSAC+PNP算法完成位姿估计。

                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
                vpPnPsolvers[i] = pSolver;
                nCandidates++;

           PnPsolver* pSolver = vpPnPsolvers[i];
            cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

（4）优化相机位姿Tcw

 

总结：以上三种方法都是基于帧间匹配，优化3D-2D重投影误差得到当前帧的初始位姿。这样得到的初始位姿不是可靠的，其只利用了两帧数据的信息，如果前一帧质量太差，得到的位姿可信度低。因此为了利用更多的信息，需要进一步将当前帧与局部地图进行匹配和优化，也就是TrackLocalMap() 。

---

跟踪局部地图

TrackLocalMap（）

对相机位姿有个基本估计和初始特征匹配，将局部地图投影到当前帧寻找更多对应的匹配地图点MapPoints，优化相机位姿。

随着相机运动，我们向局部地图添加新的关键帧和3D地图点来维护局部地图，这样，即使跟踪过程中某帧出现问题，利用局部地图，我们仍然可以求出之后那些帧的正确位姿。以下是进行局部地图跟踪的流程：

（1）UpdateLocalMap（）更新局部地图（关键帧UpdateLocalKeyFrames()+局部地图点UpdateLocalPoints()）；

更新局部关键帧mvpLocakKeyFrames步骤如下：

1. 搜索具有共视关系的关键帧。由之前的帧间匹配，我们得到当前帧的地图点（MapPoint），通过遍历这些地图点，得到也能观测到这些地图点的关键帧（地图点上记录了共视信息）。同时记录关键帧的共视次数。关键帧和共视次数存储在map keyframeCounter;
2. 将步骤1得到的关键帧全部插入到mvpLocakKeyFrames；
3. 遍历步骤1keyframeCounter中每个关键帧，得到与之共视程度最高的10个关键帧，也全部插入到mvpLocakKeyFrames；
4. 遍历步骤1keyframeCounter中每个关键帧，将每个关键帧的子关键帧（>=1）和父关键帧(=1，共视程度最高的之前的关键帧)都插入到mvpLocakKeyFrames ；
5. 更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧。
    

更新局部地图点mvpLocalMapPoints步骤如下：

1. 清空mvpLocalMapPoints；mvpLocalMapPoints.clear();
2. 遍历局部关键帧mvpLocakKeyFrames，将局部关键帧的地图点插入到mvpLocalMapPoints。

（2）SearchLocalPoints（）匹配局部地图视野范围内的点和当前帧特征点；

步骤如下：

1. 遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索，因为当前的MapPoints一定在当前帧的视野内。这些MapPoints是在帧间匹配过程中用到的。
2. 将局部地图点mvpLocalMapPoints投影到当前帧，投影矩阵是通过帧间匹配得到。判断局部地图点是否在当前帧的视野内isInFrustum（视野内，mbTrackInView 为true），只有视野内的地图点才进行投影匹配。判断要求包括：投影点是否在图像内；地图点到相机中心距离是否在尺寸变换范围内；相机中心到地图点向量与地图点平均视角夹角是否小于60°。
3. 对视野内的地图点通过投影进行特征匹配：

        matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);，得到一组3D局部地图点-2D特征点匹配对。

（3）PoseOptimization（）根据新的匹配点重新优化相机位姿。

总结：可以看出，这几步和TrackWithMotionModel类似，只不过TrackWithMotionModel匹配点对是通过帧间匹配得到的，而TrackLocalMap是剔除TrackWithMotionModel中的匹配点，从局部地图中找新的更多的匹配点，从而实现对位姿的进一步优化。