orbslam2代码解析--特征跟踪及初始化

ORBSLAM分定位和SLAM两个模式，定位模式localmap不工作，不插入新的关键帧以及更新地图

在跟踪时，初始化完成后首先调用的是GrabImageMonocular函数，对图像格式进行转换，将RGB转为灰度图。

然后通过图片初始化一个Frame类

mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth)

进入Track()函数，完成四个任务：

1.单目初始化；
2.通过上一帧获得初始位姿估计或者重定位。也就是求出当前帧在世界坐标系下的位姿
3.跟踪局部地图TrackLocalMap() 求得位姿估计, 这一步判断了当前帧的特征点和map中的哪些mappoint匹配。
4.判断是否需要给localmapping插入关键帧

1. 单目初始化

双目和RGBD只需要一帧图片即可初始化，单目需要两帧才能完成，其流程为：

1.根据得到的两帧图像的特征匹配，我们就可以计算单应矩阵H，和基础矩阵F

2.计算出的单应矩阵H，和基础矩阵F会有对应的得分，根据得分情况来选择使用单应矩阵H，还是基础矩阵F来进行初始化

3.通过H或者F分解为R，t时，会有多个R，t的组合，需要将匹配的特征点通过三角化重投影后，更具投影结果来选择具体是哪个R，t的组合

4.R，t后，我们自然也通过三角化得到了通过三角化重投影成功的匹配特征点的相对第一帧的3d坐标。剩下那些不能通过R，t重投影成功的匹配点，在后面将它剔除

5.将初始化第一帧和第二帧转化为关键帧keyframe，将三角化重投影成功的匹配特征点的3d坐标包裹为mappoint，全部添加到map中。

6.在创建mappoint过程中，会记录它能被哪些keyframe的哪些特征点匹配。

// step 4.3：表示该KeyFrame的哪个特征点可以观测到哪个3D点
    pKFini->AddMapPoint(pMP,i);
    pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
// a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);

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

7.更新关键帧间的图连接关系

​ 1.首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。

map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数

​ 2.并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）

for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) { // 更新具有最佳共视关系的关键帧信息 if(mit->second>nmax) { nmax=mit->second; // 找到对应权重最大的关键帧（共视程度最高的关键帧） pKFmax=mit->first; }

if(mit->second>=th)//th=15 { // 对应权重需要大于阈值，对这些关键帧建立连接 vPairs.push_back(make_pair(mit->second,mit->first)); // 对方关键帧也要添加这个信息 // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重 (mit->first)->AddConnection(this,mit->second); } }

​ 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理

​ 4.更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树

    // 更新图的连接(权重)
        mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end()); 更新生成树的连接
    if(mbFirstConnection && mnId!=0)
    {
        // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
        mpParent = mvpOrderedConnectedKeyFrames.front();
        // 建立双向连接关系
        mpParent->AddChild(this);
        mbFirstConnection = false;
    }`

8. BA优化两帧以及地图点

9. 将MapPoints的中值深度归一化到1，并归一化两帧之间变换

评估当前关键帧场景深度，q=2表示中值. 只是在单目情况下才会使用，其实过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值也就是中位数作为当前场景的平均深度

float medianDepth = pKFini->ComputeSceneMedianDepth(2);
float invMedianDepth = 1.0f/medianDepth;//两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于100
if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
{
    cout << "Wrong initialization, reseting..." << endl;
    Reset();
    return;
}`

将深度中值作为单位一，归一化第二帧的位姿与所有的地图点 。

//将两帧之间的变换归一化到平均深度1的尺度下 // Scale initial baseline cv::Mat Tc2w = pKFcur->GetPose(); // x/z y/z 将z归一化到1 Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth; pKFcur->SetPose(Tc2w);

// 把3D点的尺度也归一化到1 //? 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? 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); } }

至此单目初始化完成。

2.相机位姿跟踪

初始化完成后，对于相机获取当前图像mCurrentFrame，通过跟踪匹配上一帧mLastFrame特征点的方式，可以获取一个相机位姿的初始值；为了兼顾计算量和跟踪鲁棒性，追踪部分主要用了三种模型：运动模型（TrackWithMotionModel）、关键帧（TrackReferenceKeyFrame）和重定位（Relocalization）。三种跟踪模型都是为了获取相机位姿一个粗略的初值，后面会通过跟踪局部地图TrackLocalMap对位姿进行BundleAdjustment（捆集调整），进一步优化位姿。

几个命名的区别

1. 上一帧mLastFrame：即上一帧图像
2. 上一关键帧mpLastKeyFrame：最邻近当前帧的关键帧，不一定是上一帧，因为图像帧要经过判断后，满足条件才能称为关键帧
3. 参考关键帧mpReferenceKF：就是当前帧的上一关键帧；若创建了新的关键帧，参考关键帧就更新为新创建的关键帧；或者是局部地图中与当前帧共享地图点最多的关键帧。

1. TrackwithMotionModel

当上一帧跟踪正常的时候，我们会生成一个速度模型mVelocity，使得mVelocity不为空，下一帧就会进入TrackWithMotionModel()。

速度模型mVelocity就是上一帧和上上帧的位姿变化。由于在短时间内，速度模型不会变化太大，这样我们可以认为上一帧的速度模型（即上一帧和上上帧的位姿变化）等于当前帧的速度模型（即当前帧和上帧的位姿变化）。根据上一帧的求得的速度模型，以及上一帧在世界坐标系的位姿得到当前帧在世界坐标系中的初始位姿T0

       if(!mLastFrame.mTcw.empty())
        {
            // step 2.3：更新恒速运动模型 TrackWithMotionModel 中的mVelocity
            cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
            //转换成为了相机相对世界坐标系的旋转
            mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
            mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
            mVelocity = mCurrentFrame.mTcw*LastTwc; // 其实就是 Tcl
        }
        else
            //否则生成空矩阵
            mVelocity = cv::Mat();

我们将上一帧匹配的mappoint，通过T0 投影到当前帧上，然后在投影位置附近搜索当前帧的特征点以加速上一帧mappoint和当前帧的特征点匹配。也就是说，以上一帧为跳板，将当前帧的特征点与空间中mappoint进行匹配。

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

有了当前帧的初始位姿，以及当前帧的特征点与空间中mappoint进行匹配关系。可以在通过bundle adjustment方法优化得到当前帧的较为精确的位姿T1

得到当前帧较为精确的位姿后，当前帧的有些特征点和mappoint匹配通过这个T1 将不成立，形成误匹配于是将它标记为outliner后取消它们之间的匹配。

2. TrackReferenceKeyFrame

如果恒速模型不被满足,那么就只能够通过参考关键帧来定位了， 可以尝试和最近一个关键帧（即参考关键帧）去做匹配。利用了bag of words（BoW）来加速匹配。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配；最后，利用匹配的特征优化位姿

int nmatches = matcher.SearchByBoW( mpReferenceKF, //参考关键帧 mCurrentFrame, //当前帧 vpMapPointMatches); //存储匹配关系

3.Relocalization

如果正常的初始化不成功,或者 mState=LOST ，那么就只能重定位了。

当速度模型不能用，参考关键帧也没有的时候，就意味着tracking已经跟丢了。这时候，我们先将当前帧转化为BOW，然后将代表当前帧的BOW交给KeyFrameDatabase，它就会返回一个和当前帧很相似的集合作为候选帧。

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

接着，我们对当前帧和候选关键帧进行通过BOW加速的特征点匹配。并剔除特征匹配数少于15的候选帧。

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

对于剩下的候选关键帧我们通过epnp结合RANSAC算法进行筛选，并算出当前帧在世界坐标系中的位姿T2

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

对求解结果使用BA优化，如果内点较少，则反投影当前帧的地图点到候选关键帧获取额外的匹配点；若这样依然不够，放弃该候选关键帧，若足够，则将通过反投影获取的额外地图点加入，再进行优化。

如果内点满足要求(>50)则成功重定位，将最新重定位的id更新：mnLastRelocFrameId = mCurrentFrame.mnId;　否则返回false。

3.TrackLocalMap

通过上面三种模型获取了初始的相机位姿和初始的特征匹配，就可以将完整的地图（地图点）投影到当前帧中，去搜索更多的匹配。但是投影完整的地图，在large scale的场景中是很耗计算而且也没有必要的，因此，这里使用了局部地图LocalMap来进行投影匹配。对局部地图的更新包括对局部关键帧和局部地图点的更新。局部地图包含：与当前帧相连的关键帧K1（所有能观察到当前帧对应地图点的关键帧，图中Pos2），以及与K1相连的关键帧K2（一级二级相连关键帧，图中Pos1），并且限制了关键数量不超过80；K1、K2对应的地图点（图中X1）；参考关键帧Kf。局部地图就是红色椭圆圈出的部分，参与局部优化，其中红色代表取值会被优化，灰色代表取值保持不变。

1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints

UpdateLocalMap();

局部关键帧的选择策略：

策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧

策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧

​ 最佳共视的10帧，如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧；自己的子关键帧

2.在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪

SearchLocalPoints();

3.更新局部所有MapPoints后对位姿再次优化

Optimizer::PoseOptimization(&mCurrentFrame);

4. 更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果

4.LocalMapping局部建图

LocalMapping线程由LocalMapping::Run()进入；
它主要做了以下几件事：

• 向map中插入关键帧；
• 新近插入的mappoint的剔除；
• 新建mappoint；
• 局部捆集调整（BA）；
• 剔除冗余关键帧；

首先判断是否需要插入关键帧

1：如果用户在界面上选择定位模式，那么将不插入关键帧

2：判断是否距离上一次插入关键帧的时间太短

if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && //距离上一次重定位不超过1s nKFs>mMaxFrames) //地图中的关键帧已经足够 return false;

3:得到参考关键帧跟踪到的MapPoints数量, which 参考帧地图点的被观测的数目大于等于 nMinObs

int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);

4.查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧

bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

5.决策是否需要插入关键帧：

1. 距离上一次重定位距离至少20帧；
2. 局部地图线程空闲，或者距离上一次加入关键帧过去了20帧；如果需要关键帧插入（过了20帧）而LocalMapping线程忙，则发送信号给LocalMapping线程，停止局部地图优化，使得新的关键帧可以被及时处理（20帧，大概过去了不到1s）；
3. 当前帧跟踪至少50个点；确保了跟踪定位的精确度；
4. 当前帧跟踪到LocalMap中参考帧的地图点数量少于90%；确保关键帧之间有明显的视觉变化。

创建新的关键帧

void Tracking::CreateNewKeyFrame()

将关键帧传递到LocalMapping线程（mpLocalMapper->InsertKeyFrame(pKF)），再由LocalMapping完成其他工作。此外，在创建关键帧之后，对于双目、RGB-D的情况，会使用深度值大于0的关键点重投影得到地图点，得到的地图点会和当前关键帧关联，并加入到Map。

最后记录位姿信息，用于轨迹复现