ORB特征点
ORB-SLAM的整个地图重建过程都是围绕着ORB特征点进行的。实际上伴随着机器视觉的发展，人们开发出了各种各样的局部特征点，比如SIFT、SURF、ORB等。作者之所以选择ORB，估计还是因为它快， 消耗资源少。根据ORB特征点的论文中的说法， ORB特征点具有较好的旋转不变性和抗噪声能力，关键是它很快，实验表明它要比SIFT算法快上两个数量级，而且在很多情形下都能取得近似SIFT算法的效果。
1. ORB特征点
ORB 特征点的全称是 Oriented FAST and Rotated BRIEF，是一种局部的特征点。很多人都说它是由 FAST 角点和 BRIEF 描述子两部分构成。 其实从全称上的定语可以看出 ORB 并不是简单的 FAST + BRIEF。
原始的 FAST 角点是缺少方向的。右侧是我从 OpenCV 官方文档中拷过来的图。 它大体讲述了 FAST 角点的计算过程。对于给定的一个像素，假设其像素值为 \(I_p\)。FAST 会选择一个阈值 \(t\)，然后在该像素周围的 16 个像素中按照顺时针或者逆时针的方向数数。 如果有连续 \(n\) 个像素值大于 \(I_p + t\) 或者小于 \(I_p - t\)，FAST 就认为这个点是一个角点。通常 \(n\) 被选择为 12。从算法的原理上来讲，FAST 压根儿就没有考虑过方向的问题， 它只是找到一个角点而已。
OpenCV 也对 BRIEF 描述子进行了简单的介绍，其大致计算过程如下。 以目标像素为中心，在其周围 \(S \times S\) 的窗口内选择 \(n\) 个点对。对于每个点对的两个像素值，如果 \(I_x < I_y\)，则将之记为 1 否则为 0。 如此就得到了一个长度为 \(n\) 的二值数组，也就是所谓的 BRIEF 描述子。显然点对的选择不同，最后得到的描述子也是不一样的。 在 ORB-SLAM2 中，这个窗口大小为 \(S = 31\)，点对数为 \(n = 256\)，正好可以用 32 个字节来记录。 它还有一个 \(256 \times 4\) 的大数组 bit_pattern_31_ 预制了一个选点模式。 对于 BRIEF 这种二进制的描述子，我们可以通过汉明距离(Hamming Distance)，即不同位数来判定两个特征点的相似程度。显然 BRIEF 也不考虑旋转的问题。
FAST + BRIEF 的思想很简单，所以它们也很快，但是缺乏方向信息，如果图像发生了旋转，很容易就跟丢了。缺啥补啥，ORB 在二者的基础上增加了特征点方向的估计， 如此得到的特征点和描述子就有了较好的旋转不变性。
ORB 的思想也很直接。它在计算 FAST 角点的同时计算一个方向， 从而得到 Oriented FAST角点。再根据该方向，对选点模板进行旋转，就能得到 Rotated BRIEF 描述子。 为了计算 FAST 角点的方向，ORB 在一个以目标点 \(O\) 为圆心半径为 \(r\) 的圆形窗口中计算像素质心 \(C\)。 那么向量\(\vec{OC}\) 的方向就是角点的方向。下面我们来结合源码看看 ORB-SLAM 是如何计算 ORB 特征点的。
2. ORBextractor 构造函数
在解读轨迹跟踪器的时候， 我们就曾见过 ORB_SLAM2::Tracking 中定义了三个 ORBextractor 的对象用来提取 ORB 特征点。 下面是其构造函数的代码片段。我们需要指定特征点数量、金字塔配置、角点阈值来构造一个 ORBextractor 对象。
ORBextractor::ORBextractor(int _nfeatures, // 要提取的特征点数量
float _scaleFactor, // 金字塔比例系数
int _nlevelsn, // 金字塔层数
int _iniThFAST, // 初始 FAST 角点阈值
int _minThFAST) // 若以初始阈值无法找到足够的特征点，就降低检测阈值
: nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
iniThFAST(_iniThFAST), minThFAST(_minThFAST) {
图像金字塔是提供尺度不变性的一种常用手段。ORB-SLAM2 的默认配置中，以 1.2 的缩放比例构建了一个 8 层的图像金字塔，然后再每层图像上计算 FAST 角点。 在下面的代码片段中，计算了金字塔中各层图像的配置。其中成员数组 mvScalFactor 和 mvlevelsigma2 中分别记录的是各层图像相对于初始图像的尺度因子以及因子的平方。 mvInvScaleFactor 和 mvInvLevelSigma2 则是尺度因子和因子平方的倒数，可以理解为缩放比例。
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); // 图像金字塔数组, 一层对应一个元素
根据尺度不同，将 nfeatures 个特征点名额分配给每层金字塔，由成员数组 mnFeaturesPerLevel 记录各层需要检出的特征点数量。
mnFeaturesPerLevel.resize(nlevels);
float factor = 1.0f / scaleFactor; // 特征点数量, 按照缩放比例, 逐层递减
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);
sumFeatures += mnFeaturesPerLevel[level];
nDesiredFeaturesPerScale *= factor;
}
// 上述分配可能因为取整操作剩下几个特征点，都丢给顶层图像
mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
ORB-SLAM2 在一个 \(31 \times 31\) 的窗口中选点计算一个 256 位的 BRIEF 描述子，因此计算一个描述子需要 512 个采样点。 作者将选点模式硬编码在了一个长度为 \(256 \times 4\) 的整型大数组里面 bit_pattern_31_，256 表示点对数量，4 是指每个点对需要四个整型数据记录点对相对于窗口中心的像素坐标。 在下面的代码中，将选点模板拷贝到成员 pattern 中。
const int npoints = 512;
const Point* pattern0 = (const Point*)bit_pattern_31_;
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
接下来，为了方便计算特征点的方向信息，ORBextractor 预先计算了一个圆形窗口的采样模版，将每行的窗口边界记录在成员数组 umax 中。 在下面的代码片段中，它只计算了 1/4 的圆形窗口，如下面右图所示。
umax.resize(HALF_PATCH_SIZE + 1);
int v, v0, 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;
for (v = 0; v <= vmax; ++v)
umax[v] = cvRound(sqrt(hp2 - v * v));
// Make sure we are symmetric
for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v) {
while (umax[v0] == umax[v0 + 1]) ++v0;
umax[v] = v0;
++v0;
} }
3. 特征点计算总体过程
ORBextractor 重载了括号操作符来计算一帧图像的 ORB 特征点。如下面的代码片段所示，它的输入是要处理的图像 _image 和图像掩码 _mask掩码其实没有用， 输出的就是特征点坐标 _keypoints 和描述子 _descriptors。
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 );
ComputePyramid(image);
vector<vector<KeyPoint>> allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);
总体的计算过程无非是计算金字塔 → 计算各层图像的 FAST 角点 → 计算各个角点的描述子。
左侧的代码片段先检查输入图像是否为空以及数据类型。接着调用成员函数 ComputePyramid 计算图像金字塔，保存在成员变量 mvImagePyramid 中。 然后通过函数 ComputeKeyPointsOctTree 完成各层的关键点计算，其传参 allKeypoints 是一个2维的数组，对应着金字塔的每一层，分别记录了各层检出的关键点。
接下来根据刚才计算的所有特征点数量，清理输出对象 _keypoints 和 _descriptors 的内存，为它们分配足够大的空间。
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);
然后在一个 for 循环中逐层遍历金字塔，计算其中检出的各个特征点的描述子，并根据金字塔的尺度因子，将特征点的像素坐标投射到原始图像上。 下面的代码片段先获取了各图层检出的关键点及其数量。局部变量 offset 用于标记各图层特征点的偏移量。
int offset = 0;
for (int level = 0; level < nlevels; ++level) {
vector<KeyPoint>& keypoints = allKeypoints[level]; // 获取各图层检出的关键点
int nkeypointsLevel = (int)keypoints.size();
if(nkeypointsLevel==0) continue;
为了提升系统的抗噪能力，这里根据原始 ORB 的设计，先用 OpenCV 的接口对金字塔图层进行了高斯滤波，然后调用成员函数 computeDescriptors 在滤波后的图像上计算描述子。
Mat workingMat = mvImagePyramid[level].clone();
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
computeDescriptors(workingMat, keypoints, desc, pattern);
offset += nkeypointsLevel;
在 for 循环的最后将金字塔高层图像检出的特征点坐标，根据尺度因子，投射到原始图像上。
if (level != 0) {
float scale = mvScaleFactor[level];
for (auto keypoint = keypoints.begin(); keypoint != keypoints.end(); ++keypoint)
keypoint->pt *= scale;
}
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
} }
4. 计算 FAST 关键点
从上面的总体过程中，我们看到计算金字塔的关键点是在成员函数 ComputeKeyPointsOctTree 中完成的。该函数也是调用 OpenCV 的 FAST 接口来计算关键点的， 值得我们关注的是它的计算方式。如果所有的关键点都集中在一些局部的区域，将不利于相机的位姿估计。 所以 ORB-SLAM2 希望提取的关键点能够尽量在整幅图像上均匀分布。函数 ComputeKeyPointsOctTree 对金字塔的每层图像都进行了删格化， 将每个图层划分成若干个 \(30 \times 30\) 的小块。然后在每个小块中提取 FAST 角点。
下面是该函数的代码片段，它只有一个参数 allKeypoints，用于输出从图像金字塔中提取的关键点。函数先给 allKeypoints 分配了内存，同时定义了局部常量 W 来设置栅格化的图像小块尺寸。
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint>>& allKeypoints) {
allKeypoints.resize(nlevels);
const float W = 30;
然后在一个 for 循环中为遍历金字塔的每个图层。首先整理图层的边界，保证提取的关键点周围有足够的像素，可以进行后续的描述子计算。 然后在此边界的基础上计算图层实际的有效尺寸，并计算划分的栅格数量，以及各栅格的实际尺寸。同时定义了一个局部的容器 vToDistributeKeys 来保存该图层提取出的 FAST 角点。 作者特意分配了一个很大的空间，来采集尽可能多的关键点。后面会看到调用函数 DistributeOctTree 对关键点进行筛选。
在下面的第25行代码片段中，我们看到 ComputeKeyPointsOctTree 直接调用了 OpenCV 的 FAST 接口计算各个栅格单元的角点。如果以 iniThFAST 的阈值不能提取出角点， 就降低标准，以 minThFAST 的阈值再次提取角点。如果成功提取出角点，得到的像素坐标是相对于栅格左上角的，还需要将其转换到图层坐标系下，再添加到容器 vToDistributeKeys 中备选。
for (int level = 0; level < nlevels; ++level) {
const int minBorderX = EDGE_THRESHOLD-3; // 计算图像的边界
const int minBorderY = minBorderX; // 常量 EDGE_THRESHOLD 的值为 19
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
// 实际有效的图层尺寸
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);
vector<cv::KeyPoint> vToDistributeKeys;
vToDistributeKeys.reserve(nfeatures*10);
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);
// 若真提取到关键点
if (!vKeysCell.empty()) {
for(auto vit = vKeysCell.begin(); vit != vKeysCell.end(); vit++) {
(*vit).pt.x+=j*wCell; (*vit).pt.y+=i*hCell; // 转换到图层坐标系下
vToDistributeKeys.push_back(*vit);
}
} } }
对于每个图层，遍历了所有栅格单元提取出关键点之后，还需要做两件事情。其一，如下面第 42 行所示，调用函数 DistributeOctTree 对关键点进行筛选只保留分配给图层的数量个关键点。 其二，如44到50行所示，根据图层的尺度因子，将关键点坐标从图层坐标转换到原始图像的像素坐标上，并保存下来。
auto & keypoints = allKeypoints[level];
keypoints.reserve(nfeatures);
keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
// 记录筛选后的关键点点
const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
for (int i = 0; i < nkps ; i++) {
keypoints[i].pt.x+=minBorderX;
keypoints[i].pt.y+=minBorderY;
keypoints[i].octave=level;
keypoints[i].size = scaledPatchSize;
} }
最后遍历所有提取出来的关键点，调用函数 computeOrientation 计算其方向。
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
5. 关键点的方向和 BRIEF 描述子
函数 computeOrientation 用于计算关键点的方向，该函数的内部也就是在一个 for 循环中遍历金字塔图层中的所有关键点，调用函数 IC_Angle 通过灰度质心来估计关键点的方向。 下面是该函数的代码片段，它有三个参数，分别是金字塔图层，目标关键点，和构造函数中预先计算的圆形窗口的采样模版。 一开始定义了局部变量 m_01 和 m_10 分别记录灰度质心的 y 坐标和 x 坐标。center 则是目标关键点的像素值地址。
static float IC_Angle(const Mat & image, Point2f pt, const vector<int> & u_max) {
int m_01 = 0; // 灰度质心的 y 坐标
int m_10 = 0; // 灰度质心的 x 坐标
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
然后特别关照一下圆形窗口的水平中心线。这里的 center[u] 与 *(center + u) 等价，表示获取水平偏移量为 u 的像素值。
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
m_10 += u * center[u];
接着在两层 for 循环中，根据采样模版，遍历圆形窗口中其它各点的元素，计算 y 和 x 轴的像素加权和。由于圆形的对称特性，这里同时处理了两行像素。 第6行中调用 Mat 对象 step1 得到的是每行数据的字节数。
int step = (int)image.step1();
for (int v = 1; v <= HALF_PATCH_SIZE; ++v) {
int v_sum = 0; // process two lines
int d = u_max[v]; // 对称的处理两行像素
// 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_sum += (val_plus - val_minus);
m_10 += u * (val_plus + val_minus);
}
m_01 += v * v_sum;
}
return fastAtan2((float)m_01, (float)m_10);
}
最后，如上面第 19 行所示，函数 IC_Angle 调用了 OpenCV 的接口 fastAtan2 计算得到目标关键点的方向角度值，精度为 0.3°。完成方向角的估计之后，就可以对计算 BRIEF 描述子选点模板进行旋转， 得到 Rotated BRIEF 描述子了。函数 computeOrbDescriptor 具体完成了这项任务。
下面是函数 computeOrbDescriptor 的代码片段，它以参数 kpt, img, 和 pattern 作为输入，接收目标关键点坐标、图像、选点模板。以参数 desc 作为输出，返回特征点的 256 位描述子。 与函数 IC_Angle 类似，一开始直接获取了特征点像素值的地址 cener 以及图像每行所占用的字节数 step。
static void computeOrbDescriptor(const KeyPoint & kpt, // 目标关键点坐标
const Mat & img, // 提取特征点的图像
const Point* pattern, // 预定义的选点模板
uchar* desc) { // 输出的 BRIEF 描述子
const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
const int step = (int)img.step;
下面根据刚刚计算得到的特征点方向角度，计算了其余弦和正弦值。然后在一个宏定义 GET_VALUE 中，先将对比点对坐标进行旋转，再通过指针 center 和 step 获取对应点的像素值。
float angle = (float)kpt.angle*factorPI;
float a = (float)cos(angle), b = (float)sin(angle);
#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 循环中，遍历所有的采样点对，进行比较得到描述子 desc。
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;
} }