ORB-SLAM2中使用的是ORB特征点,在OpenCV中是自带ORB特征提取函数的,不过存在着特征点分布不均匀等问题,ORB-SLAM2中的特征点提取部分相比OpenCV中的函数有很大的改进,其具体调用之前有提过,在Frame
中,通过类函数的方法实现特征提取类的一个调用。
void Frame::ExtractORB(int flag, const cv::Mat &im)
{
if(flag==0)
(*mpORBextractorLeft)(im,cv::Mat(),mvKeys, mDescriptors);
else
(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}
目录
成员变量
在ORBextractor.h
中共定义了两个类,分别是分配四叉树时用到的ExtractorNode
类和ORB特征提取器ORBextractor
这里分开把两个类的一些成员变量记录一下。
ExtractorNode
ExtractorNode
类的成员变量都没有以m开头,可能是因为这个类只是用作ORBextractor
类的一个辅助,成员变量也不存在被其他类调用的情况,所以以最简单的方式直接命名了。
变量名 | 访问控制 | 简单解释 |
---|---|---|
std::vector<cv::KeyPoint> vKeys | public | 在四叉树分配特征点时,用以保存当前节点的特征点 |
cv::Point2i UL, UR, BL, BR | public | 节点对应的图像坐标边界 |
std::list<ExtractorNode>::iterator lit | public | ExtractorNode 类中重要的变量,储存总的节点,用list 方便操作 |
bool bNoMore | public | 节点是否还有特征点(没有就直接删除) |
ORBextractor
变量名 | 访问控制 | 简单解释 |
---|---|---|
std::vector<cv::Point> pattern | protected | 用于计算描述子的随机采样点集合 |
int nFeatures | protected | 整个图像金字塔中,需要提取的特征点的数目 |
double scaleFactor | protected | 图像金字塔每层之间的缩放因子 |
int nlevels | protected | 图像金字塔的层数 |
int iniThFAST int minThFAST | protected | FAST响应阈值,主要是在提取FAST特征点时,如果提取效果不好,可以改变阈值再次提取 |
std::vector<int> mnFeaturesPerLevel | protected | 分配到每层图像要提取的特征点数量 |
std::vector<int> umax | protected | 这个是计算特征点方向时,用到的像素圆的界限 |
std::vector<float> mvScaleFactor std::vector<float> mvInvScaleFactor std::vector<float> mvLevelSigma2 std::vector<float> mvInvLevelSigma2 | protected | 图像金字塔参数,缩放因子,包括sigma^2 |
成员函数
构造函数ORBextractor
ORBextractor
的构造函数具体的调用在Tracking
中,因为Tracking
的构造函数会加载系统的配置文件,因此在这里初始化了提取器,并在Frame
的构造函数中直接使用了初始化的ORB提取器。
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
if(sensor==System::STEREO)
mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
if(sensor==System::MONOCULAR)
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
- 这里初始化了三个ORB提取器,第一个是所有情况下都会用到的,第二个是为双目准备的,第三个是在单目初始化中特殊使用到的。
构造函数依旧是列表初始化了成员变量:
nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
iniThFAST(_iniThFAST), minThFAST(_minThFAST)//设置这些参数
之后是为mvScaleFactor
和mvLevelSigma2
以及他们的逆进行赋值,就是对缩放因子的一个累乘,之后每一层金字塔的数目等信息是和这些变量有关的。
mvScaleFactor.resize(nlevels);
mvLevelSigma2.resize(nlevels);
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];
}
之后计算每层金字塔应该分配到的特征点数量。关于数量的计算,这里直接引用知乎专栏的一篇文章[ORB-SLAM2] ORB-SLAM中的ORB特征(提取)
这里要注意最后一句话,也就是ORB-SLAM里是直接按边长的比例分配到各层的,具体在代码中的体现也很直接:
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;
}
我们看到第0层直接用的就是factor
,后边每一层缩放过程中乘的也是factor
,其中有一个变量是sumFeatures
,因为我们在设置特征点总数量是都是设置的如1000、2000这样的整数,因此分配完之后可能还会有剩余,这个时候就将剩余的数量全部在最高图像提取
mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
之后是对计算BRIEF描述子需要用到的随机采样点的操作,这里涉及到一个变量是bit_pattern_31_[256*4]
,这个变量每一行就是代表一对比较的点,然后两个点比较之后得到这一行的值是0或是1,最后就构成了一个256bit的描述子信息。在构造函数里,主要就是进行了类型转换:
const int npoints = 512;
//注意到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));
最后,在构造函数里又进行了计算特征点的旋转相关的设置,为了计算旋转不变性我们是需要用到一个圆的,但是这个圆又不是我们理解的那种平滑的圆,而是一个像素一个像素那样的。在代码中我们也可以看到:
//cvFloor返回不大于参数的最大整数值,cvCeil返回不小于参数的最小整数值,cvRound则是四舍五入
int v,v0,vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); //计算圆的最大行号,+1应该是把中间行也给考虑进去了
//NOTE 注意这里的最大行号指的是计算的时候的最大行号,此行的和圆的角点在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就会很容易出现不对称的情况,
//同时这些随机采样的特征点集也不能够满足旋转之后的采样不变性了)
// NOTE 这样保证上下1/8圆的umax对称,这里的圆不是严格意义的圆
for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
{
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}
我们把其中的umax都计算一下,里面的组成是这样的:
umax = {15,15,15,15,15,14,14,13,13,12,11,10,9,8,6,4}
画一个图的话大概就是这样:
这样形成的圆是一个标准对称的,就是可能和平常理解的那种圆不太一样。
至此,完成了ORBextractor
构造函数的全部工作,总结一下就是三个方面:金字塔、BRIEF和角度计算的相关初始工作。
ORB特征提取的主体,仿函数operator()
仿函数就是让一个类的使用看上去像一个函数,可以把迭代和计算分离开来。在ORB-SLAM2里,通过这个仿函数实现了ORB特征提取的主要工作。这里的输入参数中,描述子的类型暗藏玄机,使用的是OpenCV中的OutputArray
,本身就是一个引用类型了,也就是说会将描述子输出,无需我们自己定义为引用类型。
在确定图像为单通道灰度图像后,先进行图像金字塔的计算:
ComputePyramid(image);
然后计算图像的特征点,这里是通过四叉树的方式将其均匀化:
vector < vector<KeyPoint> > allKeypoints;
//使用四叉树的方式计算每层图像的特征点并进行分配
ComputeKeyPointsOctTree(allKeypoints);
之后是计算图像的描述子,描述子的操作是有对金字塔图层的一个遍历的,同时是先对图像进行了一个高斯模糊,为了避免图像噪声的影响:
GaussianBlur(workingMat, //源图像
workingMat, //输出图像
Size(7, 7), //高斯滤波器kernel大小,必须为正的奇数
2, //高斯滤波在x方向的标准差
2, //高斯滤波在y方向的标准差
BORDER_REFLECT_101);//边缘拓展点插值类型
然后计算高斯模糊后图像的描述子:
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
computeDescriptors(workingMat, //高斯模糊之后的图层图像
keypoints, //当前图层中的特征点集合
desc, //存储计算之后的描述子
pattern); //随机采样模板
这里有一个变量offset
,这个主要就是记录描述子在总矩阵中的一个位置,每次加的就是一层的关键点的个数,这样就和对应的关键点都对应上了。
之后要把金字塔非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;
}
计算图像金字塔ComputePyramid
计算图像金字塔是先把原图像缩放到对应的尺寸,再在得到的大小基础上做了一个扩边操作:
扩充后包括绿色在内的矩形是用来计算FAST角点的,包括白色边界在内的矩形是用来计算描述子的。
void ORBextractor::ComputePyramid(cv::Mat image)
{
//开始遍历所有的图层
for (int level = 0; level < nlevels; ++level)
{
//获取本层图像的缩放系数
// NOTE 这里用的已经是缩放因子的倒数了,也就是说对图像进行了多层的缩小
float scale = mvInvScaleFactor[level];
//计算本层图像的像素尺寸大小
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
//全尺寸图像。包括无效图像区域的大小。将图像进行“补边”,EDGE_THRESHOLD区域外的图像不进行FAST角点检测
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的中间部分(这里为浅拷贝,内存相同)
// TAG Rect 取矩形的一块,参数分别是x,y,width,height,从(x,y)开始,height行,width列
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
// Compute the resized image
//计算第0层以上resize后的图像
if( level != 0 )
{
//将上一层金字塔图像根据设定sz缩放到当前层级
resize(mvImagePyramid[level-1], //输入图像
mvImagePyramid[level], //输出图像
sz, //输出图像的尺寸
0, //水平方向上的缩放系数,留0表示自动计算
0, //垂直方向上的缩放系数,留0表示自动计算
cv::INTER_LINEAR); //图像缩放的差值算法类型,这里的是线性插值算法
//把源图像拷贝到目的图像的中央,四面填充指定的像素。图片如果已经拷贝到中间,只填充边界
//这样做是为了能够正确提取边界的FAST角点
//EDGE_THRESHOLD指的这个边界的宽度,由于这个边界之外的像素不是原图像素而是算法生成出来的,所以不能够在EDGE_THRESHOLD之外提取特征点
copyMakeBorder(mvImagePyramid[level], //源图像
temp, //目标图像(此时其实就已经有大了一圈的尺寸了)
EDGE_THRESHOLD, EDGE_THRESHOLD, //top & bottom 需要扩展的border大小
EDGE_THRESHOLD, EDGE_THRESHOLD, //left & right 需要扩展的border大小
BORDER_REFLECT_101+BORDER_ISOLATED); //扩充方式,opencv给出的解释:
// 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);
}
}
}
关于这个边界扩充也是用的OpenCV自带的函数:
- 这里的源代码应该是有些问题的,就是它的图像金字塔并没有实现扩边,就直接传到下一层接着操作了,但看具体的使用还没什么问题,可能因为图像边缘的特征点其实也没那么重要?所以不扩边也无所谓了。
计算四叉树特征点ComputeKeyPointsOctTree
ORB-SLAM2没有直接使用OpenCV自带的ORB特征提取函数,因为这样提取得到的特征点分布更加均匀,这个很好实现对比,这里也是引用一篇知乎专栏的图片[ORB-SLAM2] ORB特征提取策略对ORB-SLAM2性能的影响
我们可以明显看到使用ORB-SALM2的方法得到的特征点分布是更加均匀合理的,这样也能使得之后恢复位姿更加精准。函数输入的是所有的特征点allKeypoints
,定义了每个格子的预初始大小const float W = 30
,之后就开始对每一层图像做处理。
先是把图像再变成可以提取特征点的大小(就是之前那张图,阈值EDGE_THRESHOLD=19
,相当于先把白色和绿色的部分一起去掉了,然后再加个3又得到了绿色的部分,也就是提取FAST需要用到的大小)。
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;
之后重新设置一下Cell的大小,之前的初始值是30,但可能导致最后一行或一列和30差得比较多,所以相当于把误差重新分配给了各个格子,这样得到了一个新的格子大小:
const int nCols = width/W;
const int nRows = height/W;
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);
以行开始遍历当前层得图像,这样就先只需要顾虑y方向得格子坐标:
//计算当前网格初始行坐标
const float iniY =minBorderY+i*hCell;
//目测一个图像网格的大小是25*25啊
float maxY = iniY+hCell+6;
//如果初始的行坐标就已经超过了有效的图像边界了,这里的“有效图像”是指原始的、可以提取FAST特征点的图像区域
if(iniY>=maxBorderY-3)
//那么就跳过这一行
continue;
//如果图像的大小导致不能够正好划分出来整齐的图像网格,那么就要委屈最后一行了
if(maxY>maxBorderY)
maxY = maxBorderY;
然后再对每一行进行列的遍历,在这个遍历里就完成了FAST的提取:
//计算初始的列坐标
const float iniX =minBorderX+j*wCell;
//计算这列网格的最大列坐标,+6的含义和前面相同
float maxX = iniX+wCell+6;
//判断坐标是否在图像中
// BUG 正确应该是maxBorderX-3,但是问题不大
// if(iniX>=maxBorderX-6)
if(iniX>=maxBorderX-3)
continue;
//如果最大坐标越界那么委屈一下
if(maxX>maxBorderX)
maxX = maxBorderX;
vector<cv::KeyPoint> vKeysCell;
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像,这里就是当前遍历到的图像块
vKeysCell, //存储角点位置的容器
iniThFAST, //检测阈值
true); //使能非极大值抑制
//如果这个图像块中使用默认的FAST检测阈值没有能够检测到角点
if(vKeysCell.empty())
{
//那么就使用更低的阈值来进行重新检测
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像
vKeysCell, //存储角点位置的容器
minThFAST, //更低的检测阈值
true); //使能非极大值抑制
}
//当图像cell中检测到FAST角点的时候执行下面的语句
if(!vKeysCell.empty())
{
//遍历其中的所有FAST角点
for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
{
//NOTICE 到目前为止,这些角点的坐标都是基于图像cell的,现在我们要先将其恢复到当前的【坐标边界】下的坐标
//这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标
//在后面将会被继续转换成为在当前图层的扩充图像坐标系下的坐标
(*vit).pt.x+=j*wCell;
(*vit).pt.y+=i*hCell;
//然后将其加入到”等待被分配“的特征点容器中
vToDistributeKeys.push_back(*vit);
}//遍历图像cell中的所有的提取出来的FAST角点,并且恢复其在整个金字塔当前层图像下的坐标
}//当图像cell中检测到FAST角点的时候执行下面的语句
这里也有一个恢复坐标的过程,因为我们用FAST得到的坐标是相对于网格Cell的,之后要恢复到基于当前图像的才可以。
之后就是对这些特征点进行一个四叉树的分配:
keypoints = DistributeOctTree(vToDistributeKeys, //当前图层提取出来的特征点,也即是等待剔除的特征点
//NOTICE 注意此时特征点所使用的坐标都是在“半径扩充图像”下的
minBorderX, maxBorderX, //当前图层图像的边界,而这里的坐标却都是在“边缘扩充图像”下的
minBorderY, maxBorderY,
mnFeaturesPerLevel[level], //希望保留下来的当前层图像的特征点个数
level); //当前层图像所在的图层
之后又是把分配保留下来的特征点坐标恢复到相对于之前图片白色边界情况下的坐标:
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的横坐标边界
四叉树分配DistributeOctTree
四叉树分配的大概流程是:
- 如果图片的宽度比较宽,就先把分成左右w/h份。一般的640×480的图像开始的时候只有一个node。
- 如果node里面的点数>1,把每个node分成四个node,如果node里面的特征点为空,就不要了,删掉。
- 新分的node的点数>1,就再分裂成4个node。如此,一直分裂。
- 终止条件为:node的总数量> N,或者无法再进行分裂。
- 然后从每个node里面选择一个质量最好的FAST点。
这里引用一张图片:[ORB-SLAM2] ORB-SLAM中的ORB特征(提取)
具体的代码实现里,先确定了初始节点的数目(源代码要是图像宽高比小于0.5的情况会报错)。然后先生成了一个初始提取器节点:
for(int i=0; i<nIni; i++)
{
//生成一个提取器节点
ExtractorNode ni;
//设置提取器节点的图像边界
//注意这里和提取FAST角点区域相同,都是“半径扩充图像”,特征点坐标从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());
//将刚才生成的提取节点添加到链表中
//虽然这里的ni是局部变量,但是由于这里的push_back()是拷贝参数的内容到一个新的对象中然后再添加到列表中
//所以当本函数退出之后这里的内存不会成为“野指针”
lNodes.push_back(ni);
//存储这个初始的提取器节点句柄
vpIniNodes[i] = &lNodes.back();
}
再把特征点分配给子提取器:
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
//获取这个特征点对象
const cv::KeyPoint &kp = vToDistributeKeys[i];
// NOTE 初始的提取器节点只会根据X轴分
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}
然后对节点进行遍历,把不可分的标记一下,没有特征点的节点直接删除:
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++;
}
之后声明了一个用于存储节点的vSize和句柄对:
// VAL vSizeAndPointerToNode 这个变量记录了在一次分裂循环中,那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
// NOTE 这里使用了pair类型,int对应特征点的数目,ExtractorNode对应的是节点类型
vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
再把初始化节点分成四个:
vSizeAndPointerToNode.reserve(lNodes.size()*4);
之后开始利用四叉树方法对图像划分区域,均匀分配特征点,里面用到了DivideNode
函数,用来把特征点分配给各个子节点:
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;
//再细分成四个子区域
// NOTE 这个函数将特征点也分配到各个子节点了
lit->DivideNode(n1,n2,n3,n4);
// Add childs if they contain points
//如果这里分出来的子区域中有特征点,那么就将这个子区域的节点添加到提取器节点的列表中
//注意这里的条件是,有特征点即可
if(n1.vKeys.size()>0)
{
//注意这里也是添加到列表前面的
// NOTE 添加到列表前面是因为每次都是把特征点数目最多的节点先分
lNodes.push_front(n1);
//再判断其中子提取器节点中的特征点数目是否大于1
if(n1.vKeys.size()>1)
{
//如果有超过一个的特征点,那么待展开的节点计数加1
nToExpand++;
//保存这个特征点数目和节点指针的信息
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
// HACK 这个访问用的句柄貌似并没有用到?
// 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;
}//判断当前遍历到的节点中是否有超过一个的特征点
}
循环停止的条件有两种:
- 当前节点数超过了要求的特征点数
- 当前所有节点都只包含一个特征点
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
{
//停止标志置位
bFinish = true;
}
如果再划分一次就满足总结点数大于N的情况,那就慢慢划分让其刚刚达到或者超过要求的特征点数目(这里没有太多不一样的,只是对节点的特征点数量也排序了一下,优先再分裂多的)
sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
最后保留的是每个节点里响应值最大的关键点,使用OpenCV里KeyPoint
自带的response
属性,对每个网格找到最大的就可以。
for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
{
//得到这个节点区域中的特征点容器句柄
vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;
//得到指向第一个特征点的指针,后面作为最大响应值对应的关键点
cv::KeyPoint* pKP = &vNodeKeys[0];
//用第1个关键点响应值初始化最大响应值
// NOTE 这里使用了opencv KeyPoint 类型自带的response
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);
}
分配特征点到子节点DivideNode
这个函数没什么好说的,就是把特征点的坐标判断一下,然后放在对应的节点里就行。
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 存储左上区域的边界
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());
//Associate points to childs
//遍历当前提取器节点的vkeys中存储的特征点
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);
}//遍历当前提取器节点的vkeys中存储的特征点
//判断每个子特征点提取器节点所在的图像中特征点的数目(就是分配给子节点的特征点数目),然后做标记
//这里判断是否数目等于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;
}
计算特征点的主方向IC_Angle
IC_Angle
函数是一个全局静态函数,实现它是通过另一个全局静态函数computeOrientation
实现的,这个主方向用到的方法是灰度质心法,一个通俗的理解是,我们把每一个像素都可以想象成有质量的小质量块,然后我们找到所有像素构成的大质量块的质心,再由这个的形心指向这个质心,就作为我们要求的主方向。对于每一个特征点,我们都是可以计算得到一个主方向的。
灰度质心法的具体计算方法如下:
1. 在一小块图像B中,定义图像的矩为
当然如果严格按照这个方法计算,计算过程会显得很慢,ORB-SLAM2中就利用圆自身的对称性来实现了一个加速:
像上边的图一样,我们每次处理的就是两行。
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
{
//图像的矩,前者是按照图像块的y坐标加权,后者是按照图像块的x坐标加权
int m_01 = 0, m_10 = 0;
//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
// Treat the center line differently, v=0
//这条v=0中心线的计算需要特殊对待
//后面是以中心行为对称轴,成对遍历行数,所以PATCH_SIZE必须是奇数
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
//注意这里的center下标u可以是负的!中心水平线上的像素按x坐标(也就是u坐标)加权
m_10 += u * center[u];
// Go line by line in the circular patch
//这里的step1表示这个图像一行包含的字节总数。参考[https://blog.csdn.net/qianqing13579/article/details/45318279]
int step = (int)image.step1();
//注意这里是以v=0中心线为对称轴,然后对称地每成对的两行之间进行遍历,这样处理加快了计算速度
for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
{
// Proceed over the two lines
//本来m_01应该是一列一列地计算的,但是由于对称以及坐标x,y正负的原因,可以一次计算两行
int v_sum = 0;
// 获取某行像素横坐标的最大范围,注意这里的图像块是圆形的!
int d = u_max[v];
//在坐标范围内挨个像素遍历,实际是一次遍历2个
// 假设每次处理的两个点坐标,中心线下方为(x,y),中心线上方为(x,-y)
// 对于某次待处理的两个点:m_10 = Σ x*I(x,y) = x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
// 对于某次待处理的两个点:m_01 = Σ y*I(x,y) = y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
for (int u = -d; u <= d; ++u)
{
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);
}
计算特征点描述子computeOrbDescriptor
实现具体的描述子计算过程的函数是computeOrbDescriptor
,它的本身就是一个全局静态函数,在具体的调用中也是通过一个全局静态函数computeDescriptors
来实现的,我个人理解这样的好处是因为计算描述子是对每个关键点都会操作的一个大循环,所以这样将循环和计算分离开来。
我们是得到过特征点的主方向的,所以这里是先将特征点周围的像素都进行了一个旋转,类似于下边图片这样:
之后就是用预定义好的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<uchar>(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(θ)
// 下面表示 y'* step + x'
// NOTE 这里要用step是旋转后的点也要用指针表示
#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);
// NOTE |=先进行数据合并,<<向左移
val |= (t0 < t1) << 1; //描述子本字节的bit1
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;
}
//为了避免和程序中的其他部分冲突在,在使用完成之后就取消这个宏定义
#undef GET_VALUE
总结
ORB-SLAM2中ORB特征提取实现的效果要比OpenCV更好,当然和OpenCV很多函数和类型的混合使用可能看着有些乱。把这个类拿出来直接进行一个使用是可行的,也可以和OpenCV的对比一下,是可以很明显看到这个区别的。这里推荐几篇博客,里面讲的都很好:
ORB-SLAM2代码实现之灰度质心法学习记录
ORB_SLAM2特征点提取策略及描述子的计算
零基础学习ORB-SLAM2特征点提取-从原理到源码【李哈哈】