ORB_SLAM3源码阅读笔记
Posted Mortal
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了ORB_SLAM3源码阅读笔记相关的知识，希望对你有一定的参考价值。
LoopClosing 线程
1 LoopClosing 线程的创建
LoopClsing 线程的创建与启动和LocalMapping 线程一样，该线程的核心也在于Run（）函数，以下对LoopClosing 线程进行逐步的分析。
- 创建LoopClosing 对象
mpLoopCloser
mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC);
LoopClosing构造函数中完成各种初始化，基本上就是对各种vector类型的存储容器进行初始化（清空），这一部分没有太多需要进行说明的。
- 启动LoopClosing 线程
同样的，LoopClosing 线程的关键在于其Run()函数，下面开始对Run()函数进行分析。
2 Run 函数
进入Run()函数首先将完成标志位置 false,然后映入眼帘的就又是“死循环”了。重点也就是Run()函数中的“死循环”部分。现在进入循环中对代码的功能进行分析。
CheckNewKeyFrames()函数，回环检测所有的主要功能都是在这个函数返回为真的情况下进行的，这个函数的主要功能就是检查mlpLookKeyFrameQueue列表中是否存在待处理的关键帧，这里我们先将没有关键帧的情况进行分析，队列中没有关键帧那么就会进入下一条函数ResetIfRequested()，这个函数其实就是一个重置函数，将队列中的所有关键帧都清除，相当于重新开始建图和定位（当然前提是重置请求已经收到）如果是收到地图激活那么就会从当前开始利用已有的关键帧建立起局部的小地图，这也是ORB_SLAM3算法地图集的强大之处。最后通过检查全局标志位来跳出循环，完成回环检测。备注：关键帧的数据来自于定位建图线程- 在进入下一个函数前这里进行了一个判断：
if(mpLastCurrentKF)
mpLastCurrentKF->mvpLoopCandKFs.clear();
mpLastCurrentKF->mvpMergeCandKF.clear();
这一部分的功能暂时还没有完完全全的理解透彻（虽然知道是对存储的数据进行清空），所以暂时先不分析，后面在整体的细节分析环节在进行分析描述。直接进入下一个函数。
NewDetectCommonRegions()函数，进入函数开始进行回环检测，首先取出mlpLoopKeyFrameQueue队列中的第一个关键帧作为当前关键帧，接着将当前帧的mbNotErase标志位设置为真，用于防止此关键帧在别的线程中被删除，并将当前帧的回环检测标志位置为真，且获得当前关键帧的地图。后面的几个连续性的判断分别对应着不同情况下所执行的操作：
- IMU模式下：如果在IMU模式下还没有进行第二次初始化则不进行回环检测，并且添加关键帧到关键帧数据库。
- 双目模式下：如果当前地图的关键帧5，则不进行回环检测并添加当前关键帧到关键帧数据库。
- 若当前地图的关键帧数量小于12则不进行回环检测，并将当前关键帧添加至关键帧数据库。
关键的部分即将开始，首先需要知道mnLoopNumCoincidences表示的是回环成功的次数，初始化时该变量初始化为0。DetectAndReffineSim3FromLastKF()函数，该函数简单来说就是对当前的关键帧进行匹配判断（暂时不做更多深入的说明）。- 融合。
- 若当前关键帧没有被检测到回环或融合，则寻找当前关键帧的三个回环候选帧和融合候选帧。
- 若当前关键帧没有检测到回环且回环候选帧不为空，那么就对候选回环帧再次进行检测，判断是否发生回环。
- 若当前帧没有被检测到融合，且融合选帧不为空，那么就对融合候选帧再次进行检测，并判断是否发生融合。
- 最后，当检测到回环或者融合的时候返回true。
当NewDetectCommonRegions()函数返回值为真，即检测到回环或者融合的时候开始进行后面的操作，后续的操作主要分为检测到融合和检测到回环两种情况，这里进行分开讨论。
mbMergeDetected == true，也就是检测到融合的情况。此时进入该部分的处理，首先对IMU是否已经进行了初始化进行判断，如果没有初始化则中断，否则开始继续进行处理。假设已经进行了初始化，此时分别获取到当前关键帧的位姿和融合匹配帧的位姿，最后经过一系列处理得到一个最终的四元数，这里需要注意的是MergeLocal2()函数和MergeLocal()函数。mbLoopDetected==true，也就是检测到回环，此时就会进行回环矫正。这里的矫正其实就是通过阈值判断与变换矩阵参数的修改实现的。
最终完成整个回环检测与优化的过程。
ORB_SLAM2 源码解析 ORB特征提取
目录
1、定义一个枚举类型用于表示使用HARRIS响应值还是使用FAST响应值
二、计算特征点的方向 computeOrientation()
5、计算特征点的方向（computeOrientation）
五、金字塔的计算（ORBextractor::ComputePyramid）
6.3、四叉树筛选特征点: DistributeOctTree()
一、各成员函数变量
在阅读代码之前我们先来介绍变量的命名规则
1、定义一个枚举类型用于表示使用HARRIS响应值还是使用FAST响应值
nfeatures | 指定要提取出来的特征点数目 |
scaleFactor | 图像金字塔的缩放系数 |
nlevels | 指定需要提取特征点的图像金字塔层 |
| iniThFAST | 初始的默认FAST响应值阈值 |
minThFAST | 较小的FAST响应值阈值 |
2、内联函数都是用来直接获取类的成员变量的
GetScaleFactor() | 获取当前提取器所在的图像的缩放因子 |
mvScaleFactor | 图像金字塔中每个图层相对于底层图像的缩放因子 |
GetInverseScaleFactors() | 获取上面的那个缩放因子s的倒数 |
GetScaleSigmaSquares() | 获取sigma^2，就是每层图像相对于初始图像缩放因子的平方 |
GetInverseScaleSigmaSquares() | 获取上面sigma平方的倒数 |
mvImagePyramid | 用来存储图像金字塔的变量，一个元素存储一层图像 |
3、保护成员
保护成员就是私有的别人不可以调用
ComputePyramid | 计算其图像金字塔 |
ComputeKeyPointsOctTree | 以八叉树分配特征点的方式，计算图像金字塔中的特征点 |
vToDistributeKeys | 等待分配的特征点 |
mnFeaturesPerLevel | 分配到每层图像中，要提取的特征点数目 |
umax | 计算特征点方向的时候，有个圆形的图像区域，这个vector中存储了每行u轴的边界（四分之一，其他部分通过对称获得） |
二、计算特征点的方向 computeOrientation()
计算特征点的方向是为了使得提取的特征点具有旋转不变性
方法是灰度质心法：以几何中心和灰度质心的连线作为该特征点方向
2.1、灰度质心法算法步骤
1、计算一个半径为15的近似圆
后面计算的是特征点主方向上的描述子,计算过程中要将特征点周围像素旋转到主方向上,因此计算一个半径为16的圆的近似坐标,用于后面计算描述子时进行旋转操作.
PATCH_SIZE | 图像块的大小,或者说是直径 | 31 |
HALF_PATCH_SIZE | 上面这个大小的一半，或者说是半径 | 15 |
EDGE_THRESHOLD | 算法生成的图像边 | 19 |
u_max | 图像块的每一行的坐标边界 | |
| float | 返回特征点的角度，范围为[0,360)角度，精度为0.3° |
int vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); // 45°射线与圆周交点的纵坐标
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2); // 45°射线与圆周交点的纵坐标
// 先计算下半45度的umax
for (int v = 0; v <= vmax; ++v) {
umax[v] = cvRound(sqrt(15 * 15 - v * v));
}
// 根据对称性补出上半45度的umax
for (int v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v) {
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}
2、计算特征点角度
点v 绕 原点旋转θ 角，得到点v’，假设 v点的坐标是(x, y) ，那么可以推导得到 v’点的坐标（x’, y’)
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;
// 旋转公式
// x'= xcos(θ) - ysin(θ)
// y'= xsin(θ) + ycos(θ)
#define GET_VALUE(idx) \\
center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + cvRound(pattern[idx].x*a - pattern[idx].y*b)] 3、IC_Angle 计算技巧
在一个圆域中算出m10（x坐标）和m01（y坐标），计算步骤是先算出中间红线的m10，然后在平行于 x轴算出m10和m01，一次计算相当于图像中的同个颜色的两个line
4、灰度质心法计算公式
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)
{
//得到需要进行加运算和减运算的像素灰度值
//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);
}
///乘数因子，一度对应着多少弧度
const float factorPI = (float)(CV_PI/180.f);
5、计算特征点的方向（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 = IC_Angle(image, //特征点所在的图层的图像
keypoint->pt, //特征点在这张图像中的坐标
umax); //每个特征点所在图像区块的每行的边界 u_max 组成的vector
}
}三、FAST描述子
BRIEF算法的核心思想是在关键点P的周围以一定模式选取N个点对，把这N个点对的比较结果组合起来作为描述子。
BRIEF描述子生成步骤
1.以关键点P为圆心，以d为半径做圆O。
2.在圆O内某一模式选取N个点对。这里为方便说明，N=4，实际应用中N可以取512.
假设当前选取的4个点对如上图所示分别标记为：
3.定义操作T
4.分别对已选取的点对进行T操作，将得到的结果进行组合。
假如：
原始的BRIEF描述子没有方向不变性，通过加入关键点的方向来计算描述子，称之为Steer BRIEF，具有较好旋转不变特性
具体地，在计算的时候需要将这里选取的采样模板中点的x轴方向旋转到特征点的方向。
获得采样点中某个idx所对应的点的灰度值,这里旋转前坐标为(x,y), 旋转后坐标(x',y')，他们的变换关系:
x'= xcos(θ) - ysin(θ), y'= xsin(θ) + ycos(θ)
下面表示 y'* step + x'
#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
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
}五、金字塔的计算（ORBextractor::ComputePyramid）
金字塔是为了实现尺度不变性
具体实现方式如上图所示，当摄像机靠近图像，特征点变大，能提取的特征点变少；当摄像机远离图像时特征点变小，能提取到的特征点变多。我们可以观察到摄像机在正常位置时，第0层的特征点与摄像机往前移动第1层的特征点差不多大，利用这个特性我们可以实现尺度不变性。
iniThFAST | 指定初始的FAST特征点提取参数，可以提取出最明显的角点 |
minThFAST | 如果初始阈值没有检测到角点，降低到这个阈值提取出弱一点的角点 |
ORBextractor::ORBextractor(int _nfeatures, //指定要提取的特征点数目
float _scaleFactor, //指定图像金字塔的缩放系数
int _nlevels, //指定图像金字塔的层数
int _iniThFAST, //指定初始的FAST特征点提取参数，可以提取出最明显的角点
int _minThFAST): //如果初始阈值没有检测到角点，降低到这个阈值提取出弱一点的角点
nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
iniThFAST(_iniThFAST), minThFAST(_minThFAST)//设置这些参数
{
//存储每层图像缩放系数的vector调整为符合图层数目的大小
mvScaleFactor.resize(nlevels);
//存储这个sigma^2，其实就是每层图像相对初始图像缩放因子的平方
mvLevelSigma2.resize(nlevels);
//对于初始图像，这两个参数都是1
mvScaleFactor[0]=1.0f;
mvLevelSigma2[0]=1.0f;函数void ORBextractor::ComputePyramid(cv::Mat image)逐层计算图像金字塔,对于每层图像进行以下两步:
1、先进行图片缩放,缩放到mvInvScaleFactor对应尺寸.
2、在图像外补一圈厚度为19的padding(提取FAST特征点需要特征点周围半径为3的圆域,计算ORB描述子需要特征点周围半径为16的圆域).
下图表示图像金字塔每层结构:
深灰色为缩放后的原始图像.
包含绿色边界在内的矩形用于提取FAST特征点.
包含浅灰色边界在内的整个矩形用于计算ORB描述子.
//计算这层图像的坐标边界， NOTICE 注意这里是坐标边界，EDGE_THRESHOLD指的应该是可以提取特征点的有效图像边界，后面会一直使用“有效图像边界“这个自创名词
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;
void ORBextractor::ComputePyramid(cv::Mat image) {
for (int level = 0; level < nlevels; ++level) {
// 计算缩放+补padding后该层图像的尺寸
float scale = mvInvScaleFactor[level];
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
Size wholeSize(sz.width + EDGE_THRESHOLD * 2, sz.height + EDGE_THRESHOLD * 2);
Mat temp(wholeSize, image.type());
// 缩放图像并复制到对应图层并补边
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
if( level != 0 ) {
resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, cv::INTER_LINEAR);
copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101+BORDER_ISOLATED);
} else {
copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
}
}opyMakeBorder函数实现了复制和padding填充,其参数BORDER_REFLECT_101参数指定对padding进行镜像填充
六、提取FAST特征点
6.1、分cell搜索特征点
分CELL搜索特征点,若某CELL内特征点响应值普遍较小的话就降低分数线再搜索一遍.
CELL搜索的示意图如下,每个CELL的大小约为30✖30,搜索到边上,剩余尺寸不够大的时候,最后一个CELL有多大就用多大的区域.
需要注意的是相邻的CELL之间会有6像素的重叠区域,因为提取FAST特征点需要计算特征点周围半径为3的圆周上的像素点信息,实际上产生特征点的区域比传入的搜索区域小3像素.
//遍历所有图像
for (int level = 0; level < nlevels; ++level)
{
//计算这层图像的坐标边界， NOTICE 注意这里是坐标边界，EDGE_THRESHOLD指的应该是可以提取特征点的有效图像边界，后面会一直使用“有效图像边界“这个自创名词
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;
//计算当前网格最大的行坐标，这里的+6=+3+3，即考虑到了多出来3是为了cell边界像素进行FAST特征点提取用
//前面的EDGE_THRESHOLD指的应该是提取后的特征点所在的边界，所以minBorderY是考虑了计算半径时候的图像边界
//目测一个图像网格的大小是25*25啊
float maxY = iniY+hCell+6;
//如果初始的行坐标就已经超过了有效的图像边界了，这里的“有效图像”是指原始的、可以提取FAST特征点的图像区域
if(iniY>=maxBorderY-3)
//那么就跳过这一行
continue;
//如果图像的大小导致不能够正好划分出来整齐的图像网格，那么就要委屈最后一行了
if(maxY>maxBorderY)
maxY = maxBorderY;
//开始列的遍历
for(int j=0; j<nCols; j++)
{
//计算初始的列坐标
const float iniX =minBorderX+j*wCell;
//计算这列网格的最大列坐标，+6的含义和前面相同
float maxX = iniX+wCell+6;
//判断坐标是否在图像中
//如果初始的列坐标就已经超过了有效的图像边界了，这里的“有效图像”是指原始的、可以提取FAST特征点的图像区域。
//并且应该同前面行坐标的边界对应，都为-3
//!BUG 正确应该是maxBorderX-3
if(iniX>=maxBorderX-6)
continue;
//如果最大坐标越界那么委屈一下
if(maxX>maxBorderX)
maxX = maxBorderX;
这里指的应该是FAST角点可以存在的坐标位置范围，其实就是原始图像的坐标范围 注意这里没有提前进行+3的操作，而是在后面计算每个网格的区域的时候使用-3的操作来处理FAST角点半径问题 本质上和前面的思想是一样的
//计算这个容许坐标区域的宽度和高度
const int W = maxBorderX - minBorderX;
const int H = maxBorderY - minBorderY;
//同时计算每个图像cell的宽度和高度
const int cellW = ceil((float)W/levelCols);
const int cellH = ceil((float)H/levelRows);
//计算本层图像中的总cell个数
const int nCells = levelRows*levelCols;
//ceil:返回大于或者等于表达式的最小整数，向上取整
//这里计算了每个cell中需要提取出来的特征点数量，由于存在小数取整问题，所以都是往多了取整
const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells);
6.2、提取特征点
FAST提取兴趣点, 自适应阈值 并且这个向量存储这个cell中的特征点
//这个向量存储这个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); //使能非极大值抑制
}
//得到的特征点的坐标，依旧是在当前图层下来讲的
keypoints = DistributeOctTree(vToDistributeKeys, //当前图层提取出来的特征点，也即是等待剔除的特征点
//NOTICE 注意此时特征点所使用的坐标都是在“半径扩充图像”下的
minBorderX, maxBorderX, //当前图层图像的边界，而这里的坐标却都是在“边缘扩充图像”下的
minBorderY, maxBorderY,
mnFeaturesPerLevel[level], //希望保留下来的当前层图像的特征点个数
level); //当前层图像所在的图层
//PATCH_SIZE是对于底层的初始图像来说的，现在要根据当前图层的尺度缩放倍数进行缩放得到缩放后的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;
keypoints[i].pt.y+=minBorderY;
//记录特征点来源的图像金字塔图层
keypoints[i].octave=level;
//记录计算方向的patch，缩放后对应的大小， 又被称作为特征点半径
keypoints[i].size = scaledPatchSize;
}
}
// compute orientations
//然后计算这些特征点的方向信息，注意这里还是分层计算的
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], //对应的图层的图像
allKeypoints[level], //这个图层中提取并保留下来的特征点容器
umax); //以及PATCH的横坐标边界
}6.3、四叉树筛选特征点: DistributeOctTree()
将提取器节点分成4个子节点，同时也完成图像区域的划分、特征点归属的划分，以及相关标志位的置位
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];
//判断这个特征点在当前特征点提取器节点图像的哪个区域，更严格地说是属于那个子图像区块
//然后就将这个特征点追加到那个特征点提取器节点的vkeys中
//NOTICE BUG REVIEW 这里也是直接进行比较的，但是特征点的坐标是在“半径扩充图像”坐标系下的，而节点区域的坐标则是在“边缘扩充图像”坐标系下的
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中存储的特征点
step1.如果图片的宽度比较宽，就先把分成左右w/h份。一般的640×480的图像开始的时候只有一个 node。
step2.如果node里面的点数>1，把每个node分成四个node，如果node里面的特征点为空，就不要了， 删掉。
step3.新分的node的点数>1，就再分裂成4个node。如此，一直分裂。
step4.终止条件为：node的总数量> [公式] ，或者无法再进行分裂。
step5.然后从每个node里面选择一个质量最好的FAST点
//这里判断是否数目等于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;6.4、 最后计算这些特征点的方向信息
//遍历图像金字塔中的每个图层
for (int level = 0; level < nlevels; ++level)
//计算这个图层所有特征点的方向信息
computeOrientation(mvImagePyramid[level], //这个图层的图像
allKeypoints[level], //这个图层的特征点对象vector容器
umax); //patch区域的边界
}
//注意这是一个不属于任何类的全局静态函数，static修饰符限定其只能够被本文件中的函数调用
/**
* @brief 计算某层金字塔图像上特征点的描述子
*
* @param[in] image 某层金字塔图像
* @param[in] keypoints 特征点vector容器
* @param[out] descriptors 描述子
* @param[in] 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)); //提取出来的描述子的保存位置
}
七、总结
void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors)
{
// Step 1 检查图像有效性。如果图像为空，那么就直接返回
if(_image.empty())
return;
//获取图像的大小
Mat image = _image.getMat();
//判断图像的格式是否正确，要求是单通道灰度值
assert(image.type() == CV_8UC1 );
// Pre-compute the scale pyramid
// Step 2 构建图像金字塔
ComputePyramid(image);
// Step 3 计算图像的特征点，并且将特征点进行均匀化。均匀的特征点可以提高位姿计算精度
// 存储所有的特征点，注意此处为二维的vector，第一维存储的是金字塔的层数，第二维存储的是那一层金字塔图像里提取的所有特征点
vector < vector<KeyPoint> > allKeypoints;
//使用四叉树的方式计算每层图像的特征点并进行分配
ComputeKeyPointsOctTree(allKeypoints);
//使用传统的方法提取并平均分配图像的特征点，作者并未使用
//ComputeKeyPointsOld(allKeypoints);
// Step 4 拷贝图像描述子到新的矩阵descriptors
Mat descriptors;
//统计整个图像金字塔中的特征点
int nkeypoints = 0;
//开始遍历每层图像金字塔，并且累加每层的特征点个数
for (int level = 0; level < nlevels; ++level)
nkeypoints += (int)allKeypoints[level].size();
//如果本图像金字塔中没有任何的特征点
if( nkeypoints == 0 )
//通过调用cv::mat类的.realse方法，强制清空矩阵的引用计数，这样就可以强制释放矩阵的数据了
//参考[https://blog.csdn.net/giantchen547792075/article/details/9107877]
_descriptors.release();
else
{
//如果图像金字塔中有特征点，那么就创建这个存储描述子的矩阵，注意这个矩阵是存储整个图像金字塔中特征点的描述子的
_descriptors.create(nkeypoints, //矩阵的行数，对应为特征点的总个数
32, //矩阵的列数，对应为使用32*8=256位描述子
CV_8U); //矩阵元素的格式
//获取这个描述子的矩阵信息
// ?为什么不是直接在参数_descriptors上对矩阵内容进行修改，而是重新新建了一个变量，复制矩阵后，在这个新建变量的基础上进行修改？
descriptors = _descriptors.getMat();
}
//清空用作返回特征点提取结果的vector容器
_keypoints.clear();
//并预分配正确大小的空间
_keypoints.reserve(nkeypoints);
//因为遍历是一层一层进行的，但是描述子那个矩阵是存储整个图像金字塔中特征点的描述子，所以在这里设置了Offset变量来保存“寻址”时的偏移量，
//辅助进行在总描述子mat中的定位
int offset = 0;
//开始遍历每一层图像
for (int level = 0; level < nlevels; ++level)
{
//获取在allKeypoints中当前层特征点容器的句柄
vector<KeyPoint>& keypoints = allKeypoints[level];
//本层的特征点数
int nkeypointsLevel = (int)keypoints.size();
//如果特征点数目为0，跳出本次循环，继续下一层金字塔
if(nkeypointsLevel==0)
continue;
// preprocess the resized image
// Step 5 对图像进行高斯模糊
// 深拷贝当前金字塔所在层级的图像
Mat workingMat = mvImagePyramid[level].clone();
// 注意：提取特征点的时候，使用的是清晰的原图像；这里计算描述子的时候，为了避免图像噪声的影响，使用了高斯模糊
GaussianBlur(workingMat, //源图像
workingMat, //输出图像
Size(7, 7), //高斯滤波器kernel大小，必须为正的奇数
2, //高斯滤波在x方向的标准差
2, //高斯滤波在y方向的标准差
BORDER_REFLECT_101);//边缘拓展点插值类型
// Compute the descriptors 计算描述子
// desc存储当前图层的描述子
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
// Step 6 计算高斯模糊后图像的描述子
computeDescriptors(workingMat, //高斯模糊之后的图层图像
keypoints, //当前图层中的特征点集合
desc, //存储计算之后的描述子
pattern); //随机采样模板
// 更新偏移量的值
offset += nkeypointsLevel;
// Scale keypoint coordinates
// Step 6 对非第0层图像中的特征点的坐标恢复到第0层图像（原图像）的坐标系下
// ? 得到所有层特征点在第0层里的坐标放到_keypoints里面
// 对于第0层的图像特征点，他们的坐标就不需要再进行恢复了
if (level != 0)
{
// 获取当前图层上的缩放系数
float scale = mvScaleFactor[level];
// 遍历本层所有的特征点
for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
// 特征点本身直接乘缩放倍数就可以了
keypoint->pt *= scale;
}
// And add the keypoints to the output
// 将keypoints中内容插入到_keypoints 的末尾
// keypoint其实是对allkeypoints中每层图像中特征点的引用，这样allkeypoints中的所有特征点在这里被转存到输出的_keypoints
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
}
}
参考文献：
ORB_SLAM2源码解析
以上是关于ORB_SLAM3源码阅读笔记的主要内容，如果未能解决你的问题，请参考以下文章