ORB-SLAM2 关键问题之特征提取

最新推荐文章于 2022-11-27 18:30:52 发布

SLAM||DUNK

最新推荐文章于 2022-11-27 18:30:52 发布

阅读量617

点赞数

分类专栏： vslam

本文链接：https://blog.csdn.net/qq_42700518/article/details/103259332

版权

vslam 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

ORBSLAM2-特征提取

1、流程

1、Tracking对象被构造时，创建了特征提取器对象。
在创建System对象的构造函数内，创建了Tracking对象，而在Tracking对象的构造函数中，创建了特征提取器对象。 Tracking类中声明了几个特征提取器对象：

///作者自己编写和改良的ORB特征点提取器
ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
///在初始化的时候使用的特征点提取器,其提取到的特征点个数会更多
ORBextractor* mpIniORBextractor;

这几个特征提取器对象在Tracking的构造函数中完成初始化：

// tracking过程都会用到mpORBextractorLeft作为特征点提取器
mpORBextractorLeft = new ORBextractor(
    nFeatures,      
    fScaleFactor,
    nLevels,
    fIniThFAST,
    fMinThFAST);

// 如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器
if(sensor==System::STEREO)
    mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

// 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器      可以看到  提取的特征点数量是原来的2倍  
if(sensor==System::MONOCULAR)
    mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

可以看到在单目中，mpIniORBextractor提取器比mpORBextractorLeft提取器多提取一倍的特征点。

2、在创建Frame时，利用特征提取器对象进行特征提取
Frame构造函数中，传入了在构造Tracking时创建的特征提取器对象mpIniORBextractor和mpORBextractorLeft。在Frame构造函数中，又是通过ExtractORB()函数提取特征点的,
ExtractORB()函数中通过调用ORBextractor::operator()来提取特征点和描述子，
对于单目和RGBD相机，提取的特征点放在mvKeys中，描述子放在mDescriptors。

2、特征点

说了这么久的特征点，那么这个特征点到底是什么呢？特征点的定义如下。
ORBSLAM2中提取的特征点都是cv::KeyPoint类型的，这个类型里面有什么呢,查看声明可以看到

CV_PROP_RW Point2f pt; // 提取特征点的坐标
CV_PROP_RW float size; // 大小，或者特征点的范围
CV_PROP_RW float angle; // 特征点的方向
                        //!< it's in [0,360) degrees and measured relative to
                        //!< image coordinate system, ie in clockwise.
CV_PROP_RW float response; // 特征点的响应强度 响应越大特征点越好
CV_PROP_RW int octave; // 提取出来所在金子塔的层数
CV_PROP_RW int class_id; //!< object class (if the keypoints need to be clustered by an object they belong to)

这里面 pt, size, angle, octave 是我们在后面的特征提取过程需要自己设置的，response则是在求特征点时直接计算出来的。

3、构造函数

先从构造函数开始分析：

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

参数： nfeatures：提取的特征点总数，单目初始化时是2000, 其他时候以及双目\RGBD都是1000。
scaleFactor：层与层间的缩放系数，默认是1.2
nlevels：金字塔层数，8
iniThFAST：FAST阈值 10 minThFAST：FAST阈值 5

流程：先明确几个Vector和变量的作用。
mvScaleFactor：存放各层相对与原始图片的缩放倍数的容器。
mvLevelSigma2：为 mvScaleFactor 的平方。
mvInvScaleFactor：缩放倍数的倒数，相当于当前层与原始层的百分比。
mvInvLevelSigma2： mvInvScaleFactor的平方。
mvImagePyramid：储存各层图片的金字塔。
mnFeaturesPerLevel：金字塔中每层图像特征点的数量。
函数实现：

// ORB特征提取的构造函数                                        
ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
         int _iniThFAST, int _minThFAST):
    nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
    iniThFAST(_iniThFAST), minThFAST(_minThFAST)
{
  //*********************** 计算每层缩放倍数的值 mvScaleFactor保存每层图像相对与原始图像的放大倍数 ****************
  // mvLevelSigma2为mvScaleFactor的平方
    mvScaleFactor.resize(nlevels);
    mvLevelSigma2.resize(nlevels);
    mvScaleFactor[0]=1.0f;
    mvLevelSigma2[0]=1.0f;
    for(int i=1; i<nlevels; i++)
    {   // scaleFactor: 1.2    后一层是前一层的1.2倍
        mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;       // 计算第i层的缩放倍数  越高层倍数越大，也就是实际图片越小
        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);      // 容器resize为金字塔的层数   
     /***********************************计算每层的特征点数量  mnFeaturesPerLevel**************************************/
    mnFeaturesPerLevel.resize(nlevels);  // mnFeaturesPerLevel保存每层特征点的数量 容量为金字塔层数 
    float factor = 1.0f / scaleFactor;   // 放大率的倒数
    /* 求第一层图像的特征点的数量   因为特征点应该在金字塔所有层均匀分布，所以每层的特征点数量与图像大小成正比，而知道所有层特征点的总数为nfeatures，所以可以求出每一层的特征点数量
     每层特征点的数量成等比数列 比值为factor  根据等比求和公式 Sn = a1*(1-q^n)/(1-q) => a1 = Sn*(1-q)/(1-q^n)  */
    float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
    
    int sumFeatures = 0;
    // 求取每层的特征点的数量
    for( int level = 0; level < nlevels-1; level++ )   // 0-6
    {
        mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale); // cvRound()求取最接近的整数
        sumFeatures += mnFeaturesPerLevel[level];     // 特征点求和
        nDesiredFeaturesPerScale *= factor;           // 乘以缩放系数
    }
    // 最后一层的特征点数量单独赋值  这样能保证总数为nfeatures
    mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
    /*******************************为计算breif描述子准备采样点的pattern**********************/
    const int npoints = 512;                                // 256对点   
    const Point* pattern0 = (const Point*)bit_pattern_31_;  // 数组名强制转换为指向Point的常量指针 Point = Point_<int>模板类
    // 将512个点复制到pattern容器中  
    std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));    
    
    /************************计算patch圆不同高度v时的横截线的长度  用于计算特征点方向时使用***************************/
    //This is for orientation
    // pre-compute the end of a row in a circular patch
    umax.resize(HALF_PATCH_SIZE + 1);         // HALF_PATCH_SIZE = 15
    
    // vmax当角度为45度时，最大的横截线长度
    int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);  // cvFloor()返回不大于参数的最大整数值   11
    //  int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);              // cvCeil(）返回不小于参数的最小整数值
    int vmin = vmax+1;                                                 // 12
    const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;              // 半径的平方
    // 先计算高度从v到vmax处各处的横截长度   即高度为0-11的长度
    for (v = 0; v <= vmax; ++v)                  // v = 0,1,2,3,4,5....11
        umax[v] = cvRound(sqrt(hp2 - v * v));    // 通过勾股定理计算 这里取最小整数  

    // 计算高度从15-12 横截长度  
    // 根据对称性  
    for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)   
    {
        // 求出umax
        while (umax[v0] == umax[v0 + 1])    
            ++v0;
        umax[v] = v0;
        ++v0;
    }
}

构造函数最后面实现的是用于计算特征点方向的PATCH圆(代码有改动)，这个圆的半径为15，直径31，通过上面的计算构造的PATCH圆的结构如下图

圆第i层的长度length
在这里插入图片描述
应该很明显能看出对称性。

4、operator()

输入图像_image的特征点与描述子的计算就是通过这个函数完成的。

// 计算ORB特征的括号重载   
// InputArray:这个类只能作为函数的形参参数使用，不要试图声明一个InputArray类型的变量, InputArray这个接口类可以是Mat、Mat_<T>、Mat_<T, m, n>、vector<T>、vector<vector<T>>、vector<Mat>
// OutputArray是InputArray的派生类。使用时需要注意的问题和InputArray一样。和InputArray不同的是，需要注意在使用OutputArray：：getMat（）之前一定要调用OutputArray：：create（）为矩阵分配空间。
void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors)

参数： _image：输入图片 _keypoints：储存最终检测出来的特征点的容器 _descriptors ：输出的描述子

这里注意一下InputArray和OutputArray 的使用， InputArray和OutputArray是接口类，且OutputArray是InputArray的派生类，他们能接收Mat、Mat_、Mat_<T, m, n>、vector、vector<vector>、vector类型的参数，对于InputArray类型的变量，可以通过getMat()获取Mat值，不过OutputArray的变量，还需要先用create()创建空间，然后再getMat()获取其Mat值。另外，
InputArray和OutputArray只能用于作为函数的行参，不能用它来定义实际的变量。参考链接。

该函数的具体实现如下：

void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
                      OutputArray _descriptors)
{ 
    if(_image.empty())
        return;
    //  InputArray输出Mat
    Mat image = _image.getMat();
    // 检测像素是否为CV_8UC1类型    即必须要是灰度图
    assert(image.type() == CV_8UC1 );
    // Pre-compute the scale pyramid  构建高斯金字塔   
    ComputePyramid(image);
    vector < vector<KeyPoint> > allKeypoints;    // 建立储存高斯金字塔中全部特征点的vector
    ComputeKeyPointsOctTree(allKeypoints);       // 计算Keypoints  并生成四叉树
    Mat descriptors;                             // 描述子
    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);   // 创建一个行为nkeypoints 列为32的CV_8U矩阵
        descriptors = _descriptors.getMat();          // 将这个矩阵转为Mat
    }

    _keypoints.clear();
    _keypoints.reserve(nkeypoints);       // 创建能保存所有特征点的Vector   reserve()用于创建一个储存区 

    int offset = 0;
    // 遍历高斯金子塔所有层
    // 这个for循环主要完成： 1、求取所有特征点的描述子  2、将所有特征点的坐标转换到原始图片中
    for (int level = 0; level < nlevels; ++level)   
    {
        //××××××××××××××××× 取本层特征点 ×××××××××××××××××××××××        
        vector<KeyPoint>& keypoints = allKeypoints[level];   // 取出第level层的特征点   KeyPoint的Vector
        int nkeypointsLevel = (int)keypoints.size();         // 获取该层特征点数

        if(nkeypointsLevel==0)                               // 本层没有特征点
            continue;
        //*****************计算本层特征点的描述子*****************
        // preprocess the resized image
        Mat workingMat = mvImagePyramid[level].clone();  // 取出该层图片
	// 高斯模糊   高斯核7×7  方差2
        GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);

        // 计算描述子 Compute the descriptors
        Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);    // 取出offset--(offset + nkeypointsLevel-1)行组成Mat    
        computeDescriptors(workingMat, keypoints, desc, pattern);             // 计算描述子

        offset += nkeypointsLevel;                                            // 移动到下一层
        /******************将本层的特征点坐标转换到原始图片中  这里即尺度不变性的核心********************************
	 ******************从这也能看出，一幅图片的特征点，其实是整个不同尺度的图片的特征点的集合**********************/
	
        // Scale keypoint coordinates    根据尺度关系转换特征点的坐标
        if (level != 0)    // 原始层就不用转了
        {
            float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);   获取该层相比与原始层的缩小倍数
            // 遍历本层所有特征点
            for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
                 keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
	        // 坐标乘以缩小倍数 将坐标转换到原始图片
                keypoint->pt *= scale;     // 坐标放大到原始坐标系中
        }
        
        // And add the keypoints to the output   将本层金字塔转换坐标后的特征点插入到_keypoints容器中
        _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());   // insert()这里的用法是  将迭代器[keypoints.begin(),keypoints.end())之间的元素插入到_keypoints.end()的位置
    }
}

上面总结下来可以分成3步：
1、输入图像构建图像金子塔 ComputePyramid(image)
构造的金字塔保存在 mvImagePyramid 容器中。
2、计算特征点，同时通过四叉树进行处理 ComputeKeyPointsOctTree(allKeypoints)
处理的结果放置在 allKeypoints 容器。
遍历图像金字塔的每一层：
3、计算该层特征点的描述子（特征点的描述子是一个256位的二进制数）
计算描述子流程：
1、遍历每一层图片,并获取该层图片的特征点容器.
2 对图片进行高斯滤波, 接着调用computeDescriptors()计算描述子.
4 将该层图片的特征点乘以尺度转换到原始图片上,

可以看到这个函数内部用到了很多其他的内部功能函数，下面一步一步分析。

4.1 computeDescriptors()

// 对图像金子塔的一层图片求描述子
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
                               const vector<Point>& pattern)
{  // 储存描述子的mat   每一个描述子为一行   每一行 32*8 = 256位
    descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
    // 遍历全部特征点  
    for (size_t i = 0; i < keypoints.size(); i++)     
        // 为该特征点计算描述子     结果放在 descriptors的第i行
        computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));
}

提取描述子的 pattern 由 bit_pattern_31_[]数组构造, 一共有256对点,点的坐标是通过学习的方法在一个直径为31的Patch中提取出的.
computeOrbDescriptor()的流程:
1 首先根据特征点的方向对采样的Patch进行旋转,这是实现方向不变性的关键.

 // 获取该特征点的方向      在之前的ComputeKeyPointsOctTree()函数中就计算出来了 
 float angle = (float)kpt.angle*factorPI;
 float a = (float)cos(angle), b = (float)sin(angle);
 // 该特征点中心像素的指针  
 const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
 const int step = (int)img.step;
 // 对pattern中第idx个点的坐标按照特征点方向进行旋转, 获取旋转后的像素值   
 #define GET_VALUE(idx) \
     center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
            cvRound(pattern[idx].x*a - pattern[idx].y*b)]

2 旋转后进行采样, 一共 8*32 = 256对点.

// 8×32位    
for (int i = 0; i < 32; ++i, pattern += 16)
{  
    int t0, t1, val;
    t0 = GET_VALUE(0); t1 = GET_VALUE(1);      
    val = t0 < t1;
    t0 = GET_VALUE(2); t1 = GET_VALUE(3);
    val |= (t0 < t1) << 1;
    t0 = GET_VALUE(4); t1 = GET_VALUE(5);
    val |= (t0 < t1) << 2;
    t0 = GET_VALUE(6); t1 = GET_VALUE(7);
    val |= (t0 < t1) << 3;
    t0 = GET_VALUE(8); t1 = GET_VALUE(9);
    val |= (t0 < t1) << 4;
    t0 = GET_VALUE(10); t1 = GET_VALUE(11);
    val |= (t0 < t1) << 5;
    t0 = GET_VALUE(12); t1 = GET_VALUE(13);
    val |= (t0 < t1) << 6;
    t0 = GET_VALUE(14); t1 = GET_VALUE(15);
    val |= (t0 < t1) << 7;

    desc[i] = (uchar)val;
}

4.1、ComputePyramid() 计算图像金子塔

这个函数感觉原代码写的太冗余了，我简化了一下，不影响实际效果，现在这个实现就精简多了。

void ORBextractor::ComputePyramid(cv::Mat image)
{
  for(int level = 0; level < nlevels; ++level)
  {
     float scale = mvInvScaleFactor[level];         // 获取尺寸的缩放系数    <1        
     Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));   // 该层金字塔图片的尺寸 
     // 如果不是第一层
     if(level != 0)
     {
       resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
     }
     else{   // 第一层
       mvImagePyramid[level] = image;
    }
  }
}

4.2、ComputeKeyPointsOctTree()

1、首先遍历图像金字塔每一层的图片,为图像金子塔中的每层图片提取特征点。

2、计算参与提取特征点的区域的范围 -minBorderX minBorderY maxBorderX maxBorderY

// 靠近边缘的部分不计算特征点  距离边缘EDGE_THRESHOLD以内的点不计算FAST
const int minBorderX = EDGE_THRESHOLD-3;     // EDGE_THRESHOLD设定为19  16
const int minBorderY = minBorderX;       
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;

注意 EDGE_THRESHOLD表示的是特征点出现的范围即特征点中心的范围，-3是因为计算fast特征点的圆的半径为3，如下图
在这里插入图片描述
3、提取特征点时要将图片分割成大小大约为30的cell，然后分别对这些cell提取特征点，这样来确保每个区域都能尽可能提到特征点，这是保证特征点分布均匀的关键。
cell切分的数量以及size计算如下：

// 计算范围
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
// cell的数量     默认 cell的size是30  
const int nCols = width/W;
const int nRows = height/W;
// 计算每个cell的size    
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);

这样设置cell的尺寸的目的是让cell覆盖全部图片提取区域。因为 hCell×nRows>height ，wCell×nCols>width 。

4、按行遍历每一个cell，从左到右，从上到下遍历，然后获取cell提取特征点的区域范围
[(iniY,maxY),(iniX,maxX)]，下面有一点小小的修改，即if(iniY>=maxBorderY-6)，源码里是减3，但是感觉因为FAST特征点的提取圆直径是7，所以最小的提取特征点区域应该是7*7的，所以应该减6才对吧？也不知道理解对不对，写出来请大家指正。

// 按行遍历每一个cell
for(int i=0; i<nRows; i++)
{   // 提取特征点的区域是[(iniY,maxY),(iniX,maxX)]
const float iniY =minBorderY+i*hCell;    // cell左上角的起始坐标
// 加6是为了增大提取面积
float maxY = iniY+hCell+6;               
// 左上角位置太靠近下边界则跳出 这个3是参数 我咋感觉是6？？？？改成6试试   因为FAST特征点提取的圆直径是7     否则就作为提取特征点的区域   因为这样的话  这一块区域就太小了
if(iniY>=maxBorderY-6)      
   continue;            
// 对maxY进行限制   所以最小的提取区域高度至少为7
if(maxY>maxBorderY)
   maxY = maxBorderY;
// 遍历一行的所有cell
for(int j=0; j<nCols; j++)
{
   const float iniX =minBorderX+j*wCell;      // cell左上角X坐标
   float maxX = iniX+wCell+6;                 // 同样加6扩大提取面积
   if(iniX>=maxBorderX-6)                     // 左上距离边界至少7
       continue;
   if(maxX>maxBorderX)
       maxX = maxBorderX;
       ..........
       ..........
 }
}

关于为什么 float maxY = iniY+hCell+6 要+6我的理解是：
在这里插入图片描述
iniY+hCell 也就是cell的区域只能提取 / 部分的特征，而 \ 的部分则变成死区了，+6可以提取出死区部分的特征。

2、对cell区域提取特征点,提取的特征点保存在vKeysCell中，代码这里只为正常情况和提不到特征点时两种情况分配了两个阈值，如果能考虑更多实际情况自动的调整阈值是不是效果会更好呢?

 vector<cv::KeyPoint> vKeysCell;     // 存取每个小cell的FAST特征点的Vector
 // 提取FAST特征点   对mvImagePyramid[level]图像的[(iniY,maxY),(iniX,maxX)]范围提取FAST
 cv::FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
      vKeysCell,iniThFAST,true);     // 采用阈值iniThFAST  true: 开启极大值抑制算法
 // 如果提取的特征点数为0   则阈值换成minThFAST
 if(vKeysCell.empty())
 {
     cv::FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
          vKeysCell,minThFAST,true);    
 }

3、遍历vKeysCell中的全部特征点，将特征点的坐标转换到原图像中，这里图像依然是去掉边缘大小
EDGE_THRESHOLD-3后的图像。

// 提取到了特征点了  将这个cell的特征点坐标转换后全部存放到vToDistributeKeys之中
if(!vKeysCell.empty())
{   // 遍历全部提取提取出来的特征点  将特征点的坐标从cell转换到全部cell的坐标再保存到vToDistributeKeys
    for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
    {
        (*vit).pt.x+=j*wCell;                             // 原始坐标加上cell的偏移
        (*vit).pt.y+=i*hCell;
        vToDistributeKeys.push_back(*vit);
    }
}

4、当前层的特征点提取完后，通过DistributeOctTree()函数通过四叉树将提取的特征点进行处理，

// 创建第level层allKeypoints的引用   keypoints
vector<KeyPoint> & keypoints = allKeypoints[level]; 
keypoints.reserve(nfeatures);
// 利用四叉树处理特征点  使特征点的数量满足条件 且分布均匀
keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
                              minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);

5、为keypoints中每个特征点的pt,size,octave信息赋值：

// 特征点size的计算
const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
// Add border to coordinates and scale information
const int nkps = keypoints.size();       
for(int i=0; i<nkps ; i++)
{
    keypoints[i].pt.x+=minBorderX;      // 转换到图像x坐标
    keypoints[i].pt.y+=minBorderY;      // 转换到图像y坐标
    keypoints[i].octave=level;          // octave储存图像金字塔的层数          
    keypoints[i].size = scaledPatchSize;// 特征点的size
}

size的作用是什么？？？？？？？？？？？？？？？？？？？？？？？？？

6、遍历每层的特征点，通过灰度质心法计算每个特征点的方向信息：

// compute orientations     计算每层图像所有特征点的方向   为计算描述子提供信息
for (int level = 0; level < nlevels; ++level)
    computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);

下面看其中的关键的函数——四叉树特征点处理DistributeOctTree()。

4.2.1、DistributeOctTree()

/**
 * @brief vToDistributeKeys: 从金字塔某一层中提取出来的特征点
 * @param minX，maxX，minY，maxY :特征点区域    minBorderX, maxBorderX,minBorderY, maxBorderY
 * @param N 该层图像上应该提取的特征点个数
 * @param level 图像的层数
 */ 
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)

算法流程：
1、找出根节点。
2、循环执行对当前所有节点(即lNodes中的节点)进行分割，这里的过程比较有意思，比如说根节点只有一个的时候，然后接下来分割出来4个节点，它们存放在lNodes中的先后顺序是右下节点，左下节点，右上节点，左上节点，然后循环执行第二次分割，这时首先是右下节点进行分割，右下节点分割完后，由于分割后的节点只是被插入到lNodes最前面，所以下一次的分割的节点仍然是之前分割节点的下一节点，直到之前lNodes中所有节点分割完了，才会重新返回到lNodes最前面的节点进行分割，这样能使分割更大程度的均匀，而不会出现只对某一个节点不停地分割的情况。
3、一次分割循环结束后，检查分割是否完毕，分割完毕的判断标准是：（1)、节点数是否已经达到需要提取的特征点数。 (2)、节点的数目已经稳定。
另外，如果当前节点继续分割一次后节点的总数可能大于所要提取的特征数时，则将要分割的节点按内部特征点的数量从大到小排序，先分割特征点数量多的节点，因为特征点多的节点更有可能分割出4个子节点出来，这样能使分割尽快达到目标。
4、分割完毕后，遍历每个节点，将响应最大的特征点找出来并保存在vResultKeys中。

详细分析：
1、计算需要多少根节点，以及创建根节点并存放到 lNodes容器，节点地址保存在 vpIniNodes 容器。
根节点放置如下
在这里插入图片描述
代码部分

// Compute how many initial nodes   根据图像的长宽选择初始的根节点数     宽 / 高   ( 一般宽>高 )
// 由于要求根节点的范围尽量为正方形，所以若区域长与宽的比值接近1那么只需要一个根节点 而长于宽的比值越大则需要的根节点就越多
const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));
// 一个根节点所占的宽度
const float hX = static_cast<float>(maxX-minX)/nIni;

list<ExtractorNode> lNodes;                             // 四叉树节点   保存四叉树所有的节点
vector<ExtractorNode*> vpIniNodes;                      // 保存四叉树的节点的地址
vpIniNodes.resize(nIni);                                // 首先申请大小为根节点的大小
// 构造根节点
for(int i=0; i<nIni; i++)
{
    ExtractorNode ni;
// 根节点的范围    这里
    ni.UL = cv::Point2i(hX*static_cast<float>(i),0);    // 左上
    ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);  // 右上
    ni.BL = cv::Point2i(ni.UL.x,maxY-minY);             // 左下
    ni.BR = cv::Point2i(ni.UR.x,maxY-minY);             // 右下
    ni.vKeys.reserve(vToDistributeKeys.size());         // 分配存储特征点的空间
    
    lNodes.push_back(ni);                               // 将根节点放入lNodes        

    vpIniNodes[i] = &lNodes.back();                     // 地址保存至vpIniNodes  
}

2、遍历所有特征点，将特征点分配给位置靠近的根节点中(放置到根节点vKeys容器中)

// 遍历所有特征点   将特征点分配到根节点中     
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
   const cv::KeyPoint &kp = vToDistributeKeys[i];      // 第i个特征点的坐标  
   vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);        // 基于该特征点坐标将其分配到根节点中
}

3、遍历根节点，如果其子节点为空则从lNodes中删除…

// 获得节点的迭代器    此时保存的只有根节点
list<ExtractorNode>::iterator lit = lNodes.begin();
// 遍历每个根节点   将不需要分割的标记或者无效根节删除
while(lit!=lNodes.end())
{   // 如果该节点范围内  特征点的数量为1   则bNoMore=true表示不继续分割
   if(lit->vKeys.size()==1)
   {
       lit->bNoMore=true;
       lit++;
   }                               // 如果节点为空  则直接删除
   else if(lit->vKeys.empty())
       lit = lNodes.erase(lit);    // list  删除了节点lit后会返回下一个lit      
   else
       lit++;
}

4、循环一直到分割完成，循环内部对lNodes进行遍历，对于每个节点，若特征点数>1，则进行分裂 lit->DivideNode(n1,n2,n3,n4)，将分裂后子节点特征点数>0的插入到lNodes的首部

while(!bFinish)                      
{
  // iteration++;
  // 节点数量   
  int prevSize = lNodes.size();
  // 最开始节点的迭代器    后面分割的节点都插入到了开始的位置
  lit = lNodes.begin();
  // 分裂出来的节点中   可以再次分裂的数量   
  int nToExpand = 0;
   
  vSizeAndPointerToNode.clear();
  // 遍历其全部节点  将需要分割的进行分割   没有特征点数为0的节点
  while(lit!=lNodes.end())
  {   // 如果节点  只包含一个特征点了  那么跳到下一个
      if(lit->bNoMore)
      {
          // If node only contains one point do not subdivide and continue
          lit++;
          continue;
      }
      else                              // 可以分割   
      {   
          // If more than one point, subdivide   还有超过一个节点  则继续分割
          ExtractorNode n1,n2,n3,n4;    // 分成4个子节点
          lit->DivideNode(n1,n2,n3,n4); // 对lit节点进行分割
  
          // Add childs if they contain points
// 如果n1节点有分布特征点  
          if(n1.vKeys.size()>0)     
          {
              lNodes.push_front(n1);      // 节点存储到lNodes中  插入到最前面             
              if(n1.vKeys.size()>1)       // 可以继续分割
              { 
    // 下面几步是是用于最后如果分割的节点可能超过特征点数  则执行单独的分割步骤
                  nToExpand++;            // 可继续分割的数量++  
// 储存节点的地址与节点的特征数                                                   
                  vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
// 记录该节点的迭代器  用于后面删除根节点
                  lNodes.front().lit = lNodes.begin();
              }
          }
          // 如果n2节点有分布的特征点
          if(n2.vKeys.size()>0)
          {
              lNodes.push_front(n2);              
              if(n2.vKeys.size()>1)
              {
                  nToExpand++;     // 数量加一
                  vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
                  lNodes.front().lit = lNodes.begin();     // 记录该节点的迭代器  用于后面删除根节点
              }
          }   
          // 如果n3节点有分布的特征点
          if(n3.vKeys.size()>0)
          {
              lNodes.push_front(n3);
              if(n3.vKeys.size()>1)
              {
                  nToExpand++;
                  vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
                  lNodes.front().lit = lNodes.begin();     // 记录该节点的迭代器  用于后面删除根节点
              }
          }
          // 如果n4节点有分布的特征点
          if(n4.vKeys.size()>0)
          {
              lNodes.push_front(n4);
              if(n4.vKeys.size()>1)
              {
                  nToExpand++;
                  vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
                  lNodes.front().lit = lNodes.begin();     // 记录该节点的迭代器  用于后面删除根节点
              }
          }

          lit=lNodes.erase(lit);    // 分割完了将该节点删除
          continue;
      }
  }       
  
  // 当前所有的节点都分割完毕后   判断分割是否可以结束
  // Finish if there are more nodes than required features
  // or all nodes contain just one point
 // if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)  // 如果节点的数量超过特征点数量 或与上一次没有变化    
  if((int)lNodes.size()>=N || nToExpand == 0)
  {
      bFinish = true;    
  }   // 如果下一次分割后的节点数量大于N那么进行如下处理， 由于一个节点分割为四个节点是需要删除原节点的，所以实际上增加了3个节点   
  else if(((int)lNodes.size()+nToExpand*3)>N)
  {   //  一直循环执行直到节点的数量大于特征点数量于是分割结束
      while(!bFinish)
      {               
          prevSize = lNodes.size();
          // 上一次分割后  待分割的节点的容器 
          vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
// 清空  接下来会保存这一轮分割后仍然需要分割的节点
          vSizeAndPointerToNode.clear();
          // 特征点数量从小到大排序   
          sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
// 按特征点的数量从大到小访问节点   先分割特征点数量多的  因为特征点多的节点更有可能分割出4个子节点出来，这样能使分割尽快达到目标。    
         // for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
for(int j=0;j<vPrevSizeAndPointerToNode.size();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);
// 数量大于1需要继续分割
                  if(n1.vKeys.size()>1)
                  {   // 放置到容器vSizeAndPointerToNode   准备下一次的分割 
                      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();   // 记录该节点的迭代器  用于后面删除根节点
                  }
              }
              // 原节点分割后被删除   erase()函数中的参数需要是迭代器
              lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
              // 如果这时节点数量大于N  则退出
              if((int)lNodes.size()>=N)
                  break;
          }
          // 节点数量大于N   或分割停止就结束    
          if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
              bFinish = true;
      }
  }
}

5、分裂完毕后，每个区域选取一个响应最大的特征点，保存在 vResultKeys 。

// Retain the best point in each node
vector<cv::KeyPoint> vResultKeys;
vResultKeys.reserve(nfeatures);
// 遍历全部节点    为每个节点找到响应最大的那个特征点
for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
{
    vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;     // 获得节点中的特征点  
    // 将第一个特征点的响应值最为max
    cv::KeyPoint* pKP = &vNodeKeys[0];                
    float maxResponse = pKP->response;                // 获取响应值   
    // 然后遍历其他的特征点 找出响应最大的特征点作为该节点的代表特征点  
    for(size_t k=1;k<vNodeKeys.size();k++)
    {
        if(vNodeKeys[k].response>maxResponse)
        {
            pKP = &vNodeKeys[k];
            maxResponse = vNodeKeys[k].response;
        }
    }
    // 每个节点响应最大的保存在这里
    vResultKeys.push_back(*pKP);
}

4.2.1.1、DivideNode()

介绍：一个节点中有多个特征点 ,那么将该节点分成4个子节点。
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
将一个节点分割成4个的主要思路是：
0、求出该cell的半径。
1、求出各个子节点n1,n2,n3,n4的区域范围，UL，UR，BL，BR。
2、将原节点中所有的特征点分配到子节点中 vKeys。
3、将不需要分割的节点设置bNoMore = true。

// 一个节点中有多个特征点   那么将该节点分成4个子节点  
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
{
    // 求该节点的区域中心位置
    const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
    const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);

    //Define boundaries of childs   
    // 设定第一个子节点的区域    左上角的节点
    n1.UL = UL;
    n1.UR = cv::Point2i(UL.x+halfX,UL.y);
    n1.BL = cv::Point2i(UL.x,UL.y+halfY);
    n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);
    n1.vKeys.reserve(vKeys.size());
    // 第二个节点   
    n2.UL = n1.UR;
    n2.UR = UR;
    n2.BL = n1.BR;
    n2.BR = cv::Point2i(UR.x,UL.y+halfY);
    n2.vKeys.reserve(vKeys.size());
    // 第三个节点
    n3.UL = n1.BL;
    n3.UR = n1.BR;
    n3.BL = BL;
    n3.BR = cv::Point2i(n1.BR.x,BL.y);
    n3.vKeys.reserve(vKeys.size());
    // 第4个节点
    n4.UL = n3.UR;
    n4.UR = n2.BR;
    n4.BL = n3.BR;
    n4.BR = BR;
    n4.vKeys.reserve(vKeys.size());

    //Associate points to childs   根据特征点的坐标将特征点分配到节点中
    for(size_t i=0;i<vKeys.size();i++)
    {
        const cv::KeyPoint &kp = vKeys[i];   // 获取特征点
        if(kp.pt.x<n1.UR.x)
        {
            if(kp.pt.y<n1.BR.y)
                n1.vKeys.push_back(kp);   // 左上
            else
                n3.vKeys.push_back(kp);   // 左下
        }
        else if(kp.pt.y<n1.BR.y)          
            n2.vKeys.push_back(kp);        // 右上
        else
            n4.vKeys.push_back(kp);         // 右下
    }
    // 看有没有不需要分割的节点
    if(n1.vKeys.size()==1)
        n1.bNoMore = true;
    if(n2.vKeys.size()==1)
        n2.bNoMore = true;
    if(n3.vKeys.size()==1)
        n3.bNoMore = true;
    if(n4.vKeys.size()==1)
        n4.bNoMore = true;

}

SLAM||DUNK

关注

0
点赞
踩
7

收藏

觉得还不错? 一键收藏
1
评论
ORB-SLAM2 关键问题之特征提取

ORBSLAM2-特征提取欢迎使用Markdown编辑器新的改变功能快捷键合理的创建标题，有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants创建一个自定义列表如何创建一个注脚注释也是必不可少的KaTeX数学公式新的甘特图功能，丰富你的文章UML 图表FLowchart流程图导出与导入导出导入欢迎使...
复制链接

扫一扫