ORBSLAM3代码笔记——特征点提取

(文中图片皆来自于网上,本篇文章只当笔记使用,少量代码,重在理解代码含义,有什么错误请留言)

两大部分:

一、头文件

二、源代码

一、头文件

class ExtractorNode//分配四叉树的节点类型
{
public:
    ExtractorNode():bNoMore(false){}

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

    std::vector vKeys;
    cv::Point2i UL, UR, BL, BR;
    std::list::iterator lit;
    bool bNoMore;
};

补充知识:四叉树索引的大致思想是将已知范围的平面或者空间均等分成四个相等的子平面或者子空间,再对四个相等的子平面或者子空间中的每一个再均等分成四个相等的子平面或者子空间,如此递归下去,直至树的层次达到一定的深度或者满足设定的某种要求后,停止分割。

二、源代码

该代码按功能来分可分为如下几大块:

ORBextractor::ComputeKeyPointsOctTree 根据四叉树提取特征点(ORB)
ORBextractor::ORBextractor 特征点提取器的构造函数
ORBextractor::ComputePyramid 构建图像金字塔

引言:构造函数(初始化)

ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
                               int _iniThFAST, int _minThFAST):
            nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
            iniThFAST(_iniThFAST), minThFAST(_minThFAST)

调用参数:

_nfeatures 指定要提取的特征点数目
_scaleFactor 指定图像金字塔的缩放系数
_nlevels 指定图像金字塔的层数
_iniThFAST 指定初始的FAST特征点提取参数,可以提取出最明显的角点
_minThFAST

指定FAST特征点提取替补阈值

注:(步骤皆为代码顺序)

第一步:存储每层图像缩放系数的vector(类似数组中的元素)调整为符合图层数目的大小。

第二步:储存每层图像缩放系数平方的vector(同上),后面没见用到过。

第三步:给上述两个缩放系数初始化。

第四步:开始遍历图像金字塔,逐层计算图像金字塔图像各层相对于初始图像缩放的系数

第五步:上述两个缩放系数的倒数,逐层计算图像金字塔图像各层相对于初始图像缩放的系数(方法同上)

第六步:调整图像金字塔vector以使得其符合咱们设定的图像层数

第七步:每层需要提取出来的特征点个数,这个向量也要根据图像金字塔设定的层数进行调整

第八步:设定每个单位缩放系数所希望的特征点个数,公式计算。(其中使用pow函数,计算图片降采样缩放系数的倒数金字塔层数的次幂。)

第九步:遍历金字塔层数,开始逐层分配特征点个数,除了顶层图像。

              这里把上述每个单位缩放系数所希望的特征点个数取整分配到储存特征点的容器中。并                  累计特征点的数目。

第十步:由于前面的特征点个数取整操作,可能会导致剩余一些特征点个数没有被分配,所以这里就将这个余出来的特征点分配到最高的图层中(这就是上述不在循环里分配顶层图像的原因。)

第十一步:获取用于计算BRIEF描述子的随机采样点点集头指针。

第十二步:将在全局变量区域的、int格式的随机采样点以cv::point格式复制到当前类对象中的成员变量(pattern)中。

                 这段代码使用std::back_inserter函数,目的是可以快覆盖掉这个容器pattern之前的数据

下面的代码是和特征点的旋转有关:(主要是计算umax)还是构造函数的一部分,为后续的计算特征点的旋转打基础。

// 预先计算圆形patch中行的结束位置
        // +1中的1表示那个圆的中间行
        umax.resize(HALF_PATCH_SIZE + 1);

        // cvFloor返回不大于参数的最大整数值,cvCeil返回不小于参数的最小整数值,cvRound则是四舍五入
        //注:因为像素点都是整数,所以对勾股定理计算出来的浮点数需要进行向下取整,这就可能导致出现索引不对称的情况,所以作者选择分成了两个八分之一圆弧进行计算

        //以下计算的是下半部分的八分之一的一个圆弧

        int v,  // 循环辅助变量
        v0,    // 辅助变量
        vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); // 计算圆的最大行号,+1应该是把中间行也给考虑进去了
        // NOTICE 注意这里的最大行号指的是计算的时候的最大行号,此行的和圆的角点在45°圆心角的一边上,之所以这样选择
        // 是因为圆周上的对称特性
        
        // 这里的二分之根2就是对应那个45°圆心角
        int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
        // 半径的平方
        const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
        // 利用圆的方程计算每行像素的u坐标边界(max)
        for (v = 0; v <= vmax; ++v)
            umax[v] = cvRound(sqrt(hp2 - v * v));  // 结果都是大于0的结果,表示x坐标在这一行的边界

        // Make sure we are symmetric
        // 这里其实是使用了对称的方式计算上四分之一的圆周上的umax,目的也是为了保持严格的对称(如果按照常规的想法做,由于cvRound就会很容易出现不对称的情况,
        // 同时这些随机采样的特征点集也不能够满足旋转之后的采样不变性了)
        for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
        {
            while (umax[v0] == umax[v0 + 1])
                ++v0;
            umax[v] = v0;
            ++v0;
        }
    }

这里,作者是根据圆的对称性进行计算(主要计算的是最长行的边界umax),又把第一象限的四分之一个圆分成上下两部分(八分之一圆)v表示y轴,u表示x轴

代码中cvFloor返回不大于参数的最大整数值,cvCeil返回不小于参数的最小整数值,cvRound则是四舍五入。明白这三点后,我们开始进行代码的解读。

第一个for循环计算的是首先计算下半部分。

这里是从v=0到v=vmax遍历,根据勾股定理可得每个v点对应的u点。

vmax在下图虚线部分,对应umax红色的弧线部分(D点到B点)这里的弧线是像素块近似的弧线,没有那么圆滑。

第二个for循环计算的是下半部分。

同理,也就是蓝色的弧线。

注:为什么不让vmax=vmin,用同一个值当循环体的条件。

个人认为,像素块是整数,当出现浮点类型的时候,使用同一个点会增加误差。

注:为什么使用圆型patch来计算图像重心:

因为图像旋转以后方形patch可能会导致像素不一致性,就无法保证后面的旋转的不变性。圆形可以避开这个问题。(如下图)

ORBSLAM3代码笔记——特征点提取_第1张图片

1、使用重载括号运算符的方法来计算图像特征点以及描述子

这个函数是这个ORBextractor.cpp的主体,通过它调用里面所有的函数。

int ORBextractor::operator()( InputArray _image, InputArray _mask, vector& _keypoints,OutputArray _descriptors, std::vector &vLappingArea)

调用参数:

_image 输入原始图的图像
_mask 掩膜mask
_keypoints 存储特征点关键点的向量
_descriptors 储存特征点描述子的矩阵
vLappingArea 应该是左右相机模型

代码功能应用如下:

【1】检测图像的有效性:检查图像的有效。

第一步:检测原始图像是否存在,不存在则直接返回;

第二步:获取图像的大小,并判断;图像的格式是否正确,要求是单通道灰度值。

【2】构建图像金字塔(调用函数ORBextractor::ComputePyramid

         注:详细讲解此功能见后面。

【3】计算图像的特征点,并且将特征点进行均匀化(调用函数ORBextractor::ComputeKeyPointsOctTree

        注:此代码中调用的函数详细讲解见后面。

【4】拷贝图像描述子到新的矩阵descriptors

第一步:统计整个图像金字塔中的特征点数量;

               这里通过遍历每层图像金字塔,累加每层的特征点个数。

第二步:判断整个图像金字塔是否没有任何特征点;

             若没有特征点,则通过调用cv::mat类的.realse方法,强制清空矩阵的引用计数,这样就可                   以强制释放矩阵的数据了。

             若如果图像金字塔中有特征点,那么就创建这个存储描述子的矩阵,注意这个矩阵是存储                   整个图像金字塔中特征点的描述子的。

第三步:特征点数量从int类型转变成向量类型(_keypoints)

 【5】 对图像进行高斯模糊

第一步:遍历金字塔每层的图像,并获取每层特征点的地址。

第二步:在循环中,判断本层的特征点数是否为0,是则跳出循环,继续下一次层图像。

第三步:在循环中,若本层的特征点数不为0,则对本层的图像进行高斯模糊。

              深拷贝当前金字塔所在层级的图像后,调用高斯模糊函数(目的在于计算描述子。提升                  特征点对噪声的影响)

【6】计算高斯模糊后图像的描述子(调用函数computeDescriptors)注:这个【6】还在上述                      【5】的循环中.这里的坐标为边缘扩充下的坐标

【7】对非第0层图像中的特征点的坐标恢复到第0层图像(原图像)的坐标系下

第一步:在循环内,获取当前图层上的缩放系数。

第二步:在循环内,开始遍历本层所有的特征点(注:上一个循环里开始了新一轮的循环遍历)

              在这个循环里,若不是第一层图像的特征点,则把特征点乘以缩放倍数,目的在于非第0                 层图像中的特征点的坐标恢复到第0层图像(原图像)的坐标系下。

              在这个循环里,判断特征点的横坐标是否满足这个(keypoint->pt.x >= vLappingArea[0]                   && keypoint->pt.x <= vLappingArea[1])条件,满足的话则把描述子放在特征点向量的                    后面,否则放在前面。

第三步:【7】中的遍历本层特征点的循环结束。【5】中的遍历金字塔层数的循环结束。

2、上述步骤【2】的构建金字塔

ORBextractor::ComputePyramid(cv::Mat image)

中心思想:

遍历设定好的的图层数,对原始图像,按照图层数依次向上补边和缩放。(目的使得特征点具有尺度信息)。

调用参数:image 原始图像

ORBSLAM3代码笔记——特征点提取_第2张图片

代码功能应用如下:

第一步:开始遍历图层level(这里的图层数nlevels是指定好的),for循环。

第二步:在循环内,获取本层图像的缩放系数(SLAM 系统默认设置的是1.2的比例)。

第三步:在循环内,计算本层图像的像素尺寸大小。

第四步:在循环内,对图像进行“补边”,就是在原始的图像围上EDGE_THRESHOLD长度或者宽度。

第五步:在循环内,声明了一个Mat类型的temp变量。

第六步:在循环内,把图像金字塔该图层的图像copy给temp(这里为浅拷贝,内存相同),这里使用的Rect函数的功能(rect对象用来存储一个矩形框的左上角坐标、宽度和高度(从零点开始,开始原始的图像边界加扩充边界))。

第七步:在循环内,计算除原图层的图像尺寸(调用resize,这里图像缩放的插值算法类型使用的是线性插值算法)。

第八步:在循环内,把源图像拷贝到目的图像的中央,四面填充指定的像素。图片如果已经拷贝到中间,只填充边界。

              这里的源图像指的是不同尺寸的图像。

循环结束

3、上述步骤【3】的计算图像的特征点,并且将特征点进行均匀化。

 void ORBextractor::ComputeKeyPointsOctTree(vector >& allKeypoints)

中心思想: 

1、计算当层的坐标边界,定义提取特征点的图像cell块(这里不是同时对整张图像提取特征点,而是按一个cell块内提取特征点,然后坐标恢复到原有的坐标体系下)

2、cell块对当层的整张图像进行遍历。

3、把特征点坐标恢复到当下坐标系上(半径扩充的坐标系),然后在通过四叉树进行均匀分配特征点

4、当前图层的尺度缩放倍数进行缩放得到缩放后的PATCH大小和特征点,然后进行方向计算。

5、遍历特征点,把半径扩充边界的特征点坐标恢复成边缘扩充图像下的坐标,计算特征点的方向。

     函数传入参数是allKeypoints (二维)所有的特征点,这里的特征点储存方式为 (第一维存储的是金字塔的层数,第二维存储的是那一层金字塔图像里提取的所有特征点)。

第一步:修改allKeypoints第一维金字塔的层数,和定义图像cell块(边长为35的正方形)

第二步:遍历所有图像,对每一层的图像做处理

//计算这层图像的坐标边界, 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 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);

{minBorderX,minBorderY, maxBorderX, maxBorderY}如下图:

第三步:在所有图像循环内,开始遍历图像网格,还是以行开始遍历的

            一个正方形大小的cell,从第一行开始遍历图像

for(int i=0; i=maxBorderY-3)
                    //那么就跳过循环体剩下的部分,开始新一轮的循环条件判定
                    continue;
                //如果图像的大小导致不能够正好划分出来整齐的图像网格,那么就要委屈最后一行了
                if(maxY>maxBorderY)
                    maxY = maxBorderY;

第四步:在所有图像遍历循环内,在以行开始遍历循环内,开始以列遍历

            一个正方形大小的cell,从行遍历,遍历一行的所有列,再换到下一行。这里的一行指的是cell的边长长度。

 for(int j=0; j=maxBorderX-6)
                        continue;
                    //如果最大坐标越界那么委屈一下
                    if(maxX>maxBorderX)
                        maxX = maxBorderX;

{iniY,maxY,iniX, maxX}如下图:

 图像引用:ORB-SLAM2 ---- ORBextractor::ComputeKeyPointsOctTree函数_Courage2022的博客-CSDN博客

第五步:在所有图像遍历循环循环内,在以行开始遍历循环内,开始以列遍历循环内,调用opencv的库函数来检测FAST角点。

         这里在设置检测FAST角点的阈值设立两个,第一个初始值,条件比较苛刻,第二个阈值是为了满足设置的特征点数量,降低了门槛。

        当图像cell中检测到FAST角点的时候,遍历其中的所有FAST角点,使得这些角点的坐标(都是基于图像cell),恢复到当前的【坐标边界】(半径扩充边,也就是上图红色的框)下的坐标。然后将其加入到”等待被分配“的特征点容器中(vToDistributeKeys.push_back(*vit))。

第七步:在所有图像遍历循环循环内,前两个行、列遍历循环结束,声明一个对当前图层的特征点的容器的引用,并扩大容器尺寸。

第八步:在所有图像遍历循环循环内,调用DistributeOctTree使用四叉树方法寻找特征点聚集群,然后使用极大值抑制算法,剔除一些特征不明显的特征点。

keypoints = DistributeOctTree(vToDistributeKeys, //当前图层提取出来的特征点,也即是等待剔除的特征点
                                          //NOTICE 注意此时特征点所使用的坐标都是在“半径扩充图像”下的,当前图层图像的边界,而这里的坐标却都是在“边缘扩充图像”下的
                                          minBorderX,maxBorderX,
                                          minBorderY, maxBorderY,
                                          mnFeaturesPerLevel[level],  //希望保留下来的当前层图像的特征点个数
                                          level);  //当前层图像所在的图层

第九步:把根据当前图层的尺度缩放倍数进行缩放得到缩放后的PATCH大小和特征点的方向计算有关。

第十步:在所有图像遍历循环循环内,开始遍历这些特征点,恢复其所在图层坐标系下的坐标,并记录层数和缩放比例。

把半径扩充边界的特征点坐标恢复成边缘扩充图像下的坐标并记录特征点来源的图像金字塔图层。

第十一步:所有图像遍历循环循环结束,计算这些特征点的方向信息(特征点描述子的方向,使得特征点具有旋转不变形),注意这里还是分层计算的(调用computeOrientation函数)。

ORBSLAM3代码笔记——特征点提取_第3张图片

4、上述的【6】中的计算高斯模糊后图像的描述子computeDescriptors(注意这是一个不属于任何类的全局静态函数,static修饰符限定其只能够被本文件中的函数调用)

函数的中心思想:

1、遍历所有的特征点(灰像素),对每个特征点提取描述子

2、给描述子添加它本身相匹配的特征点的方向信息。

3、根据给出的随机点集,两两进行像素比值(灰度值),总共256位,516个点。

static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors,
                                   const vector& pattern)

这里的函数主要是遍历特征点,计算所有特征点的描述子(调用computeOrbDescriptor)

static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors,
                                   const vector& pattern)//全部特征点的描述子
    {
        //清空保存描述子信息的容器(n*32的区域)256bit长度每行
        descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
        //开始遍历特征点
        for (size_t i = 0; i < keypoints.size(); i++)
            //计算这个特征点的描述子(块)
            computeOrbDescriptor(keypoints[i],  //要计算描述子的特征点
                                 image,   //以及其图像
                                 &pattern[0],  //随机点集的首地址
                                 descriptors.ptr((int)i));   //提取出来的描述子的保存位置(uchar* desc = descriptors.ptr(i); )
    }

这里调用的函数只计算一个特征点的描述子,代码如下:

 static void computeOrbDescriptor(const KeyPoint& kpt,
                                     const Mat& img, const Point* pattern,
                                     uchar* desc)//计算特征点的描述子,所包括参数有特征点对象kpt,提取特征点的图像img,预定义好的随机采样点集pattern,保存计算好的描述子
    {
        // 得到特征点的角度,用弧度制表示。kpt.angle是角度制,范围为[0,360)度
        float angle = (float)kpt.angle*factorPI;
        // 然后计算这个角度的余弦值和正弦值
        float a = (float)cos(angle), b = (float)sin(angle);

        // 获得图像中心指针
        const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
        //获得图像的每行的字节数
        const int step = (int)img.step;

        // 原始的BRIEF描述子不具有方向信息,通过加入特征点的方向来计算描述子,称之为Steer BRIEF,具有较好旋转不变特性
        // 具体地,在计算的时候需要将这里选取的随机点点集的x轴方向旋转到特征点的方向。
        // 获得随机“相对点集”中某个idx所对应的点的灰度,这里旋转前坐标为(x,y), 旋转后坐标(x',y')推导:
        // x'= xcos(θ) - ysin(θ),  y'= xsin(θ) + ycos(θ)
#define GET_VALUE(idx) \
        center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
               cvRound(pattern[idx].x*a - pattern[idx].y*b)] //宏定义 宏名(形参表)
        //这里的宏定义指的是在算y'* step + x',即pattern点集下的(x,y)点的坐标。center取其灰度值,这个其实就是算(x,y)点的灰度值。
        // y'* step
        // x'
        // brief描述子由32*8位组成
        // 其中每一位是来自于两个像素点灰度的直接比较,所以每比较出8bit结果,需要16个随机点,这也就是为什么pattern需要+=16的原因
        for (int i = 0; i < 32; ++i, pattern += 16)
        {
            int t0, //参与比较的一个特征点的灰度值
            t1, //参与比较的另一个特征点的灰度值        
            val; //描述子这个字节的比较结果
            
            
            t0 = GET_VALUE(0); t1 = GET_VALUE(1);  //GET_VALUE(idx)的主要作用就是获取坐标点的像素值,
            val = t0 < t1;  //描述子本字节的bit0
            t0 = GET_VALUE(2); t1 = GET_VALUE(3);
            val |= (t0 < t1) << 1;  //描述子本字节的bit1     |= 为A与B按位或运算最终值给A,<<代表将该数位左移
            t0 = GET_VALUE(4); t1 = GET_VALUE(5);
            val |= (t0 < t1) << 2;  //描述子本字节的bit2
            t0 = GET_VALUE(6); t1 = GET_VALUE(7);
            val |= (t0 < t1) << 3;  //描述子本字节的bit3
            t0 = GET_VALUE(8); t1 = GET_VALUE(9);
            val |= (t0 < t1) << 4;  //描述子本字节的bit4
            t0 = GET_VALUE(10); t1 = GET_VALUE(11);
            val |= (t0 < t1) << 5;  //描述子本字节的bit5
            t0 = GET_VALUE(12); t1 = GET_VALUE(13);
            val |= (t0 < t1) << 6;  //描述子本字节的bit6
            t0 = GET_VALUE(14); t1 = GET_VALUE(15);
            val |= (t0 < t1) << 7;  //描述子本字节的bit7

            // 保存当前比较的出来的描述子的这个字节
            desc[i] = (uchar)val;
        }// 通过对随机点像素灰度的比较,得出BRIEF描述子,一共是32*8=256位
        // 为了避免和程序中的其他部分冲突在,在使用完成之后就取消这个宏定义

#undef GET_VALUE
    }

代码实现功能:图像的中心指针是特征点的坐标取整所在图像的地址,为了保证描述子与特征点具有方向的一致性,则对pattern的二维点(x,y)进行旋转,后坐标为(x',y')。

5、上述的3、特征点均匀化的第八步,调用了DistributeOctTree函数。(这里是个难点)

函数主要思想介绍:

我们要把一层金字塔图像分成若干个节点,然后判断每个节点的特征点数目:

                若该节点内特征点数目大于1,则将节点分裂成四个小结点。

                若该节点内特征点数目等于1,则不分裂该节点。

                若该节点内特征点数目等于0,则删除该节点。

        最终得到等于参数N或者大于参数N的节点数量,对特征点进行评分,留下评分好的前N个特整点。

 

原文链接:https://blog.csdn.net/qq_41694024/article/details/126303179

Step 1 、根据宽高比确定初始节点数目(这里的iniY,maxY,iniX, maxX是这minBorderY,maxBorderY, minBorderX, maxBorderX,)

const int nIni = round(static_cast(maxX-minX)/(maxY-minY));//round指的是四舍五入的值

        // 一个初始的节点的x方向有多少个像素
        const float hX = static_cast(maxX-minX)/nIni;
        // 存储有提取器节点的列表
        list lNodes;
        // 存储初始提取器节点指针的vector
        vector vpIniNodes;
        // 然后重新设置其大小
        vpIniNodes.resize(nIni);

Step 2、 生成初始提取器节点

for(int i=0; i(i),0); // UpLeft
            ni.UR = cv::Point2i(hX*static_cast(i+1),0);  // UpRight
            ni.BL = cv::Point2i(ni.UL.x,maxY-minY);  // BottomLeft
            ni.BR = cv::Point2i(ni.UR.x,maxY-minY); // BottomRight
            
            // 重设vkeys大小
            ni.vKeys.reserve(vToDistributeKeys.size());

            // 将刚才生成的提取节点添加到列表中
            // 虽然这里的ni是局部变量,但是由于这里的push_back()是拷贝参数的内容到一个新的对象中然后再添加到列表中
            // 所以当本函数退出之后这里的内存不会成为“野指针”
            lNodes.push_back(ni);
            // 存储这个初始的提取器节点句柄
            vpIniNodes[i] = &lNodes.back();//
        }

一个初始节点分成四个节点,如下图。这里的UL,UR,BL,BR是四个角点坐标。

ORBSLAM3代码笔记——特征点提取_第4张图片

 在程序中,定义了一个平面区域的四个子区域索引号,左上为第一象限0,右上为第二象限1,左下为第三象限2,右下为第四象限3。一个矩形区域的象限划分如上图。

Step 3 、将特征点分配到子提取器节点中 

for(size_t i=0;ivKeys.push_back(kp);
        }

Step 4 、遍历此提取器节点列表,标记那些不可再分裂的节点,删除那些没有分配到特征点的节点

while(lit!=lNodes.end())
        {
            // 如果初始的提取器节点所分配到的特征点个数为1
            if(lit->vKeys.size()==1)
            {
                // 那么就标志位置位,表示此节点不可再分
                lit->bNoMore=true;
                // 更新迭代器
                lit++;
            }
            // 如果一个提取器节点没有被分配到特征点,那么就从列表中直接删除它
            else if(lit->vKeys.empty())
                // 注意,由于是直接删除了它,所以这里的迭代器没有必要更新;否则反而会造成跳过元素的情况
                lit = lNodes.erase(lit);
            else
                // 如果上面的这些情况和当前的特征点提取器节点无关,那么就只是更新迭代器
                lit++;
        }

Step 5 、根据兴趣点分布,利用4叉树方法对图像进行划分区域

上述的四个节点区域,每个节点区域分别根据里面的特征点再分成四份,直到满足停止这个过程的两个条件之一,即可:
            //1、当前的节点数已经超过了要求的特征点数
            //2、当前所有的节点中都只包含一个特征点

注意这里的节点分裂添加和母节点(指节点分裂前的点)删除是下面这个图的步骤:

 Step 6 、当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数

       if((int)lNodes.size()>=N 				//判断是否超过了要求的特征点数
			|| (int)lNodes.size()==prevSize)	//prevSize中保存的是分裂之前的节点个数,如果分裂之前和分裂之后的总节点个数一样,说明当前所有的
												//节点区域中只有一个特征点,已经不能够再细分了
        {
			//停止标志置位
            bFinish = true;
        }
 
        // Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数
        //可以展开的子节点个数nToExpand x3,是因为一分四之后,会删除原来的主节点,所以乘以3
        else if(((int)lNodes.size()+nToExpand*3)>N)
        {
			//如果再分裂一次那么数目就要超了,这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出
			//这里的nToExpand和vSizeAndPointerToNode不是一次循环对一次循环的关系,而是前者是累计计数,后者只保存某一个循环的
			//一直循环,直到结束标志位被置位
            while(!bFinish)
            {
				//获取当前的list中的节点个数
                prevSize = lNodes.size();
 
				//保留那些还可以分裂的节点的信息, 这里是深拷贝
                vector > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
				//清空
                vSizeAndPointerToNode.clear();
 
                // 对需要划分的节点进行排序,对pair对的第一个元素进行排序,默认是从小到大排序
				// 优先分裂特征点多的节点,使得特征点密集的区域保留更少的特征点
                //! 注意这里的排序规则非常重要!会导致每次最后产生的特征点都不一样。建议使用 stable_sort
                sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
 
				//遍历这个存储了pair对的vector,注意是从后往前遍历
                for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
                {
                    ExtractorNode n1,n2,n3,n4;
					//对每个需要进行分裂的节点进行分裂
                    vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);
 
                    // Add childs if they contain points
					//其实这里的节点可以说是二级子节点了,执行和前面一样的操作
                    if(n1.vKeys.size()>0)
                    {
                        lNodes.push_front(n1);
                        if(n1.vKeys.size()>1)
                        {
							//因为这里还有对于vSizeAndPointerToNode的操作,所以前面才会备份vSizeAndPointerToNode中的数据
							//为可能的、后续的又一次for循环做准备
                            vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n2.vKeys.size()>0)
                    {
                        lNodes.push_front(n2);
                        if(n2.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n3.vKeys.size()>0)
                    {
                        lNodes.push_front(n3);
                        if(n3.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n4.vKeys.size()>0)
                    {
                        lNodes.push_front(n4);
                        if(n4.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
 
                    //删除母节点,在这里其实应该是一级子节点
                    lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
 
					//判断是是否超过了需要的特征点数?是的话就退出,不是的话就继续这个分裂过程,直到刚刚达到或者超过要求的特征点个数
					//作者的思想其实就是这样的,再分裂了一次之后判断下一次分裂是否会超过N,如果不是那么就放心大胆地全部进行分裂(因为少了一个判断因此
					//其运算速度会稍微快一些),如果会那么就引导到这里进行最后一次分裂
                    if((int)lNodes.size()>=N)
                        break;
                }//遍历vPrevSizeAndPointerToNode并对其中指定的node进行分裂,直到刚刚达到或者超过要求的特征点个数
 
                //这里理想中应该是一个for循环就能够达成结束条件了,但是作者想的可能是,有些子节点所在的区域会没有特征点,因此很有可能一次for循环之后
				//的数目还是不能够满足要求,所以还是需要判断结束条件并且再来一次
                //判断是否达到了停止条件
                if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
                    bFinish = true;				
            }//一直进行nToExpand累加的节点分裂过程,直到分裂后的nodes数目刚刚达到或者超过要求的特征点数目
        }//当本次分裂后达不到结束条件但是再进行一次完整的分裂之后就可以达到结束条件时
    }// 根据兴趣点分布,利用4叉树方法对图像进行划分区域

Step 7 、保留每个区域响应值最大的一个兴趣点 

// 使用这个vector来存储我们感兴趣的特征点的过滤结果
        vector vResultKeys;
        // 调整大小为要提取的特征点数目
        vResultKeys.reserve(nfeatures);
        
        // 遍历这个节点列表
        for(list::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
        {
            // 得到这个节点区域中的特征点容器句柄
            vector &vNodeKeys = lit->vKeys;
            // 得到指向第一个特征点的指针,后面作为最大响应值对应的关键点
            cv::KeyPoint* pKP = &vNodeKeys[0];
            // 用第1个关键点响应值初始化最大响应值
            float maxResponse = pKP->response;

            // 开始遍历这个节点区域中的特征点容器中的特征点,注意是从1开始哟,0已经用过了
            for(size_t k=1;kmaxResponse)
                {
                    // 更新pKP指向具有最大响应值的keypoints
                    pKP = &vNodeKeys[k];
                    maxResponse = vNodeKeys[k].response;
                }
            }
            // 将这个节点区域中的响应值最大的特征点加入最终结果容器
            vResultKeys.push_back(*pKP);
        }
        
        //返回最终结果容器,其中保存有分裂出来的区域中,我们最感兴趣、响应值最大的特征点
        return vResultKeys;
    }

6、在上述特征点进行均匀化中调用了调用computeOrientation函数

中心思想:

1、为了保证旋转不变性,所以要在圆内计算;

2、为了在圆内计算,故要知道圆的边界,才能去索引;(边界umax在构造函数中已经计算出来了)

3、为了知道圆的边界,在已知纵坐标的情况下,还需知道横坐标umax;

4、最终利用IC_Angle函数进行并行求解方向。

static void computeOrientation(const Mat& image, vector& keypoints, const vector& umax)//特征点向量
    {
        // 遍历所有的特征点
        for (vector::iterator keypoint = keypoints.begin(),
                     keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
        {
            // 调用IC_Angle 函数计算这个特征点的方向
            // image  特征点所在的图层的图像
            //keypoint->pt 特征点在这张图像中的坐标
            //umax  每个特征点所在图像区块的每行的边界 u_max 组成的vector
            keypoint->angle = IC_Angle(image, keypoint->pt, umax);
        }
    }

这个函数主要是调用调用IC_Angle 函数计算这个特征点的方向。

这个函数的中心思想:

1、首先计算圆最中间的那一行像素之合,也就是直径(通过特征点为中心点来确定的)

2、然后根据圆的对称性,分别计算剩下像素之和。(这里坐标下的像素点怎么确定,是利用了opencv存储二维数组(矩阵)的一个机制)

3、通过图像质心公式和角度公式,计算出质心和角度。在转换成弧度制。

    static float IC_Angle(const Mat& image, Point2f pt,  const vector & u_max)//IC_Angle为加速计算灰度值,计算特征点的主方向
    //包含的参数:要进行操作的某层金字塔图像imag,当前特征点的坐标,图像块的每一行的坐标边界
    {
        // 图像的矩,前者是按照图像块的y坐标加权,后者是按照图像块的x坐标加权,
        int m_01 = 0, m_10 = 0;

        // 获得这个特征点所在的图像块的中心点坐标灰度值的指针center
        const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x));//cvRound():返回跟参数最接近的整数值,即四舍五入;uchar的范围是0-255,at像素的读取值

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

        // Go line by line in the circuI853lar 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)//内嵌循环,第一层循环为v=1从中心线往上,第二层循环为u从左往右
        {
            // 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相当于中心水平线向下
                // 在v(y轴)上,2行所有像素灰度值之差
                v_sum += (val_plus - val_minus);//-v所以要加上负号
                // 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);
    }

///乘数因子,一度对应着多少弧度
    const float factorPI = (float)(CV_PI/180.f);

这里面用了图像质心来确定角度,特征点来确定要计算的像素块。

使用圆对称的方法计算像素值

ORB-SLAM2特征点提取IC_Angle函数的理解_changym5的博客-CSDN博客

你可能感兴趣的:(slam,c++)