ORB-SLAM2 单目部分技术路线（五）

特征点提取

# 本部分代码主要位置：src/Frame.cc，src/ORBextractor.cc

        上一讲我们主要还是先大致了解了TrackMonocular()，GrabImageMonocular()，以及frame()函数的主要内容，文章的最后一个部分，我们提到了两个很重要的函数，分别是特征点提取ExtractORB()与畸变矫正UndistortKeyPoints()；这一讲里面，我们先对特征点提取部分进行讲解。

1. 仿函数：

        从frame的框架中我们进入ExtractORB()函数，在这里面定义了传入值为0或其他时的情况，为“0”代表只有左图（即单目图像）；

if(flag==0)
        (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);

        我们进入这个有点看不懂的带*的函数中，发现进入了frame.h头文件，*mpORBextractorLeft是被定义在ORBextractor下的；

ORBextractor* mpORBextractorLeft, *mpORBextractorRight;

        进而进入ORBextractor中，即进入了ORBextractor.h头文件，我们根据传入参数，马上就能找到对应的仿函数定义：

void operator()( cv::InputArray image, cv::InputArray mask,
      std::vector<cv::KeyPoint>& keypoints,
      cv::OutputArray descriptors);

        这里使用到了仿函数（Functors），简单来说仿函数是指一种能够像函数一样被调用的对象。在C++中，仿函数通常是重载了operator()的类或结构体，使得该对象的实例能够像函数一样使用，仿函数的作用和优势包括：

        代码封装：仿函数使得某些逻辑或操作封装成对象，并可以通过修改对象状态或参数来控制行为。这提供了比传统函数更灵活的接口。

        与STL算法的兼容：许多标准模板库（STL）算法都可以接收仿函数作为参数。通过自定义仿函数，可以改变算法的行为，例如定制排序规则或遍历操作。

        状态保存：与普通函数不同，仿函数不仅可以进行计算，还能保存一些状态信息，这对于某些需要持续跟踪状态的操作非常有用。

        灵活性和可扩展性：仿函数可以有成员变量和方法，可以根据实际需要提供比普通函数更多的功能。例如，它们可以通过成员函数来修改参数或持有一些内部状态，具有更多灵活性。

2. 仿函数的搭建

        这一段代码的总体作用用一句话其实就能总结：提取图像的特征点（Keypoints）并计算它们的描述符（Descriptors）。

        解析仿函数的代码，在图像验证之后，先是关于特征点的部分：

ComputePyramid(image);
vector < vector<KeyPoint> > allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);

        上述内容包括计算尺度金字塔以及提取特征点的操作。

        接下来是关于初始化在特征点周围的描述子以及处理每个层级的特征点部分：

int nkeypoints = 0;
for (int level = 0; level < nlevels; ++level)
    nkeypoints += (int)allKeypoints[level].size();
if( nkeypoints == 0 )
    _descriptors.release();
else
{
    _descriptors.create(nkeypoints, 32, CV_8U);
    descriptors = _descriptors.getMat();
}
_keypoints.clear();
_keypoints.reserve(nkeypoints);
int offset = 0;
for (int level = 0; level < nlevels; ++level)
{
    vector<KeyPoint>& keypoints = allKeypoints[level];
    int nkeypointsLevel = (int)keypoints.size();
    if(nkeypointsLevel==0)
        continue;

         之后是高斯模糊处理，有助于减少噪声并提高特征的稳定性

Mat workingMat = mvImagePyramid[level].clone();
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);

        接下来计算描述子：

Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
computeDescriptors(workingMat, keypoints, desc, pattern);
offset += nkeypointsLevel;

        这里主要涉及computeDescriptors()函数，即计算之前的256个点，形成128维向量。

         在进行完上述处理后，调整特征点坐标，保证尺度一致性。

if (level != 0)
{
    float scale = mvScaleFactor[level];
    for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
         keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
        keypoint->pt *= scale;
}
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());

3. 构建尺度金字塔

        这里我们关注ComputePyramid()函数，在之前的第二讲，我们已经初始化过金字塔的相关参数，这里函数的主要作用是构建图像金字塔，涉及到计算每一层的面积以及对其进行填充。

        如上图所示，这是金字塔的一层所有的面积，包括原图像（中心深灰色），半径扩充图像（绿色），以及高斯模糊区域（灰白） 三块区域。对应到代码：

for (int level = 0; level < nlevels; ++level)
    {
        float scale = mvInvScaleFactor[level];
        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);

            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);            
        }
    }

        从最底层（第0层）开始，在得到金字塔层数以及缩放比例后，计算了图像尺寸以及层级边界，首先通过四舍五入，计算图像的宽和高得到size；之后为当前层级创建边界，在当前图像的边缘部分添加额外的区域（通过EDGE_THRESHOLD），以处理边界区域，避免在图像边缘提取特征点时出现问题。然后创建mvImagePyramid[level]，并通过裁剪获得大小为sz的有效图像区域。

        那么对于第0层（原始图像），直接使用copyMakeBorder在四周添加边界，对应方法为BORDER_REFLECT_101（反射填充），图像没有缩放处理；对于非0层，图像尺寸会通过 resize 函数缩小。mvImagePyramid[level-1] 是前一层金字塔的图像，使用 INTER_LINEAR 插值方法将其缩放到当前层级的目标尺寸。然后，使用 copyMakeBorder 在图像的四个边添加额外的边界（由 EDGE_THRESHOLD 控制），并使用 BORDER_REFLECT_101 和 BORDER_ISOLATED （反射填充+隔离模式，避免外部数据影响）来避免边缘处理中的不连续问题。

4. 通过四叉树记录特征点

       这一过程主要可以分为两个部分，分别是特征点寻找以及四叉树分区。

4.1 特征点寻找

        通过遍历每一层金字塔，设定边际值，并根据设定的网格大小计算每个网络尺寸以及网络数量，这里用到向上取整确保覆盖整个区域。

        遍历整个网格区域进行FAST特征点检测，FAST算法会在每个网格区域内检测特征点，并将检测到的点存储到vKeysCell中。FAST特征点的选取是根据周围像素差决定的，这里我们直接调用函数即可，初次设定的阈值是iniTHFAST

for (int level = 0; level < nlevels; ++level)
    {
        const int minBorderX = EDGE_THRESHOLD-3;
        const int minBorderY = minBorderX;
        const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
        const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;

        vector<cv::KeyPoint> vToDistributeKeys;
        vToDistributeKeys.reserve(nfeatures*10);

        const float width = (maxBorderX-minBorderX);
        const float height = (maxBorderY-minBorderY);

        const int nCols = width/W;
        const int nRows = height/W;
        const int wCell = ceil(width/nCols);
        const int hCell = ceil(height/nRows);

        for(int i=0; i<nRows; i++)
        {
            const float iniY =minBorderY+i*hCell;
            float maxY = iniY+hCell+6;

            if(iniY>=maxBorderY-3)
                continue;
            if(maxY>maxBorderY)
                maxY = maxBorderY;

            for(int j=0; j<nCols; j++)
            {
                const float iniX =minBorderX+j*wCell;
                float maxX = iniX+wCell+6;
                if(iniX>=maxBorderX-6)
                    continue;
                if(maxX>maxBorderX)
                    maxX = maxBorderX;

                vector<cv::KeyPoint> vKeysCell;
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
                     vKeysCell,iniThFAST,true);

        如果检测结果为空，则选用更低的阈值minTHFAST。

        如果结果不为空，处理每个检测到的特征点，将它们的坐标调整为相对于整个图像的坐标，并将这些特征点加入vToDistributeKeys，它存储了所有检测到的特征点。

if(vKeysCell.empty())
{
    FAST(mvImagePyramid[level].rowRange(iniY, maxY).colRange(iniX, maxX),
         vKeysCell, minThFAST, true);
}

if(!vKeysCell.empty())
{
    for(vector<cv::KeyPoint>::iterator vit = vKeysCell.begin(); vit != vKeysCell.end(); vit++)
    {
        (*vit).pt.x += j * wCell;
        (*vit).pt.y += i * hCell;
        vToDistributeKeys.push_back(*vit);
    }
}

4.2 四叉树

        使用四叉树的主要目的是加快计算以及减少不必要的算力投入，并选取最合适的特征点，同时四叉树是一个多级的网格结构，通过将图像划分成多个区域（称为“单元格”）来分配特征点，从而实现特征点的均匀分布。这种方法通常用于避免特征点在某些区域过度集中或分布不均的情况。

        四叉树的函数太过于繁琐，我们在这里只介绍其思想，四叉树的主体思想其实简单来说就是在确定图像大小后，将一整个图像一分四，设定分好图像的编号，再次进行一分四，确保每个区域中都有特征点的存在；直到某一区域没有特征点，则删去该区域；或者图像分割达到阈值，若此时仍在某一区域内有多个特征点，我们对其根据进行筛选，选出最好的即可。最后确保一个区域对应一个特征点。（如一共50个特征点，阈值为31，则一般分3次后加之进行筛选即可得到最终结果）

keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
                              minBorderY, maxBorderY, mnFeaturesPerLevel[level], level);

        在这里提一下四叉树函数最后有一个computeOrientation()函数：

static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)

        这个函数的作用就是为了保证旋转不变性，通过灰度质心法，在代码中的体现就是求出水平/垂直方向的梯度，从而求得灰度质心，图像中心与灰度质心连线，从而得到一个向量，保证特征点及其周围描述子不会因为图像旋转发生改变，其中核心函数为IC_ANGLE()，目的是计算角度值。

static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{
    int m_01 = 0, m_10 = 0;

    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
        m_10 += u * center[u];

    // Go line by line in the circuI853lar patch
    int step = (int)image.step1();
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        // Proceed over the two lines
        int v_sum = 0;
        int d = u_max[v];
        for (int u = -d; u <= d; ++u)
        {
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
            v_sum += (val_plus - val_minus);
            m_10 += u * (val_plus + val_minus);
        }
        m_01 += v * v_sum;
    }

    return fastAtan2((float)m_01, (float)m_10);
}

5. 去畸变，边界划分，网格划分

        这里主要对应的是UndistortKeyPoints()函数以及frame.cc最后部分的内容，去畸变是一个固定的过程，主要和给定的相机参数有关，示意图如下：