视觉里程计之非直接法

在slam的前端中有两大主流算法，根据是否提取特征点，分为直接法和非直接法。那本文章就带大家领略一下优秀的非直接法叭 wink~
我们将分为三部分进行讲解，分别为 特征提取、特征匹配、位姿估计算法设计

1.1 特征提取
对于计算机而言，一幅图像就是由一组像素组成的数字阵列，图像可以认为是一种数学表达。特征是图像中一些特别的地方，足够数量特征的集合一定程度上能够代表一幅图像。这就要求特征具备一些稳定鲁棒的性质，如：
1）可复制性：同一特征在不同图像中的表达式一样的。
2）可区分性：不同特征在同一图像中的表达是不一样的。
3）有效性：特征应是像素区域的精炼表达，特征数量必然要小于像素数量。
4）代表行：特征是一块图像区域的表达，应保证细致度。
一般而言，特征由关键点及其描述子组成。关键点就是在图像中选取的特别的点，描述子则是对该关键点的一个数学描述，一般以该关键点周围部分区域其他点进行对照来获取这个数学描述。目前，在视觉SLAM领域研究较多的典型特征有Harris、SIFT、SURF、ORB等。本文章选用ORB特征作为视觉里程计的非直接法的算法基础。

ORB特征由FAST关键点和BRIEF描述子构成，并人为地赋予了旋转和尺度不变性。

FAST（Features from Accelerated Segment Test）关键点提取分为五步：粗提取、初步筛选FAST关键点、提出局部密集FAST关键点、确定FAST关键点、添加尺度不变性和旋转不变性。

第一步，粗提取。对于一点p，其像素值为Ip，选取以p为中心，3为半径的圆上的16个点，若有12个以上的点的像素值与Ip之间的差值大于阈值，则认为p为FAST关键点候选点。在检测时，可以先将这16个点排序，然后依次取圆上1，5，9，13位置的像素，若有三个及以上点差值超过阈值，则继续判断，否则，直接排除，判断其为非FAST关键点。

第二步，初步筛选FAST关键点。经过第一步的粗提取我们会得到大量的FAST关键点，而这一步我们要通过机器学习中的决策树思想，来进一步筛选FAST关键点。
对于第一步中的任一关键点p，与其半径为3的圆上16个像素点的像素关系有以下三种可能：

对于所有候选点，可以得到三个映射集Pd、Ps、Pb，这三个映射就代表关键点的三个特征，有这三个特征建立决策树，由下面的式子可以计算每个子集Q的信息熵：

其中c为子集内FAST关键点的数量，c非(…好难啊)表示子集内非FAST关键点的数量（这16个点中很可能包含第一步中提取的其他关键点）。由下式计算以p为根节点的信息增益：
然后以信息增益为标准，其值最高的像素点设定为决策树的根节点，并继续对其子集进行迭代，知道确定该点的性质，即是否为FAST关键点。于是便生成了ID3决策树，利用该树筛选出最优的FAST关键点。

第三步，剔除局部密集FAST关键点。经过上两步，我们已经剔除了许多点了，但是此时的点还有一个缺点，就是在局部很密集，因此我们这一步将利用非极大值抑制来剔除局部密集的点。我们根据下式计算每个FAST关键点的得分：

计算方式是特征点P和其周围16个特征点偏差的绝对值和。在比较临近（局部）的特征点中，保留响应值（得分）较大的特征点，删除其余的特征点。

第四步，确定FAST关键点。即使经过上述两次筛选，得到的FAST关键点数量仍可能很大，而往往希望获取固定的数量特征。这一步进行最后的筛选，确定提取数量。这次我们利用Harris特征的原理，假设指定特征数量为N，按照式对所有FAST关键点分别计算Harris响应值，然后筛选响应值排在前N位的FAST关键点作为最终的特征集合。
介绍一下Harris特征，其是利用图像中邻近区域像素灰度值一阶梯度进行检测。核心思想是如果该点沿多个方向灰度变化都较大，则为特征点（角点）；若只在一个方向上灰度变化较大，则为边缘点；若在各个方向灰度变化值变化均不大，则为内点。那我们要怎么来提取Harris特征呢？滑动窗口法。通过下式评价灰度变化的程度：

其中[u,v]是窗口W的偏移量，(x,y)是窗口所对应的像素点位置，I(x,y)是对应位置的像素值，w(x,y)这里一般选择的是高斯
加权函数，即离中心点近的梯度贡献更大，距离中心远的点贡献小。
通过上式可以看出，在平坦区域的灰度变化不大，E(u,v)很小；而在纹理丰富的区域，E(u,v)会比较大。
下面看一组图辅助理解：

那现在我们要开始计算E(u,v)了，怎么计算呢？为了简化计算量，我们采用泰勒公式对其简化：

其中
M为协方差矩阵，采用主成分分析法（PCA），将其对角化：
其中， λ1和 λ2是M的特征值，如图：

从上图可知，λ1和 λ2处于什么关系式时为Harris特征点。实际检测时，用下式进行度量：

第五步，赋予特征尺度和旋转不变性。尺度不变性利用金字塔原理实现，将图像降采样处理得到图像金字塔，对每一层都完成上述四步特征提取，实现FAST关键点尺度不变性。旋转不变性通过灰度质心法实现，计算以关键点为中心图像块U的质心，把中心到质心的向量定义为关键点的方向，实现FAST关键点的旋转不变性。
特征由关键点和描述子两部分组成，提取完FAST关键点后，就要计算他们的描述子。本文采用BRIEF（Binary Robust Independent Elementary Features）描述子，这是一种二进制描述子，仅由0和1组成。
为减少噪声干扰，先对图像进行高斯滤波（方差为2，高斯窗口为9x9）：
以特征点为中心，取SxS的邻域窗口。在窗口内随机选取一对（两个）点，比较二者像素的大小，进行如下二进制赋值。
其中，p(x)，p(y)分别是随机点x=(u1,v1),y=(u2,v2)的像素值。
选取n（一般为256）个点即可得到二进制字符串：

1.2 特征匹配

特征提取算法中使用的特征描述子为二进制描述子，使用汉明距离来完成度量是否为同一点，并考虑旋转不变性来达到对旋转的图像的特征匹配。汉明距离的计算可以使用异或操作。
提取图像特征的数量总是很大的，然而我们暴力匹配效率太低。因此我们选用快速最近邻FLANN算法来进行匹配。即将特征点投影到哈希空间，相邻点在哈希空间相邻几率高，不相邻的点在哈希空间相邻几率小，这样即可在哈希空间进行匹配，有效缩小了范围。这样实现了特征匹配的最近邻搜索算法，大幅度提高了匹配效率。

1.3 位姿估计计算
在提取并匹配两个相邻的特征之后，使用匹配关系来估计相机的运动和姿态。相机成像过程称为观测过程。
这里就不赘述各坐标系之间的转换了，可以看这篇博客计算机视觉：摄像机标定（循序渐进理解到应用）
最后得到这个关系式：
视觉里程计位姿的估计是对于相邻两帧图像而言，多个这样的帧位姿估计累计就是相机的运动轨迹，如下式：

其中， Tcnw表示第 n 帧的世界坐标系到相机坐标系的变换， Tcncn-1表示相机坐 标系下第 n-1 帧到第 n 帧的变换。显而易见，两帧间位姿的估计关键在于求解转换 矩阵 Tcncn-1。
两帧间位姿估计常用算法有对极几何、ICP、PnP等算法。对极几何进利用两帧的像素坐标。ICP利用两帧的相机坐标，但相机坐标的获取由像素点的深度转换而来。PnP利用n个点的优化后的相机坐标、世界坐标和当前帧的像素坐标。本文采用PnP进行两帧间位姿的粗略估计、再利用RANSAC算法进行该位姿的帧间一致性优化，避免误匹配。
PnP算法的核心思想是已知前一帧优化好的点的世界坐标和当前帧中对应的像素坐标，然后求得当前帧的相机坐标，转换为简单的三维到三维的位姿估计问题。因为用的RGB-D相机，可以直接初始化Tc0w，然后按上式进行视觉里程计位姿估计。PnP最少需要四个点，利用三个点的几何关系和一个点做验证。如下图：
可得关系式：
除以OC^2, 并记x = OA/OC, y = OB/OC, v = AB^2 / OC^2, u = BC^2 / AB^2, w = AC^2 / AB^2:

最后解得：

于是就将位姿估计转换成了三维坐标的问题，很容易求出R，t。最后利用RANSAC算法对该问题进行优化。
RANSAC算法是一种利用迭代的方式消除噪声拟合模型的方法。即随机选n个点假设为内点，建立模型，然后寻找符合该模型的其余点。经过多次迭代，找到最适合的模型。那问题来了，应该迭代多少次呢？设 P 中 N 个数据中内点数 据比例为 ω，z 表示迭代 k 次后至少有一次选取的 n 个数据都是内点的概率：
则k要满足：
为了优化特征误匹配问题，本次选择拟合模型为单应矩阵模型。单应矩阵通常描述处于共同平面的一些点在两帧之间地变换关系。对于一对匹配好的特征点p1和p2，位于平面P上，P满足：



记单应矩阵H = K(R-(t * n^T)/d) K^-1，H是一个3x3矩阵：

令h9 = 1，则有：

由此，一组匹配点可以构造两个约束，求解八个未知数地单应矩阵需要四组匹配点，也就是RANSAC每次迭代中选择地数据数量为4。由此4组数据计算单应矩阵H，然后利用此矩阵计算其他数据的拟合程度，代价函数为：

k次迭代后，选取代价函数最小的模型即为最优模型，选择该模型地内点作为PnP算法的特征点，即可消除错误匹配带来的干扰和误差。

至此，视觉里程计 完结，撒花！