特征点参数初始化
ORBSLAM2的整体跟踪程序运行之前,进行了系统的初始化ORB_SLAM2::System SLAM(...)
,在系统初始化中,开启了Tracking、LoopMaping等线程。
对于图像刚进来的第一步操作:特征点提取,作为ORBSLAM2最重要的部分,在这里会详细讲解。因为特征点的提取过程中会使用到一些参数,这些参数不仅仅是配置文件中读取的,还有一些计算的过程,因此,特别的将初始化过程单独列出来
ORBSLAM2在程序中设定了一个特征提取器(其实就是一个类)ORBextractor
,而它的初始化过程也都在类的构造函数中(这个类的构造在Tracking类的构造函数中),这个初始化过程包含三部分:金字塔、BRIEF描述子和特征点方向,因此初始化的代码是由下面三部分组合而成
金字塔-初始化
金字塔原理
在视觉slam十四讲中,提到的ORB特征添加了尺度的描述,就是使用金字塔(对图像进行多次的缩放)进行特征点提取,实现尺度不变性
就如上图中,两帧图像进行相机的位置不同导致的尺度不同,可能出现特征点匹配不上的情况,因此使用金字塔方式,对图像进行缩放,就可以在金字塔的其他层中找到特征点的匹配,这就是尺度不变性
金字塔初始化
初始化过程:
- 将配置文件中的数据放置在对应的类成员变量中
- 计算金字塔每层需要提取的特征点
需要注意的👇
- ⏩从配置文件中读取的特征点数量指的是金字塔所有层加一起的数量,即
ORBextractor.nFeatures: 1200
中的1200是8层金字塔总共提取的特征点数量 - ⏩特征点的总数量确定了,那么就需要计算金字塔每层需要提取的特征点数量:主要是按照面积计算的,计算公式可以参考大佬的文章
- ⏩在计算特征点数量的时候,由于每次都会进行取整操作,因此就只计算前7层的特征点数量,最后一层(最小的图像)就是剩下多少就给它多少
/***************************************************************/
/*************下面是将配置文件中数据放置对应的类成员变量中**************/
/***************************************************************/
//存储每层图像缩放系数的vector调整为符合图层数目的大小
mvScaleFactor.resize(nlevels);
//存储这个sigma^2,其实就是每层图像相对初始图像缩放因子的平方
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层图像应该分配的特征点数量
// ## pow函数,pow(x,y)就是x的y次幂
float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
//用于在特征点个数分配的,特征点的累计计数清空
int sumFeatures = 0;
//开始逐层计算要分配的特征点个数,顶层图像除外(看循环后面)
// ## 对每一层分配特征点数量,只计算了前7层
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);
BRIEF描述子-初始化
描述子原理
BRIEF算法的核心思想就是在特征点P周围以一定模式选取N个点对,比较点对的两个点(比如p,q)所在的灰度值大小:如果p比q大,则取1,反之取0。如果取了256个这样的p,q,就会得到256维由0,1组成的向量,而这个向量就是这个点的唯一表示,就可以称之为描述子。在工程上,这个一定模式的随机N个点对,是设计好的固定的坐标点对pattern(注意:这里的pattern坐标点对都是以特征点位置为0,0点)
Steer BRIED描述子:上面所说的BRIEF算法,在将图像旋转后,描述子就会发生变化,为了使得描述子对旋转鲁棒,提出了灰度质心法(这个原理在下面讲解)具体做法就是将固定的pattern进行坐标转换,转换的坐标就是灰度质心法求解的向量作为x轴所在的坐标系,然后用转换后的pattern坐标下所在的像素进行对比,同样会得到唯一的描述子。如下图所示,Q就是灰度质心,坐标转换成PQ是x轴的坐标系:
一些问题解答👇
- ⏩坐标转换后为什么就有旋转不变性了?如果使用了灰度质心法,那么计算质心Q的时候,PQ向量的夹角会随着图像的旋转同样的发生变化,因此在将pattern坐标转换的时候,即使图像发生了旋转,依然会转换到一样的位置
描述子初始化
按照上面的原理,需要在特征点周围固定的pattern坐标对对应的像素值进行对比,因此初始化的过程就只需要将这个固定的pattern转化为坐标的形式,其中的pattern一开始就是一个大的数组:
初始化过程就是将上面这个数组转化成cv::Point
数据类型:
//成员变量pattern的长度,也就是点的个数,这里的512表示512个点(上面的数组中是存储的坐标所以是256*2*2)
const int npoints = 512;
//获取用于计算BRIEF描述子的随机采样点点集头指针
//注意到pattern0数据类型为Points*,bit_pattern_31_是int[]型,所以这里需要进行强制类型转换
const Point* pattern0 = (const Point*)bit_pattern_31_;
//使用std::back_inserter的目的是可以快速覆盖掉这个容器pattern之前的数据
//其实这里的操作就是,将在全局变量区域的、int格式的随机采样点以cv::point格式复制到当前类对象中的成员变量中
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
特征点方向-初始化
灰度质心法原理
FAST角点是没有方向信息的,那么在图像发生旋转的时候,其描述子也会发生变化,为了是的特征点具有旋转不变形,引出了灰度质心法来计算特征点的方向,下面简单讲解原理:
灰度质心法主要就是连接几何中心(特征点位置)和灰度中心(类似于重心)形成的向量,由此计算这个向量相对于x轴方向的夹角。流程如下:
- 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 p,q={0,1} mpq=x,y∑xpyqI(x,y),p,q=0,1
-
- 再根据设置的圆形图像区域,得到两个坐标轴方向的图像矩:
m 10 = ∑ x = − R R ∑ y = − R R x I ( x , y ) m 01 = ∑ x = − R R ∑ y = − R R y I ( x , y ) \begin{align} m_{10}=\sum_{x=-R}^R\sum_{y=-R}^RxI(x,y) \\ m_{01}=\sum_{x=-R}^R\sum_{y=-R}^RyI(x,y) \end{align} m10=x=−R∑Ry=−R∑RxI(x,y)m01=x=−R∑Ry=−R∑RyI(x,y)
- 圆形图像区域内所有的灰度值总和:
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=−R∑Ry=−R∑RI(x,y)
-
2.图像的质心为:
C = ( c x , c y ) = ( m 10 m 00 , m 01 m 00 ) C=(c_x,c_y)=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}}) C=(cx,cy)=(m00m10,m00m01)
- 3.求得质心相对于几何中心(特征点)的角度:
θ = 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)
灰度质心法初始化
按照上面的原理,需要计算一个图像块的矩,在ORBSLAM2中图像块是圆形,那么初始化主要做的工作就是要确定这个图像块的边界坐标,知道了边界坐标,就可以计算图像块的灰度值了
需要注意的👇
- ⏩ORBSLAM2没有将全部的一圈圆的坐标计算出来,而是仅计算1/4圆的边界,其他的是可以镜像出来的
- ⏩由于图像是矩阵的形式存放,圆形坐标在矩阵中的计算主要就是勾股定理,作者在计算边界的时候,也并没有全都计算,而是计算的1/8圆的边界坐标,剩下的1/8圆就是按照中间45°线的对称得到,因此作者在计算的时候设定了vmax和vmin,就是用来分开1/8圆
- ⏩作者使用了umax存放边界坐标,这个变量可以认为是一个二维数组,存放着边界坐标:
umax[0] = 15
就是v=0时对应的u=15,这里的u,v就是坐标轴,作者是先从v=0开始计算u对应的值,然后令v一步步+1,一直到vmax为止(就是从下往上计算),剩下的就是关于45°线(实线是圆的45°线,虚线是矩阵的45°计算后的线)的对称坐标而已(从上往下计算),如下图显示,每个方块都是一个像素格,初始化就是求解橘红色的像素格坐标:
- ⏩上图中的蓝色框就是一一对应的(关于45°线对称),那么为什么上半部分和下半部分对称的不一样?是因为后面计算矩的过程中,作者是一行一行的计算每个块的灰度值的,因此,上半部分的边界就只需要最左边的边界坐标就可以了
//这个初始化的过程,需要知道要干什么:求一个圆的一圈边界坐标,就像上图显示的
//HALF_PATCH_SIZE是15,+1中的1表示那个圆的中间,也就是特征点
umax.resize(HALF_PATCH_SIZE + 1);
//cvFloor 返回不大于参数的最大整数值
//cvCeil 返回不小于参数的最小整数值
//cvRound 则是四舍五入
int v,
v0,
//计算45度线对应的坐标,也就是在1/8圆上最大的v值
//用于从下往上计算时,所用的最大边界
vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);
//用于从上往下计算时,所用的最小边界
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
//利用勾股定理计算每行像素的u坐标边界(max)
//这里就是从下往上计算u值
for (v = 0; v <= vmax; ++v)
umax[v] = cvRound(sqrt(hp2 - v * v));
//这里就是从上往下计算u值
//可以看着上面的图片中的蓝色框对比这里的逻辑
for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
{
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}
特征点计算
特征点的计算是在图像进入程序Tracking线程后,构造Frame的构造函数中,在构造函数中开启的特征点提取的线程,运行了ExtractORB
函数,在这个函数中,使用了重载括号运算符的方式,对特征点进行了提取:
特征点的提取过程包含构建金字塔、计算特征点和计算描述子,下面进行分别讲解:
构建图像金字塔
金字塔的构建不仅仅是将图像缩放就行了,还需要进行一定的填充(如下图展示)👇
- ⏩因为要对每一层的图像提取特征点,而在提取边缘的FAST角点时,需要周围一圈的点的灰度值,就需要在图像的外围添加一圈点,因为FAST角点是使用的半径为3的一圈点计算,因此需要在图像的外围添加一圈3像素的点
- ⏩后续的操作中,需要对图像进行高斯滤波处理,为了考虑到使滤波后的图像边界处的像素也能够携带有正确的图像信息,扩充了16个像素。
//为了方便说明,对几个图像设置别名:
//没有进行扩充的图像:---------------源图像
//扩充的3像素的图像:----------------半径扩充图像
//在扩充3的基础上,再扩充16像素:------全尺寸图像
void ORBextractor::ComputePyramid(cv::Mat image)
{
//开始遍历所有的图层
for (int level = 0; level < nlevels; ++level)
{
//获取本层图像的缩放系数
float scale = mvInvScaleFactor[level];
//计算本层源图像的像素尺寸大小 cvRound是取整
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
//计算全尺寸图像,EDGE_THRESHOLD是19,×2是因为左右上下都扩充了19
Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
// 定义了两个变量:temp是扩展了边界的图像,masktemp并未使用
Mat temp(wholeSize, image.type()), masktemp;
// mvImagePyramid 刚开始时是个空的vector<Mat>
// 把图像金字塔该图层的图像指针mvImagePyramid指向temp的中间部分(这里为浅拷贝,内存相同)
//Rect初始化一个矩阵对象,参数:左上角坐标,宽高
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
//计算第0层以上resize后的图像
//由于第0层指的是原来图像大小,没有缩放,因此放在else中了
if( level != 0 )
{
//将上一层金字塔图像根据设定sz缩放到当前层级
resize(mvImagePyramid[level-1], //输入图像
mvImagePyramid[level], //输出图像
sz, //输出图像的尺寸
0, //水平方向上的缩放系数,留0表示自动计算
0, //垂直方向上的缩放系数,留0表示自动计算
cv::INTER_LINEAR); //图像缩放的差值算法类型,这里的是线性插值算法
//把源图像拷贝到目的图像的中央,四面填充指定的像素。图片如果已经拷贝到中间,只填充边界
//copyMakeBorder在源图像的周围扩充像素
copyMakeBorder(mvImagePyramid[level], //源图像
temp, //目标图像(扩充后的大小)
EDGE_THRESHOLD, EDGE_THRESHOLD, //上下需要扩展的像素大小
EDGE_THRESHOLD, EDGE_THRESHOLD, //左右需要扩展的像素大小
BORDER_REFLECT_101+BORDER_ISOLATED);//扩充方式,opencv给出的解释:
//BORDER_REFLECT_101 镜像填充
//BORDER_ISOLATED 表示对整个图像进行操作
//https://docs.opencv.org/3.4.4/d2/de8/group__core__array.html#ga2ac1049c2c3dd25c2b41bffe17658a36
}
else
{
//对于第0层未缩放图像,直接将图像深拷贝到temp的中间,并且对其周围进行边界扩展。此时temp就是对原图扩展后的图像
copyMakeBorder(image, //这里是原图像
temp,
EDGE_THRESHOLD, EDGE_THRESHOLD,
EDGE_THRESHOLD,EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
}
}
一些问题解答👇
- ⏩为什么要在图像的基础上再扩充一共19个像素? 在之前说过扩充的3像素是用于计算FAST角点时所需要的,剩下的16像素是由于高斯模糊对边界的处理,其实对于高斯模糊扩充16像素比较大,那么为什么选择16?这是由于在计算Steer BRIEF描述子的时候,需要在特征点的周围随机取点,这个随机取点是在pattern中固定了,而在pattern中有一个点坐标是(-13,-13);假想这样一种极端的情况,特征点在左上角边界(非常极端的情况),那么假设这个(-13,-13)坐标点经过灰度质心法的坐标转换后,这个点刚好在图像的x轴或者y轴上,那么这个点到原点(特征点)的距离就是 13 ∗ 2 = 18.382 13*\sqrt{2}=18.382 13∗2=18.382,虽然这种情况很少发生,但是为了安全起见,就直接扩充了19个像素,足够图像的各种计算了
提取特征点(复杂)
提取特征点就是计算FAST角点、四叉树均匀化和计算特征点方向
以下的几个部分,都是在讲解ComputeKeyPointsOctTree
函数中的内容
计算FAST角点
看过视觉slam十四讲的对下面的图应该并不陌生,就是FAST角点的提取(是对金字塔每层分别提取),这里就将提取角点的过程放在这里:
- 选取像素p,假设它的亮度为Ip
- 设置一个阈值T(这个阈值指的是亮度差,代码中是
iniThFAST=20
和minThFAST=7
) - 以像素p为中心,选取半径为3的圆上的16个像素点
- 假设在这个圆上,有连续N个点的亮度大于Ip+T或小于Ip-T,那么像素p就被认为是特征点
- 循环上面几步,对每个像素都执行相同操作
需要注意的👇
- ⏩ORBSLAM2使用的是OpenCV提供的FAST函数进行特征点的提取,但是并不是将整个图像进行特征点的提取,而是将图像分成了一个个网格cell,对每个网格进行FAST角点提取
- ⏩ORBSLAM2对FAST角点设置了两个阈值
iniThFAST=20
和minThFAST=7
,是为了在阈值20情况下没有提取到特征点时,降低阈值,达到争取在每个网格中都有特征点提取到 - ⏩OpenCV提供的FAST函数中,有对边界的处理:就是不对前三行/列和后三行/列提取特征点,因为边界没法找到半径为3的圆。因此,我们分割的网格就需要在半径扩充图像范围内分割(就是扩充了3像素的图像),然后送进FAST函数提取,具体的FAST函数的源代码点击这里
- ⏩由于FAST函数是在网格中提取的特征点,那么提取之后的特征点坐标都是在网格这个坐标系下的(就是网格的左上角是0,0点),那么就必须要经过一个坐标转换,将所有的特征点转换到统一的坐标系下
/**************以下是ComputeKeyPointsOctTree函数中的一部分***********/
//vector < vector<KeyPoint> > allKeypoints;是个两层的vector
//第一层是金字塔的某一层,第二层是某层金字塔对应的特征点
//重新调整图像层数
allKeypoints.resize(nlevels);
//图像分割成网格的大小,后面还会根据图像大小调整
const float W = 30;
//遍历每一层图像
for (int level = 0; level < nlevels; ++level)
{
//计算这层半径扩充图像(扩充3像素的图像)的坐标边界
//minBorderX、minBorderY就是左上角坐标(16,16)
//maxBorderX、maxBorderY就是右下角坐标
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;
//一般地都是过量采集,所以这里预分配的空间大小是nfeatures*10
//过量采集的,因为后面会使用四叉树均匀分配特征点
vToDistributeKeys.reserve(nfeatures*10);
//计算进行特征点提取的图像区域尺寸
//其实下面这里应该+1,但是并不影响
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
//下面就是计算分割的网格大小和数量
//计算网格的数量nCols是横着有多少个,nRows是竖着有多少个
const int nCols = width/W;
const int nRows = height/W;
//计算每个网格的大小wCell是网格的宽(x),hCell是网格的高(y)
//ceil函数是返回不小于 value 的下一个整数
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);
//开始遍历每个网格,两个for循环,先循环竖着的
for(int i=0; i<nRows; i++)
{
//计算当前网格初始行坐标(左上角y坐标)
//在minBorderY半径扩充图像左上角坐标基础上,偏移hCell网格的高,就是这个小网格左上角的y坐标
const float iniY =minBorderY+i*hCell;
//计算当前网格最大的行坐标(左下角y坐标)
//在iniY网格左上角y坐标的基础上,添加hCell网格的高,在添加提取FAST所用的6
//这里的+6=+3+3,即考虑到了多出来3是为了cell边界像素进行FAST特征点提取用
/**********这里为什么+6在后面讲解**********/
float maxY = iniY+hCell+6;
//如果初始的行坐标就已经超过了半径扩充图像边界,那么就跳过这一行
if(iniY>=maxBorderY-3)
continue;
//如果图像的大小导致不能够正好划分出来整齐的图像网格,那么就要委屈最后一行了
if(maxY>maxBorderY)
maxY = maxBorderY;
//再循环横着的
for(int j=0; j<nCols; j++)
{
//下面计算网格的x坐标跟上面一样
const float iniX =minBorderX+j*wCell;
float maxX = iniX+wCell+6;
//!BUG 正确应该是maxBorderX-3
if(iniX>=maxBorderX-6)
continue;
if(maxX>maxBorderX)
maxX = maxBorderX;
//开始对这个网格提取特征点,先使用的iniThFAST=20的阈值检测
//如果20的阈值没有检测到,就使用minThFAST=7的阈值检测
vector<cv::KeyPoint> vKeysCell;
//调用opencv的库函数来检测FAST角点
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像,这里就是当前遍历到的网格
vKeysCell, //存储角点位置的容器
iniThFAST, //检测阈值
true); //使能非极大值抑制
if(vKeysCell.empty())
{
//那么就使用更低的阈值来进行重新检测
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,
minThFAST, //更低的检测阈值
true);
}
//当检测到特征点后,进行坐标转换
//因为使用FAST函数计算角点后,特征点坐标是在网格坐标系下
//ORBSLAM2将坐标都转换都半径扩充图像的坐标系中
if(!vKeysCell.empty())
{
//遍历其中的所有FAST角点
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);
}
}
}
}
/*************这里的内容依然在遍历金字塔图层的for循环里**************/
/*************是用四叉树对特征点进行均匀分配(下面讲解)**************/
}
一些问题解答👇
- ⏩为什么
float maxY = iniY+hCell+6;
这个是+6而不是+3? ORBSLAM2中对图像进行网格分割,其实并不是想象中每个网格都在并列紧挨着,而是每个网格都是有重叠的,而这个重叠的部分就是这个6像素,这样设置的主要原因是FAST函数在提取特征的时候,对于边界3像素不会提取,那就导致不重叠的话就出现:两个网格中间有6像素没有提取到特征点。所以这里是+6,而如果+3的话,依然有3像素的区域提取不到特征点
- ⏩使用网格提取特征和整体图像一起提取特征有什么不同? ORBSLAM2有一个优点,就是提取的特征点可以均匀分布,如果使用整体图像送给FAST函数提取的话,那么有些区域就不好提取特征,所以ORBSLAM2使用了两个阈值去提取特征,这样可以在特征不明显的地方降低提取要求,尽量提取特征,为后续的四叉树均匀分布特征点打基础
四叉树均匀化
特征点提取之后,就需要对这些特征点进行均匀分布,在B站上有个视频,很清晰的展示了四叉树怎么均匀分配特征点的(点击跳转),四叉树的流程如下:
- 先把图像分成w/h份,就是初始的节点,如果是640x480的图像,就只设置一个节点node
- 如果node里面的特征点数量>1,就把这个node分成4个node,如果node里面的特征点为空,就不要了,删除掉
- 重复上面的步骤,一直到node总数连>设定的该图层应该提取的特征点数,停止
- 最后从每个node里面选择一个质量最好的FAST角点
需要注意的👇
- ⏩ORBSLAM2使用自定义的
DistributeOctTree
函数实现四叉树的特征点分配(后面讲解这个函数),需要注意的时,输入的是半径扩充图像的边界,也就是说,特征点分配之后的坐标系是在半径扩充图像的坐标系下,因此最后我们需要转换到全尺寸图像的坐标系下
下面的代码是ComputeKeyPointsOctTree
函数中for循环金字塔图层中一部分(上面计算FAST角点的程序后面有说明):
//声明一个对当前图层的特征点的容器的引用
vector<KeyPoint> & keypoints = allKeypoints[level];
//nfeatures是1200,其实扩大了,但是并不要紧
keypoints.reserve(nfeatures);
//返回值是一个保存有特征点的vector容器,含有剔除后的保留下来的特征点
//##注意:此时的特征点坐标是在半径扩充图像的坐标系上,所以后面还有坐标转换
keypoints = DistributeOctTree(vToDistributeKeys, //当前图层提取出来的特征点(未均分)
minBorderX, maxBorderX, //输入的边界是半径扩充图像的
minBorderY, maxBorderY,
mnFeaturesPerLevel[level], //计算的当前层图像应该提取的特征点数量
level); //当前层图像所在的图层
//PATCH_SIZE是对于底层的初始图像来说的,现在要根据当前图层的尺度缩放倍数进行缩放得到缩放后的PATCH大小 和特征点的方向计算有关
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;
}
DistributeOctTree
内容较多,为了防止混乱,先说明一下这里是要干什么:
- 我们已经提取了特征点,现在需要使用四叉树均匀分配
- 那么就需要创建节点,然后根据节点中特征点的数量(0:删除节点,1:保留节点但是不分裂,>1:保留节点并分裂此节点),实现不同操作
- 一直到节点数量达到了我们初始化时计算的该图层所需要的特征点数量,再停止
- 最后对特征点数量>1的节点进行最优选择,选择最好的那个特征点保留,其他的删除
- 保存最后剩下的所有特征点
需要注意的👇
- ⏩ORBSLAM2中对于节点的应用,使用类存储每个节点的信息,并使用链表存储所有节点。因此四叉树平均分配特征点的过程,就是在不断的处理下面这个节点类的信息(存入数据,删除节点等):
class ExtractorNode{
public:
//构造函数
ExtractorNode():bNoMore(false){}
//分裂节点的函数
void DivideNode(...);
///保存当前节点的特征点,是个KeyPoint类型的vector
std::vector<cv::KeyPoint> vKeys;
///对应这个节点的四个边界点坐标,如下图
cv::Point2i UL, UR, BL, BR;
//存储提取器节点的列表(其实就是双向链表)的一个迭代器
//这指向了节点列表
std::list<ExtractorNode>::iterator lit;
//如果节点中只有一个特征点的话,说明这个节点不能够再进行分裂了,这个标志置位
//这个节点中如果没有特征点的话,这个节点就直接被删除了
bool bNoMore;
};
- ⏩一个节点分裂成4个节点之后,那么这个父节点需要删除,因为它就不存在了
- ⏩为了让该图层能够分裂的节点与该图层要提取的特征点数量一致(因为最后每个节点就只有一个特征点),ORBSLAM2在分裂的过程中,做了判断:是否下一次分裂就会使节点数量超过预定数量。因此最后一次分裂的时候,仅仅分裂特征点多的节点,不会全部分裂,在节点数量达标的时候就停止
- ⏩四叉树均匀分配的程序流程:
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)
//注:这里输入的minX、maxX、minY、maxY是半径扩充图像的边界
{
//##1.根据宽高比确定初始节点数目,一般就是1或者2##
// ! bug: 如果宽高比小于0.5(就是说图像是手机那种竖着的),nIni=0, 后面hx会报错,
//static_cast<float>强制类型转换、round四舍五入取整
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;
//重新设置其大小,这个时候节点也就1或2个
vpIniNodes.resize(nIni);
//给初始节点赋值
for(int i=0; i<nIni; i++)
{
//生成一个提取器节点
ExtractorNode ni;
//##1-1.给节点赋值坐标信息##
//注意:这里坐标的起始左上角是0,0,因此这个坐标系是在半径扩充图像上的
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);
//存储这个初始的提取器节点句柄,其实就是指向了初始节点,第一个和第二个
//back()返回最后一个元素的引用
vpIniNodes[i] = &lNodes.back();
}
//##1-2.将特征点信息赋值给节点##
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
const cv::KeyPoint &kp = vToDistributeKeys[i];
//遍历该图层所有的特征点
//按特征点的横轴位置,分配给属于那个图像区域的提取器节点(最初的提取器节点)
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}
//##1-3.按照特征点数量设置节点标志位##
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<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
//调整大小,这里的意思是一个初始化节点将“分裂”成为四个
vSizeAndPointerToNode.reserve(lNodes.size()*4);
//##2.开始分裂节点##
//根据上面设置的标志位,判断是否要分裂
/***************这里的while循环在代码后面有讲解**************/
while(!bFinish)
{
//更新迭代次数计数器,只是记录,并未起到作用
iteration++;
//保存当前节点个数,prev在这里理解为“保留”比较好
int prevSize = lNodes.size();
//重新定位迭代器指向列表头部,依靠这个才会不断循环节点列表
lit = lNodes.begin();
//需要分裂的节点计数,这个一直保持累计,不清零
int nToExpand = 0;
//为了安全起见,才clear了这个变量
vSizeAndPointerToNode.clear();
//##2-1.开始遍历当前的所有节点##
/***************这里的while循环在代码后面有讲解**************/
while(lit!=lNodes.end())
{
//##2-1-1.该节点只有一个特征点,跳过##
if(lit->bNoMore)
{
lit++;
continue;
}
//##2-1-2.该节点有超过一个节点,分裂##
else
{
//创建分裂需要的节点类
ExtractorNode n1,n2,n3,n4;
//##2-1-2-1.分裂节点,调用函数##
//这个函数在后面有讲解,就是分裂节点后,给每个节点赋值边界坐标,特征点信息和标志位
lit->DivideNode(n1,n2,n3,n4);
//分裂之后节点有特征点,需要保存下来
//分别对四个节点都进行保存操作
if(n1.vKeys.size()>0)
{
//##2-1-2-2.将分裂的节点存放到节点列表##
//注意这里是添加到列表前面的
/************这里为什么在前面添加在代码后面有讲解***********/
lNodes.push_front(n1);
//判断该节点中特征点数量>1
if(n1.vKeys.size()>1)
{
//如果有超过一个的特征点,那么待展开的节点计数加1
//这个变量在后面是用来判断是否是最后一次分裂
nToExpand++;
//vSizeAndPointerToNode只是计算用的中间变量,实际的节点信息都存储在lNodes中
vSizeAndPointerToNode.push_back(
make_pair(n1.vKeys.size(),&lNodes.front()));
//?这个访问用的句柄貌似并没有用到?
lNodes.front().lit = lNodes.begin();
}
}
//后面的操作都是相同的,这里不再赘述
if(n2.vKeys.size()>0){...}
if(n3.vKeys.size()>0){...}
if(n4.vKeys.size()>0){...}
//##2-1-2-3.删除父节点##
lit=lNodes.erase(lit);
continue; //当前次分裂结束,进行下一次
}
}
//##2-2.判断当前节点数量是否够了,够了就退出循环##
//停止这个过程的条件有两个,满足其中一个即可:
//1、当前的节点数已经超过了要求的特征点数
//2、当前所有的节点中都只包含一个特征点
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
//prevSize中保存的是分裂之前的节点个数
//如果分裂之前和分裂之后的总节点个数一样
//说明当前所有的节点区域中只有一个特征点,已经不能够再细分了
{
//停止标志置位
bFinish = true;
}
//##2-3.判断是否下一次循环后,节点数量就会超过计算的该图层应该有的数量##
//可以展开的子节点个数nToExpand x3,是因为一分四之后,会删除原来的主节点,所以乘以3
else if(((int)lNodes.size()+nToExpand*3)>N)
{
//如果再分裂一次那么数目就要超了,这里尽可能使其刚刚达到或者超过要求的特征点个数时就退出
while(!bFinish)
{
//获取当前的list中的节点个数
prevSize = lNodes.size();
//保留那些还可以分裂的节点的信息, 这里是深拷贝
vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
//清空
vSizeAndPointerToNode.clear();
// 对需要划分的节点进行排序,对pair对的第一个元素进行排序,默认是从小到大排序
//! 注意这里的排序规则非常重要!会导致每次最后产生的特征点都不一样。建议使用 stable_sort
sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
//##2-3-1.寻找特征点数量最多的进行分裂##
//注意是从后往前遍历,因此就是特征点多的先分裂
for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
{
//##2-3-2.分裂的4个节点都存放到节点列表中##
//下面的操作跟之前一样,分裂,存储
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.push_back(
make_pair(n1.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n2.vKeys.size()>0){...}
if(n3.vKeys.size()>0){...}
if(n4.vKeys.size()>0){...}
//##2-3-3.删除父节点##
//vPrevSizeAndPointerToNode[j].second->lit指向的是节点列表lNodes
lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
//##2-3-4.如果此时节点数量够了,就退出##
if((int)lNodes.size()>=N)
break;
}
//判断是否达到了停止条件,跟之前一样
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
bFinish = true;
}
}
}
// Retain the best point in each node
//##3.在每个节点中寻找响应值最大的特征点保存,其他的删除##
//使用这个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;
}
一些问题解答👇
- ⏩程序中的两个while循环是如何实现遍历所有节点列表的? 主要依靠
lit = lNodes.begin();
和lNodes.push_front(n1);
推动着循环一直进行,用下图表示:
DivideNode
此函数是在DistributeOctTree
中的,是ExtractorNode
类的成员函数,对当前节点进行分裂:lit->DivideNode(n1,n2,n3,n4);
对lit节点进行分裂,形成了n1,n2,n3,n4一共4个子节点
其实这个函数没有什么难点,分裂节点就是创建新的节点,根据当前父节点的信息,计算子节点的边界坐标、特征点信息和标志位
void ExtractorNode::DivideNode(ExtractorNode &n1, //这是左上节点
ExtractorNode &n2, //这是右上节点
ExtractorNode &n3, //这是左下节点
ExtractorNode &n4) //这是右下节点
{
//得到当前提取器节点所在图像区域的一半长宽,当然结果需要取整
//这里的UR、UL、BR、UL都是父节点的信息
const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);
//下面的操作大同小异,将一个图像区域再细分成为四个小图像区块
//n1 存储左上区域的边界坐标
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);
//用来存储在该节点对应的图像网格中提取出来的特征点的vector
n1.vKeys.reserve(vKeys.size());
//n2 存储右上区域的边界坐标
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 存储左下区域的边界坐标
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());
//n4 存储右下区域的边界坐标
n4.UL = n3.UR;
n4.UR = n2.BR;
n4.BL = n3.BR;
n4.BR = BR;
n4.vKeys.reserve(vKeys.size());
//遍历当前图层的所有特征点
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);
}
//设置节点的标志位
//这里判断是否数目等于1的目的是确定这个节点还能不能再向下进行分裂
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;
}
灰度质心法求解特征点方向
灰度质心法求解特征点方向在ComputeKeyPointsOctTree
函数的最后一部分:
//最后计算这些特征点的方向信息,注意这里还是分层计算的
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], //对应的图层的图像
allKeypoints[level], //这个图层中提取并保留下来的特征点容器
umax); //以及PATCH的横坐标边界
computeOrientation
//就是对图层中的每个特征点,挨个计算其方向
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是角度
keypoint->angle = IC_Angle(image, //特征点所在的图层的图像
keypoint->pt, //特征点在这张图像中的坐标
umax); //每个特征点所在图像区块的每行的边界 u_max 组成的vector
}
}
IC_Angle
此函数是计算特征点的方向,就是按照灰度质心法原理中的公式,计算其灰度质心的夹角。原理很简单,但是代码中作者使用了比较巧妙的方法进行了加速计算,并不好理解(但终归代码比较少),因此这里建议对着图片和代码去理解
IC_Angle逻辑👇
首先明白这里是要干什么:计算特征点方向,按照公式需要求解
m
10
,
m
01
m_{10},m_{01}
m10,m01,然后再计算arctan值就好了,这个arctan值可以使用OpenCV提供的fastAtan2
函数直接求解,那么重点就是需要求
m
10
,
m
01
m_{10},m_{01}
m10,m01。这两个量分别代表的是这个圆形区域两个坐标轴方向的矩,就是如下公式:
m 10 = ∑ x = − R R ∑ y = − R R x I ( x , y ) m 01 = ∑ x = − R R ∑ y = − R R y I ( x , y ) \begin{align} m_{10}=\sum_{x=-R}^R\sum_{y=-R}^RxI(x,y)\\ m_{01}=\sum_{x=-R}^R\sum_{y=-R}^RyI(x,y) \end{align} m10=x=−R∑Ry=−R∑RxI(x,y)m01=x=−R∑Ry=−R∑RyI(x,y)
那么就需要计算圆形区域每个像素块的坐标和灰度值,再按照上面的公式计算,就可求得
注:公式中的x,y对应到计算的坐标中就是u,v,对应着umax[v]=u
ORBSLAM2在计算这个矩的时候,使用了非常巧妙的方式进行了加速计算,对照上图理解下面的逻辑:
- 1.先确定了一个指向中心位置(特征点位置)的指针center,指向的是这个像素块的内容(灰度值)
- 2.根据之前计算的umax边界坐标对center指针进行横向偏移(u轴偏移),首先求解了u轴的矩(就是中间灰色那一横条)
- 3.建立一个step:图像一行的字节数,用于使得center指针能够纵向偏移。这是OpenCV中的函数参考这里
- 4.依然根据umax对center进行横向偏移,同时使用step进行纵向偏移,求得上图中黄色部分的矩(上下都求了,因为step偏移一横行,也就可以偏移上一行或者下一行,所以一起算了)
- 5.不断使用umax和step对center指针进行横向、纵向偏移,求解圆形区域所有部分的矩,在进行相加减(为什么会有减,因为公式里面的x,y在坐标轴上是有正负的)
//输入是金字塔该层图像、单个特征点、初始化计算的umax
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
{
//图像的矩
//m_01是y坐标加权,m_10是x坐标加权
int m_01 = 0, m_10 = 0;
//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
//注意,这里先放进去的是y然后是x,因为OpeCV就这样设定了,跟我们常识是反着的
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
//按照上面的逻辑,先计算中间灰色横条部分的矩,使用center指针偏移求得灰度值
//HALF_PATCH_SIZE=15 这里也就是umax[0]对应的u值15
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
//注意这里的center下标u可以是负的!中心水平线上的像素按x坐标(也就是u坐标)加权
m_10 += u * center[u];
//step是偏移了整个图像一行(是整个图像不是原型区域)的字节
//就是说可以用这个作为偏移量,实现找到这个点下面的那个点的位置(就是这个点上面或者下面一列对应的点)
int step = (int)image.step1();
//由此,就有了横纵偏移的方法了,就可以计算每一条的矩了
//这里以v=0为中心线,先求黄色两条再求蓝色两条,依次求解就得到整个圆形区域的矩
//这种一次求上下两条的方式,相当于加速了计算
//因为v=0已经计算了,就是灰色横条的部分,所以这里从1开始
for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
{
//存放中间值
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)
{
//一次循环可以使用step偏移,得到对称的上下两个点的灰度值
//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 10 = ∑ x = − R R ∑ y = − R R x I ( x , y ) = > ∑ x = − R R [ x I ( x , y ) + x I ( x , − y ) ] = > ∑ x = − R R x [ I ( x , y ) + I ( x , − y ) ] m 01 = ∑ x = − R R ∑ y = − R R y I ( x , y ) = > ∑ x = − R R [ y I ( x , y ) − y I ( x , − y ) ] = > ∑ x = − R R y [ I ( x , y ) − I ( x , − y ) ] \begin{align} m_{10}=\sum_{x=-R}^R\sum_{y=-R}^RxI(x,y)=>\sum_{x=-R}^R[xI(x,y)+xI(x,-y)]=>\sum_{x=-R}^Rx[I(x,y)+I(x,-y)] \\ m_{01}=\sum_{x=-R}^R\sum_{y=-R}^RyI(x,y)=>\sum_{x=-R}^R[yI(x,y)-yI(x,-y)]=>\sum_{x=-R}^Ry[I(x,y)-I(x,-y)] \end{align} m10=x=−R∑Ry=−R∑RxI(x,y)=>x=−R∑R[xI(x,y)+xI(x,−y)]=>x=−R∑Rx[I(x,y)+I(x,−y)]m01=x=−R∑Ry=−R∑RyI(x,y)=>x=−R∑R[yI(x,y)−yI(x,−y)]=>x=−R∑Ry[I(x,y)−I(x,−y)]
- ⏩计算上下横条的矩,为什么y有负值,而x没有负值?其实x是有负值的,但是这个分解都是为了说明代码的计算,所以要仔细看代码的逻辑,x(也就是u)在代码中循环是从-d到d的,也就是说x本身就带有的正负值;而y(也就是v)在代码中循环是从1到HALF_PATCH_SIZE的,没有负值,因此分解时就需要带有负号
- ⏩为什么在计算m_10时将灰色横条计算进去了,而在计算m_01时没有计算灰色横条?计算图像的矩应该将区域内所有的灰度值进行计算,但是灰色横条是u轴对应的矩,它对应的v(也就是y)是0,因此没有必要计算
计算描述子
根据原理的说明,需要按照pattern的坐标点对寻找对应的灰度值,然后农户进行对比,得到描述子。但是在此之前还需要对图像进行高斯模糊,再计算特征点的描述子(注意,描述子是在对应的图层上计算),最后把所有的特征点装换到原图像的下(金字塔最底层)方便后面计算。
需要注意的👇
- ⏩高斯模糊:也叫高斯平滑,主要是用来减少图像噪声,从数学角度就是图像与正态分布做卷积操作,而正态分布就是中间高两边低的形态,那么对图像的高斯模糊指的就是图像中的一个像素块是由周围其他像素块的平均而来的
- ⏩代码中建立一个descriptors矩阵,矩阵的行是特征点数量,矩阵的列是描述子。因此在遍历金字塔图层的时候,使用了offset偏移,对每层图像的描述子进行存储
//拷贝图像描述子到新的矩阵descriptors
Mat descriptors;
//统计整个图像金字塔中的特征点
int nkeypoints = 0;
//开始遍历每层图像金字塔,并且累加每层的特征点个数
for (int level = 0; level < nlevels; ++level)
nkeypoints += (int)allKeypoints[level].size();
//如果本图像金字塔中没有任何的特征点
if( nkeypoints == 0 )
//通过调用cv::mat类的.realse方法,强制清空矩阵的引用计数,这样就可以强制释放矩阵的数据了
_descriptors.release();
else
{
//创建一个描述子的矩阵,行:特征点数量,列:描述子
_descriptors.create(nkeypoints, //矩阵的行数,对应为特征点的总个数
32, //矩阵的列数,对应为使用32*8=256位描述子
CV_8U); //矩阵元素的格式
//获取这个描述子的矩阵信息,转换成Mat类型
descriptors = _descriptors.getMat();
}
//特征点处理
_keypoints.clear();
_keypoints.reserve(nkeypoints);
//对金字塔每层图像分别进行高斯模糊和描述子计算,用offset偏移存放描述子
int offset = 0;
//开始遍历每一层图像
for (int level = 0; level < nlevels; ++level)
{
//得到该图层的额特征点
vector<KeyPoint>& keypoints = allKeypoints[level];
int nkeypointsLevel = (int)keypoints.size();
//如果特征点数目为0,跳出本次循环,继续下一层金字塔
if(nkeypointsLevel==0)
continue;
//高斯模糊
//深拷贝当前金字塔所在层级的图像
Mat workingMat = mvImagePyramid[level].clone();
//注意:提取特征点的时候,使用的是清晰的原图像;计算描述子时,为了避免图像噪声的影响,使用了高斯模糊
GaussianBlur(workingMat, //源图像
workingMat, //输出图像
Size(7, 7), //高斯滤波器kernel大小,必须为正的奇数
2, //高斯滤波在x方向的标准差
2, //高斯滤波在y方向的标准差
BORDER_REFLECT_101);//边缘拓展点插值类型
//计算描述子
//desc存储当前图层的描述子
//这里使用rowRange其实desc是指向这个矩阵的一部分,后面对desc计算就是在改变descriptors内容
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
//计算高斯模糊后图像的描述子
computeDescriptors(workingMat, //高斯模糊之后的图层图像
keypoints, //当前图层中的特征点集合
desc, //存储计算之后的描述子
pattern); //随机采样模板
//更新偏移量的值
offset += nkeypointsLevel;
//对非第0层图像中的特征点的坐标恢复到第0层图像(原图像)的坐标系下,方便后面计算
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());
}
computeDescriptors
这里就是遍历该图层中的所有特征点,并将pattern传入用于计算描述子
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors, const vector<Point>& pattern)
{
//清空保存描述子信息的容器
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)); //提取出来的描述子的保存位置
}
computeOrbDescriptor
按照之前原理中的计算Steer BRIEF描述子的步骤:
- 1. 将pattern中的坐标对按照灰度质心法的角度进行旋转(坐标转换)
-
- 设pattern提取的坐标是v点,旋转后(坐标转换)的坐标是v’,这个 θ \theta θ就是灰度质心法计算的角度
x
′
=
r
c
o
s
(
θ
+
ϕ
)
=
r
c
o
s
(
θ
)
c
o
s
(
ϕ
)
−
r
s
i
n
(
θ
)
s
i
n
(
ϕ
)
=
x
c
o
s
(
θ
)
−
y
s
i
n
(
θ
)
y
′
=
r
s
i
n
(
θ
+
ϕ
)
=
r
s
i
n
(
θ
)
c
o
s
(
ϕ
)
+
r
c
o
s
(
θ
)
s
i
n
(
ϕ
)
=
x
s
i
n
(
θ
)
+
y
c
o
s
(
θ
)
\begin{align} x'=rcos(\theta+\phi)=rcos(\theta)cos(\phi)-rsin(\theta)sin(\phi)=xcos(\theta)-ysin(\theta)\\ y'=rsin(\theta+\phi)=rsin(\theta)cos(\phi)+rcos(\theta)sin(\phi)=xsin(\theta)+ycos(\theta) \end{align}
x′=rcos(θ+ϕ)=rcos(θ)cos(ϕ)−rsin(θ)sin(ϕ)=xcos(θ)−ysin(θ)y′=rsin(θ+ϕ)=rsin(θ)cos(ϕ)+rcos(θ)sin(ϕ)=xsin(θ)+ycos(θ)
[
x
′
y
′
]
=
[
c
o
s
θ
−
s
i
n
θ
s
i
n
θ
c
o
s
θ
]
∗
[
x
y
]
\begin{bmatrix}x'\\y'\end{bmatrix}= \begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{bmatrix} * \begin{bmatrix} x\\y \end{bmatrix}
[x′y′]=[cosθsinθ−sinθcosθ]∗[xy]
- 2. 按照转换后的v’坐标,寻找对应的灰度值(以特征点为坐标中心)
- 3. 对比坐标对的大小,一共得到256个由0,1组成的变量,将这个变量存放到描述子矩阵中
需要注意的👇
- ⏩提取灰度值的方法,跟之前灰度质心法时候一样,使用一个指针center指向中心位置的灰度值,然后向两个坐标轴偏移,就会得到周围所有其他点的灰度值
- ⏩由于这个坐标转换的过程需要多次使用,因此ORBSLAM2中使用了有参宏定义
GET_VALUE(idx)
,后面多次转换的时候就只需要使用这个宏定义就好了。GET_VALUE(idx)
是输入pattern坐标点的索引,输出的是坐标转换后的对应的灰度值 - ⏩描述子的计算是得到位bit的数据,因此使用了一些位操作:
<<左移
和|或运算
,这些都是为了将对比结果存储在desc矩阵中
static void computeOrbDescriptor(const KeyPoint& kpt, const Mat& img, const Point* pattern, uchar* desc)
{
//得到特征点的角度,用弧度制表示。其中kpt.angle是角度制,范围为[0,360)度
//这就是灰度质心法计算得到的角度
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;
//获得采样点中某个idx所对应的点的灰度值,这里旋转前坐标为(x,y), 旋转后坐标(x',y'),他们的变换关系:
//x'= xcos(θ) - ysin(θ), y'= xsin(θ) + ycos(θ)
//下面表示 center[y'* step + x'] 得到的就是转换后坐标点对应的灰度值
//pattern[idx]提取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)]
//brief描述子由32*8位组成
//其中每一位是来自于两个像素点灰度的直接比较,所以每比较出8bit结果,需要16个随机点
//这也就是为什么pattern需要+=16的原因
for (int i = 0; i < 32; ++i, pattern += 16)
{
int t0, //参与比较的第1个特征点的灰度值
t1, //参与比较的第2个特征点的灰度值
val; //描述子这个字节的比较结果,0或1
t0 = GET_VALUE(0); t1 = GET_VALUE(1);
val = t0 < t1; //描述子本字节的bit0
t0 = GET_VALUE(2); t1 = GET_VALUE(3);
val |= (t0 < t1) << 1; //描述子本字节的bit1,左移1位,或运算,bit0位置的数保存下来
t0 = GET_VALUE(4); t1 = GET_VALUE(5);
val |= (t0 < t1) << 2; //描述子本字节的bit2,左移2位,或运算,之前位置的数保存下来
t0 = GET_VALUE(6); t1 = GET_VALUE(7);
val |= (t0 < t1) << 3; //描述子本字节的bit3,左移3位,或运算,之前位置的数保存下来
t0 = GET_VALUE(8); t1 = GET_VALUE(9);
val |= (t0 < t1) << 4; //描述子本字节的bit4,左移4位,或运算,之前位置的数保存下来
t0 = GET_VALUE(10); t1 = GET_VALUE(11);
val |= (t0 < t1) << 5; //描述子本字节的bit5,左移5位,或运算,之前位置的数保存下来
t0 = GET_VALUE(12); t1 = GET_VALUE(13);
val |= (t0 < t1) << 6; //描述子本字节的bit6,左移6位,或运算,之前位置的数保存下来
t0 = GET_VALUE(14); t1 = GET_VALUE(15);
val |= (t0 < t1) << 7; //描述子本字节的bit7,左移7位,或运算,之前位置的数保存下来
//保存当前比较的出来的描述子的这个字节
desc[i] = (uchar)val;
}
//为了避免和程序中的其他部分冲突在,在使用完成之后就取消这个宏定义
#undef GET_VALUE
}
特征点提取总结
到此,特征点的提取就都结束了,总结一下都干了什么:计算FAST角点、使用四叉树均匀化特征点、灰度质心法求解特征点方向、根据方向计算Steer BRIEF描述子。一共就做了这几步,但是为了后续能够方便进行三角化计算,将各个图层的特征点转换到原图像(金字塔的最底层)下,然后将特征点信息和描述子信息(因为提取特征点主要就为了求解这两个)分别传递给重载括号运算符的形参中(这部分放在了计算描述子中,因为在一个for循环中),实现传递。