ORB-SLAM2从理论到代码实现（三）：ORB特征提取和匹配理论和代码详解

欢迎交流，本人邮箱[e-mail]

我将在写Tracking.cc的博客前，先写一下ORBextractor.cc、ORBmatcher.cc和Frame.cc。

三 特征提取和匹配

1. 理论知识

    特征点由关键点（Key-point）和描述子（Descriptor）两部分组成。ORB特征点（Oriented FAST and Rotated BRIEF）是由Oriented FAST角点和 BRIEF （Binary Robust Independent Elementary Features）描述子构成，其计算速度是sift特征点的100倍，是surf特征点的10倍。

a.Fast 角点提取

FAST 是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的思 想是：如果一个像素与它邻域的像素差别较大（过亮或过暗）, 那它更可能是角点。相比于 其他角点检测算法，FAST 只需比较像素亮度的大小，十分快捷。

提取步骤：

1. 在图像中选取像素 p，假设它的亮度为 Ip。

2. 设置一个阈值 T(比如 Ip 的 20%)。

3. 以像素 p 为中心, 选取半径为 3 的圆上的 16 个像素点。

4. 假如选取的圆上，有连续的 N 个点的亮度大于 Ip + T 或小于 Ip −T，那么像素 p 可以被认为是特征点 (N 通常取 12，即为 FAST-12。其它常用的 N 取值为 9 和 11， 他们分别被称为 FAST-9，FAST-11)。

5. 循环以上四步，对每一个像素执行相同的操作。

为了提高效率，可以采用额外的加速办法。具体操作为，对于每个像素，直接检测邻域圆上的第 1，5，9，13 个像素的亮度。至少有3个和候选点的灰度值同时大于 Ip + T 或小于 Ip −T 时，当前像素才有可能是一个角点，否则应该直接排除。为了提高比较的效率，通常只使用N个周边像素来比较，也就是大家经常说的FAST-N，其中Fast-9，Fast-12使用最多。

Fast角点本不具有方向，但是由于特征点匹配需要，ORB对Fast角点进行了改进，，改进后的 FAST 被称为 Oriented FAST，具有旋转和尺度的描述。

尺度不变性是由构建图像金字塔，并在金字塔的每一层上检测角点来实现，这在SLAM十四讲中并没有具体介绍。这里进行介绍。

高斯金字塔构建：

•  对图像做不同尺度的高斯模糊

为了让尺度体现其连续性，高斯金字塔在简单降采样的基础上加上了高斯滤波。将图像金字塔每层的一张图像使用不同参数做高斯模糊，使得金字塔的每层含有多张高斯模糊图像，将金字塔每层多张图像合称为一组(Octave)，金字塔每层只有一组图像，组数和金字塔层数相等，使用下列公式计算，每组含有多张(也叫层Interval)图像。另外，降采样时，高斯金字塔上一组图像的初始图像(底层图像)是由前一组图像的倒数第三张图像隔点采样得到的。

其中M，N为原图像的大小,t为塔顶图像的最小维数的对数值。如，对于大小为512*512的图像，金字塔上各层图像的大小如表3.1所示，当塔顶图像为4*4时，n=7，当塔顶图像为2*2时，n=8。

• 对图像做降采样(隔点采样)

 

总结：

设置一个比例因子scaleFactor（opencv默认为1.2）和金字塔的层数nlevels（pencv默认为8）。将原图像按比例因子缩小成nlevels幅图像。缩放后的图像为：I’= I/scaleFactork(k=1,2,…, nlevels)。nlevels幅不同比例的图像提取特征点总和作为这幅图像的oFAST特征点。

特征的旋转是由灰度质心法（Intensity Centroid）实现的。下面介绍灰度质心法。

灰度质心法：

1. 在一个小的图像块 B 中，定义图像块的矩为： 

2. 通过矩可以找到图像块的质心：

3. 连接图像块的几何中心 O 与质心 C，得到一个方向向量 ，于是特征点的方向可以定义为： 

上面公式摘自高翔《视觉SLAM14讲》。但是如果看程序可能还是不懂，来个更直观的公式

 

其中，图片大小为M*N，（，）表示质心，表示第i行的坐标，表示第j列的坐标，f(i,j)表示第i行第j列的像素值。 

b.ORB特征匹配

我们将Fast角点提取出来后，要描述它。否则我们无法进行匹配。ORB采用BRIEF算法来计算一个特征点的描述子。其核心思想是在关键点P的周围以一定模式选取N个点对，把这N个点对的比较结果组合起来作为描述子。

步骤：（此处参考了https://www.cnblogs.com/zjiaxing/p/5616653.html）
1.以关键点P为圆心，以d为半径做圆O。
2.在圆O内某一模式选取N个点对。这里为方便说明，N=4，实际应用中N可以取512.
假设当前选取的4个点对如上图所示分别标记为：

3.定义操作T

4.分别对已选取的点对进行T操作，将得到的结果进行组合。
假如：

则最终的描述子为：1011

ORB特征点匹配用的是汉明距离，两个等长字符串之间的汉明距离是两个字符串对应位置的不同字符的个数。换句话说，它就是将一个字符串变换成另外一个字符串所需要替换的字符个数。例如1011101 与 1001001 之间的汉明距离是 2。当两个特征点的汉明距离小于设定的阈值时，可以认为匹配。

    与ORB特征的提取和匹配相比SIFT要复杂的多，但是SIFT效果要更好。

2.代码实现

ORB-SLAM2中提取ORB特征是由ORBextractor.cc实现的。我们先来看看主要函数。

static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)

//计算特征点的方向，采取的就是我们上面讲的质心法公式
static void computeOrbDescriptor(const KeyPoint& kpt, const Mat& img, const Point *pattern,uchar* desc)////计算ORB描述子
ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
         int _iniThFAST, int _minThFAST)//提取ORB特征，我写具体点
{

//定义尺度大小
    mvScaleFactor.resize(nlevels);
    mvLevelSigma2.resize(nlevels);
    mvScaleFactor[0]=1.0f;
    mvLevelSigma2[0]=1.0f;
    for(int i=1; i<nlevels; i++)
    {
        mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;
        mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i];
    }

    mvInvScaleFactor.resize(nlevels);
    mvInvLevelSigma2.resize(nlevels);

    //计算逆尺度大小
    for(int i=0; i<nlevels; i++)
    {
        mvInvScaleFactor[i]=1.0f/mvScaleFactor[i];
        mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];
    }

    mvImagePyramid.resize(nlevels);


    mnFeaturesPerLevel.resize(nlevels);
    float factor = 1.0f / scaleFactor;
    //每层要包含的特征点数
    float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));

    int sumFeatures = 0;
    for( int level = 0; level < nlevels-1; level++ )
    {
        //对上层所包含的特征点数进行求整
        mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
        //特征点总数
        sumFeatures += mnFeaturesPerLevel[level];
        //下一层所含有的特征点数
        nDesiredFeaturesPerScale *= factor;
    }
    //最大层需要的特征点数=需要的特征点数-其他所有层的特征点总合
    mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);

    //复制训练的模板
    const int npoints = 512;
    const Point* pattern0 = (const Point*)bit_pattern_31_;
    std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));

    //This is for orientation
    // pre-compute the end of a row in a circular patch
    // 定义一个vector，用来保存每个v对应的最大坐标u
    umax.resize(HALF_PATCH_SIZE + 1);

    // 将v坐标划分为两部分计算，为了确保计算特征主方向的时候，x,y方向对称
    int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);
    int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
    const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
    for (v = 0; v <= vmax; ++v)
        umax[v] = cvRound(sqrt(hp2 - v * v)); //勾股定理

    // Make sure we are symmetric
    for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
    {
        while (umax[v0] == umax[v0 + 1])
            ++v0;
        umax[v] = v0;
        ++v0;
    }

}
tatic void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)//计算每个关键点的角度
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)//很多时候图像的容易集中在某个局部，另外有很大一部区域角点很稀疏，这样计算出
//来的描述子很不理想，为了让角点分布更均匀合理，这里运用四叉树算法对角点进行再分布。将图像以中
//点为远点划分为四个象限然后分别查看每个象限里面包含的特征点数，如果特征点数大于1，
//则在该象限继续划分，直到特征点数为1，这样自然会有很多没有特征点的叶子，
//直接去掉，然后将特征点重新进行分配
vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)///计算FAST选出来的特征点是否合格
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)////对影像金字塔中的每一层图像进行特征点的计算。具体计算过程是将影像网格分割成小区域，每一
//个小区域独立使用FAST角点检测
//检测完成之后使用DistributeOcTree函数对检测到所有的角点进行筛选，使得角点分布均匀
//计算描述子
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors, const vector<Point>& pattern)//计算描述子


void ORBextractor::ComputePyramid(cv::Mat image)//建立金字塔，这个我写具体点
{
    // 计算n个level尺度的图片
    for (int level = 0; level < nlevels; ++level)
    {
        //获取尺度
        float scale = mvInvScaleFactor[level];
        //获取当前尺度下图片的尺寸，根据金字塔模型，层数越高，尺度越大，scale是取的逆尺度因子
        // 所以层数越高，图片尺寸越小
        Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
        Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
        Mat temp(wholeSize, image.type()), masktemp;
        //图片的初始化
        mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));

        // Compute the resized image
        if( level != 0 )
        {
            resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
            // 在图像周围以对称的方式加一个宽度为EDGE_THRESHOLD的边，便于后面的计算
            copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
                           BORDER_REFLECT_101+BORDER_ISOLATED);            
        }
        else
        {
            //第一张为原图分辨率，无需缩放
            copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
                           BORDER_REFLECT_101);            
        }
    }

}

一起看ORBmatcher.cc。。。算了，写下一篇吧，不然太长了！