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

?? Tracking 系统主线程中的线程运行，负责特征提取、位置估计、地图跟踪、关键帧选择等工作，可简单理解为 SLAM 前端里程计部分，但也有一些优化。

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

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

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

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

• ① 运动模型跟踪 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 求解当前位姿。

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

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

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

• 在构造帧之间，初始化跟踪线程的时候，创建了三个特征提取器
  • tracking 过程都会用到 mpORBextractorLeft 作为特征点提取器，在单目初始化的时候，会用 mpIniORBextractor 来作为特征点提取器，两者的区别是后者比前者最多提出的点数多一倍。

  // 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；

    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 倍，以此类推，最后一层兜底；

    // 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，用于后面计算描述子的随机采样点集合；

    std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
    

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

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

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

  (*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 继续提取；

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

      • 然后对提取出了的关键点 vKeysCell 换算出其位于 level 层的被裁掉边界的图像中的位置，并将每个窗口中的关键点存入 vToDistributeKeys 容器中，暂时保存着第 level 层图像的关键点；
    • 步骤 ④： 将每层的 vToDistributeKeys 送入到 ORBextractor::DistributeOctTree() 中进行关键点剔除和平分配
      • 步骤 A： 确定四叉树有几个初始节点，每个初始节点的 x 方向有多少个像素

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

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

        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 特征的介绍。

       /* @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() 函数具体实现

    /** * @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 进行畸变矫正
• 步骤四： 初始化本帧的地图点，先默认所有的地图点均为内点

  // 初始化存储地图点句柄的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；
  • 从类的成员变量中获取已经去畸变后的特征点的每一个特征点

    const cv::KeyPoint &kp = mvKeysUn[i];
    

    • 并利用 Frame::PosInGrid() 找到该特征点所处的网格，输出为指定的图像特征点所在的图像网格的横纵 id（其实就是图像网格的坐标）
  • 根据上一步返回的网格坐标，将该特征点分配到网格中

    if(PosInGrid(kp,nGridPosX,nGridPosY))
        mGrid[nGridPosX][nGridPosY].push_back(i);
    

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

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

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