一起学ORBSLAM（2）ORB特征点提取
转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125
ORBSLAM2的主要特点在于他的所有工程实现都用到了ORB这个特征点提取方法，包括他实现的在线单目，双目以RGBD的SLAM,下面我们就看看他是怎么一步一步一步一步一步一步一步一步一步一步实现ORB特征的提取的。
理论部分：
关于ORB特征点的实现方法以及其工作原理，网上资料很多，大家可以广泛阅读，当然也要筛选正确的信息，下面给出一个参考博客：http://www.cnblogs.com/ronny/p/4083537.html ，ORB特征自从2011年由Ethan Rublee,Vincent Rabaud,Kurt Konolige以及Gary R.Bradski在一篇名为“ORB：An Efficient Alternative to SIFT or SURF” (http://www.willowgarage.com/sites/default/files/orb_final.pdf)的文章中提出之后，逐渐引起了人们的注意，并逐渐熟悉了这一种方法。
ORB的原理概括为以下几个部分
1.特征点：参考FAST特征点的提取方式寻找角点，从而确定特征点的位置，并在其基础上用灰度质心法给特征点加上了方向，使其具有旋转不变性，而且引入了高斯金字塔，使其具有尺度不变性。
2.描述子：在特征点周围选择合适的Patch，并在该Patch内按一定的概率分布规律取128点对，用没对点进行比较大小，比如说点对(p,q)：如果I(p)>I(q)则用1代表这个点对，否则用0代表，依次形成128维的二进制向量来代表描述子。
（1）FAST角点提取
如图所示，以待检测像素为圆心，3为半径，做一个圆，与圆相交的共有16个像素，检测这16个像素中与中心点的像素差大于某个阈值T时n +1，，若这16个点检测完成时n>N（N一般取11或者9）则认为该待检测像素为角点，然后循环这个过程检测没一个像素。
通常情况下，还会进行预测试来加快检测步伐，即检测1,5,9,13这四个点，如果有其中3个与中心像素值的差大于阈值T，而进一步进行检查，否则认为该点不是角点，同时还有极大值检测的方式，防止角点扎堆现象。具体实现看上边推荐的博客，这里不展开讲了就。否则篇幅就过于冗杂了。
（2）给FAST角点添加尺度不变性
将原始图像做成高斯金字塔模型，即对原始图像用高斯卷积核进行高斯模糊处理，然后对其进行降采样，形成分辨率更小的图像，依次将原始图像分成若干个level，在各个level上进行FAST角点的提取，解决了FAST角点的尺度问题。高斯金字塔的建立过程请见下面的链接：
理论：http://blog.csdn.net/poem_qianmo/article/details/26157633
代码实现：http://amitapba.blog.163.com/blog/static/203610207201281992239/
（3）给FAST角点添加旋转不变性
借用高翔书中的一段话来解释一下怎么给FAST关键点加上方向
算出此方向后，在计算描述子时要将相应图像的坐标转换到以该方向为x轴上，使得描述子具有旋转不变性
（4）描述子的建立（参见 http://www.cnblogs.com/ronny/p/4081362.html）
选择合适的Patch，一般取15为半径的圆，当然这个得根据图像的分辨率进行改变，然后取该Patch内的若干个点对，一般去128个，也可以取256等。。。取法是根据某种概率分布随机取，通常有高斯分布等，形成128维的二进制向量，然后在进行匹配时就可以通过汉明距离进行匹配，快速便捷。
ORBSLAM2中代码的实现
在ORBSLAM2中是通过ORBextractor这个类来实现每张图片ORB特征的提取的，具体的代码在ORBextractor.cc和ORBextractor.h中进行实现的。下边一步步看一下他的头文件和源文件的内容。
class ORBextractor
{
public:
enum {HARRIS_SCORE=0, FAST_SCORE=1 };
//nfeatures ORB特征点数量 scaleFactor相邻层的放大倍数 nlevels层数 iniThFAST提取FAST角点时初始阈值 minThFAST提取FAST角点时更小的阈值
//设置两个阈值的原因是在FAST提取角点进行分块后有可能在某个块中在原始阈值情况下提取不到角点，使用更小的阈值在进一步提取
//构造函数
ORBextractor(int nfeatures, float scaleFactor, int nlevels,
int iniThFAST, int minThFAST);
~ORBextractor(){}
// Compute the ORB features and descriptors on an image.
// ORB are dispersed on the image using an octree.
// Mask is ignored in the current implementation.
//重载了()运算符，作为提取器的对外接口
void operator()( cv::InputArray image, cv::InputArray mask,
std::vector<cv::KeyPoint>& keypoints,
cv::OutputArray descriptors);
int inline GetLevels(){
return nlevels;}
float inline GetScaleFactor(){
return scaleFactor;}
std::vector<float> inline GetScaleFactors(){
return mvScaleFactor;
}
std::vector<float> inline GetInverseScaleFactors(){
return mvInvScaleFactor;
}
std::vector<float> inline GetScaleSigmaSquares(){
return mvLevelSigma2;
}
std::vector<float> inline GetInverseScaleSigmaSquares(){
return mvInvLevelSigma2;
}
//图像金字塔 存放各层的图片
std::vector<cv::Mat> mvImagePyramid;
protected:
//计算高斯金字塔
void ComputePyramid(cv::Mat image);
//计算关键点并用四叉树进行存储
void ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
//为关键点分配四叉树
std::vector<cv::KeyPoint> DistributeOctTree(const std::vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level);
void ComputeKeyPointsOld(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
//存储关键点附近patch的点对
std::vector<cv::Point> pattern;
//提取特征点的最大数量
int nfeatures;
//每层之间的缩放比例
double scaleFactor;
//高斯金字塔的层数
int nlevels;
//iniThFAST提取FAST角点时初始阈值
int iniThFAST;
//minThFAST提取FAST角点时更小的阈值
int minThFAST;
//每层的特征数量
std::vector<int> mnFeaturesPerLevel;
//Patch圆的最大坐标
std::vector<int> umax;
//每层的相对于原始图像的缩放比例
std::vector<float> mvScaleFactor;
//每层的相对于原始图像的缩放比例的倒数
std::vector<float> mvInvScaleFactor;
std::vector<float> mvLevelSigma2;
std::vector<float> mvInvLevelSigma2;
};该类实现了ORB特征点的提取，外部调用时只需要调用ORB的构造函数ORBextractor(int nfeatures, float scaleFactor, int nlevels,
int iniThFAST, int minThFAST);和外部接口函数void operator()( cv::InputArray image, cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,cv::OutputArray descriptors);具体的各个参数的含义在代码注释中已经给出，这里不再罗嗦了。
下面看各个成员函数是如何实现的：
构造函数：
ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
int _iniThFAST, int _minThFAST):
nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
iniThFAST(_iniThFAST), minThFAST(_minThFAST)
{
//计算每一层相对于原始图片的放大倍数
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;
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);
mnFeaturesPerLevel.resize(nlevels);
float factor = 1.0f / scaleFactor; //scaleFactor：1.2 nfeatures：1000 nlevels：8
//为什么第一层的特征点数量为这些，这是由于
// 第一层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels))
// 第二层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels))*q
// 第三层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels))*q*q
// .........
// 第nlevels层：nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels))*q^nlevels
// 那么前nlevels层的和为总的特征点数量nfeatures（等比数列的前n相和）
// 可以看出ORB特征点是如何实现尺度不变性的，原始图像那一层特征点最多，依次递减
// 主要是将每层的特征点数量进行均匀控制
float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels)); //计算第一层特征点的数量
int sumFeatures = 0;
for( int level = 0; level < nlevels-1; level++ )
{
mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale); //mnFeaturesPerLevel数组存储每层特征点的数量
sumFeatures += mnFeaturesPerLevel[level];
nDesiredFeaturesPerScale *= factor;
}
mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
//复制训练的模板
const int npoints = 512;
const Point* pattern0 = (const Point*)bit_pattern_31_;
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
//This is for orientation 用于计算特征点方向
// pre-compute the end of a row in a circular patch
//用于计算特征方向时，每个v坐标对应最大的u坐标
umax.resize(HALF_PATCH_SIZE + 1);
// 将v坐标划分为两部分进行计算，主要为了确保计算特征主方向的时候，x,y方向对称
int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); //cvFloor含义是取不大于参数的最大整数值
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2); //cvCeil含义是取不小于参数的最小整数值
//利用勾股定理计算
const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
//V坐标的第一部分
for (v = 0; v <= vmax; ++v)
umax[v] = cvRound(sqrt(hp2 - v * v));
// Make sure we are symmetric 确保对称，即保证是一个圆
//V坐标的第二部分
for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
{
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}
}此函数为构造函数，主要就是将一些外部参数赋值到该类的成员变量，包括特征点的数量nfeatures，高斯金字塔每层之间的缩放比例scaleFactor，高斯金字塔的层数：nlevels，FAST角点提取时的阈值：iniThFAST，minThFAST，以及将各层相对于原图像的缩放比例存放到mvScaleFactor等等，分配各层图像应取的特征点数量，保证每层的特征点数量是均匀的，用到等比数列进行分配。之后又对Patch进行了处理，这个Patch是用来计算FAST特征点方向时使用。
上边简单话了一下PATCH的形状就是一个圆，代码中umax存储的是u坐标绝对值的最大值。
void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors)
{
if(_image.empty())
return;
Mat image = _image.getMat();
assert(image.type() == CV_8UC1 );
// Pre-compute the scale pyramid 构建高斯金字塔
ComputePyramid(image);
//计算关键点并生成四叉树
vector < vector<KeyPoint> > allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);
//ComputeKeyPointsOld(allKeypoints);
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);
descriptors = _descriptors.getMat();
}
_keypoints.clear();
_keypoints.reserve(nkeypoints);
//计算每个关键点对应的描述子
int offset = 0;
for (int level = 0; level < nlevels; ++level)
{
vector<KeyPoint>& keypoints = allKeypoints[level];
int nkeypointsLevel = (int)keypoints.size();
if(nkeypointsLevel==0)
continue;
// preprocess the resized image
Mat workingMat = mvImagePyramid[level].clone();
//进行高斯模糊，用BORDER_REFLECT_101方法处理边缘
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
// Compute the descriptors
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel); //offset是偏移量，此处取出的是该层的描述子，
//计算描述子
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.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
}
}这一部分是用来提取ORB特征的外部接口函数，_image是输入图像，_keypoints为输出的关键点，_descriptors是对应的描述子，实现了从构建高斯金字塔到计算高斯金字塔中关键点的位置之后进行关键点描述子的计算。返回了关键点和描述子。
//构建高斯金字塔 修改过，感觉源代码中有好多不必要的的东西
void ORBextractor::ComputePyramid(cv::Mat image)
{
for (int level = 0; level < nlevels; ++level)
{
float scale = mvInvScaleFactor[level];
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
// Compute the resized image
if( level != 0 )
{
resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
}
else
{
//此函数将图像转化为更大的图像，并用BORDER_REFLECT_101的方式来处理边缘图像
mvImagePyramid[level] = image;
}
}
}构建高斯金字塔，此函数被我删减过，感觉有许多冗余的代码不需要。该函数直接利用了resize这个函数来构建高斯金字塔中的各层图像，并将图像存储在mvImagePyramid变量中。
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{
allKeypoints.resize(nlevels);
//设定每个格子的大小
const float W = 30;
//每层提想分别提取特征点
for (int level = 0; level < nlevels; ++level)
{
//设定该层图像中检测的X，Y最大最小坐标
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;
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);
// 在每个格子内进行fast特征检测
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;
vector<cv::KeyPoint> vKeysCell;
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);
}
//如果检测不为空就将检测到的特征点放到vToDistributeKeys中
if(!vKeysCell.empty())
{
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);
}
}
}
}
vector<KeyPoint> & keypoints = allKeypoints[level];
keypoints.reserve(nfeatures);
keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
//计算特征点Patch的大小，根据每层的尺度的不同而不同
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; //特征点所在的层数
keypoints[i].size = scaledPatchSize;//特征点Patch的大小将来计算描述子时使用
}
}
// compute orientations 计算特征点的方向
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}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));
}static void computeOrbDescriptor(const KeyPoint& kpt,
const Mat& img, const Point* pattern,
uchar* desc)
{
//取关键点的方向角度并进一步转化为弧度
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;
//使得该关键点具有旋转不变性
#define GET_VALUE(idx) \
center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
cvRound(pattern[idx].x*a - pattern[idx].y*b)] 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;
}
#undef GET_VALUE
}static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
{
int m_01 = 0, m_10 = 0;
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
// Treat the center line differently, v=0 v=0这一行是特殊的，需要单独计算
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
m_10 += u * center[u];//m_10=SUM(u*I(u,v)) m_01=SUM(v*I(u,v))
// Go line by line in the circuI853lar patch
int step = (int)image.step1();
for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
{
// Proceed over the two lines
int v_sum = 0;
int d = u_max[v];
for (int u = -d; u <= d; ++u)
{
int val_plus = center[u + v*step], val_minus = center[u - v*step];
v_sum += (val_plus - val_minus); //计算上下的时候是有符号的，所以这边是减
m_10 += u * (val_plus + val_minus);//将(u,v)和(u,-v)两个点的像素一起计算 这边加是由于u已经确定好了符号
}
m_01 += v * v_sum; //将x=v这条直线上所有的坐标点的像素值求和在进行计算
}
return fastAtan2((float)m_01, (float)m_10);
}
这三个函数都是进行描述子的计算的，其中static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)这个函数使用来计算关键点方向的，其主要原理是：灰度质心法。static void computeOrbDescriptor(const KeyPoint& kpt, const Mat& img, const Point* pattern, uchar* desc)是为了使该描述子具有旋转不变性，将主方向加进描述子中，使得相应的点坐标进行坐标变换。
好了ORB的特征计算就是这个样子了。到这里这篇博客也就结束了。有些乱，讲究这看吧，之后我会把我整理的有中文注释版的ORBSLAM2源代码上传，大家可以下载，只是现在还不行，因为还有部分代码我没有完全弄清楚。等我研究清楚了在和大家分享喽！下期见！
