2.1ORB-SLAM3之ORB特征提取和BRIEF描述子计算

1.模块功能简述

该模块主要是进行ORB特征点的提取,描述子的计算;输入一张原始图像,输出KeyPoints和Descriptor。主要包含创建图像金字塔,对金字塔图像进行特征提取,对特征点进行基于四叉树的均匀化分布，计算特征点方向,计算描述子等四个主要模块。

2.构造函数

构造函数初始化一个特征提取器，规定了特征提取的数量，金字塔尺度因子，金字塔层数，特征提取时的大小阈值。根据系统选择的传感器不同，在Tracking线程中会使用不同的参数构造特征提取器。主要差别在于，对于单目模式，初始化之前提取的特征数量是平时的5倍。

/**
 * @brief Construct a new ORBextractor::ORBextractor object
 * 
 * @param _nfeatures 单张图像需要提取的特征数量
 * @param _scalFactor 金字塔图像的尺度因子
 * @param _nlevels 金字塔层数
 * @param _iniThFAST 宽松阈值
 * @param _minThFast 严刻阈值
 */
ORBextractor::ORBextractor(int _nfeatures, float _scalFactor, int _nlevels, int _iniThFAST, int _minThFast)

构造函数在ORBSLAM的Tracking线程的构造函数完成，只构造一次，主要完成的事情如下：

1. 初始化图像金字塔的一些参数
   主要包括尺度因子和每层金字塔图像需要提取的特征数，计算每层金字塔图像的特征数时，主要基于等比数列进行计算，已经知道等比数列前n项和以及比值q(尺度因子_scalFactor)，可以得到每一层金字塔图像需要提取的特征数量(有些解析是根据每层金字塔图像占总面积的比值得到的，代码中比较简单粗暴直接将尺度因子作为了比值);

2. 初始化计算描述子的256组点对
   这256个点对即512个点是论文里通过训练得到的位置，可以让特征点拥有更好的区分度。要说明的是，ORBSLAM的BRIEF描述子有256维，这一点和《十四讲》中的128维有所区别
   这些点对的坐标系使用的是图像块的局部坐标系，以图像块中心像素为原点，中心线为坐标轴，因此x,y方向的坐标范围均为[-15 ,15]

3. 初始化计算特征点方向的圆形区域边界
   因为考虑到像素坐标都是整数的问题，所以这里的圆形区域只是一个近似的计算，在图像中行用v表示，列用u表示，作者的思路是先通过勾股定理和四舍五入得到0-45度区间内的圆形区域边界坐标，然后通过对称操作得到45-90度区间的边界值，最终可以得到坐标系第一象限下的1/4圆的坐标。

   对于第一象限的每个v值对应的u值如下：
   下半部分：
   u[0]=15, u[1]=15, u[2]=15, u[3]=15, u[4]=14, u[5]=14, u[6]=14, u[7]=13, u[8]=13, u[9]=12, u[10]=11, u[11]=10
   上半部分：
   u[15]=3, u[14]=6, u[13]=8, u[12] = 9, u[11]=10, u[10]=11
   

3 特征提取

/**
 * @brief ()重载函数，特征提取的外层调用函数
 * @param _image 输入图像
 * @param _mask 掩膜，没有用到
 * @param _keypoints 输出，整副图像的特征点
 * @param _descriptors 输出，特征点描述子
 * @param vLappingArea 双单目模式下两个相机图像的重叠区域
 * @return int 特征点的数量
 */
int ORBextractor::operator()(InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
                                OutputArray _descriptors, std::vector<int> &vLappingArea)

特征提取函数是该模块的核心函数，包括创建图像金字塔，对每层金字塔图像进行特征提取，基于四叉树对特征进行均匀化处理，计算fast特征点的方向，计算特征点对应的描述子

3.1 创建图像金字塔

对ORBSLAM代码进行修改，去掉扩边处理，通过对图像进行resize获得金字塔的每一层图像，根据对两种方法得到的图像金字塔进行对比，所得到的结果一致,获得的金字塔图像存储在成员变量mvImagePyramid中。

void ORBextractor::ComputePyramid(cv::Mat image);

在对图像resize时，关于图像的宽和高会进行四舍五入的取整操作。图像像素坐标表示某个像素在图像的第几行第几列，一般都是整数，目前只在进行双目之间特征匹配时使用到了亚像素级别的精度，但这只用于3D点的恢复。

(通过对比，直接对图像进行resize和对图像进行扩边得到的金字塔图像在尺寸以及像素上相同，所以创建图像金字塔中的扩边处理感觉就很鸡肋)

3.2 对每层图像提取Fast特征点并计算特征点的方向

函数入口：

void ORBextractor::ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> > &allKeypoints)// 函数总入口
---->void ORBextractor::ComputeKeyPointsOctTreeEveryLevel(int level, std::vector<std::vector<cv::KeyPoint> > &allKeypoints)// 提取单层的图像特征点
---->static void computeOrientation(cv::Mat &image, vector<cv::KeyPoint>& keypoints, const vector<int> &umax)// 计算单层图像特征点的方向

3.2.1 Fast关键点提取

1. 本质上调用的是OpenCV的函数接口，首先将金字塔图像划分为wCell列hCell行个像素块，每个像素块的边长大概在30左右，提取特征点时遍历每个像素块，并将像素块送入OpenCV函数进行特征提取，提取结束后将特征点的坐标恢复成图像上的全局坐标

2. 特征提取设置了两个阈值，Fast特征提取就是比较某个像素与周围一圈像素的差异，如果与连续N个像素差值均大于某个阈值就认为该像素是特征点，ORBSLAM系列设置了两个判断阈值，一个比较严格(_iniThFAST=20),一个比较宽松(_minThFast=7),首先使用严格的阈值对像素块中的图像进行检测，如果检测不到特征点再使用宽松的阈值检测
   

3. 将图像划分成小的像素块就是为上述两种阈值服务的，这里的思想就是尽量采用梯度变化较明显的像素作为特征点，如果检测不到再放宽限制。但是这种方法在纹理较少的区域可能会浪费较多时间，因为相当于对整副图像进行了两次特征提取。

3.2.2 基于四叉树的特征均匀化分布

参考链接: https://zhuanlan.zhihu.com/p/61738607
主要步骤如下：

1. 根据图像的宽度与高度的比例确定初始节点的数量(一般初始节点只有1个node,kitty数据集图像的分辨率是720x360会被划分成两个)

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

2. 如果node里面的点数>1，把每个node分成四个node;如果node中的特征点为1,该node停止分裂;如果node里面的特征点为空，删掉。当一个父节点被分成4个子节点时，该父节点就没用了，会被从节点列表中删掉

void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)

3. 重复过程2。终止条件为：node的总数量> N_a，或者无法再进行分裂即节点的数量不再增加。然后从每个node里面选择一个相应程度最大的FAST点

示意图：假设某一层图像的特征点N=25
Step1:只有1个node

Step2:第一次分裂
(1)1个node分裂为4个node
(2)node数量4<25，还要继续分裂

Step3:第二次分裂
(1)4个node分裂成15个node,因为其中一个node没有点所以删除该node
(2)node数量16<25，还要继续分裂

Step4:第三次分裂
(1)15个node分裂为30个node，删掉了没有特征点的node
(2)node数量30>25，停止分裂

Step5:从每个node中选择响应值最大的一个点

3.2.3 计算特征点方向

static void computeOrientation(cv::Mat &image, vector<cv::KeyPoint>& keypoints, const vector<int> &umax)
---->static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)// 核心函数，给定一个特征点坐标，计算特征点方向

根据灰度质心法计算特征点方向，以特征点像素为圆心，HALF_PATCH_SIZE=15为半径，计算圆型区域内像素的灰度质心，连接圆心与灰度质心即得到特征点的方向。

1. 圆形区域选择
   在构造函数中已经确定了圆形区域的边界，对于每一行(即每一个v值)都存在一个列坐标边界值，具体见## 2.构造函数部分。

2. 关键点方向计算

   1. 简单的来说分别求得圆形区域内所有像素的灰度值总和m00，图像沿x方向的矩m10,图像沿y方向的矩m01
   2. 根据公式可以计算得到该圆形区域的灰度质心C=(cx,cy)
   3. 特征点的方向就是向量OC的方向

代码中的计算技巧：
在计算圆形区域的边界时可以知道，关于特征点周围圆形区域的边界是关于x轴对称的，因此在代码实现时采用了一个小技巧，一次性可以计算关于x轴对称的两行像素的矩：

上图中相同颜色的像素带表示关于中心轴对称，假设对于某对对称的像素，其坐标为[u,v],[u,-v]，则可以分别计算他们在x和y方向上的矩

x方向：m_10 = u * I(u,v) + u * I(u,-v) = u * (I(u,v) + I(u,-v))
y方向：m_01 = v * I(u,v) + (-v) * I(u,-v) = v* (I(u,v) - I(u,-v))

这样遍历圆形区域上半部分所有行即可计算整个圆形区域图像在x方向和y方向上的矩,整体上减少了一半的遍历时间

为什么选择计算圆形区域图像的矩计算特征点方向
对于同一特征点在不同图像中的方向或者描述子计算，需要尽可能的选择对统一块区域的像素进行处理，保证同一位置特征点的属性相同，如果选择正方形区域会导致采集点的时候绿色和黄色部分不是相同的像素

3.3 计算特征点的描述子

在ORBSLAM系列中还考虑到了BRIEF的旋转性，基于描述子计算两帧图像之间的特征匹配的本质就是比较对应特征点周围像素之间的灰度值差异，当相机发生旋转时，图像内特征点周围的像素也会发生相应的旋转，因此在进行比较时需要将特征点周围的像素块旋转相同的方向，使得进行比较的两个像素块是同一块区域。

3.3.1 BRIEF描述子简介

BRIEF是一个二进制的描述子，计算和匹配的速度都很快。计算步骤如下：
(1)以关键点为中心，选择一个31×31的像素块
(2)在这个块内按照一定的方法选择N对点，N一般取256,这里的方法一般是提前通过学习选择的效果最好的256组位置
(3)对于点对{A,B},通过比较这两个点的灰度大小确定一个二值结果，0或者1.

(4)这样把256个点对的结果排起来，就形成了BRIEF描述子。
[1 0 1 0 0 ... 1 0 0]

BRIEF的匹配采用汉明距离，非常快，简单说就是看一下不相同的位数有多少个。如下的两个描述子，不同的位数为4。实际上我们选择的点对数为256，那么距离范围就是0~256。
[1 0 1 1 1 0]
[1 1 0 0 1 1]

3.3.2 Steered BRIEF

思想简述：不管是计算特征点的方向还是计算描述子的方向，最终的目的是为了更加准确的匹配上两张图像中对应的特征点，匹配的依据则是通过描述子进行判断。两个特征点周围的像素信息分布相差越小，描述子距离就越小，两个特征点是goodMatch的可能性就越高。

这样，判断两个特征点是否匹配的问题就转化成计算特征点周围像素分布情况差异的问题，然后就又转化为对周围像素的选取以及这种差异性计算方式的设计。

对于BRIEF描述子来说，计算方式就是计算汉明距离，点对的选择上面也提到了使用学习的方法进行设计，现在就剩下一个问题了，怎么确定参与比较的特征点周围的像素。

BRIEF把关键点"周围像素"成为一个像素快patch，如果不考虑旋转则选择patch就简单了，直接以特征点像素坐标为中心，选择长和宽固定的一个patch就可以了，如下图所示。

但是存在一个问题，随着相机运动，特征点在x方向和y方向上的像素会发生变化，这样选取的图像块中的像素也会发生很大的变化，如同上图的PATCH1和PATCH2，如果拿这两个像素快去计算秒素子距离，肯定会有比较大的差异。

基于这一现象，rBRIEF在原来的基础上增加了旋转的信息，就是当图像发生旋转了，特征点的方向肯定发生旋转，选取的的图像块范围也给他整体进行一个旋转，就变成了下图所示。
使用PATCH1’中的像素和PATCH2中的像素进行比较。

BRIEF的旋转不变性原理大致是这样了，下面看一下具体的实现。

BRIEF描述子是没有考虑旋转不变性的，Steered BRIEF根据Oriented FAST计算出的角度，把原始的256个点对的坐标旋转之后，再取灰度。从而实现了旋转不变性。

原始的256个点对坐标为S，旋转后的为S_θ

具体实现代码：

// 在特征提取的最后，会在computeOrientation函数中计算特征点的角度
        float angle = (float)kpt.angle*factorPI;// 将角度转换为弧度
        float a = (float)cos(angle), b = (float)sin(angle);// 计算余弦值与正弦值
        // 和之前一样，这里获取的是一个指向特征点像素的uchar类型的指针，并非是特征点所对应像素的灰度值，需要注意
        // 如果把img前面的取地址符&去掉，获取的就是该位置对应的灰度值了
        // 换句话说是可以对center进行索引操作获取其它元素的值的，这在下面定义的宏中就有体现
        const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
        // 这里的step是Mat的属性之一，表示一个数据所占字节长度，方便后面迭代时候跳过指定步长
        // 例如一个单通道灰度影像一个像素值在我这里字节长度是700,而RGB影像一个像素的长度是2100
        const int step = (int)img.step;
// 这里定义了一个宏，用于后续计算的方便
// 这里的center就是上面获取的uchar的指针，而pattern就是传入的参数
// 这里面一长串其实都是在计算索引，而对center取索引获取到的就是该索引(位置)所对应的灰度值
// 像素位置 = 行的值×每行的字节数 + 列的值
#define GET_VALUE(idx) center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + cvRound(pattern[idx].x*a - pattern[idx].y*b)]

需要计算的像素点位置在旋转前后的对应关系如下图所示，即像素的坐标跟着旋转了一个特征点方向。

符号说明：

-θ:关键点的角度
-φ：像素块中某个像素在旋转前与x轴夹角
-r:像素块到原点的距离
-p(x,y):旋转前像素点坐标
-p'(x,y):旋转后像素点坐标

现在就变成了已知θ，p(x,y)求p'(x,y)，推导公式如下：

4.返回值

在计算特征点和描述子后，需要将特征点和描述子的索引进行一一对应。

特征点最后的返回形式是一个cv::KeyPoint类型的vector,描述子最终返回的形式是一个nkeys行，32列，每列占8个字节的cv::Mat

5.一点拓展和思考

在ORBSLAM3中，基于ROS发布的位姿是直接从Tracking中拿到的，通过时间统计Tracking的耗时主要在ORB特征检测部分。因为特征检测是在8层金字塔图像上依次遍历，所以可以使用并行线程同时对8层金字塔进行处理，但结果差强人意。

经统计，在自己的工作站上在单目模式下的耗时统计如下：

可以看到，跟踪总时间不降却升先不说，特征检测部分并行化处理并没有明显提升检测速度。经测试，是因为线程频繁创建和销毁产生的时间开销和并行化节省的时间差不多，因此没有达到理想效果。

有过测试，在特征提取模块，对每层金字塔处理时强制睡眠1s，结果并行话特征检测大概花费1s+左右，串行却花费了8s+，可见并行化处理是起了作用的，但是原本时间开销就比较少了，在这里起到的作用不大。

另，一点其他的改进思路：
有个设想，可以像回环检测，局部建图线程那样在SLAM系统初始化时创建一个线程池，等待图像输入进来，这样线程只需要创建一次，系统结束时销毁，但是这种方法有点投机取巧，而且从长远看SLAM算法终究还是要移植到嵌入式或者NVIDIA边缘服务器这种性能较差的设备上，哪里会有那么多CPU核供SLAM使用。

因此目前以为应该从直接法入手从本质上提升特征检测速度，参考SVO半直接法和特征点法的结合，可以达到比较高的FPS。

另，一点思路的一点思路：
再扯远一点，相机的FPS大概在20-30左右，也就是说相邻图像帧之间的时间间隔在33ms-50ms左右，也就是说普通的VSLAM算法跟踪一帧的时间小于单帧时间间隔就可以满足使用了。