ORBSlam2学习研究(Code analysis)-ORBSlam2中的视觉里程计Tracking

写在前面

代码理解帮助

ORBSlam2之所以易于学习不仅仅因为它所使用的方法非常的规范，而且他的变量与函数命名也非常规整，非常易于阅读，在这里我整理出一些其函数的规则。

变量命名规范

• Im 图像 Image 如：mImGray
• m 地图或映射 map 如：mState或mObservations
• p 指针 pointer
• b 布尔 bool 如：bOK 代表该变量是一个bool值的变量
• v 向量 vector 如：vpAllMapPoints 代表该变量是一个vector向量
• n 数量 number 如：nmatches 表示计数
• l 列表 list 如：lLocalKeyFrames 表示一个列表

比如：
mvpMapPoints的意思就是地图点MapPoint的指针向量vector

代码入口

本文基于ORBSlam2的Monocular模式撰写，所涉及的算法都是单目视觉下的算法。
ORBSlam2的线程启动写在System类的构造函数，Tracking，LocalMapping和LoopClosing三个线程在System函数中启动，其中Tracking运行在主线程中。

线程启动后，ORBSlam2再通过调用TrackMonocular(im,tframe)将图像传给Tracking线程，而TrackMonocular函数中调用了Tracking类的GrabImageMonocular(im,timestamp)，才真正进入Tracking的图像处理过程。

Tracking

Tracking的主要流程

图像预处理 Pre-process Input

拿到图像之后

1. 程序先对图像进行灰度化处理，将RGB图像或RGBA图像转换为灰度图
2. 接着初始化帧数据，也就是将图像数据转换为系统能够识别的Frame帧，计算图像的ORB特征，在这一步进行。

位姿估计 Pose Prediction

在位姿估计的部分里，在三处地方都进行了位姿的估计和优化，分别是地图自动初始化，初始化后的相机位姿估计和重定位，在进行位姿估计的时候，均使用到了对极约束，三角测量，纯运动BA算法，局部BA算法和全局BA算法。

1. 地图自动初始化

刚刚启动程序的时候，进入MonocularInitialization函数进行相机的初始化。
第一次进入该函数的时候，因为只有一帧数据，没有历史数据，所以要将该帧赋值给初始帧和最后一帧。

第二次进入该函数的时候，系统的到了两帧的数据，即当前帧和初始帧，那么就可以用这两帧数据进行位姿估计，从而初始化。

• 首先 判断关键点是否少于100，如果少于100的话，就不用该帧作为初始化帧。
• 接着 用ORB匹配器的匹配方法在当前帧和初始帧之间进行匹配，得到两帧之间关键点的两两匹配结果。
• 然后 使用八点法构造对极约束，并用对极约束开启两个线程，分别计算单应矩阵和基础矩阵，通过参数的比值来确定本次恢复相机姿态的方式是使用单应矩阵还是基础矩阵。得到一个大概准确的位姿。

thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));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);//从基础矩阵中恢复运动轨迹

• 接着 将这两帧当做关键帧，计算他们的BoW值，并通过之前三角测量时记录下的点的深度计算点的3D坐标并生成MapPoint三维点，并添加到地图中。
• 然后 通过全局BundleAdjustmentOptimizer::GlobalBundleAdjustemnt(mpMap,20); 去同时更加精确的优化相机的位姿和地图点的位置。
• 最后 由于单目相机无法确定尺度，所以最后需要初始化一下单位尺度信息，并初始化点信息和基线信息。
  

2. 初始化后的相机位姿估计

分别针对两种不同的情况，代码中共有两个地方进行了相机的位姿估计

• TrackWithMotionModel() 当上一帧的相机位姿有速度，可以通过上一帧的图像数据来估计当前这一帧数据的相机位姿。
• TrackReferenceKeyFrame() 如果上一帧相机没有速度，或通过上一帧估计位姿失败，那么使用附近关键帧来估计当前相机的位姿。
  

2.1 TrackWithMotionModel

1. 先通过上一帧的位姿和速度预测当前帧相机的位姿
   
2. 通过PnP方法估计相机位姿，在将上一帧的地图点投影到当前固定大小范围的帧平面上，如果匹配点少，那么扩大两倍的采点范围。
   
   从上面的代码可以看出，只有在匹配点大于20个的时候，才算是匹配成功
3. 然后进行一次BA算法，通过最小二乘法优化相机的位姿。
   
4. 优化位姿之后，对当前帧的关键点和地图点，抛弃无用的杂点，剩下的点供下一次操作使用。
   

2.2 TrackReferenceKeyFrame

1. 按照关键帧进行Track的方法和运动模式恢复相机运动位姿的方法接近。首先求解当前帧的BOW向量。
   
2. 再搜索当前帧和关键帧之间的关键点匹配关系，如果这个匹配关系小于15对的话，就Track失败了。
   
   在这里的关键帧，指的是初试关键帧或者是局部地图中看到MapPoint最多的关键帧，在Tracking和Frame中都有一个mpReferenceKF变量，这里使用的是Tracking的成员变量。
3. 接着将当前帧的位置假定到上一帧的位置那里
   mCurrentFrame.SetPose(mLastFrame.mTcw);
4. 并通过最小二乘法优化相机的位姿。
   Optimizer::PoseOptimization(&mCurrentFrame);
5. 最后依然是抛弃无用的杂点，当match数大于等于10的时候，返回true成功。

3. 重定位 Relocalization

Relocalization() 如果相机Tracking丢失了，那么使用重定位方式来估计相机当前的位姿。
从之前的关键帧中找出与当前帧之间拥有充足匹配点的候选帧，利用Ransac迭代，通过PnP求解位姿。

1. 先计算当前帧的BOW值，并从关键帧数据库中查找候选的匹配关键帧
   
   这里的查找方法是查找所有关键帧中和当前帧有相同BoW，并去计算这些筛选出来的帧和当前帧之间的similarity score，再通过Covisibility图去累计算得的分数，这样离当前帧越近的关键帧得分就越高，就可以得到附近的邻居关键帧的指针，最后返回所有分数大于75%最高分的关键帧，作为候选帧。

   

   PnP之前的预处理，标记杂点并构造PnP求解器

2. 用PnP算法求解位姿，进行若干次P4P Ransac迭代，并使用非线性最小二乘优化，直到发现一个有充足inliers支持的相机位置。
   

   int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
   

3. 返回成功或失败
   
   只有在优化后的inliers数量大于50的时候，才会返回true

精确估计相机位姿 Track Local Map

在估计好相机位姿和地图点位姿之后，需要跟踪局部地图，这个过程相当于是通过投影方法，从刚才生成的地图点MapPoint中找到更多的匹配关系，来精确我们的估计结果。

1. 更新局部关键帧
   统计观察到当前帧观察到的每个地图点MapPoint的关键帧，如果这个地图点是不可用的，那么排除该地图点。
   
   然后通过上面的统计，把所有观察到地图点的关键帧都囊括到局部地图中，同时检查哪些关键帧共同包含了大部分的地图点。
   注意这里的pKFmax 在后面他会被赋值给mpReferenceKF和mCurrentFrame.mpReferenceKF 也就是当前Tracking线程和当前帧的参考关键帧。
   
   接着要把这些关键帧的邻居关键帧也包含到该局部地图中，这里就不贴代码了，代码比较长

2. 更新局部地图点并进行地图点的筛选
   更新局部地图点的过程，实际上就是把刚才得到的局部图中的关键帧里面的每一个地图点囊括到局部地图点的vector变量中。这一过程比较简单，函数是void Tracking::UpdateLocalPoints()

   • 将地图点投影到当前帧上，如果超出图像范围，就将其舍弃
     

   • 计算当前地图点到相机的距离，若这个距离超出某个阈值，那么则抛弃该点
     

   • 计算当前视线方向向量v与地图点云平均视线方向向量n的夹角，舍弃n·v < cos(60°)的点云，图中的0.5代表的是cos(60)
     
     并且这里会计算图像的尺度因子
     const int nPredictedLevel = pMP->PredictScale(dist,this);

3. BA优化相机位姿 Optimizer::PoseOptimization(&mCurrentFrame);

4. 更新地图点的统计量
   

生成关键帧 New KeyFrame Decision

关键帧的生成分为两步，先判断是否需要生成关键帧，在进行关键帧的生成。

1. 是否需要生成关键帧
   生成关键帧需要满足以下几个条件

   • 在上一次进行重定位之后，过了20帧数据，或关键帧数小于20个，不满足不能生成
     
   • 在上一个关键帧插入之后，过了20帧，或 局部建图是空闲状态，不满足不能生成。
     const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
   • 当前帧跟踪到大于若干个点，不满足不能生成
     const bool c1c = mSenso r!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
   • 当前帧的跟踪点数小于90%的参考关键帧跟踪点数，并且当前帧跟踪点数大于15，不满足不能生成
     const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);

2. 关键帧的生成
   void Tracking::CreateNewKeyFrame()
   关键帧生成的部分比较少，满足条件的话，就把当前帧作为关键帧，并更新地图点，关键帧序列等变量

参考链接：
sylvester的博客http://blog.csdn.net/u010128736/article/details/53339311
ORB-SLAM2:一种开源的VSLAM方案http://www.sohu.com/a/154011668_715754