第七讲 视觉里程计1-特征点
更加详细内容请参考
视觉里程计根据相邻图像的信息估计出粗略的相机运动。视觉里程计的算法：特征点法和直接法。
特征点法优势：具有稳定，对光照、动态物体不敏感的优势，这类算法有时也称为两视图几何（Two-view geometry )
时间问题：特征点提取与匹配所耗费的时间相比较，提取时间是远大于匹配的。
文章目录
1 特征点法
1.1 特征点
特征点由关键点(Key-point)和描述子(Descriptor)两部分组成。
关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。只要两个特征点的描述子在向量空间上的距离相近，就可以认为它们是同样的特征点。
注意，一个关键点对应的描述子是向量，那么一幅图像很多特征点对应的就是一个矩阵！
2 ORB特征
ORB特征由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”,是一种改进的FAST角点（计算了特征点的主方向），描述子在BRIEF(Binary Robust Independent Elementary Features)描述子)的基础上加入了上述方向信息，称为Steered BRIEF。
2.1源码分析：ORB特征点整体计算流程
/**
* @brief 用仿函数（重载括号运算符）方法来计算图像特征点
*
* @param[in] _image 输入原始图的图像
* @param[in] _mask 掩膜mask
* @param[in & out] _keypoints 存储特征点关键点的向量
* @param[in & out] _descriptors 存储特征点描述子的矩阵
*/
void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors)
{
// 1. 判断图像是否正确
if(_image.empty())
return;
Mat image = _image.getMat();
assert(image.type() == CV_8UC1 );
// 2. 计算图像金字塔,金字塔保存到mvImagePyramid[level]
ComputePyramid(image);
// 3. 计算图像的特征点，并且将特征点进行均匀化。计算特征点主方向，均匀的特征点可以提高位姿计算精度
// 存储所有的特征点，此处为二维的vector，第一维存储的是金字塔的层数，第二维存储的是那一层金字塔图像里提取的所有特征点
vector < vector<KeyPoint> > allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);
//ComputeKeyPointsOld(allKeypoints); //使用传统的方法提取并平均分配图像的特征点
// 4. 创建图像描述子矩阵descriptors
Mat descriptors;
int nkeypoints = 0;
// 计算所有的特征点总数nkeypoints（所有层）
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, CV_8U); // nkeypoints是行数，32是列数
descriptors = _descriptors.getMat(); // CV_8U矩阵元素的格式 32*8=256
} // getMat()获取这个描述子的矩阵信息
_keypoints.clear();
_keypoints.reserve(nkeypoints); // 预分配正确大小的空间
// 因为遍历是一层一层进行的，但是描述子那个矩阵是存储整个图像金字塔中特征点的描述子，
// 所以在这里设置了Offset变量来保存“寻址”时的偏移量，
// 辅助进行在总描述子mat中的定位
// 5.计算当前金字塔层的特征点对应描述子矩阵
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();
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
// 计算当前层描述子
// rowRange 函数，用于提取 descriptors 矩阵中指定行的子矩阵
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
// workingMat高斯模糊之后的图层图像 keypoints当前图层中的特征点集合
// desc当前层特征点对应的描述子矩阵 pattern随机点对模板
// 5.计算当前金字塔层的特征点对应描述子矩阵
computeDescriptors(workingMat, keypoints, desc, pattern);
offset += nkeypointsLevel;
// 6. Scale keypoint coordinates坐标尺度还原
// 对非第0层图像中的特征点的坐标恢复到第0层图像（原图像）的坐标系下
// 得到所有层特征点在第0层里的坐标放到_keypoints里面
// 对于第0层的图像特征点，他们的坐标就不需要再进行恢复了
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;
}
// 7. 将keypoints中内容插入到_keypoints 的末尾，完成最终的图像特征点向量
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
}
}
2.2 Oriented FAST角点
FAST角点有以下几个问题：
① FAST关键点很容易扎堆出现，因此第一次遍历完图像后还需要用到非极大值抑制，在一定范围内保留响应值最大的值作为该范围内的FAST关键点。
② 尺度问题—图像金字塔
③ 不具有方向性—灰度质心法
2.3 尺度不变性—图像金字塔
FAST角点存在尺度问题。根据上面的描述，FAST固定选取的是半径为3的圆，这个关键点和相机拍摄物体的分辨率有关。我在初始位置通过FAST-9判定是关键点，但是当相机靠近物体进行拍摄时,这个角点占用的像素数目就会变多，固定的半径为3的圆就检测不到角点了；或者当相机远离物体进行拍摄时,这个角点占用的像素数目就会变少，固定的半径为3的圆也检测不到角点。
ORB特征点使用图像金字塔来确保特征点的尺度不变性，故名为Oriented FAST角点。每往上一层，就对图像进行一个固定倍率的缩放，得到不同分辨率的图像。
① 源码分析
/**
* 构建图像金字塔，注意这里把每一层图像进行了扩展，这是为了方便图像边缘特征点的提取。但是源码中实际并没
* 有获取扩充的图像，获取的仍是等比例缩放的原图像
* @param image 输入原图像，这个输入图像所有像素都是有效的，也就是说都是可以在其上提取出FAST角点的
*/
void ORBextractor::ComputePyramid(cv::Mat image)
{
for (int level = 0; level < nlevels; ++level)
{
float scale = mvInvScaleFactor[level]; // 1 s s^2 构造函数中计算
// Size对象sz Size 是 OpenCV 中的一个类，用于表示图像的尺寸（宽度和高度）
// 1. 计算原图像比例缩放
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
// 2. 图像扩充。包括无效图像区域的大小。将图像进行“补边”，扩充区域图像不进行FAST角点检测
Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
// 3. 定义temp图像，它的大小为 wholeSize，类型与输入图像 image 相同 masktemp没有使用
Mat temp(wholeSize, image.type()), masktemp;
// 使用 Rect 函数，从 temp 中选取有效图像区域，即去掉边界区域的部分，大小为 sz.width 和 sz.height，
// 并将其赋值给 mvImagePyramid[level]，即存储在图像金字塔中的当前层级图像。
// 4. 从扩充图像抽取出实际的图像
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
// Compute the resized image
if( level != 0 )
{
resize(mvImagePyramid[level-1], // 输入图像
mvImagePyramid[level], // 输出图像
sz, // 输出图像的尺寸
0, // 水平方向上的缩放系数，留0表示自动计算
0, // 水平方向上的缩放系数，留0表示自动计算
INTER_LINEAR); // 图像缩放的差值算法类型，这里的是双线性插值算法
// 把源图像拷贝到目的图像的中央，四面填充指定的像素。图片如果已经拷贝到中间，只填充边界
// 这样做是为了能够正确提取边界的FAST角点
// EDGE_THRESHOLD指的这个边界的宽度，由于这个边界之外的像素不是原图像素而是算法生成出来的，所以不能够在EDGE_THRESHOLD之外提取特征点
// 第一个参数 src 是输入图像。第二个参数 dst 是输出图像，即进行边界扩展后的图像。
// 第三到第六个参数 top, bottom, left, right 分别表示在每个边界的上、下、左、右添加的像素数。在这段代码中，EDGE_THRESHOLD 被用于指定扩展的像素数。
// 第七个参数 borderType 是一个整数，用于指定边界扩展的类型。在这里，BORDER_REFLECT_101 和 BORDER_ISOLATED 用于指定边界扩展的模式
// 5. 扩充图像插值
copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101+BORDER_ISOLATED);
// BORDER_REFLECT_101 表示使用镜像反射方式进行边界扩展。边界上的像素值通过沿边界反射方式获得，即像素值从边界开始反射，例如：abc|d|cba。
// BORDER_ISOLATED 表示边界扩展时不复制原始图像的边界值，而是将边界外的像素置为0。
}
else
{
copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
}
}
② 实验测试
这个测试程序可以帮助理解上面的图像金字塔构建过程，上面源码中很奇怪的是，它的目的是为例扩展图像，但是最终并没有选择temp扩展图层。
cv::Size、cv::resize、cv::Rect、cv::copyMakeBorder函数使用方法
#include<iostream>
#include<vector>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp> // cv::resize
#include <opencv2/opencv.hpp> // imread
using namespace std;
using namespace cv;
std::vector<cv::Mat> ComputePyramid(cv::Mat image, const int nlevels, vector<cv::Mat> mvImagePyramid,
vector<float> mvInvScaleFactor, const int EDGE_THRESHOLD);
int main()
{
// 1. 参数
int pyramid_nlevels = 4;
double pyramid_scale = 0.5;
std::vector<float> mvInvScaleFactor = { 1.0, 0.5, 0.25, 0.125 };
int EDGE_THRESHOLD = 19;
std::vector<cv::Mat> mvImagePyramid, PyramidShow;
mvImagePyramid.resize(pyramid_nlevels); // 要设置容器内图像数，否则报错
PyramidShow.resize(pyramid_nlevels);
// 2. 读取图像
// 读取图像
cv::Mat image = cv::imread("222.jpg", cv::IMREAD_COLOR);
if (image.empty()) {
std::cout << "Failed to read the image." << std::endl;
return -1;
}
// 3.缩放图像,因为imread的图太大
cv::Size targetSize(640, 480);
cv::Mat resizedImage;
cv::resize(image, resizedImage, targetSize,0,0, INTER_LINEAR); // INTER_NEAREST：最近邻插值 INTER_CUBIC：双三次插值
// 4. 构建图像金字塔、扩充图像()双线性插值
PyramidShow = ComputePyramid(resizedImage, pyramid_nlevels, mvImagePyramid,
mvInvScaleFactor, EDGE_THRESHOLD);
// 5.显示原始图像和缩放后的图像
cv::namedWindow("Original Image", cv::WINDOW_AUTOSIZE);
cv::imshow("Original Image", resizedImage);
cv::namedWindow("s = 1", cv::WINDOW_AUTOSIZE);
cv::imshow("s = 1", PyramidShow[0]);
cv::namedWindow("s = 0.5", cv::WINDOW_AUTOSIZE);
cv::imshow("s = 0.5", PyramidShow[1]);
cv::namedWindow("s = 0.25", cv::WINDOW_AUTOSIZE);
cv::imshow("s = 0.25", PyramidShow[2]);
cv::namedWindow("s = 0.125", cv::WINDOW_AUTOSIZE);
cv::imshow("s = 0.125", PyramidShow[3]);
cv::waitKey(0);
cv::destroyAllWindows();
return 0;
}
std::vector<cv::Mat> ComputePyramid(cv::Mat image,const int nlevels,vector<cv::Mat> mvImagePyramid,
vector<float> mvInvScaleFactor, const int EDGE_THRESHOLD)
{
for (int level = 0; level < nlevels; ++level)
{
float scale = mvInvScaleFactor[level];
cv::Size sz(cvRound((float)image.cols * scale), cvRound((float)image.rows * scale));
cv::Size wholeSize(sz.width + EDGE_THRESHOLD * 2, sz.height + EDGE_THRESHOLD * 2);
// 扩充图像
cv::Mat temp(wholeSize, image.type());
// 使用 Rect 函数，从 temp 中选取有效图像区域，即去掉边界区域的部分，大小为 sz.width 和 sz.height，
// 并将其赋值给 mvImagePyramid[level]，即存储在图像金字塔中的当前层级图像。
// cv::Rect(int x, int y, int width, int height)：根据指定的位置和大小创建一个矩形。
mvImagePyramid[level] = temp(cv::Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
if (level != 0)
{
cv::resize(image, mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
// 将图像扩充部分插值
cv::copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101 + BORDER_ISOLATED);
}
else
{
cv::resize(image, mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
cv::copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
mvImagePyramid[level] = temp;
}
return mvImagePyramid;
}
以原始图像大小为例，它会对每层图像进行EDGE_THRESHOLD * 2的扩展，所以右图比左图大。
很明显可以看出来，右图经过插值填充了EDGE_THRESHOLD 区域，所以看起来很奇怪。但这是为了服务特征点的提取，并不会用到这些点。
2.4 旋转不变性—灰度质心法
① 原理推导
以关键点为中心选择一个圆形区域，特征点坐标（整数）是几何中心，即形心。然后计算关键点的”主方向“，这里的主方向即由形心指向质心的向量。这里计算的 θ \theta θ，将旋转描述子所旋转的所有点对，统一将像素旋转到这个“主方向”，这就引入了旋转不变性。
下面介绍灰度质心法的步骤：
定义圆心区域图像的矩：
m
p
q
=
∑
x
,
y
x
p
y
q
I
(
x
,
y
)
,
p
,
q
=
{
0
,
1
}
m_{pq}=\sum_{x,y}x^{p}y^{q}I(x,y),\quad p,q=\{0,1\}
mpq=x,y∑xpyqI(x,y),p,q={0,1}
在半径为
R
R
R的圆形图像区域，沿两个坐标轴
x
,
y
x,y
x,y方向的图像矩分别为
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{gathered} m_{10} =\sum_{x=-R}^R\sum_{y=-R}^RxI(x,y) \\ m_{01} =\sum_{x=-R}^{R}\sum_{y=-R}^{R}yI(x,y) \end{gathered}
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}^{R}I(x,y)
m00=x=−R∑Ry=−R∑RI(x,y)
图像的质心为：
C
=
(
c
x
,
c
y
)
=
(
m
10
m
00
,
m
01
m
00
)
C=(c_{x},c_{y})=\left(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}}\right)
C=(cx,cy)=(m00m10,m00m01)
于是关键点的旋转角度记为
θ
=
arctan
2
(
c
y
,
c
x
)
=
arctan
2
(
m
01
,
m
10
)
\theta=\arctan2\left(c_{y},c_{x}\right)=\arctan2\left(m_{01},m_{10}\right)
θ=arctan2(cy,cx)=arctan2(m01,m10)
② 获取角度 θ θ θ程序
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)
{
keypoint->angle = IC_Angle(image, keypoint->pt, umax); // 每个关键点主方向
}
}
③ 计算角度 θ θ θ程序
注意，十四讲中采用的是矩形框，真正的ORB算法采用的是圆形。它的质心计算使用到了一定的技巧。
/**
* @brief 这个函数用于计算特征点的方向，这里是返回角度作为方向。
* 计算特征点方向是为了使得提取的特征点具有旋转不变性。
* 方法是灰度质心法：以几何中心和灰度质心的连线作为该特征点方向
* @param[in] image 要进行操作的某层金字塔图像
* @param[in] pt 当前特征点的坐标
* @param[in] u_max 图像块的每一行的坐标边界 u_max，在类构造函数中计算
* @return float 返回特征点的角度，范围为[0,360)角度，精度为0.3°
*/
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
{
int m_01 = 0, m_10 = 0;
//cvRound表示取整，以关键点像素地址为center指针
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
// 1.单独求圆形区域水平坐标轴这一行像素灰度，这一行y=0，所以只需要计算m10=x*I(x,y)
// HALF_PATCH_SIZE = 15 = 圆半径 奇数 = 2*数+1
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
m_10 += u * center[u];
// 2.对称计算每一行的m10和m01，m10=x(I(X,Y)+I(x,-y)) m01=y(I(X,Y)-I(x,-y))
int step = (int)image.step1(); // 图像一行的字节数
for (int v = 1; v <= HALF_PATCH_SIZE; ++v) // 计算15次(行row)
{
// Proceed over the two lines
int v_sum = 0;
int d = u_max[v]; // 得到这一行的区域最大x坐标范围d(半径)
for (int u = -d; u <= d; ++u)
{
//3. 利用指针计算像素强度I(x,y) 以pt.x,pt.y为中心，列坐标+行数*行字节数 非常巧妙
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); // arctan
}
④ 验证
/* 这里分析了灰度质心法的计算、以及umax的计算
1. 求取质心m00，形心以一个随机的特征点表示
2. 注意质心坐标，它的计算值是相对与形心(0,0)的，所以在不能直接作为图像中的坐标！
3. 这里的半径R设置很大，因为半径小了看不清结果
4. 这里只画了一个特征点，后续可以拓展画出所有的特征点
*/
#include<iostream>
#include<vector>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp> // cv::resize
#include <opencv2/opencv.hpp> // imread
using namespace std;
using namespace cv;
vector<int> compute_umax(const int R);
cv::Point2f IC_Angle(const Mat& image, Point2f pt, const vector<int>& u_max,
const int HALF_PATCH_SIZE);
int main()
{
// 1. 参数设置
int R = 200;
// 2. 读取图像
cv::Mat image = cv::imread("222.jpg", 1); // 注意要读取的是灰度图，IC_Angle中是uchar
if (image.empty()) {
std::cout << "Failed to read the image." << std::endl;
return -1;
}
// 3.缩放图像,因为imread的图太大
cv::Size targetSize(1280, 720);
cv::Mat resizedImage;
cv::resize(image, resizedImage, targetSize, 0, 0, INTER_LINEAR);
// key points, using GFTT here.
vector<KeyPoint> kp;
Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // 最多500 keypoints
detector->detect(resizedImage, kp);
cv::Point2f keypoint(kp[15].pt.x, kp[15].pt.y);
画出关键点
//cv::Mat kpimg;
//cv::drawKeypoints(resizedImage, kp, kpimg, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
//cv::imshow("ORB features", kpimg);
// 4.计算角度
float angle = 0, m01 = 0, m10 = 0;
cv::Point2f centroid = IC_Angle(resizedImage, keypoint, compute_umax(R), R);
// 5.
cv::circle(resizedImage, keypoint, 5, cv::Scalar(0, 0, 255), -1); // 用红色实心圆画出第一个点
cv::circle(resizedImage, centroid, 5, cv::Scalar(0, 0, 255), -1); // 用红色实心圆画出第二个点
cv::line(resizedImage, keypoint, centroid, cv::Scalar(0, 255, 0), 2); // 用绿色线段连接两个点
//cv::circle(resizedImage, keypoint, 10, cv::Scalar(0, 0, 255), -1); // 用红色实心圆画出第一个点
//cv::circle(resizedImage, centroid, 10, cv::Scalar(0, 0, 255), -1); // 用红色实心圆画出第二个点
//cv::line(resizedImage, keypoint, centroid, cv::Scalar(0, 255, 0), 2); // 用绿色线段连接两个点
cv::imshow("Image", resizedImage);
cv::waitKey(0);
}
vector<int> compute_umax(const int R)
{
vector<int> umax;
int v, v0, vmax = cvFloor(R * sqrt(2.f) / 2 + 1); // 向下取整
int vmin = cvCeil(R * sqrt(2.f) / 2); // 向上取整
umax.resize(R + 1);
const double hp2 = R * R; // R^2
for (v = 0; v <= vmax; ++v)
{
umax[v] = cvRound(sqrt(hp2 - v * v)); // 四舍五入取整
//cout << umax[v] << ' ';
}
for (v = R, v0 = 0; v >= vmin; --v)
{
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}
cout << endl;
for (auto& k : umax)
{
//cout << k << ' ';
}
return umax;
}
Point2f IC_Angle(const Mat& image, Point2f pt, const vector<int>& u_max,
const int HALF_PATCH_SIZE)
{
int m_01 = 0, m_10 = 0, m_00 = 0;
//cvRound表示取整，以关键点像素地址为center指针
const uchar* center = &image.at<uchar>(cvRound(pt.y), cvRound(pt.x));
// 1.单独求圆形区域水平坐标轴这一行像素灰度，这一行y=0，所以只需要计算m10=x*I(x,y)
// HALF_PATCH_SIZE = 15 = 圆半径 奇数 = 2*数+1
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
{
m_10 += u *center[u];
m_00 += center[u];
}
// 2.对称计算每一行的m10和m01，m10=x(I(X,Y)+I(x,-y)) m01=y(I(X,Y)-I(x,-y)) m00 = I(X,Y)+I(x,-y)
int step = (int)image.step1(); // 图像一行的字节数
for (int v = 1; v <= HALF_PATCH_SIZE; ++v) // 计算15次(行row)
{
// Proceed over the two lines
int v_sum = 0;
int d = u_max[v]; // 得到这一行的区域最大x坐标范围d(半径)
for (int u = -d; u <= d; ++u)
{
//3. 利用指针计算像素强度I(x,y) 以pt.x,pt.y为中心，列坐标+行数*行字节数 非常巧妙
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_00 += val_plus + val_minus;
}
m_01 += v * v_sum;
}
// 5. 画出形心和质心
float C1 = float(m_10) / m_00 + pt.x;
float C2 = float(m_01) / m_00 + pt.y;
cv::Point2f centroid(C1, C2);
return centroid;
}
结果
2.5 旋转不变性—Steered BRIEF
① 原理分析
确定了关键点，就要对每个关键点的信息进行量化，计算其描述子。ORB特征点中的描述子是在BRIEF的基础上进行改进的，称为Steered BRIEF。
BRIEF是一种二进制编码的描述子，在ORB-SLAM2中它是一个256 bit的向量，其中每个bit是0或1。这样我们在比较两个描述子时，可以直接用异或位运算计算汉明距离，速度非常快。
步骤：
① 为减少噪声干扰，先对图像进行高斯滤波
② 以关键点为中心，取一定大小的图像窗口P。在窗口内随机选取一对点，比较二者像素的大小，进行如下二进制赋值。
τ
(
p
;
x
,
y
)
:
=
{
1
:
p
(
x
)
<
p
(
y
)
0
:
p
(
x
)
⩾
p
(
y
)
\tau(p;x,y):=\begin{cases}1&:p(x)<p(y)\\0&:p(x)\geqslant p(y)\end{cases}
τ(p;x,y):={10:p(x)<p(y):p(x)⩾p(y)
实际上，在ORB-SLAM中，其采用了固定选点模板。
③ 在窗口中随机选取N(在ORB-SLAM2中N=256)对随机点，重复第2步的二进制赋值，最后得到一个256维的二进制描述子。
补充：二维平面的旋转可以看作是绕Z轴（固定轴）旋转的结果，只取平面
x
o
y
xoy
xoy的结果，可以写为如下的矩阵形式。也可以从下面的图中直接推出来。
[
x
′
y
′
]
=
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
[
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]
② 源码分析
//注意这是一个不属于任何类的全局静态函数，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); // 行维=关键点总数 列维=32 矩阵元素都是CV_8UC1 8位 32*8=256
for (size_t i = 0; i < keypoints.size(); i++)
computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i)); // ptr函数，用于获取 descriptors 矩阵中指定行的指针
}
/**
* @brief 计算ORB特征点的描述子。注意这个是全局的静态函数，只能是在本文件内被调用
* @param[in] kpt 特征点对象
* @param[in] img 提取特征点的图像
* @param[in] pattern 预定义好的采样模板
* @param[out] desc 用作输出变量，保存计算好的描述子，维度为32*8 = 256 bit
*/
const float factorPI = (float)(CV_PI/180.f); // 角度转弧度
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); // cos(Θ),sin(Θ)
const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
const int step = (int)img.step;
// 旋转随机点对到主方向 \ 用作行续符号 宏定义GET_VALUE
#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; // << 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; // 8位描述子，一共16个点 16*32=512=256*2个点
}
#undef GET_VALUE // 取消宏定义
}
// ORB预定义的特殊点对
static int bit_pattern_31_[256*4] =
{
8,-3, 9,5/*mean (0), correlation (0)*/,
4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/,
-11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/,
};
③ 旋转随机点对本质
如图所示，我们把上面的随机点对全部旋转到“主方向”，也就是把整个坐标系旋转至“主方向”。其中点P是特征点位置，也就是几何中心；点Q是圆形区域质心。
2.6 均匀化策略
为了避免特征点过于集中，ORB-SLAM中采用了均匀化策略来提高精度，如位姿计算，避免冗余。
① 第一步：计算每一层金字塔特征点数、umax
（1）原理分析
根据总的图像金字塔层级数
m
m
m和待提取的特征点总数
N
N
N，计算图像金字塔中每个层级需要提取的特征点数量。第
0
0
0层图像的宽为
W
W
W,高为
H
H
H,对应的面积
H
W
=
C
HW=C
HW=C,图像金字塔缩放因子为
s
s
s,
0
<
s
<
1
0<s<1
0<s<1。
1. 计算所有层级的总面积
S
=
H
W
(
s
2
)
0
+
H
W
(
s
2
)
1
+
⋯
+
H
W
(
s
2
)
(
m
−
1
)
=
H
W
1
−
(
s
2
)
m
1
−
s
2
=
C
1
−
(
s
2
)
m
1
−
s
2
\begin{aligned}S&=HW(s^2)^0+HW(s^2)^1+\cdots+HW(s^2)^{(m-1)}\\&=HW\frac{1-(s^2)^m}{1-s^2}=C\frac{1-(s^2)^m}{1-s^2}\end{aligned}
S=HW(s2)0+HW(s2)1+⋯+HW(s2)(m−1)=HW1−s21−(s2)m=C1−s21−(s2)m
2. 单位面积被分配的特征点数量为：
N
a
v
g
=
N
S
=
N
C
1
−
(
s
2
)
m
1
−
s
2
=
N
(
1
−
s
2
)
C
[
1
−
(
s
2
)
m
]
N_{\mathrm{avg}}=\frac{N}{S}=\frac{N}{C\frac{1-(s^2)^m}{1-s^2}}=\frac{N(1-s^2)}{C[1-(s^2)^m]}
Navg=SN=C1−s21−(s2)mN=C[1−(s2)m]N(1−s2)
3. 原图像即第0层分配特征点数量
N
0
=
C
N
a
v
g
=
N
(
1
−
s
2
)
1
−
(
s
2
)
m
N_0=CN_{\mathrm{avg}}=\frac{N(1-s^2)}{1-(s^2)^m}
N0=CNavg=1−(s2)mN(1−s2)
4. 第
i
i
i层金字塔分配特征点数量
N
i
=
N
(
1
−
s
2
)
C
[
1
−
(
s
2
)
m
]
C
(
s
2
)
i
=
N
(
1
−
s
2
)
1
−
(
s
2
)
m
(
s
2
)
i
N_i=\frac{N(1-s^2)}{C[1-(s^2)^m]}C(s^2)^i=\frac{N(1-s^2)}{1-(s^2)^m}(s^2)^i
Ni=C[1−(s2)m]N(1−s2)C(s2)i=1−(s2)mN(1−s2)(s2)i
5. ORB-SLAM中实际以s即面积开方来平均特征点
N
i
=
N
(
1
−
s
)
C
[
1
−
(
s
)
m
]
C
(
s
)
i
=
N
(
1
−
s
)
1
−
(
s
)
m
(
s
)
i
N_i=\frac{N(1-s)}{C[1-(s)^m]}C(s)^i=\frac{N(1-s)}{1-(s)^m}(s)^i
Ni=C[1−(s)m]N(1−s)C(s)i=1−(s)mN(1−s)(s)i
（2）源码分析
上面这一过程在ORBextractor类构造函数中初始化完成，下面具体分析
/**
* @brief ORBextractor类构造函数，初始化参数，计算每层金字塔的特征点数，计算灰度质心法中的圆形区域
* 不同行的列范围umax
*
* @param[in & out] _nfeatures 提取器节点1：要提取的总特征点数
* @param[in & out] _scaleFactor 1/s
* @param[in & out] _nlevels 金字塔层数
* @param[in & out] _iniThFAST 初始FAST角点可信阈值
* @param[in & out] _minThFAST 指FAST角点最低可信阈值(均匀化策略，降低选取标准)
*/
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); // 1 1/s 1/s^2 1/s^3 ...
mvLevelSigma2.resize(nlevels); // 1 1/s^2 1/s^4 ....
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]; // // 1 s s^2
mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];
}
mvImagePyramid.resize(nlevels); // std::vector<cv::Mat> mvImagePyramid; 对应不同的金字塔层图像
mnFeaturesPerLevel.resize(nlevels);
float factor = 1.0f / scaleFactor; // s
// N0 = N(1-s)/(1-s^m)
float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
int sumFeatures = 0;
for( int level = 0; level < nlevels-1; level++ )
{ // Ni = (N(1-s)/(1-s^m))s^i
mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
sumFeatures += mnFeaturesPerLevel[level];
nDesiredFeaturesPerScale *= factor;
}
mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0); // 最顶层金字塔图像特征点数
const int npoints = 512; // 256点对，即512个点
const Point* pattern0 = (const Point*)bit_pattern_31_; // int bit_pattern_31_[256*4]
// 强制把整型数组指针bit_pattern_31_(数组首地址)转换为Point型，然后赋给Point型指针pattern0
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
// 类成员定义std::vector<cv::Point> pattern;
// 将从范围 [pattern0, pattern0 + npoints) 中的元素复制到 pattern 容器中，pattern数组指针
// 在计算描述子时使用，每次迭代16个点，8对，8位uchar
// std::back_inserter(pattern) 是一个迭代器，它在 pattern 容器的末尾插入元素
// 计算化灰度质心法中圆的每一行最大半径 v是行索引，从0-R之间每一行最大的半径
umax.resize(HALF_PATCH_SIZE + 1); // std::vector<int> umax;
// 1. 计算0-vmax
int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); // 向下取整
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2); // 向上取整
// 除HALF_PATCH_SIZE * sqrt(2.f) / 2是整数外，vmin=vmax
const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE; // R^2
for (v = 0; v <= vmax; ++v)
umax[v] = cvRound(sqrt(hp2 - v * v)); // 四舍五入取整
// 2.计算vmin(=vmax)-R
// 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）umax推导
这里较难理解的是umax的计算。
HALF_PATCH_SIZE就是半径R，u是列坐标，v是行坐标。以
45
°
45°
45°为区分线，按代码中两步分别计算。
R
2
=
u
2
+
v
2
R^2 = u^2 + v^2
R2=u2+v2. 代码中
v
m
i
n
、
v
m
a
x
vmin、vmax
vmin、vmax差不多为
R
/
根号
2
R/根号2
R/根号2即
45
°
45°
45°处，下面的计算和上面的计算是对称的，但实际中因为取整的关系，并不是完全的对称。在1.2.4中也单独验证了该算法，最好debug调试查看
② 第二步：划分格子，遍历提取每一个格子中的FAST角点
划分格子，在 O R B − S L A M 2 ORB-SLAM2 ORB−SLAM2中格子固定尺寸为 30 30 30像素 × 30 ×30 ×30像素。依次在每一个格子中提取 F A S T FAST FAST角点(直接调用opencv库)。如果初始的 F A S T FAST FAST角点阈值没有检测到角点，则降低 F A S T FAST FAST角点阈值，这样在弱纹理区域也能提取到更多的角点。如果降低一次阈值后还是提取不到角点，则不在这个格子里提取，这样可以避免提取到质量特别差的角点。
（1）图像边界、格子
关于这里的边界问题，首先图像时扩充过的，上下左右四个方向都扩充了19个像素。minBorder是选择减去上下左右16个像素的图像边界，这里减3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆。保证原始图像边界点的正确检测。程序中因为取整的原因，并不一定是30*30(相近)，这里以30*30分析。
const float iniY =minBorderY+i*hCell; // 第i个方格子占的像素行数hCell
float maxY = iniY+hCell+6; // 加6即3+30+3 都是为了保证边界像素进行FAST特征点提取
（2）源码分析
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{
// 重新调整图像层数
allKeypoints.resize(nlevels);
// 图像网格块cell的尺寸，是个正方形
const float W = 30;
for (int level = 0; level < nlevels; ++level)
{
// 1. 计算边界
// EDGE_THRESHOLD = 19 在构建金字塔时图像进行了扩充
// 这里的3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆
const int minBorderX = EDGE_THRESHOLD-3; // 16
const int minBorderY = minBorderX; // 16 std::vector<cv::Mat> mvImagePyramid;
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); // 10倍总特征点数
// 计算进行特征点提取的图像区域尺寸
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
const int nCols = width/W; // 列方向有多少图像网格块(30*30)
const int nRows = height/W; // 行方向有多少图像网格块(30*30)
// 计算每个图像网格块所占的像素行数和列数
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);
// 2. 开始遍历图像网格(30*30)，以行开始遍历的(从第一列的行)
for(int i=0; i<nRows; i++)
{
// 行方向第i个图像网格块
// 计算当前网格最大的行坐标，这里的+6=+3+3，即考虑到了多出来3是为了cell边界像素进行FAST特征点提取用
// 前面的EDGE_THRESHOLD指的应该是提取后的特征点所在的边界，所以minBorderY是考虑了计算半径时候的图像边界
const float iniY =minBorderY+i*hCell;
float maxY = iniY+hCell+6;
if(iniY>=maxBorderY-3)
continue;
if(maxY>maxBorderY)
maxY = maxBorderY;
// 3. 开始列的遍历
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;
// 4. FAST提取兴趣点, 自适应阈值
// 这个向量存储这个cell中的特征点
vector<cv::KeyPoint> vKeysCell;
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), // 图像块区域
// vKeysCell存储角点 iniThFAST初始阈值
vKeysCell,iniThFAST,true); // true表示使能非极大值抑制，避免角过于集中
// 5. 如果这个图像块中使用默认的FAST检测阈值没有能够检测到角点
if(vKeysCell.empty())
{
// 使用更低的阈值minThFAST来进行重新检测
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,minThFAST,true); // true表示使能非极大值抑制，避免特征点过于集中
}
if(!vKeysCell.empty())
{ // 6. 遍历其中的所有FAST角点，恢复其在整个金字塔当前层图像下的坐标
// 因为这里提取的特征点的坐标只是小方块30*30的坐标，恢复特征点在当前金字塔层的坐标
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);
// 7. 利用四叉树均匀选取当前金字塔层特征点。 vToDistributeKeys是当前层提取的角点
// mnFeaturesPerLevel[level]是当前层分配的特征点数目 level当前层级
keypoints = DistributeOctTree(vToDistributeKeys, 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++)
{
// 8.对每一个保留下来的特征点，恢复到相对于当前图层“边缘扩充图像下”的坐标系的坐标
// 因为现在是在排除上下左右都为16的区域进行的特征提取
keypoints[i].pt.x+=minBorderX;
keypoints[i].pt.y+=minBorderY;
// 记录特征点来源的图像金字塔图层
keypoints[i].octave=level;
// 记录计算方向的patch，缩放后对应的大小，又被称作为特征点半径
keypoints[i].size = scaledPatchSize;
}
}
// 9.计算每一层每一个特征点的主方向
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
③ 第三步：四叉树
（1）节点分裂算法
节点第 1 1 1次分裂， 1 1 1个根节点分裂为 4 4 4个子节点。分裂之后根据图像的尺寸划分节点的区域，对应的边界为 U L i , U R i , B L i , B R i , i = 1 , 2 , 3 , 4 UL_i,UR_i,BL_i,BR_i,i=1,2,3,4 ULi,URi,BLi,BRi,i=1,2,3,4分别对应左上角、右上角、左下角、右下角的 4 4 4个坐标。有些坐标会被多个节点共享，比如图像中心点坐标就同时被 B R 1 , B L 2 , U R 3 , U L 44 BR1,BL2,UR3,UL44 BR1,BL2,UR3,UL44个点共享。落在某个节点区域范围内的所有特征点都属于该节点的元素。
这里需要注意的是，一个母节点分裂为 4 4 4个子节点后，需要在节点链表中删掉原来的母节点，所以实际上一次分裂净增加了 3 3 3个节点。因此，下次分裂后，节点的总数是可以提前预估的，计算方式为：当前节点总数十即将分裂的 节点总数 × 3 节点总数×3 节点总数×3。下次分裂最多可以得到 4 + 4 × 3 = 16 4+4×3=16 4+4×3=16个节点
- 程序分析：类中嵌套类（节点嵌套子节点），非常适合这里的区域分配
/**
* @brief 将提取器节点分成4个子节点，同时也完成图像区域的划分、特征点归属的划分，以及相关标志位的置位
*
* @param[in & out] n1 提取器节点1：左上
* @param[in & out] n2 提取器节点1：右上
* @param[in & out] n3 提取器节点1：左下
* @param[in & out] n4 提取器节点1：右下
*/
class ExtractorNode // 定义划分节点类
{
public:
ExtractorNode():bNoMore(false){} // 构造函数，初始化列表参数
//
void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); // 分配节点函数
std::vector<cv::KeyPoint> vKeys; // 当前母节点区域内关键点数组vector
cv::Point2i UL, UR, BL, BR; // 一个矩形图像区域四个角的坐标(当前节点-母节点)
std::list<ExtractorNode>::iterator lit; // 构建list容器
bool bNoMore; // 表示属于当前节点的特征点数量=1则为true
};
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
{
// 1.计算当前节点所在图像区域的一半长宽 ceil向上取整 static_cast强制把Point2i类型转float
const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);
// 2.计算子节点图像区域边界 1分为4 左上
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);
n1.vKeys.reserve(vKeys.size()); // reserve将容器的容量设置为至少vKeys.size
// 右上
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.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.UL = n3.UR;
n4.UR = n2.BR;
n4.BL = n3.BR;
n4.BR = BR;
n4.vKeys.reserve(vKeys.size());
// 把特征点分配到对应的4个节点区域
for(size_t i=0;i<vKeys.size();i++)
{
// 地址引用
const cv::KeyPoint &kp = vKeys[i]; // 注意vector数组容器中元素是cv::KeyPoint
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，若为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;
}
（2）多次分裂—特征点处理
后续的节点分裂与上面相似，这里主要介绍一些其它情况
1 如果当前分裂后的节点区域内没有特征点，我们直接跳过该区域，删除这个节点，对应代码注释4.2。
2 如果当前分裂后的节点区域内只有1个特征点，我们不在分裂这个特征点，对应代码注释4.1。
3 如果下一次分裂的节点数 >=N,优先选择节点区域内特征点数目比较多的节点进行分裂，见注释5.6。
4 在不断的循环后，使得节点数 = N，选择每个节点区域内响应最大的节点作为该区域唯一特征点，注释7。
- 程序分析： O R B e x t r a c t o r ORBextractor ORBextractor类内置函数
/**
* @brief 使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发
*
* @param[in] vToDistributeKeys 等待进行分配到四叉树中的特征点
* @param[in] minX 当前图层的图像的边界，坐标都是在“半径扩充图像”坐标系下的坐标
* @param[in] maxX
* @param[in] minY
* @param[in] maxY
* @param[in] N 希望提取出的特征点个数
* @param[in] level 指定的金字塔图层，并未使用
* @return vector<cv::KeyPoint> 已经均匀分散好的特征点vector容器
*/
// vector<cv::KeyPoint>是返回类型
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)
{
// 1.Compute how many initial nodes 一般是1或2
const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY)); // 图像长/宽
const float hX = static_cast<float>(maxX-minX)/nIni; // 每个节点的列范围
list<ExtractorNode> lNodes; // 节点list列表，类型ExtractorNode，存放节点
vector<ExtractorNode*> vpIniNodes; // 节点指针
vpIniNodes.resize(nIni); // 最初容量至少=初始节点nIni
// 2.生成初始提取器节点
for(int i=0; i<nIni; i++)
{
ExtractorNode ni;
// cv::Point2i 类的定义位于 <opencv2/core/types.hpp> 头文件中
// 用于表示二维平面上的整数坐标点 i指int型 类模板Point_<T>
ni.UL = cv::Point2i(hX*static_cast<float>(i),0);
ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);
ni.BL = cv::Point2i(ni.UL.x,maxY-minY);
ni.BR = cv::Point2i(ni.UR.x,maxY-minY);
ni.vKeys.reserve(vToDistributeKeys.size());
// 把ni添加到 lNodes 容器的末尾
lNodes.push_back(ni);
// back() 是 list 容器的成员函数，用于获取容器中的最后一个元素
// vpIniNodes节点指针 vpIniNodes[i]指针数组，指向当前节点
// 存储这个初始的提取器节点句柄
vpIniNodes[i] = &lNodes.back();
}
// 3. 把特征点分配给子节点(初始节点)
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
// 引用 kp
const cv::KeyPoint &kp = vToDistributeKeys[i];
// kp.pt.x/hX初始化一般为1、2 存储到相应节点类成员对象vKeys
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}
// 4 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点
list<ExtractorNode>::iterator lit = lNodes.begin();
while(lit!=lNodes.end())
{
if(lit->vKeys.size()==1)
{
lit->bNoMore=true; // 4.1 标志位，表示此节点不可再分
lit++;
}
else if(lit->vKeys.empty())
// 4.2 如果一个节点没有被分配到特征点，那么就从列表中直接删除它
lit = lNodes.erase(lit);
else
lit++;
}
bool bFinish = false;
int iteration = 0;
// 这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄(节点指针)
vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
// 调整大小，一个初始化节点将“分裂”成为四个节点
vSizeAndPointerToNode.reserve(lNodes.size()*4);
// 5 利用四叉树方法对图像进行划分区域，均匀分配特征点
while(!bFinish)
{
iteration++;
// 节点数
int prevSize = lNodes.size();
lit = lNodes.begin();
// 需要展开的节点计数，这个一直保持累计，不清零
int nToExpand = 0;
vSizeAndPointerToNode.clear();
while(lit!=lNodes.end())
{
if(lit->bNoMore)
{
// 5.1 当前节点只有一个特征点，不再分裂该特征点
lit++;
continue;
}
else
{
// 5.2 不止一个特征点, 1分为4
ExtractorNode n1,n2,n3,n4;
lit->DivideNode(n1,n2,n3,n4);
// 5.3 如果分裂的子节点区域特征点数比0大，接下面操作；否则该区域将被抛弃
if(n1.vKeys.size()>0)
{
// 只要这个节点区域内特征点数>=1,添加n1子节点
lNodes.push_front(n1); // 把这个节点放在容器第一个位置
if(n1.vKeys.size()>1)
{
// 5.4 特征点数>1,说明还需要分裂(最终的目的就是每一个节点区域的特征点为1)
nToExpand++;
// 键值对数组容器 <n1节点特征点数，n1节点指针>
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
// lNodes.front().lit 和前面的迭代的lit 不同，只是名字相同而已
// lNodes.front().lit是node结构体里的一个指针用来记录节点的位置
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();
}
}
lit=lNodes.erase(lit); // 删除分裂的母节点
continue;
}
}
// 5.5 Finish 如果节点数>特征点数 or 所有的节点只有一个特征点
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
{
bFinish = true; // 结束循环
}
// 5.6 (int)lNodes.size()+nToExpand*3 实际分裂会得到节点数>N,那么选择特征点数比较大的节点分裂
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());
for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
{
// 5.6.1 优先分裂特征点数比较大的节点
ExtractorNode n1,n2,n3,n4;
vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);
// 5.6.2 Add childs if they contain points
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)
{
lNodes.push_front(n2);
if(n2.vKeys.size()>1)
{
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)
{
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)
{
vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
// 只要分裂的节点数>=N，那么直接跳出for循环，不在分裂节点
if((int)lNodes.size()>=N)
break;
}
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
bFinish = true;
}
}
}
// Retain the best point in each node
// 7 到此，已经固定了最终的节点，但每个节点不一定只有一个特征点，我们保留每个节点区域响应值最大的一个特征点
vector<cv::KeyPoint> vResultKeys;
// 调整容器大小为要提取的特征点数目
vResultKeys.reserve(nfeatures);
for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
{
// 当前节点的特征点数组容器 引用
vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;
// 7.1 得到指向第一个特征点的指针，后面作为最大响应值对应的关键点
cv::KeyPoint* pKP = &vNodeKeys[0];
// 7.2 用第1个关键点响应值初始化最大响应值
float maxResponse = pKP->response;
for(size_t k=1;k<vNodeKeys.size();k++)
{
if(vNodeKeys[k].response>maxResponse)
{
// 7.3 只要这个关键点的相应大，记录其地址指针
pKP = &vNodeKeys[k];
maxResponse = vNodeKeys[k].response;
}
}
// 7.4 记录每一个节点中响应最大的关键点
vResultKeys.push_back(*pKP);
}
// 8.返回最终关键点数组容器
return vResultKeys;
}
3 特征匹配
特征匹配解决了SLAM中的数据关联问题(data association),即确定当前看到的路标与之前看到的路标之间的对应关系。
- 匹配方法：暴力匹配、快速近似最近邻等
- 距离计算：对于二进制的描述子，如BRIEF，使用汉明距离(Hamming distance)作为度量(不同位数的个数)
3.1 单目初始化中的特征匹配
在ORB-SLAM2的单目纯视觉初始化中，认为参与初始化的两帧是比较接近的，即在第1帧提取的特征点坐标对应的第2帧中的位置附近画一个正方形(或圆形)，匹配点应该落在这个圆中。
① 快速确定候选匹配点
提取完特征点后会直接将特征点划分到不同的网格中并记录在mGrid里。如图所示，在搜索时是以网格为单位进行的，代码中的网格尺寸默认是64像素×48像素，这样圆内包含的网格数目相比像素数目就大大减少。
(1) 源码解析
/**
* @brief 找到在 以x,y为中心,半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点
*
* @param[in] x 特征点坐标x
* @param[in] y 特征点坐标y
* @param[in] r 搜索半径
* @param[in] minLevel 最小金字塔层级
* @param[in] maxLevel 最大金字塔层级
* @return vector<size_t> 返回搜索到的候选匹配点id
*/
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
{
// 存储搜索结果的vector
vector<size_t> vIndices;
vIndices.reserve(N);
// step 1
// 1.1 左边界 mfGridElementWidthInv一个像素占多少网格(<1) 解析见下面
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
// 1.2 右边界
const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
return vIndices;
// 1.3 上边界
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;
// 1.4 下边界
const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
const bool bCheckLevels = (minLevel>0) || (maxLevel>=0); // 逻辑或
// Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
// 2.1 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
const vector<size_t> vCell = mGrid[ix][iy];
if(vCell.empty())
continue;
// 2.2 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
if(bCheckLevels)
{
// cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据
// 要在minLevel~maxLevel层
if(kpUn.octave<minLevel)
continue;
if(maxLevel>=0)
if(kpUn.octave>maxLevel)
continue;
}
// 2.3 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内
// x和y即这个圆心，也即上一帧的特征点坐标
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
// 不超出方向搜索区域
// if(distx*distx + disty*disty < r*r) 改为圆形区域
if(fabs(distx)<r && fabs(disty)<r)
// vCell[j]是特征点索引
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
(2) mfGridElementWidthInv 与 mfGridElementHeightInv
#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64
// mnMaxX等图像边界由下面函数求解(去畸变) FRAME_GRID_COLS是预定义的值
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
mnMaxX最大列像素 - mnMinX最大行像素 = 图像宽度(多少像素)
(mnMaxX-mnMinX)/FRAME_GRID_COLS就是一个网格占多少像素(>1)
FRAME_GRID_COLS/(mnMaxX-mnMinX)就是一个像素占多少网格(<1)
(3) 去畸变图像的边界mnMinX、mnMaxX、mnMinY、mnMaxY
/**
* @brief 计算去畸变图像的边界
*
* @param[in] imLeft 需要计算边界的图像
*/
void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
// 如果相机畸变参数不为0，用OpenCV函数进行畸变矫正
if(mDistCoef.at<float>(0)!=0.0)
{
// 1 保存矫正前的图像四个边界点坐标 CV_32F
// 这里创建的是单通道 4*2矩阵，表示单精度浮点数类型
cv::Mat mat(4,2,CV_32F); // 4个坐标8个数，所以4*2矩阵
// 1.1 (0,0)左上角
mat.at<float>(0,0)=0.0; mat.at<float>(0,1)=0.0;
// 1.2 (cols,0)右上角
mat.at<float>(1,0)=imLeft.cols; mat.at<float>(1,1)=0.0;
// 1.3 (0,rows)左下角
mat.at<float>(2,0)=0.0; mat.at<float>(2,1)=imLeft.rows;
// 1.4 (cols,rows)右下角
mat.at<float>(3,0)=imLeft.cols; mat.at<float>(3,1)=imLeft.rows;
// Undistort corners
// https://docs.opencv.org/4.x/d3/d63/classcv_1_1Mat.html#a4eb96e3251417fa88b78e2abd6cfd7d8
// 第一个参数：通道数 第二个：行数 这里2指的是通道数 矩阵维度4*1 相当于成了4个点
mat=mat.reshape(2);
// https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga55c716492470bfe86b0ee9bf3a1f0f7e
// 参数： 输入图像 去畸变图像 mK即相机内参矩阵 mDistCoef相机畸变系数 cv::Mat()外参矩阵，这里为空，默认使用恒等变换
// 最后mk指畸变后新的内参矩阵
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1);
//2 校正后的四个边界点已经不能够围成一个严格的矩形，因此在这个四边形的外侧加边框作为坐标的边界
mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));//左上和左下横坐标最小的
mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));//右上和右下横坐标最大的
mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));//左上和右上纵坐标最小的
mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));//左下和右下纵坐标最小的,1));
}
else
{
// 如果畸变参数为0，无需去畸变
mnMinX = 0.0f;
mnMaxX = imLeft.cols;
mnMinY = 0.0f;
mnMaxY = imLeft.rows;
}
}
(4) cv::Mat::reshape
int main()
{
cv::Mat m(4, 2, CV_8UC1);
for (int x = 0; x < 2; x++)
{
for (int y = 0; y < 4; y++)
{
m.at<uchar>(y, x) = x + y;
}
}
// 输出原始矩阵的值
for (int row = 0; row < m.rows; ++row)
{
for (int col = 0; col < m.cols; ++col)
{
// 必须把uchar值强制转换为int
std::cout << static_cast<int>(m.at<uchar>(row, col)) << ' ';
}
cout << endl;
}
cout << endl;
// 将矩阵改为2通道 行维不设置则默认为原来的行维
m = m.reshape(2);
// 矩阵变为4*1
for (int row = 0; row < m.rows; ++row)
{
for (int col = 0; col < m.cols; ++col)
{
cv::Vec2b pixel = m.at<cv::Vec2b>(row, col);
std::cout << "Pixel at (" << row << ", " << col << "): "
<< static_cast<int>(pixel[0]) << ", "
<< static_cast<int>(pixel[1]) << ", "
<< std::endl;
}
}
}
② 方向一致性检验
原理是统计两张图像所有匹配对中两个特征，点主方向的差，构建一个直方图。由于两张图像整体发生了运动，因此特征点匹配对主方向整体会有一个固定一致的变化。通常直方图中前三个最大的格子里就是正常的匹配点对，那些误匹配的特征点对此时就会暴露出来，落在直方图之外的其他格子里，这些就是需要剔除的错误匹配。
/**
* @brief 单目初始化中用于参考帧和当前帧的特征点匹配
* 步骤
* Step 1 构建旋转直方图
* Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点
* Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
* Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
* Step 5 计算匹配点旋转角度差所在的直方图
* Step 6 筛除旋转直方图中“非主流”部分
* Step 7 将最后通过筛选的匹配好的特征点保存
* @param[in] F1 初始化参考帧
* @param[in] F2 当前帧
* @param[in & out] vbPrevMatched 本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标
* @param[in & out] vnMatches12 保存参考帧F1中特征点匹配情况，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
* @param[in] windowSize 搜索窗口
* @return int 返回成功匹配的特征点数目
*/
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
{
// 总的匹配数
int nmatches=0;
// F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间，值全为-1
// vector构造函数，(数目，数值)
vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);
// Step 1 构建旋转直方图，HISTO_LENGTH = 30
// 创建一个数组，每个元素都是vector<int>
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
//! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码
const float factor = HISTO_LENGTH/360.0f;
// 匹配的特征点对距离，按照F2特征点数目分配空间 初始值 = 最佳描述子匹配距离
vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
// 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间
vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
// 遍历帧1中的特征点
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
{
// 特征点
cv::KeyPoint kp1 = F1.mvKeysUn[i1];
// 这个特征点所属金字塔层级
int level1 = kp1.octave;
// 只使用原始图像层级(level1=0)上的特征点
if(level1>0)
continue;
// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点
// GetFeaturesInArea返回搜索到的帧2的候选匹配点索引vector 参数 x,y,r=100,minl=maxl=0
vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
// 没有帧1的候选匹配点，直接跳过
if(vIndices2.empty())
continue;
// 取出参考帧F1中当前遍历特征点对应的描述子
// 描述子矩阵每一行即一个特征点的描述子
cv::Mat d1 = F1.mDescriptors.row(i1);
int bestDist = INT_MAX; // 最佳描述子匹配距离，越小越好
int bestDist2 = INT_MAX; // 次佳描述子匹配距离
int bestIdx2 = -1; // 最佳候选特征点在F2中的index
// Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{
size_t i2 = *vit;
// 取出候选特征点对应的描述子 i2即索引，因为vit是迭代器指针，解引用vector的值
cv::Mat d2 = F2.mDescriptors.row(i2);
// DescriptorDistance函数：计算描述子距离--汉明距离 参数：两个描述子矩阵
int dist = DescriptorDistance(d1,d2);
// 描述子距离>=INT_MAX = vMatchedDistance[i2]
if(vMatchedDistance[i2]<=dist)
continue;
// 描述子距离<=INT_MAX
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestIdx2=i2; // 记录最佳匹配点的索引
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
// Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
// 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值
if(bestDist<=TH_LOW)
{
// 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高 mfNNratio=0.6
if(bestDist<(float)bestDist2*mfNNratio)
{
// 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉
// vnMatches21初始化帧2每一个特征点索引值都对应-1
if(vnMatches21[bestIdx2]>=0)
{
// vnMatches21[bestIdx2] = i1 即帧1中的特征点索引 设置为-1 也就是说明误匹配
vnMatches12[vnMatches21[bestIdx2]]=-1;
nmatches--; // 总匹配数-1
}
// i1为帧1中的特征点索引 bestIdx2帧2中的特征点索引
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1; // 互相标记，这是两者值不在是-1 两者有匹配点
vMatchedDistance[bestIdx2]=bestDist; // 距离索引标记
nmatches++;
// Step 5 计算匹配点旋转角度差所在的直方图
if(mbCheckOrientation)
{
// 计算匹配特征点的角度差，这里单位是角度°，不是弧度
float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
// 前面factor = HISTO_LENGTH/360.0f HISTO_LENGTH=30
// bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin // 因为是取整，所bin会有0、1、2...29
int bin = round(rot*factor); // 四舍五入取整
// 如果bin 满了又是一个轮回
if(bin==HISTO_LENGTH)
bin=0; // 一共30个数
assert(bin>=0 && bin<HISTO_LENGTH); // 0~29为正常数据
// 把每一个特征点加到对应的直方图区间
rotHist[bin].push_back(i1);
}
}
}
}
// Step 6 筛除旋转直方图中次要部分
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
int idx1 = rotHist[i][j];
if(vnMatches12[idx1]>=0)
{
vnMatches12[idx1]=-1;
nmatches--;
}
}
}
}
//Update prev matched
// Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatched
for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)
if(vnMatches12[i1]>=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
}
// 描述子矩阵: 特征点数*32 每个数都是8位uchar 每一行都代表了一个特征点的描述子
descriptors.create(nkeypoints, 32, CV_8U); // nkeypoints是行数，32是列数
// CV_8U矩阵元素的格式 32*8=256
③ 汉明距离计算
int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
const int *pa = a.ptr<int32_t>();
const int *pb = b.ptr<int32_t>();
int dist=0;
for(int i=0; i<8; i++, pa++, pb++)
{
// 这将两个描述符中对应整数进行异或操作，得到一个结果v，表示两个描述符在这个位置上的差异。
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
}
return dist;
}
④ComputeThreeMaxima
void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)
{
int max1=0;
int max2=0;
int max3=0;
for(int i=0; i<L; i++)
{
const int s = histo[i].size();
if(s>max1)
{
max3=max2;
max2=max1;
max1=s;
ind3=ind2;
ind2=ind1;
ind1=i;
}
else if(s>max2)
{
max3=max2;
max2=s;
ind3=ind2;
ind2=i;
}
else if(s>max3)
{
max3=s;
ind3=i;
}
}
if(max2<0.1f*(float)max1)
{
ind2=-1;
ind3=-1;
}
else if(max3<0.1f*(float)max1)
{
ind3=-1;
}
}
3.2 词袋匹配
3.3 路标点投影匹配
3.4 Sim(3)变换投影匹配
4 十四讲程序解析
这一讲程序的编译基本只是涉及opencv库，应该可顺利编译成功。当然，运行程序时候可能会报错，一般来讲都是图片的路径不对。
4.1 CMake文件.
4.1.1 CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(vo1) # 项目名
set(CMAKE_BUILD_TYPE "Release") # Release 或 Debug
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4") # 添加了一个预处理器定义
# 把${PROJECT_SOURCE_DIR}/cmake添加到CMAKE_MODULE_PATH，find时会优先查找
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 3 REQUIRED) # 这里的版本要和环境一致，或者干脆不写
find_package(G2O REQUIRED) # 作者自定义cmake文件来查找g2o
find_package(Sophus REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
"/usr/include/eigen3/"
)
add_executable(orb_cv orb_cv.cpp)
target_link_libraries(orb_cv ${OpenCV_LIBS})
add_executable(orb_self orb_self.cpp)
target_link_libraries(orb_self ${OpenCV_LIBS})
# add_executable( pose_estimation_2d2d pose_estimation_2d2d.cpp extra.cpp ) # use this if in OpenCV2
add_executable(pose_estimation_2d2d pose_estimation_2d2d.cpp)
target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBS})
# # add_executable( triangulation triangulation.cpp extra.cpp) # use this if in opencv2
add_executable(triangulation triangulation.cpp)
target_link_libraries(triangulation ${OpenCV_LIBS})
add_executable(pose_estimation_3d2d pose_estimation_3d2d.cpp)
target_link_libraries(pose_estimation_3d2d
g2o_core g2o_stuff
${OpenCV_LIBS})
add_executable(pose_estimation_3d3d pose_estimation_3d3d.cpp)
target_link_libraries(pose_estimation_3d3d
g2o_core g2o_stuff
${OpenCV_LIBS})
4.1.2 FindG2O.cmake
# Find the header files
FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
${G2O_ROOT}/include
$ENV{G2O_ROOT}/include
$ENV{G2O_ROOT}
/usr/local/include
/usr/include
/opt/local/include
/sw/local/include
/sw/include
NO_DEFAULT_PATH
)
# Macro to unify finding both the debug and release versions of the
# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
# macro.
MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
FIND_LIBRARY("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
${G2O_ROOT}/lib/Debug
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Debug
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
FIND_LIBRARY("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
)
FIND_LIBRARY(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
${G2O_ROOT}/lib/Release
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Release
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
FIND_LIBRARY(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
)
IF(NOT ${MYLIBRARY}_DEBUG)
IF(MYLIBRARY)
SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
ENDIF(MYLIBRARY)
ENDIF( NOT ${MYLIBRARY}_DEBUG)
ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
# Find the core elements
FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
# Find the CLI library
FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
# Find the pluggable solvers
FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
# Find the predefined types
FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
# G2O solvers declared found if we found at least one solver
SET(G2O_SOLVERS_FOUND "NO")
IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
SET(G2O_SOLVERS_FOUND "YES")
ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
# G2O itself declared found if we found the core libraries and at least one solver
SET(G2O_FOUND "NO")
IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
SET(G2O_FOUND "YES")
ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
4.2 opencv实现ORB特征提取与匹配
注意这里的图像参数前面是两个点，因为图像在可执行文件的上一级目录下面。
p@p:~/slambook/ch7/build$ ./orb_cv ../1.png ../2.png
extract ORB cost = 0.190616 seconds.
match ORB cost = 0.0226859 seconds.
-- Max dist : 94.000000
-- Min dist : 4.000000
# 这种就是图像路径错误
p@p:~/slambook/ch7/build$ ./orb_cv ./1.png ./2.png
extract ORB cost = 1.1364e-05 seconds.
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(3.4.16) /home/p/opencv-3.4.16/modules/imgproc/src/color.cpp:182: error: (-215:Assertion failed) !_src.empty() in function 'cvtColor'
Aborted (core dumped)
程序分析
#include<iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <chrono>
using namespace std;
int main(int argc, char** argv)
{
if(argc != 3)
{
cout << "usage:feature_extraction img1 img2" << endl;
return 1;
}
// 1. 读取图像
// cv::Mat创建任意维度的稠密矩阵，我们用它来表示图像
cv::Mat img_1 = cv::imread(argv[1],1);
cv::Mat img_1 = cv::imread(argv[2],1);
assert(img_1.data != nullptr && img_2.data != nullptr );
// cv::imshow("ss", img_1);
// cv::waitKey(0)
// 2. 初始化 关键点、描述子变量 然后使用ORB特征检测法，构建地址指针
std::vector<cv::KeyPoint> keypoint_1,keypoint_2; // KeyPoint类型动态数组，存储关键点
cv::Mat descriptors_1,descriptors_2; // 存储描述子数据，单独一个特征点的描述子是向量，那么全部的描述子就是一个矩阵
// cv::Ptr<class T>是指针模板类,等价于FeatureDetector* detector = ...
cv::Ptr<cv::FeatureDetector> detector = cv:: ORB::create(); // 关键点指针
cv::Ptr<cv::DescriptorExtractor> descriptor = cv:: ORB::create(); // 描述子指针
cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); // 模板匹配指针
// 3.cv::Feature2D::detect(pa1,pa2,pa3)用于FAST 角点位置检测。ORB类继承Feature2D类中函数
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
detector->detect(img_1,keypoints_1);
detector->detect(img_2,keypoints_2);
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
// 4. 先把关键点给画出来
cv::Mat outimg1,outimg2; cv::drawKeypoints(img_1,keypoints_1,outimg1,cv::Scalar::all(-1),cv::DrawMatchesFlags::DEFAULT); cv::drawKeypoints(img_2,keypoints_2,outimg2,cv::Scalar::all(-1),cv::DrawMatchesFlags::DEFAULT);
cv::imshow("hhh", outimg1); cv::imshow("h", outimg2);
// cv::waitkey(0);
// 5. 计算两幅图像特征点的一个匹配距离，求出最大和最小距离
std::vector<cv::DMatch> matches;
matcher->match(descriptors_1, descriptors_2, matches ); // 特征匹配！
// minmax_element第三个参数是匿名函数
auto min_max = minmax_element(matches.begin(), matches.end(),
[](const cv::DMatch &m1, const cv::DMatch &m2)
{return m1.distance < m2.distance;} );
// 求出最大和最小汉明距离
double min_dist = min_max.first->distance;
double max_dist = min_max.second->distance;
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
// 6. 筛选合理距离的特征点 应该是暴力匹配
// cout << descriptors_1.cols << descriptors_1.rows << endl; // 32,500
// std::cout <<keypoints_1.size() << std::endl ; // 500
// 就是说，描述子矩阵 行维是关键点数，而列维是一个关键点对应的描述子数
std::vector<cv::DMatch> good_matches;
for(int i = 0; i < descriptors_1.rows; i++) // 描述子矩阵行维500 列维cols = 32
{
if(matches[i].distance <= std::max(2 * min_dist,30.0))
{
good_matches.push_back(matches[i]);
}
}
// 7. 结果展示
std::cout <<" keypoints_1 :" <<keypoints_1.size() << std::endl ; // 500
std::cout <<" keypoints_2 :" <<keypoints_2.size() << std::endl ; // 500
std::cout << "matches: " << matches.size() << std::endl; // 500
std::cout << "good_matches: " << good_matches.size() << std::endl; // 79
std::cout << "descriptors_1.cols:" <<descriptors_1.cols << std::endl << "descriptors_1.rows: "<< descriptors_1.rows << std::endl;
std::cout << "descriptors_2.cols:" <<descriptors_2.cols << std::endl << "descriptors_2.rows"<< descriptors_2.rows << std::endl;
cv::Mat img_match;
cv::drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches,img_match);
cv::imshow("img_match1", img_match);
cv::Mat img_goodmatch;
cv::drawMatches(img_1, keypoints_1, img_2, keypoints_2, good_matches,img_goodmatch);
cv::imshow(" img_goodmatch", img_goodmatch);
}
4.3 手写ORB特征
这里直接执行程序还是会报路径的问题，纠正代码后编译即可运行成功。
程序解析：相当于简化版ORB，细节不在详述。
#include<iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <nmmintrin.h>
#include <chrono>
// 全局变量
std::string first_file = "./1.png";
std::string second_file = "./2.png";
// DescriptorType描述子类型，无符号32位整数 整型都是4字节，一个字节8位，4*8=32位，uint_32是unsigned int的typedef简写
typedef std::vector<uint_32> DescType;
#include <opencv2/opencv.hpp>
#include <string>
#include <nmmintrin.h>
#include <chrono>
using namespace std;
// global variables
string first_file = "./1.png";
string second_file = "./2.png";
// 32 bit unsigned int, will have 8, 8x32=256
typedef vector<uint32_t> DescType; // Descriptor type
// 计算ORB特征点，参数 1.图像 2.关键点向量 3.描述子向量
void ComputeORB(const cv::Mat &img, std::vector<cv::KeyPoint> &Keypoint, std::vector<DescType> &descriptors);
// 特征点匹配 参数 1-描述子1 2-描述子2 3-匹配结果向量
void BfMatch(const std::vector<DescType> &desc1, const std::vector<DescType> &desc2, std::vector<cv::DMatch> &matches);
int main(int argc, char **argv) {
// 加载图像
cv::Mat first_image = cv::imread(first_file, 0);
cv::Mat second_image = cv::imread(second_file, 0);
assert(first_image.data != nullptr && second_image.data != nullptr);
// detect FAST keypoints1 using threshold=40 最大距离40 图像1检测
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
vector<cv::KeyPoint> keypoints1;
cv::FAST(first_image, keypoints1, 40);
vector<DescType> descriptor1;
ComputeORB(first_image, keypoints1, descriptor1);
cout << " keypoints1.size() = "<<keypoints1.size()<<";" //638个特征点
<< "descriptor1.size() = " << descriptor1.size() <<endl // 638 对应的描述子向量
<< "descriptor1[].size() = " << descriptor1[100].size() <<endl ; //8 应该就是二维的，但是一直不知道怎么把它数出来，8*32 根据它输出的数据 < 2^32
// 图像2检测
vector<cv::KeyPoint> keypoints2;
vector<DescType> descriptor2;
cv::FAST(second_image, keypoints2, 40);
ComputeORB(second_image, keypoints2, descriptor2);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;
// 匹配
vector<cv::DMatch> matches;
BfMatch(descriptor1, descriptor2, matches);
cout << "matches: " << matches.size() << endl;
// plot the matches
cv::Mat image_show;
cv::drawMatches(first_image, keypoints1, second_image, keypoints2, matches, image_show);
cv::imshow("matches", image_show);
cv::imwrite("matches.png", image_show);
cv::waitKey(0);
cout << "done." << endl;
return 0;
}
// opencv中带的256个描述子点对pq，两张图片1个点对2组坐标，所以是4列256行。orb论文中写这些点对比较好
int ORB_pattern[256 * 4] = {8, -3, 9, 5，......};
void ComputeORB(const cv::Mat &img, std::vector<cv::KeyPoint> &keypoints, std::vector<DescType> &descriptors )
{
const int half_patch_size = 8;
const int half_boundary = 16;
int bad_points = 0;
for(auto &kp : keypoints)
{
// 1.计算坏点。以该关键点为中心的图像块，不满足下面条件时，超出了图像之外，当我们计算BRIEF描述子而随机选择像素对时，将会导致选取图像外的像素对，因此该关键点为badpoints，应当剔除掉。
if (kp.pt.x < half_boundary || kp.pt.y < half_boundary || kp.pt.x >= img.cols - half_boundary || kp.pt.y >= img.rows - half_boundary)
{
bad_points++;
descriptors.push_back({});
}
// 2.计算灰度质心 图像patch范围 dx，dy不是变化量，就是当前位置的pixel
for(int dx = -half_patch_size; dx < half_patch_size; dx++)
{
for(int dy = -half_patch_size; dy < half_patch_size; dy++)
{
// .at获取y行x列的像素，即I(x,y)
uchar pixel = img.at<uchar>(kp.pt.y + dy,kp.pt.x + dx);//按列读取
m10 += dx * pixel; // 公式 patch内全部x*I(x,y)和
m01 += dy * pixel;
}
}
// 3.计算theta，1e-18是为例防止除以0
float m_sqrt =sqrt(m01*m01 + m10 * m10) + 1e-18;
float sin_theta = m01/m_sqrt;
float cos_thrta = m10/m_sqrt;
// 4.对每一个关键点都进行下面操作 即每个关键点的描述子采用256（8*32）位二进制描述
// 即采用ORB_pattern中256对PQ
DescType desc(8, 0); // 8个0的动态数组 最后存储32*8共256位
for (int i = 0;i < 8; i++)
{
uint32_t d = 0;
for (int k = 0; k < 32; k++)
{
int idx_pq = i * 32 + k;
cv::Point2f p(ORB_pattern[idx_pq * 4], ORB_pattern[idx_pq * 4 + 1]);
cv::Point2f q(ORB_pattern[idx_pq * 4 + 2], ORB_pattern[idx_pq * 4 + 3]);
// 平面上绕关键点kp.pt(几何中心)逆时针旋转theta角后的坐标pp(x,y) qq(x,y)
cv::Point2f pp = cv::Point2f(cos_theta * p.x - sin_theta * p.y, sin_theta * p.x + cos_theta * p.y) + kp.pt;
// https://blog.csdn.net/sss_369/article/details/90182696
cv::Point2f qq = cv::Point2f(cos_theta * q.x - sin_theta * q.y, sin_theta * q.x + cos_theta * q.y) + kp.pt;
// pp(y,x)对应像素I(x,y) p>q,取1;p < q,取0
if(img.at<uchar>(pp.y, pp.x) < img.at<uchar>(qq.y,qq.x))
{
// d |= 1 等价于d = d | 1 按位 或运算符，按二进制位进行"或"运算,
// https://www.runoob.com/cplusplus/cpp-operators.html
// 1<<k指二进制数左移k位
d |= 1 << k; // 等价于 d = d|(1 << k)
// d = d | (1 << k); 根据规律退出作者意思是p>q时候记为0 p<q记为1
// pp < qq d = ... 00000000000001 k=0
// pp < qq d = ... 00000000000001 | ...00000000010 = ...00000011 k=1
// pp < qq d = ...00000011 |00000100 = 00000111 k=2
// pp > qq d = 000...00111 k=3
// pp > qq d = 000...00111 k=4
// pp < qq d = 000...00111 | 00000100000 = 0000...100111 k=5
// .....
// 一共32组 k = 31 所以这32位数字记录了这32组随机选取的PQ像素点对的大小关系，即描述子
}
}
desc[i] = d;// 每32记一组，共8组
}
descriptors.push_back(desc); // 这里相当于三维数组，关键点*256(32*8)
}
std::cout << "bad/total: " << bad_points << "/" << keypoints.size() << std::endl;
}
// 暴力匹配
void BfMatch(const std::vector<DescType> &desc1, const std::vector<DescType> &desc2, std::vector<cv::DMatch> &matches) {
const int d_max = 40; // 最大距离
// size应该是特征点的数量，一维desc存储所有特征点的描述子
for (size_t i1 = 0; i1 < desc1.size(); ++i1) {
if (desc1[i1].empty()) continue; // 若关键点没有描述子，跳过
// DMatch(int _queryIdx, int _trainIdx, float _distance);
cv::DMatch m{i1, 0, 256}; // 这里应该是初始化，没啥用，后面会不断赋值给他，覆盖
// 第二帧图像
for (size_t i2 = 0; i2 < desc2.size(); ++i2) {
if (desc2[i2].empty()) continue;
int distance = 0;
for (int k = 0; k < 8; k++) {
// 计算1的个数 = 两帧图像之间的距离 本质是256位，但它分为8组记录，只能循环8次。求总和
distance += _mm_popcnt_u32(desc1[i1][k] ^ desc2[i2][k]);
}
if (distance < d_max && distance < m.distance) {
m.distance = distance; // 找到更小的索引
m.trainIdx = i2; // 匹配到的特征点的索引
}
}
// 把最小距离记录下来
if (m.distance < d_max) {
matches.push_back(m);
}
}
}
4.4 注意点
Image.at<uchar>(j, i); //表示的是 j 行 i 列 的这个像素 I(i,j)
Image.at<uchar>(Point(j, i)); //表示的是 坐标（j,i）的像素 I(j,i)
注意，这里有很大不同，因为 行对应的是j(y) 列对应的是i(x) 所以(j,i)即j行i列对应的元素的坐标是(i,j)
所以上面就是 I(i,j)和I(j,i)的区别
// 常用下面几种具体化的Point_类来定义图像像素点
typedef Point_<int> Point2i;
typedef Point_<int64> Point2l;
typedef Point_<float> Point2f;
typedef Point_<double> Point2d;
typedef Point2i Point;
5 线特征
除此以外，还有面特征，是处理点云的，有时间再整理。
485
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
