😀 ORB-SLAM2 代码解读（二）：跟踪线程

0. 跟踪线程总体介绍

Tracking 线程运行在系统主线程中，负责对每帧图像进行特征提取、位姿估计、地图跟踪、关键帧选取等工作，可以简单理解为 SLAM 的前端里程计部分，但该线程也存在一些优化。

0.1 流程简述

1. 系统初始化：在 ==System SLAM()== 初始化 SLAM 系统时初始化了跟踪线程

1
2
3
4
5
6
7
8


mpTracker = new Tracking(this,				
                         mpVocabulary,			//字典
                         mpFrameDrawer, 			//帧绘制器
                         mpMapDrawer,			//地图绘制器
                         mpMap, 				//地图
                         mpKeyFrameDatabase, 		//关键帧地图
                         strSettingsFile, 			//配置文件路径
                         mSensor);				//传感器类型

在 Tracking.cc 的 ==Tracking()== 构造函数中主要初始化系统的一些参数，包括加载相机参数、创建 ORB 特征提取器

注意在设置特征提取数量时，单目初始化时提取的数量是双目和 RGB-D 的两倍

1
2
3
4
5
6


// 如果是双目，tracking 过程中还会用用到 mpORBextractorRight 作为右目特征点提取器
if(sensor==System::STEREO)
    mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 在单目初始化的时候，会用 mpIniORBextractor 来作为特征点提取器
if(sensor==System::MONOCULAR)
    mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

注意在初始化 SLAM 系统的时候同时也初始化并运行了其他三个线程：局部建图线程、闭环检测线程和可视化线程，但跟踪线程只是初始化了暂时并没有运行。

2. 进入跟踪线程
- 以 mono_tum.cc 为例，送入单张图片及其时间戳进入单目跟踪器接口：==SLAM.TrackMonocular(im,tframe)==
  - 在 TrackMonocular() 函数中会检查模式：实时跟踪模式、定位模式、重置等；
  - 然后再次传递图像及其时间戳到 Tracking::GrabImageMonocular() 函数。
- ==Tracking::GrabImageMonocular()== 函数
  - 步骤一： 将图像转换为灰度图；
  - 步骤二： 构造 Frame 帧 ==Frame::Frame()==（第一帧提取orb特征点数量较多,为后面帧的两倍）；
  - 步骤三： 进入 Track() 开始跟踪。
- 返回的是当前帧的相机位姿。

3. 开始跟踪：==Tracking::Track()== 跟踪线程主要实现函数

步骤一： 判断跟踪状态 mState

1
2
3
4
5
6
7
8
9


enum eTrackingState{
        SYSTEM_NOT_READY=-1,        // 系统没有准备好的状态,一般就是在启动后加载配置文件和词典文件时候的状态
        NO_IMAGES_YET=0,            // 当前无图像,图像复位过、或者第一次运行
        NOT_INITIALIZED=1,          // 有图像但是没有完成初始化
        OK=2,                       // 正常时候的工作状态
        LOST=3                      // 系统已经跟丢了的状态
        };
//跟踪状态
eTrackingState mState;

步骤二：初始化
- A. 单目初始化：==Tracking::MonocularInitialization()==（需要两帧）；
- B. 双目/RGB-D 初始化：==Tracking::StereoInitialization()==（由于具有深度信息，直接生成MapPoints）。
步骤三：两两帧相机位姿估计
- 情形一：实时 SLAM 模式：mbOnlyTracking = false
  - 情形 1： 如果初始化成功 mState==OK
    - 步骤 1： 先检查并更新上一帧：==CheckReplacedInLastFrame()==
    - 步骤 2 情形 ①： 如果当前运动模型为空或刚完成重定位，则跟踪关键帧：==TrackReferenceKeyFrame()==
    - 步骤 2 情形 ②： 如果有运动速度，则使用恒速运动模型（跟踪上一帧）：== TrackWithMotionModel()==
      - 如果恒速运动跟踪失败，则再考虑跟踪参考帧：TrackReferenceKeyFrame()
  - 情形 2： 如果初始化不成功，则只能重定位：==Relocalization()==
- 情形二：定位模式：mbOnlyTracking = true
  - 情形 1： 如果跟丢了：mState==LOST，只能进行重定位：bOK = Relocalization();；
  - 情形 2： 如果没有跟丢
    - 情形 ①： 如果跟踪了较多的地图点 mbVO=false
      - 情形 A：如果有运动速度，则使用恒速运动模型：bOK = TrackWithMotionModel();
      - 情形 B：如果当前运动模型为空或刚完成重定位，则跟踪关键帧：bOK = TrackReferenceKeyFrame();
    - 情形 ②： 如果跟踪地图点较少（少于 10 ）
      - 步骤 ①：如果有运动速度，则使用恒速运动模型：bOK = TrackWithMotionModel();
      - 步骤 ②：同时使用重定位得到当前帧的位姿
      - 步骤 ③：两者只要有一个成功了，则认为跟踪成功，并且重定位与跟踪，更相信重定位
**步骤四：局部地图跟踪（小回环优化）**
- ==TrackLocalMap()==
- 局部地图 local map：包括当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系；
- 局部地图的更新：更新关键帧和地图点，更新运动模型，清除当前帧中不好的点；
- 通过两帧之间的匹配得到初始位姿之后，现在对局部地图进行跟踪，搜索地图点，获得局部地图与当前帧的匹配，得到更多的匹配，最小化冲投影误差优化当前帧的位姿；
步骤五：生成关键帧
- 步骤 1：判断是否生成关键帧：==bool Tracking::NeedNewKeyFrame()==；
  - 生成关键帧条件 : 很⻓时间没有插入关键帧，局部地图空闲，跟踪快要跟丢，跟踪地图的 MapPoints 地图点比例比较少；
- 步骤 2：创建新的关键帧：==void Tracking::CreateNewKeyFrame()==。

0.2 两种工作模式

在 Pangolin 可视化界面可以选择两种工作模式

同时定位与建图模式：mbOnlyTracking = false
- 在跟踪线程的同时有局部建图和回环检测。
仅跟踪模式：mbOnlyTracking = true
- 不插入新的关键帧，不添加新的地图点，局部地图线程不工作，而且回环检测线程也不会工作，只跟踪地图中现有的地图点。

0.3 五种跟踪状态

SYSTEM_NOT_READY
- 系统没有准备好的状态，一般就是在启动后加载配置文件和词典文件时候的状态；
NO_IMAGES_YET
- 当前无图像，图像复位过、或者第一次运行；
- 当等待到了新的一帧，将线程状态改变为 NOT_INITIALIZED。
NOT_INITIALIZED
- 有图像但是跟踪线程没有完成初始化的状态；
- 单目相机至少需要两帧来初始化，第一帧建立初始化器，设定该帧作为初始化参考帧。第二帧作为匹配帧，通过这两帧之间进行匹配，进而通过单应性矩阵和基础矩阵计算两帧之间的位姿以及匹配点的深度信息。初始化成功之后初始化地图。
- 双目或 RGB-D 相机只需要一帧，设置初始帧位姿，并初始化地图。
OK
- 经过初始化的系统追踪线程就转为 OK 状态，在没有丢帧或者是复位的情况下系统将一直处于 OK 状态；
- 处于OK状态的系统就可以进行位姿估计，地图点追踪。
LOST
- 跟踪失败，需要进行重定位。

0.4 三种跟踪模式

跟踪线程用了三种模式进行跟踪, 分别是

① 运动模型跟踪 TrackWithMotionModel()
假设物体处于匀速运动，那么可以用上一帧的位姿和速度来估计当前帧的位姿。上一帧的速度可以通过前面几帧的位姿计算得到。这个模型适用于运动速度和方向比较一致,没有大转动的情形下,比如匀速运动的汽⻋、机器人、人等。而对于运动比较随意的目标,当然就会失效了。此时就要用到下面两个模型。
② 参考关键帧跟踪 TrackReferenceKeyFrame()
假如 motion model 已经失效,那么首先可以尝试和最近一个关键帧去做匹配( 匹配关键帧中的地图点) 。毕竟当前帧和上一个关键帧的距离还不是很远。作者利用了 bag of words ( BoW )来加速匹配。关键帧和当前帧均用字典单词线性向量表示，单词的描述子肯定比较相近，用单词的描述子进行匹配可以加速匹配。
- 首先,计算当前帧的 BoW，并设定初始位姿为上一帧的位姿；
- 其次,根据位姿和 BoW 词典来寻找特征匹配(参⻅ ORB − SLAM (六)回环检测)；
- 最后,利用匹配的特征优化位姿(参⻅ ORB − SLAM (五)优化)。
③ 重定位跟踪 Relocalization()
假如当前帧与最近邻关键帧的匹配也失败了，意味着此时当前帧已经丢了,无法确定其真实位置。此时，只有去和所有关键帧匹配，看能否找到合适的位置。
- 首先,计算当前帧的 Bow 向量。
- 其次，利用 BoW 词典选取若干关键帧作为备选(参⻅ ORB − SLAM (六)回环检测)；计算当前帧的字典单词线性表示向量和所有关键帧的字典单词线性表示向量之间的距离，选取部分距离短的候选关键帧
- 然后，当前帧和候选关键帧分别进行描述匹配，寻找有足够多的特征点匹配的关键帧;
- 最后,利用特征点匹配迭代求解位姿( RANSAC 框架下，因为相对位姿可能比较大,局外点会比较多)。
- 如果有关键帧有足够多的内点,那么选取该关键帧优化出的位姿。
选择跟踪模式的依据
- A. 优先选择通过恒速运动模型，从 LastFrame (上一普通帧)直接预测出(乘以一个固定的位姿变换矩阵)当前帧的姿态；
- B. 如果是静止状态或者运动模型匹配失效(运用恒速模型后反投影发现 LastFrame 的地图点和 CurrentFrame 的特征点匹配很少)，则采用参考帧模型，通过增大参考帧的地图点反投影匹配范围，获取较多匹配后，计算当前位姿；
- C. 若这两者均失败,即代表 tracking 失败, mState!=OK ,则在 KeyFrameDataBase 中用 Bow 搜索 CurrentFrame 的特征点匹配,进行全局重定位 GlobalRelocalization ,在 RANSAC 框架下使用 EPnP 求解当前位姿。

0.5 局部地图跟踪

前面三种跟踪模型都是为了获取相机位姿一个粗略的初值，后面会通过跟踪局部地图 TrackLocalMap 对位姿进行 Bundle Adjustment (捆集调整)，进一步优化位姿。
一旦我们通过上面三种模型获取了初始的相机位姿和初始的特征匹配，就可以将完整的地图投影到当前帧中去搜索更多的匹配。但是投影完整的地图，在 large scale 的场景中是很耗计算而且也没有必要的，因此,这里使用了局部地图 LocalMap 来进行投影匹配。

LocalMap 包含：
- 与当前帧相连的关键帧 K1，以及与 K1 相连的关键帧 K2 (一级二级相连关键帧)；
- K1 、K2 对应的地图点；
- 参考关键帧 Kf 。
匹配过程：
- 局部地图点筛选
  - ① 抛弃投影范围超出相机画面的；
  - ② 抛弃观测视⻆和地图点平均观测方向相差 60° 以上的；
  - ③ 抛弃特征点的尺度和地图点的尺度(通过高斯金字塔层数表示)不匹配的；
- 计算当前帧中特征点的尺度；
- 将地图点的描述子和当前帧 ORB 特征的描述子匹配，需要根据地图点尺度在初始位姿获取的粗略投影位置附近搜索；
- 根据所有匹配点进行 PoseOptimization 优化。

1. 构造帧

这部分其实是在进入 Track() 之前进行的，最主要的是进行 ORB 特征提取。

1.1 创建特征提取器

在构造帧之间，初始化跟踪线程的时候，创建了三个特征提取器

tracking 过程都会用到 mpORBextractorLeft 作为特征点提取器，在单目初始化的时候，会用 mpIniORBextractor 来作为特征点提取器，两者的区别是后者比前者最多提出的点数多一倍。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12


// tracking过程都会用到 mpORBextractorLeft 作为特征点提取器
    mpORBextractorLeft = new ORBextractor(  nFeatures,      /* 每一帧提取的特征点数 1000 */
                                            fScaleFactor,   /* 图像建立金字塔时的变化尺度 1.2 */
                                            nLevels,        /* 尺度金字塔的层数 8 */
                                            fIniThFAST,     /* 提取fast特征点的默认阈值 20 */
                                            fMinThFAST);    /* 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8 */
// 如果是双目，tracking 过程中还会用用到 mpORBextractorRight 作为右目特征点提取器
if(sensor==System::STEREO)
    mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 在单目初始化的时候，会用 mpIniORBextractor 来作为特征点提取器
if(sensor==System::MONOCULAR)
    mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

==ORBextractor::ORBextractor()== 构造函数
构造函数位于 ORBextractor.cc 中，传入每一帧提取的特征点数量 nFeatures（1000），高斯金字塔每层之间的缩放尺度 fScaleFactor（1.2），高斯金字塔的层数 nLevels（8），Fast 角点提取时的阈值 fIniThFAST（20）和 fMinThFAST（8）。

① 首先计算每一层相对于原始图像的缩放比例，存储在 mvScaleFactor 中，同时计算了其平方 mvLevelSigma2，其倒数 mvInvScaleFactor 及其平方的倒数 mvInvLevelSigma2；

1
2
3
4
5
6
7


for(int i = 1; i < nlevels; i++)
{
    // 累乘计算得到缩放系数
    mvScaleFactor[i] = mvScaleFactor[i-1]*scaleFactor;
    // 每层图像相对于初始图像缩放因子的平方.
    mvLevelSigma2[i] = mvScaleFactor[i]*mvScaleFactor[i];
}

② 然后分配各层图像应取的特征点数量，保证每层的特征点数量是均匀的，用到等比数列进行分配，将每层的特征点数存放在 std::vector<int> mnFeaturesPerLevel 中；

注意：第零层的特征点数是 nfeatures×(1-1/scaleFactor)/(1-(1/scaleFactor)^nlevels)，然后下一层是上一层点数的 1/scaleFactor 倍，以此类推，最后一层兜底；

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12


// STEP 将每层的特征点数量进行均匀控制
float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
// STEP 开始逐层计算要分配的特征点个数，顶层图像除外（看循环后面）
for( int level = 0; level < nlevels-1; level++ )
{
    // 分配 cvRound : 返回个参数最接近的整数值
    mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
    // 累计
    sumFeatures += mnFeaturesPerLevel[level];
    // 乘缩放系数
    nDesiredFeaturesPerScale *= factor;
}

③ 再复制训练的模板 std::vector<cv::Point> pattern，用于后面计算描述子的随机采样点集合；
1

std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
④ 最后通过求 x 坐标对应在半径为 HALF_PATCH_SIZE（15, 使用灰度质心法计算特征点的方向信息时，图像块的半径）的圆上的 y 坐标，标出了一个圆形区域用来求特征点方向；
- 代码中 umax 存储的是 u 坐标绝对值的最大值。

1.2 Frame() 构造函数构造图像帧

==Frame::Frame()== 函数传入图像，时间戳，特征点提取器，字典，相机内参矩阵等参数来构造图像帧。首先把要构造金字塔的相关参数给 Frame 类中的跟金字塔相关的元素，然后提取 ORB 特征。（以下为单目帧构造过程）

步骤一： 读取传入的特征提取器的相关参数，然后进入 ==Frame::ExtractORB(int flag, const cv::Mat &im)== 进行 ORB 特征提取，这一步实际上调用了重载函数操作符 ==ORBextractor::operator()==；

1
2
3
4


(*mpORBextractorLeft)(im,	        // 待提取特征点的图像
                      cv::Mat(),	// TODO 
                      mvKeys,	// 输出变量，用于保存提取后的特征点
                      mDescriptors);// 输出变量，用于保存特征点的描述子

步骤 1： 构建图像金字塔 ==ORBextractor::ComputePyramid(cv::Mat image)==
- 该函数对传入的图像构造 nlevels 层的金字塔，mvImagePyramid[level] 存储金字塔第 level 层的图像，它是用 resize() 函数得到大小为 level-1 层图像的 scale倍的线性插值后的图像；
- 为了方便做卷积计算，用 opencv 提供的 copyMakeBorder() 函数来做边界填充。

步骤 2： 计算金字塔每层的兴趣点，找到 FAST关键点 ==ORBextractor::ComputeKeyPointsOctTree(allKeypoints)==

步骤 ①： 依次对金字塔每层图像进行操作，首先在图像四周去掉长度为 EDGE_THRESHOLD-3 个单位的像素点的边界；
步骤 ②： 对去掉边界的图像网格化，每个窗口的大小为 W=30 个像素的正方形；

步骤 ③： 对每个图像块进行 FAST 角点提取；

前面网格化的目的是为了使得每个网格都有特征，从而使得特征点在图像上的分布相对均匀点；
如果存在有的窗口中提取的特征点数为 0，则降低阈值 minThFAST 继续提取；

1
2
3
4


FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),	// 待检测的图像，这里就是当前遍历到的【图像块】
    vKeysCell,	// 存储角点位置的容器
    iniThFAST,	// 检测阈值
    true);		// 使能非极大值抑制

然后对提取出了的关键点 vKeysCell 换算出其位于 level 层的被裁掉边界的图像中的位置，并将每个窗口中的关键点存入 vToDistributeKeys 容器中，暂时保存着第 level 层图像的关键点；

步骤 ④： 将每层的 vToDistributeKeys 送入到 ==ORBextractor::DistributeOctTree()== 中进行关键点剔除和平分配

步骤 A： 确定四叉树有几个初始节点，每个初始节点的 x 方向有多少个像素

1
2


const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));
const float hX = static_cast<float>(maxX-minX)/nIni;

步骤 B： 将关键点分配到子提取器节点 vector<ExtractorNode*> vpIniNodes 中；

1
2
3
4
5
6
7


for(size_t i=0;i<vToDistributeKeys.size();i++)
{
    // 获取这个关键点对象
    const cv::KeyPoint &kp = vToDistributeKeys[i];
    // 按点的横轴位置，分配给属于那个图像区域的提取器节点（最初的提取器节点）
    vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}

步骤 C： 当节点分配到的关键点的个数为 1 时就不再进行分裂，当节点没有分配到关键点时就删除该节点；
步骤 D： 再根据兴趣点分布,利用四叉树方法对图像进行划分区域，当 bFinish 的值为 true 时就不再进行区域划分；
- 首先对目前的区域进行划分，把每次划分得到的有关键点的子区域设为新的节点，将 nToExpand 参数加 1，并插入到节点列表的前边，删除掉其父节点；只要新节点中的关键点的个数超过一个，就继续划分，继续插入列表前面，继续删除父节点，直到划分的子区域中的关键点的个数是一个，然后迭代器加以移动到下一个节点，继续划分区域；
- 当划分的区域即节点的个数大于关键点的个数或者分裂过程没有增加节点的个数时就将 bFinish 的值设为 true，不再进行划分；
- 如果以上条件没有满足，但是满足 ((int)lNodes.size()+nToExpand * 3)>N，表示再分一次即将结束，所以开始按照特征点的数量对节点进行排序，特征点数多的节点优先划分，直到节点数量满足；
- vSizeAndPointerToNode 是前面分裂出来的子节点（n1, n2, n3, n4）中可以分裂的节点。按照它们特征点的排序，先从特征点多的开始分裂，分裂的结果继续存储在 lNodes 中；每分裂一个节点都会进行一次判断，如果 lNodes 中的节点数量大于所需要的特征点数量，退出整个 while(!bFinish) 循环，如果进行了一次分裂，并没有增加节点数量，退出整个 while(!bFinish) 循环；
- 取出每一个节点(每个区域)对应的最大响应点，即我们确定的特征点。
总结：因为经过 FAST 提取出的关键点有很多，当划分的子区域一旦大于 mnFeaturesPerLevel[level] 根据nfeatures 算出的每一个 level 层最多的特征点数的时候就不再进行区域划分了，所以每个区域内(节点)的关键点数会很多，取出响应值最大的那个就是我们想要的特征点；这个函数的意义就是根据mnFeaturesPerLevel，即该层的兴趣点数,对特征点进行剔除，根据 Harris 角点的 score 进行排序，保留正确的。
经过以上步骤，我们提出来 level 层在无边界图像中的特征点，并给特征点条件边界补偿及尺度信息。

步骤 ⑤： 分层计算关键点的方向 ==computeOrientation()==，具体方向计算在 ==IC_Angle()== 函数中

为了使得提取的特征点具有旋转不变性，需要计算每个特征点的方向；方法是计算以特征点为中心，以像素为权值的圆形区域上的重心，以中心和重心的连线作为该特征点的方向。
可参考十四讲中 ORB 特征的介绍。

1
2
3
4
5
6
7
8


 /* @param[in] image     要进行操作的原图像（块）
  * @param[in] pt        要计算特征点方向的特征点的坐标
  * @param[in] u_max     图像块的每一行的u轴坐标边界（1/4）
  * @return float        角度，弧度制
  */
static float IC_Angle(  const Mat& image,
                        Point2f pt,
                        const vector<int> & u_max)

步骤 3： 描述子计算：==computeDescriptors()== 函数中调用 ==computeOrbDescriptor()== 函数具体实现

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11


/**
 * @brief 计算ORB特征点的描述子
 * @param[in] kpt       特征点对象
 * @param[in] img       提取出特征点的图像
 * @param[in] pattern   随机采样点集
 * @param[out] desc     用作输出变量，保存计算好的描述子，长度为32*8bit
 */
static void computeOrbDescriptor(const KeyPoint& kpt,	//特征点对象
                                 const Mat& img, 	//提取出特征点的图像
                                 const Point* pattern,	//随机采样点集
                                 uchar* desc)		//用作输出变量，保存计算好的描述子

ORB 使用 BRIEF 作为特征描述子，原始的 BRIEF 描述子不具有方向信息，这里就是通过加入了特征点的方向来计算描述子，称之为 Steer BRIEF 描述子使其具有较好的旋转不变特性；
在计算的时候需要将这里选取的随机点点集 pattern 的 x 轴方向旋转到特征点的方向，并获得随机点集中某个 idx 所对应的点的灰度；
brief 描述子由 32 * 8 位组成，其中每一位是来自于两个像素点灰度的直接比较，所以每比较出 8bit 结果，需要 16 个随机点（这也就是为什么 pattern 需要+=16）；
通过对随机点像素灰度的比较，得出 BRIEF 描述子，一共是 32 * 8 = 256 位。

步骤二： 检查是否成功提取了本帧的特征点，如果没有提取到有效的特征则放弃本帧；
步骤三： 对提取的特征点进行畸变矫正 ==Frame::UndistortKeyPoints()==；
- 调用 opencv 提供的 cv::undistortPoints 进行畸变矫正

步骤四： 初始化本帧的地图点，先默认所有的地图点均为内点

1
2
3
4


// 初始化存储地图点句柄的vector
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
// 开始认为默认的地图点均为inlier
mvbOutlier = vector<bool>(N,false);

步骤五： 判断是否需要进行进行特殊初始化,这个过程一般是在第一帧或者是重定位之后进行;
步骤六： 将特征点分配到图像网格中 ==Frame::AssignFeaturesToGrid()==；
- 先创建一个 std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS] 空间存储的是每个图像网格内特征点的 id；
- 从类的成员变量中获取已经去畸变后的特征点的每一个特征点
  1
  
  const cv::KeyPoint &kp = mvKeysUn[i];
  - 并利用 ==Frame::PosInGrid()== 找到该特征点所处的网格，输出为指定的图像特征点所在的图像网格的横纵 id（其实就是图像网格的坐标）
- 根据上一步返回的网格坐标，将该特征点分配到网格中
  1 2
  
  if(PosInGrid(kp,nGridPosX,nGridPosY)) mGrid[nGridPosX][nGridPosY].push_back(i);

2. 初始化

在前面构造完 Frame 图像帧之后即进入到 Track() 函数，开始真正开始跟踪线程，第一步就是判断是否进行了初始化，分为单目初始化和双目初始化的情况。

2.1 单目初始化

单目初始化通过并行地计算基础矩阵 F 和单应矩阵 H ，恢复出最开始两帧的匹配、相机初始位姿，三角化得到 MapPoints 的深度，获得初始化点云地图。

1
2
3


同时计算两个模型：    
用于平面场景的单应性矩阵 H（4对 3d-2d点对，线性方程组，奇异值分解）    
用于非平面场景的基础矩阵 F（8对 3d-2d点对，线性方程组，奇异值分解）    

步骤一： 创建单目初始器并获取第一帧图像（未创建初始器）
- 步骤 1：获取第一帧，用 mInitialFrame 表示第一帧（初始帧）， mLastFrame 表示上一帧，并对第一帧进行特征提取，要求特征点数必须大于 100，mvbPrevMatched 用于存储第一帧中的所有特征点；
- 步骤 2：==Initializer::Initializer()== 创建初始器，获取相机内参、参考帧的特征点，初始化估计误差和最大的 RANSAC 迭代次数:；
- 用mvIniMatches 表示两帧之间的匹配标志，先填充为 -1 ，也就是均没有匹配。
步骤二： 获取单目初始化的第二帧（单目初始化器已经被创建）
- 要求这一帧的特征点数也要大于 100 ，如果小于 100 个，则删除单目初始器 Initializer ，回到步骤一重新创建；
- 也就是只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程。
- NOTE：步骤一二比较的特征点都是未经畸变矫正的点 mCurrentFrame.mvKeys。

步骤三： 在 mInitialFrame 与 mCurrentFrame 中找匹配的特征点对

创建特征匹配器并利用 ==ORBmatcher::SearchForInitialization()== 函数进行金字塔分层块匹配搜索匹配点对，返回匹配的数目；

1
2
3
4
5
6
7


// 创建特征匹配器.
ORBmatcher matcher( 0.9, true); // 第一个参数：最佳匹配与次佳匹配的比值，默认为 0.6；第二个参数：是否检查特征点的方向.
// 针对单目初始化的时候,进行特征点的匹配
int nmatches = matcher.SearchForInitialization( mInitialFrame, mCurrentFrame, // 初始化时的参考帧和当前帧
                                                mvbPrevMatched,               // 在初始化参考帧中提取得到的特征点
                                                mvIniMatches,                 // 匹配标志
                                                100);                         // 搜索窗口的大小

调用 ==ORBmatcher::DescriptorDistance()== 函数比较两帧之间的描述子距离，确保距离小于 ORBmatcher::TH_LOW = 50；

SearchForInitialization() 的功能：根据可能匹配特征点的描述子计算距离，确定最佳匹配；另外如果考虑特征点的方向，则将第一帧中的特征的方向角度减去对应第二帧的特征的方向角度，将值划分为直方图，则会在 0 度和 360 度左右对应的组距比较大，这样就可以对其它相差太大的角度可以进行剔除；
函数返回的匹配数 nmatches 小于 100，则回到步骤一，重新初始化，若大于 100，则进行后续初始化。

步骤四： ==Initializer::Initialize()== 并行计算 H 模型或 F 模型进行单目初始化，得到两帧间相对运动、初始MapPoints

1
2
3
4
5


mpInitializer->Initialize(  mCurrentFrame,      // 当前帧
                            mvIniMatches,       // 当前帧和参考帧的特征点的匹配标志
                            Rcw, tcw,           // 初始化得到的相机的位姿
                            mvIniP3D,           // 进行三角化得到的空间点集合
                            vbTriangulated))    // 以及对应于mvIniMatches来讲,其中哪些点被三角化了

步骤 1：筛选出匹配的点对，mvIniMatches 存储匹配标志，将标志 >= 0 的点索引关系保存到 mvMatches12

1
2
3
4
5


if(vMatches12[i]>=0)
{
    mvMatches12.push_back(make_pair(i,vMatches12[i]));
    mvbMatched1[i]=true;
}

步骤 2：在所有匹配特征点对中随机选择 8 对匹配特征点为一组，共选择 Initializer::mMaxIterations 组，其中 mMaxIterations 是最大的 RANSAC迭代次数，在初始化初始器的时候赋值为 200；
- 产生的点对保存在 mvSets 中，用于保存每次迭代时所使用的向量，保存八对点进行单应矩阵和基础矩阵估计。
  1 2
  
  mvSets = vector< vector<size_t> >( mMaxIterations, // 最大的RANSAC迭代次数 200 vector<size_t>(8,0)); //

步骤 3：多线程计算基础矩阵和单应矩阵

1
2
3
4
5
6
7
8


// 计算 homography 矩阵并打分.
thread threadH( &Initializer::FindHomography,	// 计算单应矩阵的函数
                this,				//NOTE 由于主函数为类的成员函数，所以第一个参数就应该是当前对象的this指针
                ref(vbMatchesInliersH), 	//输出，特征点对的 Inlier 标记
                ref(SH), 			//输出，计算的单应矩阵的 RANSAC 评分
                ref(H));			//输出，计算的单应矩阵结果
// 计算 fundamental 矩阵并打分.
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

线程一：计算单应矩阵及其得分 ==Initializer::FindHomography()==
- 步骤 ①：利用 ==Initializer::Normalize()== 函数归一化特征点的尺度，固定场景尺度；
- 步骤 ②：利用 ==Initializer::ComputeH21()== 函数八点法计算 homography 矩阵；
- 步骤 ③：利用 ==Initializer::CheckHomography()== 函数求取 利用重投影误差为 RANSAC 的结果评分；
- 步骤 ④：记录最大的得分。
线程二：计算本质矩阵及其得分 ==Initializer::FindFundamental()==
- 步骤 ①：利用 ==Initializer::Normalize()== 函数归一化特征点的尺度，固定场景尺度；
- 步骤 ②：利用 ==Initializer::ComputeF21()== 函数八点法计算 fundamental 矩阵；
- 步骤 ③：利用 ==Initializer::CheckFundamental()== 函数求取 利用重投影误差为 RANSAC 的结果评分；
- 步骤 ④：记录最大的得分。

步骤 4：计算两个矩阵的得分比，判断选择哪个模型,判断谁的评分占比更多一些，注意不是简单的评分大，而是看评分的占比。
1

float RH = SH/(SH+SF);
步骤 5：从选择的 F 矩阵或 H 矩阵中恢复当前帧相对于第一帧的位姿 R,t完成初始化；
- 情形一： RH > 0.40，偏向于平面，从单应矩阵恢复 ==Initializer::ReconstructH()==；
- 情形二： RF > 0.60，偏向于非平面，从基础矩阵恢复 ==Initializer::ReconstructF()==。

步骤五： 删除无法三角化的匹配点，其中 mvIniMatches 是两帧之间特征点的匹配标志，vbTriangulated 是其对应的三角化标志。

1
2
3
4
5
6
7
8


for(size_t i=0, iend = mvIniMatches.size(); i<iend;i++)
{
    if(mvIniMatches[i]>=0 && !vbTriangulated[i])
    {
        mvIniMatches[i]=-1;
        nmatches--;
    }
}

步骤六： 设置初始两帧的世界坐标位姿

初始帧的位姿设置为单位矩阵

1

mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));

当前帧（第二帧）的位姿有前面矩阵恢复出的 R,t 构造

1
2
3
4
5


// 由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);

步骤七： ==Tracking::CreateInitialMapMonocular()== 将三角化得到的点包装成地图点 MapPoints，创建初始地图，使用最小化重投影误差 BA 进行地图优化，优化位姿和地图点；
- 步骤 1： 创建初始关键帧，认为单目初始化时候的参考帧和当前帧都是关键帧 ==KeyFrame::KeyFrame()==
  1 2
  
  KeyFrame* pKFini = new KeyFrame(mInitialFrame, mpMap, mpKeyFrameDB); KeyFrame* pKFcur = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
- 步骤 2： 将初始化的两帧关键帧的描述子转换成 BoW ==Frame::ComputeBoW()==
  1 2
  
  pKFini->ComputeBoW(); pKFcur->ComputeBoW();
- 步骤 3： 将关键帧插入到地图，凡是关键帧都需要插入到地图 ==Map::AddKeyFrame(KeyFrame *pKF)==
  1 2
  
  mpMap->AddKeyFrame(pKFini); mpMap->AddKeyFrame(pKFcur);
- 步骤 4： 将 3D 点包装成 MapPoints
  - 步骤 ①：构造地图点 ==MapPoint::MapPoint()==
    1 2 3
    
    MapPoint* pMP = new MapPoint(worldPos, // 3D 点的世界坐标. pKFcur, // 对应的关键帧. mpMap); // 地图.
  - 步骤 ②：添加地图点到关键帧 ==KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)==
    1 2
    
    pKFini->AddMapPoint(pMP,i); // 第一个参数是地图点，第二个参数是地图点在关键帧中的索引. pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
  - 步骤 ③：记录关键帧的哪个特征点能观察到该地图点； ==MapPoint::AddObservation(KeyFrame* pKF, size_t idx)==
    1 2
    
    pMP->AddObservation(pKFini,i); pMP->AddObservation(pKFcur,mvIniMatches[i]);
  - 步骤 ④：从众多观测到该 MapPoint 的特征点中挑选区分度最高的描述子 ==MapPoint::ComputeDistinctiveDescriptors()==
  - 步骤 ⑤：更新该 MapPoint 平均观测方向以及观测距离的范围 ==MapPoint::UpdateNormalAndDepth()==
  - 步骤 ⑥：在地图中添加该 MapPoint ==AddMapPoint(MapPoint *pMP)==
    1
    
    mpMap->AddMapPoint(pMP);
- 步骤 5： 更新关键帧间的连接关系 ==KeyFrame::UpdateConnections()==
  1 2
  
  pKFini->UpdateConnections(); pKFcur->UpdateConnections();
  - 在 3D 点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共 3D 点的个数
- 步骤 6： BA 优化 ==Optimizer::GlobalBundleAdjustemnt()==
- 步骤 7： 将 MapPoints 的中值深度归一化到1，并归一化两帧之间变换；
- 步骤 8： 更新状态量，包括关键帧、地图点信息，更新地图绘制器。
单目初始化结束。

2.2 双目/RGB-D 初始化

双目相机通过视差获取深度，RGB-D 相机直接获取深度信息，得到 3D 点，创建地图点并初始化地图。

步骤一： 要求当前帧的特征点数大于 500 再进行下面的初始化步骤；
步骤二： 设置当前帧的初始位姿为单位矩阵，T = [I 0]，世界坐标系；
1

mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
步骤三： 将当前初始化帧构造为初始关键帧， ==KeyFrame::KeyFrame()== 完成关键帧对象的初始化，包括设置关键帧的 ID，位姿，将普通帧的格点关键点数量（加速匹配）统计赋值给关键帧（加速匹配）
1

KeyFrame* pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
步骤四： 将初始关键帧添加到全局地图中 ==Map::AddKeyFrame(KeyFrame *pKF)==
1

mpMap->AddKeyFrame(pKFini);
步骤五： 为每个特征点构造地图点
- 步骤 1：首先要求深度 float z = mCurrentFrame.mvDepth[i] 为正（RGB-D 相机直接获取，双目在构造帧时根据视差计算），然后通过反投影 ==KeyFrame::UnprojectStereo()== 得到该特征点世界坐标系的 3D 坐标；
- 步骤 2：将该 3D 点构造成地图点 ==MapPoint::MapPoint()==
  1
  
  MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
- 步骤 3：为该 MapPoint 添加属性，同单目初始化步骤四中的步骤 ③ ④ ⑤；
- 步骤 4：在地图中添加该 MapPoint ==AddMapPoint(MapPoint *pMP)==；
- 步骤 5：记录关键帧的哪个特征点能观察到该地图点； ==MapPoint::AddObservation(KeyFrame* pKF, size_t idx)==
  1
  
  pKFini->AddMapPoint(pNewMP,i);
- 步骤 6：将该 MapPoint 添加到当前帧的 mvpMapPoints 中，为当前 Frame 的特征点与 MapPoint 之间建立索引.
  1
  
  mCurrentFrame.mvpMapPoints[i]=pNewMP;
步骤六： 将该初始关键帧添加到局部地图中
1

mpLocalMapper->InsertKeyFrame(pKFini);

步骤七： 更新状态量

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26


// 当前帧变上一帧（变为上一个普通帧和关键帧）.
mLastFrame = Frame(mCurrentFrame);
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKFini;

// 添加到局部地图帧集合.
mvpLocalKeyFrames.push_back(pKFini);

// 局部地图中添加所有特征点.
mvpLocalMapPoints = mpMap->GetAllMapPoints();

// 更新参考帧.
mpReferenceKF = pKFini;
mCurrentFrame.mpReferenceKF = pKFini;

// 把当前（最新的）局部 MapPoints 作为地图参考地图点 ReferenceMapPoints
// ReferenceMapPoints 是 DrawMapPoints 函数画图的时候用的
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
// 地图关键帧
mpMap->mvpKeyFrameOrigins.push_back(pKFini);

// 更新可视化线程地图绘制器.
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

// 更新追踪状态.
mState = OK;

双目/RGB-D 初始化结束。

3. 帧间位姿估计

在前面完成初始化之后就开始正常两帧之间的跟踪与位姿估计了，首先需要判断跟踪的模式是 SLAM 模式还是仅定位模式，再根据不同的状态选择恒速运动模型、跟踪参考关键帧模型还是重定位模型；
在 SLAM 模式下，如果初始化成功了，先检查上一帧地图点的替换情况（因为两两帧之间的跟踪需要用到上一帧的信息），然后再根据不同的状态选择跟踪模式。
- ==Tracking::CheckReplacedInLastFrame()== 函数检查替换情况，函数内再调用 ==MapPoint* MapPoint::GetReplaced()== 函数返回替换的地图点 MapPoint* mpReplaced
  - 实际上地图点替换发生在闭环的时候 ==MapPoint::Replace(MapPoint* pMP)== 局部线程可能会对地图点进行替换。

3.1 跟踪参考关键帧模型

1
2
3
4


if(mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId+2)
{
    bOK = TrackReferenceKeyFrame();
}

==bool Tracking::TrackReferenceKeyFrame()==
采用跟踪参考关键帧模型的条件：
- 相机没有运动（运动速度 mVelocity 为空）；
- 或者刚完成重定位：mCurrentFrame.mnId < mnLastRelocFrameId+2；
- 否则会有限考虑运动模型，但恒速运动模式下匹配到的特征点较少时又会选择关键帧模型。
基本思想：在以上条件下尝试和最近一个关键帧 mpReferenceKF 去做特征匹配，匹配过程中使用 BOW 向量的正向索引进行加速
- 首先,计算当前帧的 BoW ,并设定初始位姿为上一帧的位姿；
- 其次,根据两帧的 BoW 特征向量同属于一个 node 下来加速搜索匹配点对；
- 最后,利用匹配的特征优化位姿。

步骤一： 计算当前帧特征描述子的词袋向量 ==Frame::ComputeBoW()== （ComputeBoW() 在 Frame 类和 KeyFrame 类中都有）

ComputeBoW() 函数的思想
- 将帧中的特征描述子可以用 BOW 词袋向量 mBowVec 来表示，词袋向量是一个 map ，元素是视觉词汇和权重；
- 用 BOW 向量来代替特征描述子的好处是可以用词袋向量的正向索引来加速后面的特征匹配过程；
- 正向索引 mFeatVec 是用来储存该图像生成 BoW 向量时曾经到达过的第 m 层上节点的编号，以及路过这个节点的特征的编号，意义在于当对两幅图像做涉及到特征点的匹配计算时,可以利用“属于同一单词索引的特征更有可能匹配”的设定规则来加速匹配；正向索引实际上也是一个 map, 元素是节点 ID 和路过这个节点的特征的编号的 vector。
步骤 1： 当当前帧的词袋 DBoW2::BowVector mBowVec 为空时，将当前帧以 opencv 存储的 Mat 类型描述子转换为 std::vectorcv::Mat 类型的向量格式 ==Converter::toDescriptorVector==
1

vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
- 本质上就是将 cv:Mat 转换成 std:vector
  1
  
  for (int j = 0;j < Descriptors.rows;j++)
// 从描述子这个矩阵中抽取出来存到向量中 vDesc.push_back(Descriptors.row(j)); ```

步骤 2： 将特征点的描述子向量转换成当前帧的词袋向量，利用 DoW 库提供的 ==transform== 函数

1
2
3
4


mpORBvocabulary->transform( vCurrentDesc,	// 当前的描述子vector
                            mBowVec,		// 输出，词袋向量
                            mFeatVec,		// 输出，保存有特征点的正向索引 vector
                            4);			// 计算正向索引时，储存到达第四层的节点编号

步骤二： 将当前帧与参考关键帧 mpReferenceKF 进行特征匹配，使用正向索引加速，计算匹配的特征数量 ==ORBmatcher::SearchByBoW()==

1
2
3


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

函数介绍：
- 参考关键帧与当前帧的特征匹配，利用正向索引进行加速, 原理是“属于同一单词索引的特征更有可能匹配”；
- 这个函数重载函数, 一个是匹配关键帧和普通帧（也就是这里使用的匹配），另一个是匹配关键帧和关键帧（闭环检测）；
- 函数的第三个参数 vpMapPointMatches 用于保存当前帧 F 中关键点匹配到的地图点 MapPoints，NULL表示未匹配；
- 匹配前需要有一个 ORBmatcher 类对象的初始化过程 ORBmatcher matcher(0.7,true);，初始化过程需要传入两个参数, 一个是一个阈值, 表示匹配距离最短的距离小于次短距离的 70% ; 另一个是表示特征匹配是否检测特征点的方向是否一致；
步骤 1： 对参考关键帧 KF 的每个特征点，遍历当前普通帧 F 中经过与之同一 node 的特征 KFit->first == Fit->first；

步骤 2： 求取两帧描述子的距离

步骤 ①：遍历 KF 中属于该 node 的特征点，匹配时跳过 KF 中地图点是坏点或没有地图点的特征点，然后取出该特征对应的描述子：

1
2
3
4
5
6
7


MapPoint* pMP = vpMapPointsKF[realIdxKF]; // 取出 KF 中该特征对应的MapPoint
// 剔除
if(!pMP)
    continue;
if(pMP->isBad())
    continue;
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); // 取出 KF 中该特征对应的描述子

步骤 ②：遍历 F 中属于该 node 的特征点

1

const cv::Mat &dF = F.mDescriptors.row(realIdxF);

步骤 ③：计算 orb 特征描述子距离 ==ORBmatcher::DescriptorDistance()==，保存最小的、次小的距离，和最小距离对应的 F 特征点的 ID
1

const int dist = DescriptorDistance(dKF,dF);

步骤 3： 根据距离阈值、最佳匹配比次佳匹配的比例阈值和方向投票剔除误匹配，将 F 匹配的特征点对应的 KF 的地图点保存到 vpMapPointMatches ，用于后面向 F 添加地图点；
- 距离阈值指的是: 特征描述子之间的最小距离要小于设定的阈值；
- 比例阈值指的是: 特征描述子的最小距离要比次最小距离的一定倍数要小；
- 角度投票指的是: 在特征匹配的时候，如果图像发生了旋转，那么特征点旋转方向应该一致, 所以可以根据这个原理计算特征点的旋转方向，统计方向偏差直方图，频率最高的三个保留，其他范围内的匹配点剔除。
步骤 4：返回匹配的点对数，正向索引匹配结束。
补充：保证参考关键帧和当前普通帧的正向索引的迭代器指向同一个 node 和其特征的方式
- 首先如果两个迭代器指向相同，则两个迭代器迭代的时候正常各自加一，否则，则使用迭代器的 STL 函数 lower_bound() 使二者保持同步；
- lower_bound() 在 first 和 last 中的前闭后开区间进行二分查找，返回大于或等于 val 的第一个元素位置，如果所有元素都小于 val,则返回 last 的位置，且 last 的位置是越界的。

步骤三： 如果匹配特征数量少于 15 个，返回 false，跟踪失败，否则进行下面流程。
步骤四： 使用上一帧 mLastFrame 的位姿作为初始位姿，根据特征匹配情况进行当前帧位姿估计，利用卡方检验剔除外点进行优化 ==Optimizer::PoseOptimization(&mCurrentFrame)==。

步骤五： 剔除外点对应的地图点，统计内点对应的地图点中有观测帧的地图点数量。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13


// 如果对应到的某个特征点是外点
if(mCurrentFrame.mvbOutlier[i])
{
    // 清除它在当前帧中存在过的痕迹
    MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
    mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
    mCurrentFrame.mvbOutlier[i]=false;
    pMP->mbTrackInView = false;
    pMP->mnLastFrameSeen = mCurrentFrame.mnId;
    nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
    nmatchesMap++;  // 内点中有观测帧的地图点数量

步骤七： 如果内点中有观测帧的地图点数量大于等于 10，返回 true，跟踪成功。
跟踪参考帧模型完成，估计并优化了当前帧位姿（步骤四），更新了地图。

3.2 恒速运动模型

在 SLAM 模式下，如果不是没有运动速度或刚完成重定位则会优先尝试恒速运动模型，当匹配的特征点较少时再选择关键帧模式；这个模型是假设物体处于匀速运动，例如匀速运动的汽车、机器人、行人等，就可以用上一帧的位姿和速度来估计当前帧的位姿，恒速运动模型实现函数为 ==Tracking::TrackWithMotionModel()==，匹配是通过投影来与上一帧看到的地图点匹配，使用的是 ==matcher.SearchByProjection()== 。

基本思想：
- 使用匀速模型估计的位姿，将 LastFrame 中临时地图点反投影到当前帧图像像素坐标上，和当前帧的关键点落在同一个格子内的做描述子匹配搜索可以加快匹配，在投影点附近根据描述子距离进行匹配(需要 >20 对匹配，否则匀速模型跟踪失败，运动变化太大时会出现这种情况)；
- 然后以运动模型预测的位姿为初值，优化当前位姿，优化完成后再剔除外点，若剩余的匹配依然 >=10 对，则跟踪成功，否则跟踪失败，需要 Relocalization。
步骤一： 创建 ORB 特征提取器，最小距离 < 0.9 倍次小距离才匹配成功：ORBmatcher matcher(0.9,true);；
步骤二： 更新上一帧位姿 ==Tracking::UpdateLastFrame()==
- 步骤 1： 计算上一帧的位姿，上一帧位姿 = 上一帧到其参考帧位姿 * 其参考帧到世界坐标系(系统第一帧)位姿
  - 步骤 ①：获取上一帧的参考帧到世界坐标系的位姿 Trw：
    1 2
    
    KeyFrame* pRef = mLastFrame.mpReferenceKF;// 上一帧的参考帧. Trw = pRef->GetPose();// Trw 上一帧的参考帧的位姿.
  - 步骤 ②： 上一帧的参考帧 mLastFrame.mpReferenceKF 到上一帧 mLastFrame 的变换 Tlr 存储为 mlRelativeFramePoses 的最后一个元素：
    1
    
    cv::Mat Tlr = mlRelativeFramePoses.back();
  - 步骤 ③：得到上一帧的位姿：Tlw = Tlr * Trw
    1
    
    mLastFrame.SetPose(Tlr*pRef->GetPose()); // Tlr*Trw = Tlw l:last r:reference w:world
- 步骤 2： 在上一帧非关键帧的情况下，对于双目和 RGB-D 相机为上一帧临时生成新的地图点；因为跟踪过程中需要将上一帧的 MapPoints 投影到当前帧可以缩小匹配范围，加快当前帧与上一帧进行特征点匹配；但这些临时添加的 MapPoints 不加入到 Map 中，在 tracking 的最后会删除。
  - 步骤 ①：得到上一帧有深度值的点 vector<pair<float,int> > vDepthIdx;，其中第一个元素是某个点的深度，第二个元素是对应的特征点 id；
  - 步骤 ②：按照深度值从小到大排序：sort(vDepthIdx.begin(),vDepthIdx.end());；
  - 步骤 ③：按照排序，对那些没有对应地图点的特征点或者创建了地图点之后没有被观测到的地图点创建前 100 个临时地图点 mlpTemporalPoints.push_back(pNewMP);。
步骤三： 初始化当前帧的位姿，运动速度 × 上一帧的位姿 mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);，同时清空当前帧的地图点；
- 其中上一帧的位姿 mLastFrame.mTcw 由步骤二得到；
- 运动速度由上一次的跟踪/初始化成功之后得到 mVelocity = mCurrentFrame.mTcw * LastTwc; 也就是上一帧和上一帧的参考帧之间的相对运动 Tcl。

步骤四： 根据匀速运动模型对上一帧的地图点进行跟踪，在当前帧和上一帧之间搜索匹配点 ==ORBmatcher::SearchByProjection()== 根据上一帧特征点对应的3D点投影的位置和网格搜索缩小特征点匹配范围

SearchByProjection() 函数思想：通过投影，对上一帧的特征点进行跟踪，将上一帧的 MapPoints 投影到当前帧(根据速度模型可以估计当前帧的 Tcw)，依次遍历参考帧的 pMapPoints，计算出该 3D 点在当前帧的投影位置，设定一个以该点为中心的正方形区域内的所有特征点，获得该 3D 点的描述子和这些特征点的描述子之间的距离（单目为 7，否则为 15），找到距离最小的那个特征点，就是该 3D 点在当前帧匹配到的特征点，返回成功匹配的数量。

步骤 1： 遍历上一帧的地图点，将上一帧的地图点投影到当前帧的像素坐标系

 1
 2
 3
 4
 5
 6
 7
 8
 9
10


// 获取上一帧地图点的世界坐标和相机坐标.
cv::Mat x3Dw = pMP->GetWorldPos();
cv::Mat x3Dc = Rcw*x3Dw+tcw;
// 上一帧特征点的 3D 坐标
const float xc = x3Dc.at<float>(0);
const float yc = x3Dc.at<float>(1);
const float invzc = 1.0/x3Dc.at<float>(2);  
// 利用当前帧的位姿将其投影到当前帧.
float u = CurrentFrame.fx * xc * invzc + CurrentFrame.cx;
float v = CurrentFrame.fy * yc * invzc + CurrentFrame.cy;

步骤 2： 搜索在投影的位置附近区域的匹配点集合

判断当前运动是向前还是向后

1
2


const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono; // 非单目情况，如果Z大于基线，则表示前进
const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono; // 非单目情况，如果Z小于基线，则表示前进

确定一个搜索区域，并利用 ==Frame::GetFeaturesInArea()== 函数获取这个区域网格中的地图点集合 vector<size_t> vIndices2;

1
2
3
4
5
6


// BRIEF 找到在 以x,y为中心,边长为2r的方形内且在[minLevel, maxLevel]的特征点
vector<size_t> Frame::GetFeaturesInArea(const float &x, 
                                        const float &y, 
                                        const float &r, 
                                        const int minLevel, 
                                        const int maxLevel) const

步骤 3： 计算当前点与候选匹配集合点的描述子距离，获得最近距离的最佳匹配，但是也要满足距离 <ORBdist；
- 如果该特征点已经有对应的 MapPoint 了,则退出该次循环；
- 双目和rgbd的情况，需要保证右图的点也在搜索半径以内；
- ==ORBmatcher::DescriptorDistance()== 计算描述子距离。
步骤 4： 根据距离阈值、比例阈值、方向投票剔除误匹配。

步骤五： 如果上一步搜索到的匹配点数少于 20 则将半径扩大一倍来搜索，如果还是少于 20 ，则认为恒速运动模型跟踪失败。
步骤六： 使用匹配点对对当前帧的位姿进行 G2O 图优化 ==Optimizer::PoseOptimization(Frame *pFrame)== ，仅优化单个普通帧的位姿，地图点不优化。
步骤七： 剔除外点对应的地图点，统计内点对应的地图点中有观测帧的地图点数量（与关键帧模式一样）。
步骤八： 如果内点中有观测帧的地图点数量大于等于 10，返回 true，跟踪成功。
恒速运动模型跟踪完成。

3.3 重定位

假如恒速运动模型跟踪失败，与相邻关键帧匹配也失败，则认为当前帧已丢失，需要进行重定位，通过词袋向量搜索关键帧数据库中与之对应的关键帧进行匹配跟踪。重定位实现函数在 ==Tracking::Relocalization()== 中。

基本思想
- 位置丢失之后，使用当前帧的 BoW 特征向量在关键帧数据库中匹配最相近的关键帧，从而求出当前帧位姿；
- 由于此时没有好的初始位姿信息，需要使用传统的 3D-2D 匹配点的 EPnP 算法来求解出一个初始位姿，之后再使用最小化重投影误差来优化更新位姿。
步骤一： 计算当前帧特征点的 BoW 词袋向量 ==Frame::ComputeBoW()==，参考 3.1 节跟踪参考帧模型的步骤一；
步骤二： 在关键帧数据库中找到与当前帧相似的候选关键帧集合，词典单词线性表示向量距离较近的一些关键帧；
- 步骤 1： 遍历当前帧词袋的所有 word，并搜索关键帧数据库中与之存在公共单词的关键帧 list<KeyFrame*> lKFsSharingWords；
- 步骤2： 统计所有闭环候选帧中与当前帧具有共同单词最多的单词数，并以它的 0.8 倍作为阈值 minCommonWords；
- 步骤3： 遍历所有闭环候选帧，挑选出共有单词数大于阈值 minCommonWords 且单词匹配度大于 minScore 的关键帧，将匹配度和关键帧存入 list<pair<float,KeyFrame*> > lScoreAndMatch；
- 步骤4： 计算候选帧组得分，得到最高组得分 bestAccScore ，并以此此得分的 0.75 倍作为阈值minScoreToRetain；
  - 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分；
  - 具体而言：lScoreAndMatch 中每一个 KeyFrame 都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的 KeyFrame，记录于 lAccScoreAndMatch；
- 步骤5： 得到组得分大于阈值的，组内得分最高的关键帧作为重定位的候选关键帧。
步骤三： 进行 ORB 特征匹配，BoW 加速搜索，要求匹配点数大于 15；
- 步骤 1： 创建 ORB特征点匹配器最小距离 < 0.75 倍的次小距离则匹配成功：ORBmatcher matcher(0.75,true)；
- 步骤 2： 对当前帧和候选关键帧进行 BoW 搜索，SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i])，参考 3.1 节的步骤二。
步骤四： 使用 EPnP 算法估计位姿
- 对当前帧和候选帧创建 PnPsolver 位姿变换求解器 PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
- 迭代求解位姿过程在 ==PnPsolver::iterate()== 函数中进行，返回当前帧位姿；
- 这里会结合 Ransac 随采样序列一致性算法，来提高求解的鲁棒性。
步骤五： 得到当前帧的初始位姿之后，使用最小化冲投影误差 BA 算法来优化位姿 ==Optimizer::PoseOptimization()==；
步骤六： 如果上一步优化之后返回内点较少，会把参考关键中还没有在当前帧有 2d 匹配的点反投影到当前帧下，再次搜索 2d 匹配点，实现函数在 ==ORBmatcher::SearchByProjection== 中，具体参考 3.2 节的步骤四。
步骤七： 如果内点数量大于 50，则重定位成功。

4. 局部地图跟踪

帧间位姿估计的三种模型是为了获取当前帧的一个位姿初值，在前面三种模式每次跟踪成功之后还需要跟踪局部地图，进行 BA 优化。实现函数在 ==Tracking::TrackLocalMap()== 中。

基本思想：
- 上面完成初始位姿的跟踪后，需要使用局部地图(参考帧的一二级共视帧组成) 来进行局部地图优化，来提高鲁棒性；
- 局部地图中与当前帧有公共点的关键帧序列成为一级相关帧 K1；
- 与一级相关帧K1有共视地图点的关键帧序列成为二级相关帧K2；
- 把局部地图中的局部地图点，投影到当前帧上，如果在当前帧的视野内进行位姿优化。
步骤一： 对局部地图进行更新 ==Tracking::UpdateLocalMap()== ，更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints；
- 步骤 1： 通过局部地图点设置参考地图点 ==Map::SetReferenceMapPoints()==
  1
  
  mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
- 步骤 2： 更新局部关键帧 ==Tracking::UpdateLocalKeyFrames()==，生成对应当前帧的局部地图(小图)
  - 原则：始终限制局部关键帧(小图中关键帧的数量)数量不超过 80；
  - 步骤 ①： 遍历当前帧的所有 MapPoints，记录所有能观测到这些点的关键帧；
  - 步骤 ②： 对上面的关键帧进行筛选，保存为局部关键帧序列，筛选策略：
    - 策略 A： 能观测到当前帧 MapPoints 的关键帧作为局部关键帧；
    - 策略 B： 与策略 A 得到的局部关键帧共视程度很高的关键帧作为局部关键帧；
      - 策略 B.1： 最佳共视的 10 帧，如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧；
      - 策略 B.2： 这些帧的子关键帧；
      - 策略 B.3： 这些帧的父关键帧。
  - 步骤 ③： 更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧。
- 步骤 3： 更新局部地图点 ==Tracking::UpdateLocalPoints()==，所有局部关键帧包含的地图点构成局部地图点；
  - 步骤 ①： 清空局部地图点；
  - 步骤 ②： 将局部关键帧的 MapPoints 添加到 mvpLocalMapPoints；
  - 步骤 ③： 同时设置地图点更新标志，来避免重复添加出现在多帧上的地图点。
步骤二： 在局部地图中对当前帧搜索匹配点对，实现函数在 ==Tracking::SearchLocalPoints()== 中
- 步骤 1： 遍历当前帧的特征点，如果已经有相应的 3D 地图点,则进行标记，不需要进行重投影匹配，并且标记已经被遍历过；
- 步骤 2： 遍历局部地图的所有地图点，如果没有被遍历过，把地图点反投影到当前帧下，保留在当前帧视野内的地图点；
- 步骤 3： 根据反投影后的2d位置，设置一个半径为 th 的范围进行搜索匹配点
  1
  
  matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
步骤三： 更新局部所有 MapPoints 后对位姿再次优化 ==Optimizer::PoseOptimization()==；
步骤四： 更新地图点状态；
步骤五： 如果刚刚进行过重定位则需要内点匹配点对数大于 50 才认为跟踪成功，正常情况下，找到的内点匹配点对数大于 30 算成功。

5. 关键帧生成

5.1 关键帧生成条件判断

在跟踪成功之后（三种跟踪模式和地图跟踪成功之后）判断是否需要创建关键帧，判断函数在 ==Tracking::NeedNewKeyFrame()== 中

步骤一： 系统模式判断，如果仅仅需要跟踪定位，不需要建图，那么不需要新建关键帧；
步骤二： 根据地图中关键帧的数量设置一些参数(系统一开始关键帧少的时候，可以放宽一些条件，多创建一些关键帧)；

步骤三： 如果很长时间（1s）没有插入关键帧

1

const bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId+mMaxFrames;

步骤四： 查询局部地图管理器是否繁忙，localMapper 处于空闲状态,才有生成关键帧的基本条件

==LocalMapping::AcceptKeyFrames()==

1
2


bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);

步骤五： 当前帧跟踪到点数量比较少，tracking 质量较弱

1
2
3
4


// 跟踪要跪的节奏，0.25和0.3是一个比较低的阈值
const bool c1c =  mSensor!=System::MONOCULAR &&         //只有在双目的时候才成立
                (mnMatchesInliers<nRefMatches*0.25 ||   //和地图点匹配的数目非常少
                  ratioMap<0.3f) ;                      //地图点跟踪成功的比例非常小,要挂了

步骤六： 上面条件成立之前必须当当前帧与之前参考帧（最近的一个关键帧）重复度不是太高

1
2
3
4


    // 阈值比c1c要高，与之前参考帧（最近的一个关键帧）重复度不是太高
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio ||  // 总的来说,还是参考关键帧观测到的地图点的数目太少,少于给定的阈值
                    ratioMap<thMapRatio) &&                   // 追踪到的地图点的数目比例太少,少于阈值
                mnMatchesInliers>15);                         //匹配到的内点太少

步骤七： 决策是否需要插入关键帧：(c1a || c1b || c1c) && c2
- 当局部地图不繁忙时可以直接插入关键帧，返回 true；
- tracking 插入关键帧不是直接插入，而且先插入到 mlNewKeyFrames 中，然后 localmapper 再逐个 pop 出来插入到 mspKeyFrames 中，所以队列里不能阻塞太多关键帧，队列中少于 3 个时可以插入，否则返回 false。

5.2 创建关键帧

当上一步返回 true 时，说明可以创建新的关键帧，创建关键帧具体在 ==Tracking::CreateNewKeyFrame()== 中实现。

步骤一： 在保证局部建图器开启的情况下将当前帧构造为关键帧；
1

KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

步骤二： 将当前关键帧设置为当前帧的参考关键帧；

1
2
3


// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;

步骤三： 对于双目或 rgbd 摄像头，为当前帧生成新的 MapPoints
- 步骤1： 得到当前帧深度小于阈值的特征点；
- 步骤2： 按照深度从小到大排序；
  1
  
  sort(vDepthIdx.begin(),vDepthIdx.end());
- 步骤3： 将距离比较近的点包装成 MapPoints，以下添加属性的操作是每次创建 MapPoint 后都要做的：
  - 步骤 ①： 地图点关联关键帧
    1
    
    pNewMP->AddObservation(pKF,i);
  - 步骤 ②： 关键帧关联地图点
    1
    
    pKF->AddMapPoint(pNewMP,i);
  - 步骤 ③： 地图点更新最优区别性的描述子
    1
    
    pNewMP->ComputeDistinctiveDescriptors();
  - 步骤 ④： 地图点更新深度
    1
    
    pNewMP->UpdateNormalAndDepth();
  - 步骤 ⑤： 地图添加地图点
    1
    
    mpMap->AddMapPoint(pNewMP);

步骤四： 将关键帧加入到待处理队列 mlNewKeyFrames 中供局部建图线程处理 ==LocalMapping::InsertKeyFrame(KeyFrame *pKF)==

1
2
3
4
5
6
7


void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexNewKFs);
    // 将关键帧插入到列表中
    mlNewKeyFrames.push_back(pKF);
    mbAbortBA=true;
}

6. 信息维护与线程同步

地图包含的信息：关键帧、地图点
更新地图绘制器
- 初始化的帧在更新了地图点和关键帧之后更新绘制器 mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose())；

【R】参考资料

[1] 吴博师兄 ppt 及注释代码
[1] 张维智师兄笔记
[2] ORBSLAM2学习之Tracking线程
[3] 一起学ORBSLAM2（4）tracking主线程
[4] orb_slam代码解析(2)Tracking线程（特征提取部分的主要参考）
[5] ORB-SLAM2代码整理–tracking线程
[6] ORB-SLAM2（二）跟踪线程Tracking.cpp
[7] ORB-SLAM2详解（四）跟踪
[8] ORB-SLAM2 ORB特征点法SLAM（单目初始化主要参考）
[9] 一步步实现slam3-初始位置估计1
[10] ORBmatcher类

【Q】问题

~~单目初始化时计算单应矩阵和基础矩阵，求其得分，三角化，分解出位姿部分的代码；~~
恢复出位姿之后初始化地图部分、BA 优化的代码还有点乱。
跟踪参考帧时、跟踪运动模型确定了当前帧的初始位姿之后的位姿优化 PoseOptimization，参考
采用运动模型时，先计算上一帧的位姿，通过 Tlw = Tlr * Trw 来计算，难道上一步不是直接返回的上一帧的位姿吗？还是里程计的相对位姿变换？
pnp 求解过程补充。

2019.04.27
wuyanminmax@gmail.com

文章目录