ORB_SLAM2特征点提取策略及描述子的计算

推荐一下计算机视觉life所发布的文件，里面包含了源码的详细注解、PPT等材料。下载链接如下：
https://github.com/electech6/ORBSLAM2_detailed_comments

OpenCV自带的库函数与ORB_SLAM2特征点提取策略效果对比

在提取特征点的时候，传统的ORB特征点提取策略会使得提取出的特征点扎堆出现，使得依据路标点建立的轨迹精度差，可靠性差。提取均匀分布的特征点，可提高ORB_SLAM2的精度。

OpenCV库自带的函数处理效果
可见提取的FAST特征点基本都集中在人物的边缘上。

ORB_SLAM2特征点提取策略效果

明显可见特征点的分布均匀了很多。

ORB_SLAM2特征点提取策略

使用图像金字塔实现了尺度不变性，使用灰度质心法实现了旋转不变性质。

尺度不变性

　　如果直接使用当前金字塔的底层和forward金字塔的底层进行特征点匹配时，很难精准匹配，因此可以考虑和forward中的第1层进行匹配，匹配结果会靠谱些。

src/KeyFrameDatabase.cc中存放特征点提取相关的函数

void ORBextractor::ComputePyramid(cv::Mat image)：构建图像金字塔函数
void ORBextractor::ComputeKeyPointsOctTree(vector& allKeypoints)：对图像金字塔中的每一层使用四叉树的方法计算特征点（后面会介绍具体的过程）。

旋转不变性

使用灰度质心法，获得了每个特征点的方向。灰度质心法的基本原理如下：

参考博客：https://blog.csdn.net/u014709760/article/details/87978271

其中P为几何中心，Q为灰度重心。灰度重心和特征点的方向的计算公式为：
代码中的体现

static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{
     
	//图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权
    int m_01 = 0, m_10 = 0;

	//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
	//这条v=0中心线的计算需要特殊对待
    //由于是中心行+若干行对，所以PATCH_SIZE应该是个奇数
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
		//注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权
        m_10 += u * center[u];

    // Go line by line in the circular patch  
	//这里的step1表示这个图像一行包含的字节总数。参考[https://blog.csdn.net/qianqing13579/article/details/45318279]
    int step = (int)image.step1();
	//注意这里是以v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
     
        // Proceed over the two lines
		//本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行
        int v_sum = 0;
		// 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！
        int d = u_max[v];
		//在坐标范围内挨个像素遍历，实际是一次遍历2个
        // 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y) 
        // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
        // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
        for (int u = -d; u <= d; ++u)
        {
     
			//得到需要进行加运算和减运算的像素灰度值
			//val_plus：在中心线下方x=u时的的像素灰度值
            //val_minus：在中心线上方x=u时的像素灰度值
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
			//在v（y轴）上，2行所有像素灰度值之差
            v_sum += (val_plus - val_minus);
			//u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
            m_10 += u * (val_plus + val_minus);
        }
        //将这一行上的和按照y坐标加权
        m_01 += v * v_sum;
    }

    //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
    return fastAtan2((float)m_01, (float)m_10);
}

这里在计算m_01，m_10时，在计算1/2的圆的过程中计算了全部，也算加快了算法速度。

提取策略1：分块检测特征点

传统的ORB中的FAST特征点提取针对全图。在ORB_SLAM2中，作者将图像分成30*30的小块。在每个小块中检测特征点。
代码中的体现

void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
	//重新调整图像层数
    allKeypoints.resize(nlevels);

	//图像cell的尺寸，是个正方形，可以理解为边长in像素坐标
    const float W = 30;

    // 对每一层图像做处理
	//遍历所有图像
    for (int level = 0; level < nlevels; ++level)
    {
     
		//计算这层图像的坐标边界， NOTICE 注意这里是坐标边界，EDGE_THRESHOLD指的应该是可以提取特征点的有效图像边界，后面会一直使用“有效图像边界“这个自创名词
        const int minBorderX = EDGE_THRESHOLD-3;			//这里的3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆
        const int minBorderY = minBorderX;					//minY的计算就可以直接拷贝上面的计算结果了
        const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
        const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;

		//存储需要进行平均分配的特征点
        vector<cv::KeyPoint> vToDistributeKeys;
		//一般地都是过量采集，所以这里预分配的空间大小是nfeatures*10
        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;
			//计算当前网格最大的行坐标，这里的+6=+3+3，即考虑到了多出来3是为了cell边界像素进行FAST特征点提取用
			//前面的EDGE_THRESHOLD指的应该是提取后的特征点所在的边界，所以minBorderY是考虑了计算半径时候的图像边界
			//目测一个图像网格的大小是25*25啊
            float maxY = iniY+hCell+6;

			//如果初始的行坐标就已经超过了有效的图像边界了，这里的“有效图像”是指原始的、可以提取FAST特征点的图像区域
            if(iniY>=maxBorderY-3)
				//那么就跳过这一行
                continue;
			//如果图像的大小导致不能够正好划分出来整齐的图像网格，那么就要委屈最后一行了
            if(maxY>maxBorderY)
                maxY = maxBorderY;

			//开始列的遍历
            for(int j=0; j<nCols; j++)
            {
     
				//计算初始的列坐标
                const float iniX =minBorderX+j*wCell;
				//计算这列网格的最大列坐标，+6的含义和前面相同
                float maxX = iniX+wCell+6;
				//判断坐标是否在图像中
				//TODO 不太能够明白为什么要-6，前面不都是-3吗
				//!BUG  正确应该是maxBorderX-3
                if(iniX>=maxBorderX-6)
                    continue;
				//如果最大坐标越界那么委屈一下
                if(maxX>maxBorderX)
                    maxX = maxBorderX;

                // FAST提取兴趣点, 自适应阈值
				//这个向量存储这个cell中的特征点
                vector<cv::KeyPoint> vKeysCell;
				//调用opencv的库函数来检测FAST角点
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),	//待检测的图像，这里就是当前遍历到的图像块
                     vKeysCell,			//存储角点位置的容器
					 iniThFAST,			//检测阈值
					 true);				//使能非极大值抑制

				//如果这个图像块中使用默认的FAST检测阈值没有能够检测到角点
                if(vKeysCell.empty())
                {
     
					//那么就使用更低的阈值来进行重新检测
                    FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),	//待检测的图像
                         vKeysCell,		//存储角点位置的容器
						 minThFAST,		//更低的检测阈值
						 true);			//使能非极大值抑制
                }

                //当图像cell中检测到FAST角点的时候执行下面的语句
                if(!vKeysCell.empty())
                {
     
					//遍历其中的所有FAST角点
                    for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
                    {
     
						//NOTICE 到目前为止，这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标
						//这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标
						//在后面将会被继续转换成为在当前图层的扩充图像坐标系下的坐标
                        (*vit).pt.x+=j*wCell;
                        (*vit).pt.y+=i*hCell;
						//然后将其加入到”等待被分配“的特征点容器中
                        vToDistributeKeys.push_back(*vit);
                    }//遍历图像cell中的所有的提取出来的FAST角点，并且恢复其在整个金字塔当前层图像下的坐标
                }//当图像cell中检测到FAST角点的时候执行下面的语句
            }//开始遍历图像cell的列
        }//开始遍历图像cell的行

可见在每个30*30的小块里面，检测12（iniThFAST）个特征点，如果没有12个，则检测7（minThFAST）个特征点（iniThFAST和minThFAST在KITTI00-02.yaml文件中配置）。

提取策略2：使用四叉树的方法保留特征点

代码中的体现
vector< cv::KeyPoint> ORBextractor::DistributeOctTree
具体步骤为：

1. 根据宽高比确定初始节点数目 。
2. 生成初始提取器节点 。
3. 将特征点分配到子提取器节点中。
4. 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点。
5. 根据特征点分布，利用4叉树方法对图像进行划分区域。
6. 当再划分之后所有的Node数大于要求数目时，就慢慢划分直到使其刚刚达到或者超过要求的特征点个数。
7. 保留每个区域响应值最大的一个兴趣点。

描述子的计算

  OpenCV自带的ORB特征点检测函数，使用的是FAST角点+BRIEF描述子。
  ORB_SLAM2中，每个特征点的描述子由32×8位组成。如果一个图片有500个特征点的话，则该图像的描述子是500*32的Mat。其中每个元素是8位二进制数。
  ORB_SLAM2提供了一个32（一个描述子有32个数）*8（每个数是8进制）*2（一个描述子需要两个像素点）*2（每个像素点有2个坐标（即u,v））的数组：

  根据给定的数组，取出考虑旋转不变性后，特征点周围某一处像素点的值：

  以图中的第一行为例：（1）通过比较两个像素点的像素值大小，获得1或0。（2）比较8组，即可获得描述子的1/32，如上图中的212（11010100）。（3）重复32次，即可获得第一行，即一个特征点的描述子。

图像特征点的存储

  每一帧都含有一个vector< KeyPoint>。在ORB_extractor.cc中，作者对（）运算符进行了重载，使得该帧对应的图像的所有金字塔层的特征点都存储在了std::vector< cv::KeyPoint> mvKeysUn中。在每个特征点类中，每个特征点会注明自己所在的金字塔层级。
  0层以上的金字塔图像中的特征点的坐标，通过乘以缩放系数，又恢复到了第0层中，然后存在了vector< KeyPoint>中。

结语

  至此，ORB_SLAM2通过分块提取特征点+四叉树分配特征点+提取每块中响应最大的特征点的方式获得了分布均匀的特征点。相比OpenCV自带的函数，该方法提取的特征点更加均匀，据此获得的SLAM轨迹和估计的位姿更加精确。