ORB_SLAM2代码阅读(2)——tracking线程

约定不等于承诺〃 2022-04-24 10:00 435阅读 0赞

### ORB\_SLAM2代码阅读（2）——Tracking线程 ###

*  1. 说明
 *  2. 简介
 *   *  2.1 Tracking 流程
     *  2.2 Tracking 线程的二三四
     *   *  2.2.1 Tracking 线程的二种模式
         *  2.2.2 Tracking 线程的三种方法
         *  2.2.3 Tracking 线程的四种状态
     *  2.3 Tracking 类的成员变量
 *  3. Tracking线程的各个功能模块
 *   *  3.1 初始化
     *  3.2 相机位姿跟踪
     *   *  3.2.1 运动模型跟踪
         *  3.2.2 参考关键帧跟踪
         *  3.2.3 重定位（三次匹配三次优化）
     *  3.2 局部地图跟踪
     *  3.3 关键帧处理
     *   *  3.3.1 插入关键帧的判定依据
         *  3.3.2 判定是否插入关键帧的整个流程
         *  3.3.3 创建关键帧
     *  3.4 姿态保存
 *  4.结束语

# 1. 说明 #

本文接着上一篇对于ORB-SLAM的系统介绍继续记录ORB-SLAM2的主线程tracking的相关内容。有很多细节部分还没有弄清楚，暂时先将整体思路捋顺。本文的内容最好参照着代码一起阅读，由于篇幅较长，并未在文中插入太多代码。在将整个ORB\_SLAM2系统阅读完毕后，我在公开我注释的代码。

# 2. 简介 #

## 2.1 Tracking 流程 ##

ORB-SLAM的tracking线程作为系统的主线程，也是SLAM前端视觉里程计的主要内容，实现的主要内容就是**确定每一帧图像的位姿和确定关键帧**。

由系统整体框架可知，tracking部分的主要内容有以下几个部分：  
![在这里插入图片描述][20190508153511349.png]  
在阅读了代码之后，发现将tracking部分的主要内容划分为以下几个模块比较合适：  
![在这里插入图片描述][20190510001036147.png]  
下文将对各个部分进行详述。

## 2.2 Tracking 线程的二三四 ##

### 2.2.1 Tracking 线程的二种模式 ###

Tracking线程由两种模式：1.纯追踪模式 2.同步定位与建图模式（默认模式）

1.  **纯追踪模式**：不插入新的关键帧，不添加新的地图点，局部地图线程不工作，而且回环检测线程也不会工作，只会追踪地图中现有的地图点。
2.  **同步定位与建图模式**：在追踪线程的同时有局部建图和回环检测

### 2.2.2 Tracking 线程的三种方法 ###

Tracking线程中涉及三种位姿计算方法：运动模型跟踪、参考关键帧跟踪、重定位

1.  **运动模型跟踪**：匹配方式使用上一帧特征点投影到当前帧的方式进行匹配，得到的匹配地图点作为图优化节点，然后根据上一帧的位姿和上一帧位姿的变换速度得到当前帧的初始位姿，用位姿图优化进行位姿优化得到当前帧位姿。
2.  **参考关键帧跟踪**：匹配方式使用关键帧BOW向量与当前帧进行匹配，通过该匹配方式得到地图点得到的地图点作为图优化节点，然后将上一帧的位姿作为初始位姿，用位姿图优化进行位姿优化得到当前帧位姿。
3.  **重定位**：匹配方式是将局部地图点（除去1、2已经匹配过的地图点剩下的局部地图点！！！）投影到到当前帧下进行匹配，得到的匹配地图点作为图优化节点，然后根据上面1、2得到帧位姿作为当前帧的初始位姿，用位姿图优化进行位姿优化得到当前帧位姿。

### 2.2.3 Tracking 线程的四种状态 ###

Tracking线程有四种状态：1.NO\_IMAGES\_YET 2.NOT\_INITIALIZED 3.OK 4.LOST

1.  **NO\_IMAGES\_YET**：表示当前没有图片。处于NO\_IMAGES\_YET，当新的一帧来临时，将线程状态改变为NOT\_INITIALIZED。
2.  **NOT\_INITIALIZED**：表示当前没有初始化追踪线程。处于NOT\_INITIALIZED，则针对单目相机和双目相机/RGBD相机进行不同的初始化。
3.  **OK**：表示当前追踪线程完好。处于OK，经过初始化后追踪线程就转为OK状态，在没有丢帧或者是复位的情况下系统将一直处于OK状态。处于OK状态的系统就可以进行位姿估计，地图点追踪。
4.  **LOST**：表示当前追踪线程丢失——注意这里的线程状态都是指当前帧处理之前的状态。处于LOST状态，上一帧追踪失败，当前帧进行重定位。

## 2.3 Tracking 类的成员变量 ##

Tracking 类中由很多成员变量。直接看代码会很难理解成员变量代表的意义，所以先简单介绍一下成员变量。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70]  
其中较为重要的变量有：

<table> 
 <thead> 
  <tr> 
   <th>变量名</th> 
   <th align="center">变量类型</th> 
   <th align="center">说明</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>mState</td> 
   <td align="center">eTrackingState</td> 
   <td align="center">跟踪状态标志</td> 
  </tr> 
  <tr> 
   <td>mbOnlyTracking</td> 
   <td align="center">bool</td> 
   <td align="center">跟踪模式标志</td> 
  </tr> 
  <tr> 
   <td>mCurrentFrame</td> 
   <td align="center">Frame</td> 
   <td align="center">当前帧</td> 
  </tr> 
  <tr> 
   <td>mLastFrame</td> 
   <td align="center">Frame</td> 
   <td align="center">上一帧</td> 
  </tr> 
  <tr> 
   <td>mpReferenceKF</td> 
   <td align="center">KeyFrame*</td> 
   <td align="center">参考关键帧</td> 
  </tr> 
  <tr> 
   <td>mpLastKeyFrame</td> 
   <td align="center">KeyFrame*</td> 
   <td align="center">上一关键帧</td> 
  </tr> 
  <tr> 
   <td>mvpLocalKeyFrames</td> 
   <td align="center">std::vector&lt;KeyFrame*&gt;</td> 
   <td align="center">局部地图关键帧</td> 
  </tr> 
  <tr> 
   <td>mvpLocalMapPoints</td> 
   <td align="center">std::vector&lt;MapPoint*&gt;</td> 
   <td align="center">局部地图的地图点</td> 
  </tr> 
  <tr> 
   <td>mpMap</td> 
   <td align="center">Map*</td> 
   <td align="center">指代整个地图</td> 
  </tr> 
  <tr> 
   <td>mlRelativeFramePoses</td> 
   <td align="center">list &lt; cv::Mat&gt;</td> 
   <td align="center">图像帧与其参考关键帧之间的变换关系链表（用于绘制轨迹）</td> 
  </tr> 
  <tr> 
   <td>mlpReferences</td> 
   <td align="center">list&lt;KeyFrame*&gt;</td> 
   <td align="center">每一帧的参考关键帧 （用于绘制轨迹）</td> 
  </tr> 
  <tr> 
   <td>mlFrameTimes</td> 
   <td align="center">list&lt; double &gt;</td> 
   <td align="center">每一帧的时间戳（用于绘制轨迹）</td> 
  </tr> 
 </tbody> 
</table>

其它变量在遇到的时候在做说明！

# 3. Tracking线程的各个功能模块 #

Tracking线程的入口是TrackStereo()，其中GrabImageStereo()返回位姿。GrabImageStereo()成员函数将输入图像转换为灰度图并构建当前帧，然后调用Track()函数。**调用Track()函数表示进入了真正的跟踪流程**。

进入跟踪流程后，下面开始介绍其中的各个功能模块。

## 3.1 初始化 ##

这里的初始化指的是追踪过程的初始化环节，而不是Tracking类构造函数中的初始化内容。顺便说一下，构造函数中根据配置文件设置了相应的参数并声明了ORB特征点提取器。ORB特征提取的过程在构造当前帧的时候进行。

图像传输正常的情况下，追踪过程的第一步就是判断是否已经初始化。

判断部分相应代码为：

if(mState==NOT_INITIALIZED)// ———————————————未初始化
        {
            if(mSensor==System::STEREO || mSensor==System::RGBD)
                StereoInitialization();    //双目和rgbd地图的初始化
            else
                MonocularInitialization(); // 单目初始化
    
            mpFrameDrawer->Update(this);   //更新帧的观测器
    
            if(mState!=OK)
                return;
        }
        else                      // ————————————————已初始化
        {
           。。。    
        }

下面介绍双目初始化过程。整个双目初始化的流程为：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 1]  
初始化过程相对简单，逻辑并不复杂。我在代码中进行了注释，在此不在赘述。

/*******************************************************************************
     *     函数属性：类Tracking的成员函数StereoInitialization()
     *     函数参数介绍：NULL
     *     备注：双目或者RGBDSLAM的第一帧处理函数，地图初始化
     ******************************************************************************/
    void Tracking::StereoInitialization()
    {
        //当前帧关键点的数量大于500，才将此帧作为初始帧并认为其为关键帧
        if(mCurrentFrame.N>500)    
        {
            // 设置初始帧的位姿
            mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            // 将当前帧（第一帧）作为初始关键帧（调用关键帧的构造函数）
            KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
            // 将关键帧插入地图中.  KeyFrame中包含了地图、反过来地图中也包含了KeyFrame，相互包含
            mpMap->AddKeyFrame(pKFini);
            //  创建地图点并将其与关键帧建立联系
            for(int i=0; i<mCurrentFrame.N;i++)
            {
                float z = mCurrentFrame.mvDepth[i];                       //获取当前帧第i个关键点的深度值
                if(z>0)
                {
                    cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);      //将当前帧的第i个特征点反投影到3D世界坐标系下
                    MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);   //用该特征点构造新的地图点
                    pNewMP->AddObservation(pKFini,i);                    //地图点添加关键帧  说明该地图点属于哪一关键帧
                    pKFini->AddMapPoint(pNewMP,i);                       //关键帧添加地图点  表明在该关键帧下可以看到该地图点
                    pNewMP->ComputeDistinctiveDescriptors();             //从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
                    pNewMP->UpdateNormalAndDepth();                      //更新该MapPoint平均观测方向以及观测距离的范围
                    mpMap->AddMapPoint(pNewMP);                          //将新的地图点加入到地图中
                    // 将地图点加入到当前针的mvpMapPoints中，为当前Frame的特征点与MapPoint之间建立联系
                    mCurrentFrame.mvpMapPoints[i]=pNewMP; 
                }
            }
    
            cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
           
            //在局部地图中添加该初始关键帧
            mpLocalMapper->InsertKeyFrame(pKFini);                              
            //更新上一帧为当前帧
            mLastFrame = Frame(mCurrentFrame);
            mnLastKeyFrameId=mCurrentFrame.mnId;
    	    // 更新上一关键帧为当前关键帧
            mpLastKeyFrame = pKFini;
    
            mvpLocalKeyFrames.push_back(pKFini);           //将初始关键帧加入到局部地图的关键帧
            mvpLocalMapPoints=mpMap->GetAllMapPoints();    //将全部地图点加入到当前局部地图点
            mpReferenceKF = pKFini;                        //将当前关键帧作为参考关键帧
            mCurrentFrame.mpReferenceKF = pKFini;          //将该关键帧作为当前帧的参考关键帧
    
            mpMap->SetReferenceMapPoints(mvpLocalMapPoints);//将当前局部地图点作为整个地图参考地图点,用于画图
            mpMap->mvpKeyFrameOrigins.push_back(pKFini);    //将关键帧加入地图的原始的关键帧
    
            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); //将当前帧加入到地图观测器
    
            mState=OK;  //更新跟踪状态
        }
    }

## 3.2 相机位姿跟踪 ##

这部分内容是整个跟踪流程中非常重要的一部分，逻辑也较为复杂。

首先说明一下该模块的主要目的是：**对当前帧的位姿进行初步估计并优化当前帧对应的地图点，这里之所以说是对位姿进行初步估计是因为在局部地图跟踪环节还要对位姿进行优化。**

该部分内容的逻辑流程：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 2]  
从流程图中可以看到相机位姿跟踪主要涉及三种跟踪方法：**运动模型跟踪**、**参考关键帧跟踪**、**重定位**（跟踪丢失时）。

值得一提的是，代码中有一个 mbVO 变量。这个变量只在纯定位模式下才被使用，mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常；mbVO为true表明此帧匹配了很少的MapPoints，少于10个，跟踪效果不好。

在纯跟踪模式下，当mbVO为true时，如果速度mVelocity不为空。则运动模型跟踪与重定位同时进行。定位和跟踪的结果分别用bOKMM和bOKReloc表示。 如果是跟踪成功且重定位失败，那么结果为跟踪的结果，但只要是重定位成功，那么整个跟踪过程就正常进行(定位与跟踪，更相信重定位)。 最后只要是跟踪和重定位只要一个成功，那么结果就正常。将最新的关键帧作为reference frame。

该部分代码为：

// 这时候既做跟踪又做重定位，定位和跟踪的结果分别用bOKMM和bOKReloc表示，只要mVelocity不为空就做基于恒速模型跟踪。
     // 如果是跟踪成功重定位失败，那么结果借用跟踪的结果，但只要是重定位成功，那么整个跟踪过程就正常进行(定位与跟踪，更相信重定位)，
    // 最后只要是跟踪和重定位只要一个成功，那么结果就正常。将最新的关键帧作为reference frame
     bool bOKMM = false;
     bool bOKReloc = false;
     vector<MapPoint*> vpMPsMM;
     vector<bool> vbOutMM;
     cv::Mat TcwMM;
     if(!mVelocity.empty())
      {
            bOKMM = TrackWithMotionModel();    //根据运动模型进行追踪
            vpMPsMM = mCurrentFrame.mvpMapPoints;
            vbOutMM = mCurrentFrame.mvbOutlier;
            TcwMM = mCurrentFrame.mTcw.clone();
       }
       bOKReloc = Relocalization();           //重定位获取相机位姿
    
       if(bOKMM && !bOKReloc)                 // 跟踪成功,重定位失败
         {
              mCurrentFrame.SetPose(TcwMM);
              mCurrentFrame.mvpMapPoints = vpMPsMM;
               mCurrentFrame.mvbOutlier = vbOutMM;
    
               if(mbVO)         // 表明此帧匹配了很少的MapPoints，少于10个，跟踪效果不好  
                {
                       for(int i =0; i<mCurrentFrame.N; i++)
                        {
                           if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                           {
                               mCurrentFrame.mvpMapPoints[i]->IncreaseFound();  //则将当前帧的检测到的地图点的查找次数增加
                           }
                        }
                 }
          }
         else if(bOKReloc)                      // 重定位成功  则整个跟踪进程正常进行
           {
             mbVO = false;
           }
    
         bOK = bOKReloc || bOKMM;

### 3.2.1 运动模型跟踪 ###

**运动模型跟踪的主要思想是**：

1.  根据上一帧的位姿和速度来计算当前帧的位姿。
2.  遍历上一帧中所有地图点，将上一帧的地图点向当前帧进行投影，投影过后在当前帧中找到一个描述子距离最相近的特征点作为投影点的匹配点。
3.  如果匹配点的数量满足要求，则对当前帧进行位姿优化。优化过后剔除外点。
4.  最后根据匹配点的数量判断跟踪是否成功。

具体流程如下图所示：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 3]  
需要注意的是：**这里的速度（mVelocity）指的是上一帧与上上帧之间的位姿变换关系（即，当前帧的前一帧与前两帧之间的位姿变换关系）**。

具体的匹配方法和优化方法在后面章节进行阐述。

### 3.2.2 参考关键帧跟踪 ###

当运动模型跟踪失败或mVelocity为空时，则需要进行参考关键帧跟踪。

**参考关键帧跟踪的主要思想是**：

1.  首先计算当前帧的词袋向量。
2.  根据词袋向量进行参考关键帧和当前帧进行匹配 ，得到匹配点。
3.  如果匹配点的数量满足要求，则将匹配点设为当前帧的地图点，上一帧的位姿设为当前帧的位姿，并进行优化。
4.  剔除优化后的外点。
5.  根据剔除外点后的匹配点数量判定跟踪是否成功。

具体流程如下图所示：

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NTRE4yMDI0_size_16_color_FFFFFF_t_70_pic_center]

参考关键帧跟踪与运动模型跟踪的异同点：

1.  不同点：  
    (1)、运动模型跟踪利用上一帧姿态和速度来初始化当前帧位姿，参考帧模型仅仅使用上一帧位姿来初始化当前帧位姿。  
    (2)、运动模型跟踪利用投影匹配，参考帧模型使用词袋匹配。  
    (3)、运动模型跟踪需要考虑是否为纯跟踪模式，而参考关键帧跟踪则不需要。  
    (4)、运动模型跟踪与上一帧进行匹配，而参考关键帧跟踪则是与参考关键帧进行匹配。
2.  相同点：整体思路一致，都是通过初始化位姿、匹配、优化位姿、剔除外点四步来判定跟踪状态

### 3.2.3 重定位（三次匹配三次优化） ###

重定位的过程相对与运动模型跟踪和参考关键帧跟踪要复杂一些。可以将重定位的过程总结为 **三次匹配三次优化**。

**重定位过程的主要思路为**：

1.  计算当前帧的词袋向量。
2.  根据词袋向量在关键帧数据库中挑选出候选关键帧。
3.  当前帧与每一个候选关键帧进行词袋匹配得到匹配点的数量，并根据匹配点的数量剔除一部分不合格的候选关键帧（第一次匹配）。
4.  根据匹配得到的地图点和当前帧来设置PNP求解器，PNP求解器迭代五次来初步求解当前帧的位姿。并根据是否达到最大迭代次数剔除一部分不合格的候选关键帧。
5.  将匹配得到的地图点剔除外点后赋值当前帧作为当前帧的地图点，然后进行位姿优化（第一次优化）。
6.  位姿优化之后，剔除当前帧地图点中的外点。若此时内点数较少，不能满足要求，则进行候选关键帧与当前帧之间的投影匹配，以此来增加匹配点数量。（第二次匹配）
7.  优化后的内点数与投影匹配得到的匹配点数之和 大于 阈值，则再次进行优化（第二次优化）。
8.  第二次优化后内点数若是还不满足要求，那么就再次通过缩小窗口进行投影匹配（第三次匹配）。
9.  第二次优化后的内点数与第三次投影匹配得到的匹配点数之和 大于 阈值，则进行最终优化（第三次优化）。
10. 最后，根据优化后的内点数判定重定位是否成功。

具体流程如下图所示：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 4]  
**重定位的过程中进行了三次匹配三次优化。第一次匹配为词袋匹配，用于初步确定当前帧的地图点。后两次匹配均为投影匹配，目的是为了增加匹配点，为优化位姿做准备。而三次优化的过程是为了根据匹配点不断的优化当前帧的位姿，使其满足要求。**

## 3.2 局部地图跟踪 ##

要理解局部地图跟踪的原理，首先要知道ORB\_SLAM2是如何定义局部地图的。

局部地图包括两部分：**局部地图关键帧**，**局部地图地图点**。

那么如何确定哪些关键帧应该作为局部地图的关键帧呢？ORB\_SLAM2采用的是这张策略：

1.  将所有**能够观测到当前帧地图点的关键帧**作为局部地图关键帧。
2.  如果局部地图关键帧数量不够的话将这些关键帧的共视关键帧、子关键帧、父关键帧也加入到局部地图关键帧中，直到满足局部关键帧的数量要求。
3.  将能看到当前帧最多地图点的关键帧设为参考关键帧,并更新当前帧的参考关键帧为该帧。

局部地图的地图点即为**局部关键帧的所有地图点的集合**。

在了解了局部地图的定义之后，再来看局部地图跟踪的目的。

局部地图跟踪的目的就是：**在得到当前帧的初始位姿之后，根据局部地图中的地图点和当前帧进行匹配，然后根据匹配结果进一步优化当前帧的位姿和当前帧的地图点。**

**局部地图跟踪的主要思路为：**

1.  更新局部地图。将之前的局部地图数据清空，重新构建局部地图。构建局部地图的过程就是重新确定局部地图关键帧和局部地图地图点的过程。
2.  局部地图中的地图点与当前帧的地图点进行匹配，然后利用匹配的地图点来优化当前帧的位姿。
3.  根据优化后的位姿重新更新匹配点的内点和外点。
4.  根据内点数量判定跟踪是否成功。

局部地图跟踪的流程如下图所示：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center]  
相机位姿跟踪与局部地图跟踪之间有什么区别和联系？

1.  在相机位姿跟踪过程中，不论是运动模型跟踪还是参考关键帧模型，他们都是根据上一帧的位姿来初步确定当前帧的位姿，然后对位姿进行优化。在该过程中，确定相机位姿的时候只用到了当前帧之前的一帧或两帧图像。对之前数据的利用程度不高。
2.  在局部地图跟踪过程中，局部地图更新的前提是必须知道当前帧的姿态和地图点（尽管可能不准确）。然后局部地图地图点与当前帧地图点进行匹配，然后进行当前帧的位姿优化。在该过程中，利用到了当前帧之前的多帧关键帧。这是与相机位姿跟踪过程最大的区别。可以说，局部地图跟踪也是在相机位姿跟踪基础之上做的操作，此外局部地图跟踪比相机位姿跟踪利用的信息更多。

## 3.3 关键帧处理 ##

如果相机位姿跟踪和局部地图跟踪都正常的话，接下来需要考虑的问题就是否需要将当前帧作为新的关键帧进行存储。

### 3.3.1 插入关键帧的判定依据 ###

**在ORB\_SLAM论文中，插入新关键帧的条件是：**

1.  与上次重定位相比帧数超过了20帧。
2.  建图线程空闲 或 距上次插入关键帧超过20帧。
3.  当前帧跟踪的特征点少于50个。
4.  当前帧跟踪的特征点少于参考关键帧的90%。

论文的原文如下（ORB\_SLAM论文部分内容）：  
![论文中插入关键帧的判定条件][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 1]

**代码中插入关键帧的判定条件与论文稍微有点区别。**

代码如下（ORB\_SLAM代码）：

// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
        const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
        // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
        const bool c1b = mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle;
        // Condition 2: Less than 90% of points than reference keyframe and enough inliers
        const bool c2 = mnMatchesInliers<nRefMatches*0.9 && mnMatchesInliers>15;
    
        if((c1a||c1b)&&c2)
        {
            // If the mapping accepts keyframes insert, otherwise send a signal to interrupt BA, but not insert yet
            if(bLocalMappingIdle)
            {
                return true;
            }
            else
            {
                mpLocalMapper->InterruptBA();
                return false;
            }
        }
        else
            return false;

从代码中可以看到，**插入关键帧的判定条件有三条**：

1.  此帧距离上次插入关键帧是否超过了最大帧数，其中最大帧数mMaxFrames = 18\*fps/30。
2.  此帧距离上次插入关键帧已经超过了最小帧数且此时局部地图线程处于空闲状态，其中最小帧数mMinFrames = 0。
3.  当前帧跟踪的特征点少于参考关键帧的90% 且 当前帧地图点中内点数大于15。

在做判定时，这三条判定条件的逻辑关系是 (c1a||c1b)&&c2 ，而并非单纯的" 与 "关系。此外，当满足判定条件时仍然需要在建图线程空闲时才能插入关键帧。

**以上内容是ORB\_SLAM中插入关键帧的判定依据，作者在ORB\_SLAM2中对该判定依据又进行了拓展。**

ORB\_SLAM2论文进行了如下表述。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 2]  
可以看出，ORB\_SLAM2引入了近点和原点来拓展判断依据，从而使系统能够应当更有挑战性的场景。

ORNB\_SLAM2中该部分内容的代码为：

//近点中被追踪数量小于100   并且近点未被追踪点数量大于70
        bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);     
        // Thresholds  设置阈值   参考比率(当前帧内点数量/参考关键帧中所有地图点被观察到的次数大于2或3次的地图点数量)
        float thRefRatio = 0.75f;
        if(nKFs<2)//如果整个地图中关键帧的数量小于2  则阈值设为0.4
            thRefRatio = 0.4f;
        if(mSensor==System::MONOCULAR)//  如果是单目相机   比例设为0.9
            thRefRatio = 0.9f;
    
        // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion    
        const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId + mMaxFrames;
        // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle     
        const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+ mMinFrames && bLocalMappingIdle);
        //Condition  1c: tracking is weak       
        const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
        // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
        const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers > 15);
    
        if((c1a||c1b||c1c)&&c2)
        {
            // If the mapping accepts keyframes, insert keyframe.
            // Otherwise send a signal to interrupt BA     
            if(bLocalMappingIdle)   //如果局部建图线程处于空闲状态则返回真
            {
                return true;
            }
            else   //否则中断BA优化
            {
                mpLocalMapper->InterruptBA();
                if(mSensor!=System::MONOCULAR)
                {
                    if(mpLocalMapper->KeyframesInQueue()<3)// 如果关键帧队列中关键帧数量小于3   则添加关键帧
                        return true;
                    else
                        return false;
                }
                else
                    return false;
            }
        }
        else
            return false;

从代码中可以看到，**插入关键帧的判定条件为**：

1.  此帧距离上次插入关键帧是否超过了最大帧数，其中最大帧数mMaxFrames = fps。
2.  此帧距离上次插入关键帧已经超过了最小帧数且此时局部地图线程处于空闲状态，其中最小帧数mMinFrames = 0。
3.  传感器类型为非单目 且 匹配点中内点数小于参考关键帧中被观测到两到三次的地图点数的0.25倍 且 bNeedToInsertClose标志为真。（bNeedToInsertClose标志为真时表示 近点中被追踪数量小于100 并且近点未被追踪点数量大于70）（ps:说起来真拗口，亏他想得出来）。
4.  内点数量大于15 且 匹配点中内点数小于参考关键帧中被观测到两到三次的地图点数的thRefRatio倍 或 bNeedToInsertClose标志为真。

在做判定时，这三条判定条件的逻辑关系是(c1a||c1b||c1c)&&c2，当满足判定条件时仍然考虑建图线程是否空闲。

### 3.3.2 判定是否插入关键帧的整个流程 ###

在了解了插入关键帧的判定依据后，来看判定插入关键帧整个流程（该流程为ORB\_SLAM2中的流程，而非ORB\_SLAM）。  
具体流程如下图所示。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 3]  
从流程图中可以看出，是否需要插入关键帧不仅仅需要考虑3.3.1中提到的判定条件还有其他前提条件。

### 3.3.3 创建关键帧 ###

一旦当前帧满足插入关键帧的条件，接下来要做的就是创建关键帧并插入到建图线程中。  
该过程主要由以下几步：

1.  将当前帧构造为关键帧，将该关键帧设置参考关键帧和为当前帧的参考关键帧。
2.  在非单目的情况下向地图中添加地图点。
3.  **将关键帧插入建图线程**
4.  更新mpLastKeyFrame和mpLastKeyFrameId。

## 3.4 姿态保存 ##

在经过相机位姿跟踪、局部地图跟踪和关键帧处理后，跟踪线程的大部分工作已经完成。其中还有一些其他的处理细节在这里没有写出，但是那部分代码比较好理解，属于边边角角的工作。

为了在程序结束后能够将相机的运动轨迹画出来，跟踪线程中提供了姿态保存的内容。

姿态保存部分分为两种情况：**跟踪成功和跟踪丢失**（通过当前帧的位姿是否为空来判定）。

在跟踪成功时，保存位姿的方式为：

1.  计算当前帧的参考关键帧 到 当前帧的位姿变换矩阵 `Tcr`。
2.  将变换矩阵`Tcr` 保存到`mlRelativeFramePoses`链表中。
3.  将参考关键帧`mpReferenceKF`保存到`mlpReferences`链表中。
4.  将当前帧的时间戳和跟踪状态分别保存到`mlFrameTimes`和`mlbLost`链表中。

在跟踪丢失时，保存位姿的方式为：

1.  将`mlRelativeFramePoses`链表中的最后一个元素再次存入该链表（相当于在链表末端复制了最后一个元素）。
2.  同样，将`mlpReferences`链表和`mlFrameTimes`链表的最后一个元素再次存入该链表。
3.  将跟踪状态保存到`mlbLost`链表中。

**以这种方式保存姿态，在绘制运动轨迹时需要根据参考关键帧的位姿和`Tcr`来计算每一帧的位姿。另外，跟踪丢失是运动轨迹将停止在最后一帧跟踪成功的图像帧的位置，直到重定位成功或系统重启。**

# 4.结束语 #

至此，已经把ORB\_SLAM2的Tracking线程的整体流程梳理了一遍。内容并不是很详细，后期如果有新的体会或感悟再来更新。

参考博客：  
[orb\_slam代码解析(2)Tracking线程][orb_slam_2_Tracking]  
[一起学ORBSLAM2（4）tracking主线程][ORBSLAM2_4_tracking]

若本文有错误或没有表达清楚的地方，欢迎指正！  
希望能够共同学习，共同进步！!

[20190508153511349.png]: /images/20220216/4c07ef6792ff41099e816d2b07fae0d1.png
[20190510001036147.png]: /images/20220216/b03761249f9748aba75400dc7bdafc03.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70]: /images/20220216/2af13eaa47924049b2a3fbe75e102942.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 1]: /images/20220216/1067d7d2909c4e8493356eb7b0b02c09.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 2]: /images/20220216/f2762f2f0aa64277a32c609100e6c39b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 3]: /images/20220216/540479d33c3b463985bd248ce13f1058.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NTRE4yMDI0_size_16_color_FFFFFF_t_70_pic_center]: /images/20220216/7cb8cfff8abc4d348582728d2b220c63.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70 4]: /images/20220216/9b1177856fa54dbfa1c3465ed91a54da.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center]: /images/20220216/923b4d8d7a4a4e9dabab1a36f4d43182.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20220216/fcd3c7d21aff43fc8a594c5acd5c9af5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 2]: /images/20220216/f955a62c759845aa9414be344277a387.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 3]: /images/20220216/3d820537a398410aa1ee8518564de784.png
[orb_slam_2_Tracking]: https://www.cnblogs.com/wall-e2/p/8057448.html
[ORBSLAM2_4_tracking]: https://blog.csdn.net/qq_30356613/article/details/80586872