SLAM入门之视觉里程计(1)：特征点的匹配
SLAM 主要分为两个部分：前端和后端，前端也就是视觉里程计（VO），它根据相邻图像的信息粗略的估计出相机的运动，给后端提供较好的初始值。VO的实现方法可以根据是否需要提取特征分为两类：基于特征点的方法，不使用特征点的直接方法。 基于特征点的VO运行稳定，对光照、动态物体不敏感。
图像特征点的提取和匹配是计算机视觉中的一个基本问题，在视觉SLAM中就需要首先找到相邻图像对应点的组合，根据这些匹配的点对计算出相机的位姿（相对初始位置，相机的旋转和平移）。
本文对这段时间对特征点的学习做一个总结，主要有以下几方面的内容：
- 特征点概述
- 常用的特征点算法，如SIFT,SURF，FAST等
- OpenCV3中特征点的提取和匹配
特征点概述
如何高效且准确的匹配出两个不同视角的图像中的同一个物体，是许多计算机视觉应用中的第一步。虽然图像在计算机中是以灰度矩阵的形式存在的，但是利用图像的灰度并不能准确的找出两幅图像中的同一个物体。这是由于灰度受光照的影响，并且当图像视角变化后，同一个物体的灰度值也会跟着变化。所以，就需要找出一种能够在相机进行移动和旋转（视角发生变化），仍然能够保持不变的特征，利用这些不变的特征来找出不同视角的图像中的同一个物体。
为了能够更好的进行图像匹配，需要在图像中选择具有代表性的区域，例如：图像中的角点、边缘和一些区块，但在图像识别出角点是最容易，也就是说角点的辨识度是最高的。所以，在很多的计算机视觉处理中，都是提取交掉作为特征，对图像进行匹配，例如SFM，视觉SLAM等。
但是，单纯的角点并不能很好的满足我们的需求，例如：相机从远处得到的是角点，但是在近处就可能不是角点；或者，当相机旋转后，角点就发生了变化。为此，计算机视觉的研究者们设计了许多更为稳定的的特征点，这些特征点不会随着相机的移动，旋转或者光照的变化而变化。例如：SIFT,SURF,ORB等
一个图像的特征点由两部分构成：关键点（Keypoint）和描述子（Descriptor）。 关键点指的是该特征点在图像中的位置，有些还具有方向、尺度信息；描述子通常是一个向量，按照人为的设计的方式，描述关键点周围像素的信息。通常描述子是按照外观相似的特征应该有相似的描述子设计的。因此，在匹配的时候，只要两个特征点的描述子在向量空间的距离相近，就可以认为它们是同一个特征点。
特征点的匹配通常需要以下三个步骤：
- 提取图像中的关键点，这部分是查找图像中具有某些特征（不同的算法有不同的）的像素
- 根据得到的关键点位置，计算特征点的描述子
- 根据特征点的描述子，进行匹配
这里先介绍下特征点的描述子，一个好的描述子是准确匹配的基础，关键点的提取和特征点的匹配，在后面介绍。
特征点描述子
从图像中提取到特征的关键点信息，通常只是其在图像的位置信息（有可能包含尺度和方向信息），仅仅利用这些信息无法很好的进行特征点的匹配，所以就需要更详细的信息，将特征区分开来，这就是特征描述子。另外，通过特征描述子可以消除视角的变化带来图像的尺度和方向的变化，能够更好的在图像间匹配。
特征的描述子通常是一个精心设计的向量，描述了关键点及其周围像素的信息。为了能够更好的匹配，一个好的描述子通常要具有以下特性：
- 不变性 指特征不会随着图像的放大缩小旋转而改变。
- 鲁棒性 对噪声、光照或者其他一些小的形变不敏感
- 可区分性 每一个特征描述子都是独特的，具有排他性，尽可能减少彼此间的相似性。
其中描述子的可区分性和其不变性是矛盾的，一个具有众多不变性的特征描述子，其区分局部图像内容的能力就比较稍弱；而如果一个很容易区分不同局部图像内容的特征描述子，其鲁棒性往往比较低。所以，在设计特征描述子的时候，就需要综合考虑这三个特性，找到三者之间的平衡。
特征描述子的不变性主要体现在两个方面：
- 尺度不变性 Scale Invarient
指的是同一个特征，在图像的不同的尺度空间保持不变。匹配在不同图像中的同一个特征点经常会有图像的尺度问题，不同尺度的图像中特征点的距离变得不同，物体的尺寸变得不同，而仅仅改变特征点的大小就有可能造成强度不匹配。如果描述子无法保证尺度不变性，那么同一个特征点在放大或者缩小的图像间，就不能很好的匹配。为了保持尺度的不变性，在计算特征点的描述子的时候，通常将图像变换到统一的尺度空间，再加上尺度因子。 - 旋转不变性 Rotation Invarient
指的是同一个特征，在成像视角旋转后，特征仍然能够保持不变。和尺度不变性类似，为了保持旋转不变性，在计算特征点描述子的时候，要加上关键点的方向信息。
为了有个更直观的理解，下面给出SIFT,SURF,BRIEF描述子计算方法对比
从上表可以看出，SIFT,SURF和BRIEF描述子都是一个向量，只是维度不同。其中，SIFT和SURF在构建特征描述子的时候，保存了特征的方向和尺度特征，这样其特征描述子就具有尺度和旋转不变性；而BRIEF描述子并没有尺度和方向特征，不具备尺度和旋转不变性。
常用的特征点算法
上面提到图像的特征点包含两个部分：
- 特征点的提取，在图像检测到特征点的位置
- 特征点的描述，也就是描述子。
在图像中提取到关键点的位置信息后，为了能够更有效的匹配（主要是保证尺度和旋转不变性），通常使用一个向量来描述关键点及其周围的信息。特征的描述子，在特征点的匹配中是非常重要的，上一小节中对其应该具有的性质做了介绍。但具体到一个算法来说，可能其既有特征点的提取算法也有特征点描述子的算法，也有可能其仅仅是一个特征点提取算法或者是特征点的描述子算法。在本小节就常用的特征点算法做一个简要的说明。
SIFT
提到特征点算法，首先就是大名鼎鼎的SIFT算法了。SIFT的全称是Scale Invariant Feature Transform，尺度不变特征变换，2004年由加拿大教授David G.Lowe提出的。SIFT特征对旋转、尺度缩放、亮度变化等保持不变性，是一种非常稳定的局部特征。
SIFT算法主要有以下几个步骤：
- 高斯差分金字塔的构建
使用组和层的结构构建了一个具有线性关系的金字塔（尺度空间），这样可以在连续的高斯核尺度上查找图像的特征点；另外，它使用一阶的高斯差分来近似高斯的拉普拉斯核，大大的减少了运算量。 - 尺度空间的极值检测及特征点的定位
搜索上一步建立的高斯尺度空间，通过高斯差分来识别潜在的对尺度和旋转不变的特征点。但是，在离散空间中，局部极值点可能并不是真正意义的极值点，真正的极值点有可能落在离散点的间隙中，SIFT通过尺度空间DoG函数进行曲线拟合寻找极值点。 - 特征方向赋值
基于图像局部的梯度方向，分配给每个关键点位置一个或多个方向，后续的所有操作都是对于关键点的方向、尺度和位置进行变换，从而提供这些特征的不变性。 - 特征描述子的生成
通过上面的步骤已经找到的SIFT特征点的位置、方向、尺度信息，最后使用一组向量来描述特征点及其周围邻域像素的信息。
SIFT算法中及包含了特征点的提取算法，也有如何生成描述子的算法，更进一步的SIFT算法介绍可参看SIFT特征详解
SURF
SURF全称 Speeded Up Robust Features，是在SIFT算法的基础上提出的，主要针对SIFT算法运算速度慢，计算量大的缺点进行了改进。
SURF的流程和SIFT比较类似，这些改进体现在以下几个方面：
- 特征点检测是基于Hessian矩阵，依据Hessian矩阵行列式的极值来定位特征点的位置。并且将Hession特征计算与高斯平滑结合在一起，两个操作通过近似处理得到一个核模板。
- 在构建尺度空间时，使用box filter与源图像卷积，而不是使用DoG算子。
- SURF使用一阶Haar小波在x、y两个方向的响应作为构建特征向量的分布信息。
FAST特征点提取算法
SIFT和SURF是非常好的，稳定的特征点算法，但运算速度是其一大弊端，无法做到实时的特征提取和匹配，其应用就有了很大的局限性。FAST特征提取算法弥补了这一局限，检测局部像素灰度变化明显的地方，以速度快而著称，其全称为：Features From Accelerated Segment Test。在FAST算法的思想很简单：如果一个像素与周围邻域的像素差别较大（过亮或者过暗），那么可以认为该像素是一个角点。和其他的特征点提取算法相比，FAST算法只需要比较像素和其邻域像素的灰度值大小，十分便捷。
FAST算法提取角点的步骤：
- 在图像中选择像素p，假设其灰度值为：\(I_p\)
- 设置一个阈值T，例如：\(I_p\)的20%
- 选择p周围半径为3的圆上的16个像素，作为比较像素
- 假设选取的圆上有连续的N个像素大于\(I_p + T\)或者\(I_p - T\)，那么可以认为像素p就是一个特征点。（N通常取12，即为FAST-12；常用的还有FAST-9,FAST-11）。
FAST算法只检测像素的灰度值，其运算速度极快，同时不可避免的也有一些缺点
- 检测到的特征点过多并且会出现“扎堆”的现象。这可以在第一遍检测完成后，使用非最大值抑制（Non-maximal suppression），在一定区域内仅保留响应极大值的角点，避免角点集中的情况。
- FAST提取到的角点没有方向和尺度信息
上面的介绍的SIFT和SURF算法都包含有各自的特征点描述子的计算方法，而FAST不包含特征点描述子的计算，仅仅只有特征点的提取方法，这就需要一个特征点描述方法来描述FAST提取到的特征点，以方便特征点的匹配。下面介绍一个专门的特征点描述子的计算算法。
BRIEF描述子
BRIEF是一种二进制的描述子，其描述向量是0和1表示的二进制串。0和1表示特征点邻域内两个像素（p和q）灰度值的大小：如果p比q大则选择1，反正就取0。在特征点的周围选择128对这样的p和q的像素对，就得到了128维由0，1组成的向量。那么p和q的像素对是怎么选择的呢？通常都是按照某种概率来随机的挑选像素对的位置。
BRIEF使用随机选点的比较，速度很快，而且使用二进制串表示最终生成的描述子向量，在存储以及用于匹配的比较时都是非常方便的，其和FAST的搭配起来可以组成非常快速的特征点提取和描述算法。
ORB算法
ORB的全称是Oriented FAST and Rotated BRIEF，是目前来说非常好的能够进行的实时的图像特征提取和描述的算法，它改进了FAST特征提取算法，并使用速度极快的二进制描述子BRIEF。
针对FAST特征提取的算法的一些确定，ORB也做了相应的改进。
- 使用非最大值抑制，在一定区域内仅仅保留响应极大值的角点，避免FAST提取到的角点过于集中。
- FAST提取到的角点数量过多且不是很稳定，ORB中可以指定需要提取到的角点的数量N，然后对FAST提取到的角点分别计算Harris响应值，选择前N个具有最大响应值的角点作为最终提取到的特征点集合。
- FAST提取到的角点不具有尺度信息，在ORB中使用图像金字塔，并且在每一层金字塔上检测角点，以此来保持尺度的不变性。
- FAST提取到的角点不具有方向信息，在ORB中使用灰度质心法(Intensity Centroid)来保持特征的旋转不变性。
OpenCV3中特征点的提取和匹配
OpenCV中封装了常用的特征点算法（如SIFT,SURF，ORB等），提供了统一的接口，便于调用。 下面代码是OpenCV中使用其feature 2D 模块的示例代码
Mat img1 = imread("F:\\image\\1.png");
Mat img2 = imread("F:\\image\\2.png");
// 1. 初始化
vector<KeyPoint> keypoints1, keypoints2;
Mat descriptors1, descriptors2;
Ptr<ORB> orb = ORB::create();
// 2. 提取特征点
orb->detect(img1, keypoints1);
orb->detect(img2, keypoints2);
// 3. 计算特征描述符
orb->compute(img1, keypoints1, descriptors1);
orb->compute(img2, keypoints2, descriptors2);
// 4. 对两幅图像的BRIEF描述符进行匹配，使用BFMatch，Hamming距离作为参考
vector<DMatch> matches;
BFMatcher bfMatcher(NORM_HAMMING);
bfMatcher.match(descriptors1, descriptors2, matches);
- 获取检测器的实例
在OpenCV3中重新的封装了特征提取的接口，可统一的使用Ptr<FeatureDetector> detector = FeatureDetector::create()来得到特征提取器的一个实例，所有的参数都提供了默认值，也可以根据具体的需要传入相应的参数。 - 在得到特征检测器的实例后，可调用的
detect方法检测图像中的特征点的具体位置，检测的结果保存在vector<KeyPoint>向量中。 - 有了特征点的位置后，调用
compute方法来计算特征点的描述子，描述子通常是一个向量，保存在Mat中。 - 得到了描述子后，可调用匹配算法进行特征点的匹配。上面代码中，使用了opencv中封装后的暴力匹配算法
BFMatcher，该算法在向量空间中，将特征点的描述子一一比较，选择距离（上面代码中使用的是Hamming距离）较小的一对作为匹配点。
上面代码匹配后的结果如下：
特征点的匹配后的优化
特征的匹配是针对特征描述子的进行的，上面提到特征描述子通常是一个向量，两个特征描述子的之间的距离可以反应出其相似的程度，也就是这两个特征点是不是同一个。根据描述子的不同，可以选择不同的距离度量。如果是浮点类型的描述子，可以使用其欧式距离；对于二进制的描述子（BRIEF）可以使用其汉明距离（两个不同二进制之间的汉明距离指的是两个二进制串不同位的个数）。
有了计算描述子相似度的方法，那么在特征点的集合中如何寻找和其最相似的特征点，这就是特征点的匹配了。最简单直观的方法就是上面使用的：暴力匹配方法(Brute-Froce Matcher)，计算某一个特征点描述子与其他所有特征点描述子之间的距离，然后将得到的距离进行排序，取距离最近的一个作为匹配点。这种方法简单粗暴，其结果也是显而易见的，通过上面的匹配结果，也可以看出有大量的错误匹配，这就需要使用一些机制来过滤掉错误的匹配。
- 汉明距离小于最小距离的两倍
选择已经匹配的点对的汉明距离小于最小距离的两倍作为判断依据，如果小于该值则认为是一个错误的匹配，过滤掉；大于该值则认为是一个正确的匹配。其实现代码也很简单，如下：
// 匹配对筛选
double min_dist = 1000, max_dist = 0;
// 找出所有匹配之间的最大值和最小值
for (int i = 0; i < descriptors1.rows; i++)
{
double dist = matches[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
// 当描述子之间的匹配大于2倍的最小距离时，即认为该匹配是一个错误的匹配。
// 但有时描述子之间的最小距离非常小，可以设置一个经验值作为下限
vector<DMatch> good_matches;
for (int i = 0; i < descriptors1.rows; i++)
{
if (matches[i].distance <= max(2 * min_dist, 30.0))
good_matches.push_back(matches[i]);
}
结果如下：
对比只是用暴力匹配的方法，进行过滤后的匹配效果好了很多。
-
交叉匹配
针对暴力匹配，可以使用交叉匹配的方法来过滤错误的匹配。交叉过滤的是想很简单，再进行一次匹配，反过来使用被匹配到的点进行匹配，如果匹配到的仍然是第一次匹配的点的话，就认为这是一个正确的匹配。举例来说就是，假如第一次特征点A使用暴力匹配的方法，匹配到的特征点是特征点B；反过来，使用特征点B进行匹配，如果匹配到的仍然是特征点A，则就认为这是一个正确的匹配，否则就是一个错误的匹配。OpenCV中BFMatcher已经封装了该方法，创建BFMatcher的实例时，第二个参数传入true即可，BFMatcher bfMatcher(NORM_HAMMING,true)。 -
KNN匹配
K近邻匹配，在匹配的时候选择K个和特征点最相似的点，如果这K个点之间的区别足够大，则选择最相似的那个点作为匹配点，通常选择K = 2，也就是最近邻匹配。对每个匹配返回两个最近邻的匹配，如果第一匹配和第二匹配距离比率足够大（向量距离足够远），则认为这是一个正确的匹配，比率的阈值通常在2左右。
OpenCV中的匹配器中封装了该方法，上面的代码可以调用bfMatcher->knnMatch(descriptors1, descriptors2, knnMatches, 2);具体实现的代码如下：
const float minRatio = 1.f / 1.5f;
const int k = 2;
vector<vector<DMatch>> knnMatches;
matcher->knnMatch(leftPattern->descriptors, rightPattern->descriptors, knnMatches, k);
for (size_t i = 0; i < knnMatches.size(); i++) {
const DMatch& bestMatch = knnMatches[i][0];
const DMatch& betterMatch = knnMatches[i][1];
float distanceRatio = bestMatch.distance / betterMatch.distance;
if (distanceRatio < minRatio)
matches.push_back(bestMatch);
}const float minRatio = 1.f / 1.5f;
const int k = 2;
vector<vector<DMatch>> knnMatches;
matcher->knnMatch(leftPattern->descriptors, rightPattern->descriptors, knnMatches, 2);
for (size_t i = 0; i < knnMatches.size(); i++) {
const DMatch& bestMatch = knnMatches[i][0];
const DMatch& betterMatch = knnMatches[i][1];
float distanceRatio = bestMatch.distance / betterMatch.distance;
if (distanceRatio < minRatio)
matches.push_back(bestMatch);
}
将不满足的最近邻的匹配之间距离比率大于设定的阈值（1/1.5）匹配剔除。
- RANSAC
另外还可采用随机采样一致性（RANSAC）来过滤掉错误的匹配，该方法利用匹配点计算两个图像之间单应矩阵，然后利用重投影误差来判定某一个匹配是不是正确的匹配。OpenCV中封装了求解单应矩阵的方法findHomography,可以为该方法设定一个重投影误差的阈值，可以得到一个向量mask来指定那些是符合该重投影误差的匹配点对，以此来剔除错误的匹配，代码如下：
const int minNumbermatchesAllowed = 8;
if (matches.size() < minNumbermatchesAllowed)
return;
//Prepare data for findHomography
vector<Point2f> srcPoints(matches.size());
vector<Point2f> dstPoints(matches.size());
for (size_t i = 0; i < matches.size(); i++) {
srcPoints[i] = rightPattern->keypoints[matches[i].trainIdx].pt;
dstPoints[i] = leftPattern->keypoints[matches[i].queryIdx].pt;
}
//find homography matrix and get inliers mask
vector<uchar> inliersMask(srcPoints.size());
homography = findHomography(srcPoints, dstPoints, CV_FM_RANSAC, reprojectionThreshold, inliersMask);
vector<DMatch> inliers;
for (size_t i = 0; i < inliersMask.size(); i++){
if (inliersMask[i])
inliers.push_back(matches[i]);
}
matches.swap(inliers);const int minNumbermatchesAllowed = 8;
if (matches.size() < minNumbermatchesAllowed)
return;
//Prepare data for findHomography
vector<Point2f> srcPoints(matches.size());
vector<Point2f> dstPoints(matches.size());
for (size_t i = 0; i < matches.size(); i++) {
srcPoints[i] = rightPattern->keypoints[matches[i].trainIdx].pt;
dstPoints[i] = leftPattern->keypoints[matches[i].queryIdx].pt;
}
//find homography matrix and get inliers mask
vector<uchar> inliersMask(srcPoints.size());
homography = findHomography(srcPoints, dstPoints, CV_FM_RANSAC, reprojectionThreshold, inliersMask);
vector<DMatch> inliers;
for (size_t i = 0; i < inliersMask.size(); i++){
if (inliersMask[i])
inliers.push_back(matches[i]);
}
matches.swap(inliers);
之前写过一篇OpenCV的特征点匹配及一些剔除错误匹配的文章，OpenCV2:特征匹配及其优化，使用的是OpenCV2，在OpenCV3中更新了特征点检测和匹配的接口，不过大体还是差不多的。上一篇的文末附有练习代码的下载链接，不要直接打开sln或者project文件，有可能vs版本不一样打不开，本文的测试代码还没有整理，等有时间好好打理下github，练习的代码随手都丢了，到想用的时候又找不到了。
后记
翻了下，上一篇博客还是6月30号发布的，而今已是12月底，半年6个月时间就这样过去了。而我，好像没有什么成长啊，工资还是那么多，调试bug的技术倒是积累了很多，知道多线程程序调试;多进程通信;学会了用Windebug:分析dump文件，在无代码环境中attach到执行文件中分析问题或者拿着pdb文件和源代码在现场环境中进行调试...;实实在在的感受到了C++的内存泄漏和空指针导致的各种奇葩问题；知道了使用未初始化的变量的不稳定性；知道了项目设计中扩展性的重要的...
写之前觉得自己虚度了半年，总结下来，这半年下来时间还是成长了不少的，心里的愧疚感降低了不少。不过以后还是要坚持写博客记录下学习的过程...
