本文为原创博客，转载请注明出处：https://blog.csdn.net/q_z_r_s

机器感知 一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号

视觉里程计

特征点法

ORB特征

一种基于FAST角点提取,BRIEF描述子的算法，由于FAST不具备尺度不变性和旋转不变性，虽然FAST检测可以说是非常高效的，为了克服这两个缺点,针对尺度问题，像著名的算法SIFT、SURF类似，在ORB中通过构建图像金字塔来实现FAST的尺度不变性；针对旋转不变形问题，也是通过对像素进行某种运算操作来确定主方向，在ORB中使用检测到的特征点周围某一小块图像的所谓的质心来构建主方向，两点确定一条直线，再根据两点的像素坐标就可以算出与坐标轴的夹角，此种方法美其名曰：Oriented FAST。

BRIEF是一种二进制描述子，特点也是快，看论文上的数据表现的性能还是很不错的，其生成的方法是按照某种概率分布来选取特征点周围的像素，然后比较选中的两点，产生一个bit的0或者1，而且其匹配时采用的是Hamming distance，也就是一个位异或操作就可以算出来有多少个对应的位不同，这在现代计算机、甚至是单片机都拥有这样的指令，可想而知，根据距离进行匹配的操作，相比于最常见的欧氏距离来说，那是相当快了。其缺点是不具备旋转不变形(或者论文中说的比较大的旋转的场景)，不过，由于改进Oriented FAST生成了方向信息，那么在生成BRIEF描述子之前，只要把坐标系统一到主方向上去就可以去除旋转带来的影响了，即拥有旋转不变性。

综上，两种高效的算法的结合在平移、旋转和缩放的情况下仍可良好的工作，这也是为什么ORB特征在SLAM中如此的受宠的原因所在。OpenCV中使用方法：

Mat image = imread("picture.jpg");
Ptr<ORB>* orb = ORB::creat();
vector<Keypoint> keypoints;
orb->detect(image, keypoints);
Mat descriptor;
orb->compute(image, keypoints, descriptor);
           

特征匹配

特征匹配就是根据ORB生成的描述子来进行匹配，不过目前的描述子多是根据特征点周围像素信息生成的，所以很显然的有局限性，比如在相似纹理很多的地方，很容易造成误匹配问题，因此，下一阶段的描述子生成可以尝试加入一些全局信息，比如特征点与特征点之间的关系等等。

匹配方法有很多，最简单的就是暴力匹配(Brute-Force Matcher)，OpenCV中对应的类为

BFMatcher

，使用方法如下：

BFMatcher matcher(NORM_HAMMING);//使用汉明距离
vector<DMatch> matches;//存储相匹配描述子对应的索引值和距离信息等
matcher.match(descriptors1, descriptors2, matches);
           

然而,当特征点数量很大时,暴力匹配法的运算量将变得很大,特别是当我们想要匹配一个帧和一张地图的时候。这不符合我们在 SLAM 中的实时性需求。此时快速近似最近邻(FLANN)算法更加适合于匹配点数量极多的情况。

2D-2D：对极几何

假设我们从两张图像中,得到了一对配对好的特征点，根据旋转和平移变换可以建立匹配点之间的数学关系式，经过变形之后就可以得到对极几何约束。主要得到两个矩阵：基础矩阵(Fundamental Matrix)F 和本质矩阵(Essential Matrix)

对极约束简洁地给出了两个匹配点的空间位置关系。于是,相机位姿估计问题变为以下两步：

1. 根据配对点的像素位置，求出E或者F；
2. 根据E或者F，求出R，t。

由于E和F只相差了相机内参，而内参在SLAM中通常是已知的，所以实践当中往往使用形式更简单的 E。E具有五个自由度的事实,表明我们最少可以用五对点来求解E。但是，E的内在性质是一种非线性性质,在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性,使用八对点来估计E——这就是经典的

八点法

(Eight-point-algorithm)。接下来对E进行SVD分解，然后就可以得到R和t。OpenCV使用方法：

//由于对极约束推导时，是根据归一化平面投影点进行的，所以需要传入相机内参
Mat findEssentialMat( InputArray points1, InputArray points2,//像素坐标系下的坐标
                      InputArray cameraMatrix, //相机内攒
                      int method = RANSAC, double prob = 0.999, 
                      double threshold = 1.0, OutputArray mask = noArray() );
Mat findEssentialMat( InputArray points1, InputArray points2,
                      double focal = 1.0, Point2d pp = Point2d(0, 0),//fx=fy=focla
                      int method = RANSAC, double prob = 0.999,//pp=[cx, cy]
                      double threshold = 1.0, OutputArray mask = noArray());
           

在OpenCV中从本质矩阵恢复R和t的方法：

int recoverPose( InputArray E, InputArray points1, InputArray points2,//本质矩阵E
                 InputArray cameraMatrix, OutputArray R, OutputArray t,//相机内参
                 InputOutputArray mask = noArray() );//像素坐标系坐标
           

单应矩阵多了一个约束条件，即认为特征点都处在同一平面上，且求解单应矩阵只需要4对匹配点即可。把H矩阵看成了向量,通过解该向量的线性方程来恢复H，又称直接线性变换法(Direct Linear Transform)。与本质矩阵相似,求出单应矩阵以后需要对其进行分解，才可以得到相应的旋转矩阵R和平移向量t。单应性在SLAM中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降，这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声，这时候如果我们继续使用八点法求解基础矩阵,基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响,通常我们会同时估计基础矩阵F和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计矩阵。OpenCV中对应的方法：

//虽然也是在归一化平面上得出的，但是它们具有相同的深度，深度因子互相抵消
  findHomography( InputArray srcPoints, InputArray dstPoints,
                  int method = 0, double ransacReprojThreshold = 3,
                  OutputArray mask=noArray(), const int maxIters = 2000,
                  const double confidence = 0.995);
  findHomography( InputArray srcPoints, InputArray dstPoints,
                  OutputArray mask, int method = 0, double ransacReprojThreshold = 3 );
           

由于E本身具有尺度等价性，它分解得到的t，R也有一个尺度等价性。而 R∈SO(3)自身具有约束，所以我们认为t具有一个尺度。换言之,在分解过程中，对t乘以任意非零常数，分解都是成立的。因此，我们通常把t进行归一化，让它的长度等于1。单目初始化不能只有纯旋转，必须要有一定程度的平移。当给定的点数多于八对时，我们可以计算一个最小二乘解。不过，当可能存在误匹配的情况时,我们会更倾向于使用随机采样一致性(Random Sample Concens-us，RANSAC)来求，而不是最小二乘。RANSAC 是一种通用的做法,适用于很多带错误数据的情况,可以处理带有错误匹配的数据。

三角测量

在单目SLAM中，仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量(Triangulation)(或三角化)的方法来估计地图点的深度。三角测量是指,通过在两处观察同一个点的夹角，确定该点的距离。在SLAM中，我们主要用三角化来估计像素点的距离。由上一步解算得到的R和t之后，同样的，根据归一化坐标点之间的关系来得出约束方程，然后带入坐标点点进行求解。OpenCV对应的方法：

void triangulatePoints( InputArray projMatr1, InputArray projMatr2,//图1、2的其次位姿
                        InputArray projPoints1, InputArray projPoints2,//相机坐标系坐标
                        OutputArray points4D );//输出(x,y,z,w),w不为1，所以要points4D/w
           

3D-2D：PnP

直接线性变换

直接根据空间点P的坐标与特征点归一化平面坐标的关系建立方程式，此方法最少需要6对匹配点方可实现对矩阵T=[R|t]的求解。这种方法(也)称为直接线性变换(Direct Linear Transform, DLT)。当匹配点大于六对时，(又)可以使用 SVD 等方法对超定方程求最小二乘解。在DLT求解中，我们直接将T矩阵看成了12个未知数，忽略了它们之间的联系。因为旋转矩阵R∈SO(3)，用DLT求出的解不一定满足该约束，它是一个一般矩阵。平移向量比较好办，它属于向量空间。对于旋转矩阵 R，我们必须针对DLT估计的T的左边3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由QR分解完成 ，相当于把结果从矩阵空间重新投影到SE(3)流形上，转换成旋转和平移两部分。

P3P

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点为 A，B，C，2D点为a，b，c，其中小写字母代表的点为大写字母在相机成像平面上的投影，A，B，C为三个世界坐标系中的坐标。三角形之间存在对应关系，根据此关系就可以求解出世界坐标系的点在相机坐标系下的3D坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。

//第一张图特征点对应的世界坐标系坐标,第二张图匹配的特征点的像素坐标系坐标
bool solvePnP( InputArray objectPoints, InputArray imagePoints,
               InputArray cameraMatrix, InputArray distCoeffs,
               OutputArray rvec, OutputArray tvec,
               bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );
//其中flags有如下选择：
enum { 
    SOLVEPNP_ITERATIVE = 0,
    SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation
    SOLVEPNP_P3P  = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem 
    SOLVEPNP_DLS  = 3, //!< A Direct Least-Squares (DLS) Method for PnP
    SOLVEPNP_UPNP = 4  //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation
};
           

Bundle Adjustment

BA优化中，把位姿看做待优化变量，最有准则是：最小化重投影误差(Reprojection error)。在g2o中，已经提供了以李代数形式给出的Edge和Vertex的相关定义，不需要自己再重新定义，直接包含头文件

g2o/types/sba/types_six_dof_expmap.h

即可，详细使用可见

Bundule Adjustment 相关源码分析

。

3D-3D:ICP

SVD方法

首先根据匹配点之间的数学关系，建立误差项，然后把整个问题构建成一个最小二乘问题，最后得出最小化两个矩阵相乘的迹的问题，之所以成为SVD方法，是因为这里用到了SVD分解来求解旋转矩阵R，解得R之后，t也就确定了。

非线性优化问题

此方法与PnP中的BA非常相似，以李代数表示位姿，目标函数为最小化各个误差平方和项构成的误差函数，然后使用g2o进行优化。可以证明，ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下，只要我们能找到极小值解，那么这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况。这也意味着ICP求解可以任意选定初始值。这是已经匹配点时求解 ICP 的一大好处。

在匹配已知的情况下，这个最小二乘问题实际上具有解析解，所以并没有必要进行迭代优化。ICP 的研究者们往往更加关心匹配未知的情况。

不过,在 RGB-D SLAM 中，由于一个像素的深度数据可能测量不到，所以我们可以混合着使用 PnP和 ICP 优化：对于深度已知的特征点，用建模它们的 3D-3D 误差；对于深度未知的特征点，则建模 3D-2D 的重投影误差。于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便。

直接法

考虑某个空间点 P 和两个时刻的相机。P 的世界坐标为 [X, Y, Z],它在两个相机上成像,记非齐次像素坐标为 p1，p2 。我们的目标是求第一个相机到第二个相机的相对位姿变换。我们以第一个相机为参照系,设第二个相机旋转和平移为 R, t，两相机的内参相同。在直接法中，由于没有特征匹配，我们无从知道哪一个 p2 与 p1 对应着同一个点。直接法的思路是根据当前相机的位姿估计值，来寻找 p2 的位置。但若相机位姿不够好，p2 的外观和 p1 会有明显差别。于是,为了减小这个差别，我们优化相机的位姿，来寻找与 p1 更相似的 p2 。这同样可以通过解一个优化问题，但此时最小化的不是重投影误差,而是

光度误差(Photometric Error)

，也就是 P 的两个像的亮度误差。然后再根据扰动模型分析亮度误差函数，可得出雅克比矩阵。最后，求解位姿的问题又变成了一个非线性优化问题。

稀疏直接法

P来自于稀疏关键点(比如FAST检测到的角点等)，我们称之为稀疏直接法。通常我们使用数百个至上千个关键点，并且像 L-K 光流那样，假设它周围像素也是不变的。这种稀疏直接法不必计算描述子，并且只使用数百个像素，因此速度最快。

半稠密的直接法

P来自部分像素。如果像素梯度为零，整一项雅可比就为零，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点(或者梯度大小满足一定的要求的点)，舍弃像素梯度不明显的地方。这称之为半稠密(Semi-Dense)的直接法。

稠密直接法

计算所有像素，不过，由于梯度不明显的点，在运动估计中不会有太大贡献，所以计算梯度不明显的点并无多大意义。