【整理】ORB-SLAM2中特征点的处理

以下内容均来自对计算机视觉life公众号中资料的整理,作为自己的笔记使用

ORB-SLAM2中特征点的处理

1 ORB特征点简介

ORB特征由关键点描述子组成

FAST关键点:
1.选取像素 p p p,假设其亮度为 I p I_p Ip;
2.设置一个阈值 T T T(比如 I p I_p Ip的20%);
3.以像素 p p p为中心,选取半径为3的圆上的16个像素点;
4.假如选取的圆上,有连续的N个点的亮度大于 I p + T I_p+T Ip+T 小 于 I p − T 小于I_p-T IpT,那么像素p可以被认为是特征点;
5.循环以上4步,对每一个像素执行相同操作。

Brief描述子:

BRIEF是一种二进制描述子,描述量由许多个0和1组成.这些0和1编码了关键点附近两个随机像素(如p和q)的大小关系,如果p比q大则取1,反之取0.

BRIEF算法的核心思想是在关键点P的周围以一定模式选取N个点对,把这N个点对的比较结果组合起来作为描述子。为了保持踩点固定,工程上采用特殊设计的固定的pattern来做.

在这里插入图片描述

2.灰度质心法

原始的FAST关键点没有方向信息,如果图像发生旋转,brief描述子也会发生变化,使得特征点对旋转不鲁棒,所以通过灰度质心法计算出特征点的方向.

灰度质心:可以理解为,图像区域内,以一像素灰度值作为权重的中心,形心指向质心的向量即为关键点的主方向.
或者说的不规范一点,灰度质心可以理解为:这一区域灰度值分布的中心点

当图像发生旋转的时候,质心一定也会跟着转,这样我们构建的坐标系也随着质心一起转,从而保证了旋转不变性.

在这里插入图片描述

计算灰度质心的步骤:

  • 1.我们定义区域图像的矩为:

m p q = ∑ x , y x p y q I ( x , y ) , p , q = { 0 , 1 } m_{pq}=\sum_{x,y}x^py^qI(x,y),\quad \quad \quad p,q=\{0,1\} mpq=x,yxpyqI(x,y),p,q={0,1}

其中,p,q取值为0或1, I ( x , y ) I(x,y) I(x,y)表示像素在坐标 ( x , y ) (x,y) (x,y)处图像的灰度值; m p q m_{pq} mpq表示图像的矩.
在半径为R的图形图像区域,沿两个坐标轴x,y方向的图像的矩为:
m 10 = ∑ x = − R R ∑ y = − R R x I ( x , y ) m 01 = ∑ x = − R R ∑ y = − R R y I ( x , y ) m_{10}=\sum_{x=-R}^R\sum_{y=-R}^RxI(x,y)\\ m_{01}=\sum_{x=-R}^R\sum_{y=-R}^RyI(x,y) m10=x=RRy=RRxI(x,y)m01=x=RRy=RRyI(x,y)
上两个式子也可以理解为区域内图像的灰度值在分别 x x x方向和 y y y方向的加和.然后下式 m 00 m_{00} m00是圆内所有像素的灰度值的加和:
m 00 = ∑ x = − R R ∑ y = − R R I ( x , y ) m_{00}=\sum_{x=-R}^R\sum_{y=-R}^RI(x,y) m00=x=RRy=RRI(x,y)

  • 2.然后分别将x,y方向的加与总和做比值,得到图像的质心:

C = ( c x , c y ) = ( m 10 m 00 , m 01 m 00 ) C=(c_x,c_y)=\left( \frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}} \right) C=(cx,cy)=(m00m10,m00m01)

  • 3.然后关键点的"主方向"可以表示为从圆形图像形心O指向质心C的方向向量 O C → \overrightarrow{OC} OC ,于是关键点的旋转角度记为:
    θ = a r c t a n 2 ( c y , c x ) = a r c t a n 2 ( m 01 , m 10 ) \theta=arctan2(c_y,c_x)=arctan2(m_{01},m_{10}) θ=arctan2(cy,cx)=arctan2(m01,m10)

在ORB-SLAM2中的实现为,先计算四分之一圆的下半部分的横坐标(u轴)边界,然后根据圆对称性计算出圆的上半部分的边界

在这里插入图片描述

具体代码如下:

首先确定圆的边界:

ORBextractor::ORBextractor( ...  ):	nfeatures(_nfeatures), ... ,  minThFAST(_minThFAST)
{
	...
    ...
        
	//预先计算圆形patch中行的结束位置,+1中的1表示那个圆的中间行,HALF_PATCH_SIZE 即半径,取值为15
    umax.resize(HALF_PATCH_SIZE + 1); 
    int v, v0,		//循环辅助变量,辅助变量
		vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);	//计算圆的最大行号,+1应该是把中间行也给考虑进去了
    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;
    }
}

然后计算方向:

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

IC_Angle()函数如下:

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

    //v=0中心线的计算需要特殊对待,后面是以中心行为对称轴,成对遍历行数,所以PATCH_SIZE必须是奇数
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
		//注意这里的center下标u可以是负的!中心水平线上的像素按x坐标(也就是u坐标)加权
        m_10 += u * center[u];

	//这里的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)
    {
		//本来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);
}

3.特征点的分配(每层分配策略)

ORB-SLAM2中特征点分配的策略为:(比如一张图像一共要提取1000个特征点)
根据输入图像构建图像金字塔 -> 计算出整个图像金字塔的总面积S -> 计算出各层金字塔图像面积占总面积的百分比 -> 根据各层占比分配特征点提取数量

具体计算方法如下:

假设需要提取的特征点数目为 N N N,金字塔共 m m m层,第0层图像的宽为 W W W,高为 H H H,对应的面积为 C = H ⋅ W C=H·W C=HW,图像金字塔的缩放因子为 s , 0 < S < 1 s,0<S<1 s,0<S<1,在ORB-SLAM2中, m = 8 , s = 1 1.2 m=8,s=\frac{1}{1.2} m=8,s=1.21.

那么整个图像金字塔的总面积是:
S = H ⋅ W ⋅ ( s 2 ) 0 + H ⋅ W ⋅ ( s 2 ) 1 + ⋅ ⋅ ⋅ + H ⋅ W ⋅ ( s 2 ) m − 1 = H W 1 − ( s 2 ) m 1 − s 2 = C 1 − ( s 2 ) m 1 − s 2 \begin{aligned} S&=H·W·(s^2)^0+H·W·(s^2)^1+···+H·W·(s^2)^{m-1}\\ &=HW\frac{1-(s^2)^m}{1-s^2}\\ &=C\frac{1-(s^2)^m}{1-s^2}\\ \end{aligned} S=HW(s2)0+HW(s2)1++HW(s2)m1=HW1s21(s2)m=C1s21(s2)m
单位面积应该分配的特征点数量为:
N a v g = N S = N C 1 − ( s 2 ) m 1 − s 2 = N ( 1 − s 2 ) C ( 1 − ( s 2 ) m ) \begin{aligned} N_{avg}&=\frac{N}{S}\\ &=\frac{N}{C\frac{1-(s^2)^m}{1-s^2}}\\ &=\frac{N(1-s^2)}{C(1-(s^2)^m)} \end{aligned} Navg=SN=C1s21(s2)mN=C(1(s2)m)N(1s2)
第0层应该分配的特征点数量为:
N 0 = N a v g ⋅ C = N ( 1 − s 2 ) 1 − ( s 2 ) m \begin{aligned} N_0&=N_{avg}·C\\ &=\frac{N(1-s^2)}{1-{(s^2)^m}} \end{aligned} N0=NavgC=1(s2)mN(1s2)
第i层应该分配的特征点数量为:
N i = N ( 1 − s 2 ) C ( 1 − ( s 2 ) m ) C ( s 2 ) i = N ( 1 − s 2 ) 1 − ( s 2 ) m ( s 2 ) i \begin{aligned} N_i&=\frac{N(1-s^2)}{C(1-(s^2)^m)}C(s^2)i\\ &=\frac{N(1-s^2)}{1-(s^2)^m}(s^2)^i \end{aligned} Ni=C(1(s2)m)N(1s2)C(s2)i=1(s2)mN(1s2)(s2)i
在ORB-SLAM2 的代码里,不是按照面积均摊的,而是按照面积的开方来均摊特征点的,也就是将上述公式中的 s 2 s^2 s2换成 s s s即可。

ORBextractor::ORBextractor( ... ) :		...
{
	//存储每层图像缩放系数的vector调整为符合图层数目的大小
    mvScaleFactor.resize(nlevels);  
	//存储这个+子的平方
    mvLevelSigma2.resize(nlevels);
	//对于初始图像,这两个参数都是1
    mvScaleFactor[0]=1.0f;
    mvLevelSigma2[0]=1.0f;
	//然后逐层计算图像金字塔中图像相当于初始图像的缩放系数 
    for(int i=1; i<nlevels; i++)  
    {
		//其实就是这样累乘计算得出来的
        mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;
		//原来这里的sigma^2就是每层图像相对于初始图像缩放因子的平方
        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];
    }

    //调整图像金字塔vector以使得其符合设定的图像层数
    mvImagePyramid.resize(nlevels);

	//每层需要提取出来的特征点个数,这个向量也要根据图像金字塔设定的层数进行调整
    mnFeaturesPerLevel.resize(nlevels);
	
	//图片降采样缩放系数的倒数
    float factor = 1.0f / scaleFactor;
	//第0层图像应该分配的特征点数量
    float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
    
    	//用于在特征点个数分配的,特征点的累计计数清空
    int sumFeatures = 0;
	//开始逐层计算要分配的特征点个数,顶层图像除外(看循环后面)
    for( int level = 0; level < nlevels-1; level++ )
    {
		//分配 cvRound : 返回个参数最接近的整数值
        mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
		//累计
        sumFeatures += mnFeaturesPerLevel[level];
		//乘系数
        nDesiredFeaturesPerScale *= factor;
    }
    //由于前面的特征点个数取整操作,可能会导致剩余一些特征点个数没有被分配,所以这里就将这个余出来的特征点分配到最高的图层中
    mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
}

4.特征提取与分配

特征点提取:

ORB-SLAM2将图像金字塔每层图像分割成同样大小的网格(30×30),如下所示:

然后分别在网格中提取特征点,如果初始的FAST阈值没有检测到角点,就降低FAST阈值,保证其在弱纹理区域也能提取到更多的角点;如果这样还提取不到角点,就不再这个格子里提取,避免提取到质量特别差的角点.

这样提取出来的特征点,其坐标是基于每个网格的,然后再将这些特征点坐标分别转换到当前图层坐标边界下的坐标.

void ORBextractor::ComputeKeyPointsOctTree(
	vector<vector<KeyPoint> >& allKeypoints)	//所有的特征点,这里第一层vector存储的是某图层里面的所有特征点,第二层存储的是整个图像金字塔中的所有图层里面的所有特征点												
{
	//重新调整图像层数
    allKeypoints.resize(nlevels);
	//图像cell的尺寸,是个正方形,可以理解为边长in像素坐标
    const float W = 30;

    // 对每一层图像做处理
    for (int level = 0; level < nlevels; ++level)
    {
		//计算这层图像的坐标边界
        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;
			
            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;

                // 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++)
                    {
						//这些角点的坐标都是基于图像cell的,现在我们要先将其恢复到当前的【坐标边界】下的坐标
                        (*vit).pt.x+=j*wCell;
                        (*vit).pt.y+=i*hCell;
						//然后将其加入到”等待被分配“的特征点容器中
                        vToDistributeKeys.push_back(*vit);
                    }//遍历图像cell中的所有的提取出来的FAST角点,并且恢复其在整个金字塔当前层图像下的坐标
                }
            }
        }

        //声明一个对当前图层的特征点的容器的引用,并且调整其大小为欲提取出来的特征点个数
        vector<KeyPoint> & keypoints = allKeypoints[level];
        keypoints.reserve(nfeatures);

        // 对特征点进行剔除,返回值是一个保存有特征点的vector容器,得到的特征点的坐标,依旧是在当前图层下来讲的
        keypoints = DistributeOctTree(vToDistributeKeys, 			//当前图层提取出来的等待剔除的特征点,注意此时特征点所使用的坐标都是在“半径扩充图像”下的
									  minBorderX, maxBorderX,		//当前图层图像的边界
                                      minBorderY, maxBorderY,
									  mnFeaturesPerLevel[level], 	//希望保留下来的当前层图像的特征点个数
									  level);						//当前层图像所在的图层

		//PATCH_SIZE是用于进行描述子计算时定义的圆的直径,实在原始图像下(level 0)的尺寸,在其他层金字塔中需要进行缩放
        const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];

		//获取剔除过程后保留下来的特征点数目
        const int nkps = keypoints.size();
		//然后开始遍历这些特征点,恢复其在当前图层图像坐标系下的坐标
        for(int i=0; i<nkps ; i++)
        {
			//对每一个保留下来的特征点,恢复到相对于当前图层“边缘扩充图像下”的坐标系的坐标
            keypoints[i].pt.x+=minBorderX;
            keypoints[i].pt.y+=minBorderY;
			//记录特征点来源的图像金字塔图层
            keypoints[i].octave=level;
			//记录计算方向的patch,缩放后对应的大小, 又被称作为特征点半径
            keypoints[i].size = scaledPatchSize;
        }
    }
    //然后计算这些特征点的方向信息,注意这里还是分层计算的
    for (int level = 0; level < nlevels; ++level)
        computeOrientation(mvImagePyramid[level],	//对应的图层的图像
						   allKeypoints[level], 	//这个图层中提取并保留下来的特征点容器
						   umax);					//以及PATCH的横坐标边界
}

四叉树分配特征点

ORB-SLAM2采用四叉树分配特征点的策略,这么做是为了保证特征点可以均匀地分布在一整张图片上,防止特征点过于集中.

如果特征点过于集中,会导致一部分特征点多余,本来一个特征点可以表达清楚的区域,结果这里有10个特征点,那么其他9个特征点都是冗余的.除此之外,如果特征点太集中,不利于相机位姿的结算.特征点在空间中分布的层次越多,越均匀,那么特征点就能越精确地表达出空间的几何关系.说的极端一点,假如一张图片的特征点都集中在一点,我们是没有办法解算相机的位姿的.所以,均匀分布特征点可以提高SLAM的精度.

四叉树分配的基本原理就是,以原始图像为根节点,然后对图像进行分块,每次分成四块,也就是四个子节点,直到节点数大概等于我们需要的特征点的数目为止,然后分别在每一个节点内只挑选一个质量最好的特征点保留.

四叉树具体分配步骤如下:

  • 第一步:确定初始的节点数目.节点数目根据原始图像的宽高比确定,一般为1或2.假设我们的目标是选取25个均匀分布的特征点,那么我们最后就要分裂出25个结点
  • 第二步:节点第一次分裂,1个结点分裂为4个节点,数量小于25,继续分裂
  • 第三步:4个node分裂为15个node.(有一个node里没有节点,所以不是16个),节点数依然小于25,继续分裂
  • 第四步:15个node分裂为30个,结束分裂
  • 第五步:从每个node中,选取质量最好的一个点

参考:https://zhuanlan.zhihu.com/p/61738607
在分裂过程中,如果某个节点特征点数目为0,则删掉该节点;如果某个节点内特征点数目为1,则该节点停止分裂.需要注意的是,一个母节点分裂为 4 个子节点后,需要在节点链表里删掉原来的母节点,所以实际上一次分裂净增加了 3 个节点。所以下次分裂后节点的总数我们是可以提前预估的,计算方式为:(当前节点总数 + 即将分裂的节点总数 × 3)

从上面第五步可以看到,最终分裂出了30个节点,超出了我们需要的节点数(如果数字不凑巧,可能会超出非常多的节点,因为节点数是以指数级增长的).这就需要注意了,实际上我们不需要把所有的节点都进行分裂,只需要在分裂得到的节点数目刚好达到25时停止分裂.ORB-SLAM2的做法是对所有节点内包含的特征点数目进行排序,优先分裂特征点数目多的节点.

最后,当我们得到需要的节点之后,只需要从每个节点中选取角点响应值最高的特征点,作为该节点的唯一特征点,该节点内其他低响应值的特征点全部删掉,这样就得到了均匀化后的,需要数目的特征点.

源码如下:

/**
 * @brief 使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发
 * 
 * @param[in] vToDistributeKeys     等待进行分配到四叉树中的特征点
 * @param[in] minX                  当前图层的图像的边界
 * @param[in] maxX 
 * @param[in] minY 
 * @param[in] maxY 
 * @param[in] N                     希望提取出的特征点个数
 * @param[in] level                 指定的金字塔图层
 * @return vector<cv::KeyPoint>     已经均匀分散好的特征点容器
 */
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)
{
    // Step 1 根据宽高比确定初始节点数目
 //计算应该生成的初始节点个数,根节点的数量nIni是根据边界的宽高比值确定的,一般是1或者2
    // ! bug: 如果宽高比小于0.5,nIni=0, 后面hx会报错
    const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));

 //一个初始的节点的x方向有多少个像素
    const float hX = static_cast<float>(maxX-minX)/nIni;

 //存储有提取器节点的链表
    list<ExtractorNode> lNodes;

 //存储初始提取器节点指针的vector
    vector<ExtractorNode*> vpIniNodes;

 //重新设置其大小
    vpIniNodes.resize(nIni);

 // Step 2 生成初始提取器节点
    for(int i=0; i<nIni; i++)
    {      
  //生成一个提取器节点
        ExtractorNode ni;

  //设置提取器节点的图像边界
        ni.UL = cv::Point2i(hX*static_cast<float>(i),0);    //UpLeft
        ni.UR = cv::Point2i(hX*static_cast<float>(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());

  //将刚才生成的提取节点添加到链表中
        lNodes.push_back(ni);
  //存储这个初始的提取器节点句柄
        vpIniNodes[i] = &lNodes.back();
    }

    // Step 3 将特征点分配到子提取器节点中
    for(size_t i=0;i<vToDistributeKeys.size();i++)
    {
  //获取这个特征点对象
        const cv::KeyPoint &kp = vToDistributeKeys[i];
  //按特征点的横轴位置,分配给属于那个图像区域的提取器节点(最初的提取器节点)
        vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
    }
    
 // Step 4 遍历此提取器节点列表,标记那些不可再分裂的节点,删除那些没有分配到特征点的节点
    list<ExtractorNode>::iterator lit = lNodes.begin();
    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++;
    }

    //结束标志位清空
    bool bFinish = false;

 //记录迭代次数,只是记录,并未起到作用
    int iteration = 0;

 //声明一个vector用于存储节点的vSize和句柄对
 //这个变量记录了在一次分裂循环中,那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
    vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;

 //调整大小,这里的意思是一个初始化节点将“分裂”成为四个
    vSizeAndPointerToNode.reserve(lNodes.size()*4);

    // Step 5 利用四叉树方法对图像进行划分区域,均匀分配特征点
    while(!bFinish)
    {
  //更新迭代次数计数器,只是记录,并未起到作用
        iteration++;

  //保存当前节点个数,prev在这里理解为“保留”比较好
        int prevSize = lNodes.size();

  //重新定位迭代器指向列表头部
        lit = lNodes.begin();

  //需要展开的节点计数,这个一直保持累计,不清零
        int nToExpand = 0;

  //因为是在循环中,前面的循环体中可能污染了这个变量,所以清空
  //这个变量也只是统计了某一个循环中的点
  //这个变量记录了在一次分裂循环中,那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
        vSizeAndPointerToNode.clear();

        //将目前的子区域进行划分
  //开始遍历列表中所有的提取器节点,并进行分解或者保留
        while(lit!=lNodes.end())
        {
   //如果提取器节点只有一个特征点,
            if(lit->bNoMore)
            {
    //那么就没有必要再进行细分了
                lit++;
    //跳过当前节点,继续下一个
                continue;
            }
            else
            {
    //如果当前的提取器节点具有超过一个的特征点,那么就要进行继续分裂
                ExtractorNode n1,n2,n3,n4;

    //再细分成四个子区域
                lit->DivideNode(n1,n2,n3,n4); 

    //如果这里分出来的子区域中有特征点,那么就将这个子区域的节点添加到提取器节点的列表中
    //注意这里的条件是,有特征点即可
                if(n1.vKeys.size()>0)
                {
     //注意这里也是添加到列表前面的
                    lNodes.push_front(n1);   

     //再判断其中子提取器节点中的特征点数目是否大于1
                    if(n1.vKeys.size()>1)
                    {
      //如果有超过一个的特征点,那么待展开的节点计数加1
                        nToExpand++;

      //保存这个特征点数目和节点指针的信息
                        vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
                        // lNodes.front().lit 和前面的迭代的lit 不同,只是名字相同而已
                        // lNodes.front().lit是node结构体里的一个指针用来记录节点的位置
                        // 迭代的lit 是while循环里作者命名的遍历的指针名称
                        lNodes.front().lit = lNodes.begin();
                    }
                }
                //后面的操作都是相同的
                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();
                    }
                }
                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();
                    }
                }
                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();
                    }
                }

                //当这个母节点expand之后就从列表中删除它了,能够进行分裂操作说明至少有一个子节点的区域中特征点的数量是>1的
                // 分裂方式是后加的节点先分裂,先加的后分裂
                lit=lNodes.erase(lit);

    //继续下一次循环,其实这里加不加这句话的作用都是一样的
                continue;
            }//判断当前遍历到的节点中是否有超过一个的特征点
        }//遍历列表中的所有提取器节点

        //停止这个过程的条件有两个,满足其中一个即可:
        //1、当前的节点数已经超过了要求的特征点数
        //2、当前所有的节点中都只包含一个特征点
        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<pair<int,ExtractorNode*> > 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);

     //其实这里的节点可以说是二级子节点了,执行和前面一样的操作
                    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);

     //判断是是否超过了需要的特征点数?是的话就退出,不是的话就继续这个分裂过程,直到刚刚达到或者超过要求的特征点个数
                    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<cv::KeyPoint> vResultKeys;

 //调整容器大小为要提取的特征点数目
    vResultKeys.reserve(nfeatures);

 //遍历这个节点链表
    for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
    {
  //得到这个节点区域中的特征点容器句柄
        vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;

  //得到指向第一个特征点的指针,后面作为最大响应值对应的关键点
        cv::KeyPoint* pKP = &vNodeKeys[0];

  //用第1个关键点响应值初始化最大响应值
        float maxResponse = pKP->response;

  //开始遍历这个节点区域中的特征点容器中的特征点,注意是从1开始哟,0已经用过了
        for(size_t k=1;k<vNodeKeys.size();k++)
        {
   //更新最大响应值
            if(vNodeKeys[k].response>maxResponse)
            {
    //更新pKP指向具有最大响应值的keypoints
                pKP = &vNodeKeys[k];
                maxResponse = vNodeKeys[k].response;
            }
        }

        //将这个节点区域中的响应值最大的特征点加入最终结果容器
        vResultKeys.push_back(*pKP);
    }

    //返回最终结果容器,其中保存有分裂出来的区域中,我们最感兴趣、响应值最大的特征点
    return vResultKeys;
}
  • 2
    点赞
  • 11
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值