LSD slam with stereo cameras (使用双目相机的LSD算法)

原文：Large-scale direct SLAM with stereo cameras. J Engel，J Stückler，D Cremers. 2015. IEEE/RSJ International Conference on Intelligent Robots & Systems

摘要：

基于双目相机的LSD算法是一种在普通CPU上能实时高帧率运行的双目SLAM算法。与基于稀疏角点的一些SLAM算法不同，该算法直接使用角点、边缘和纹理等高对比度的一些像素的摄影连贯性来关联对齐图片。双目相机在同一时刻采集到的两张图片（空间维度）与双目相机前后帧之间的图片（时间维度），同时使用这时间维度和空间维度的图片来估计出这些像素的深度数据。而且双目相机可以克服单目SLAM中常出现的尺度漂移或不定的情况。此外文中还介绍了一种鲁棒方法用来对付环境光线渐变的影响，极大地提高了该双目SLAM在真实环境中的性能。

III.  LSD-Slam with stereo

单目LSD-Slam算法是一种基于关键帧的SLAM算法，包括以下主要步骤：

*相机的运动跟踪到地图中的一个参考关键帧。如果当前帧离所有关键帧都较远，则生成新的关键帧；

*通过跟踪到的参考关键帧（时间维度上的双目）与当前帧的双目匹配点对计算出当前参考帧下的深度；

*通过各关键帧之间相互调整对齐和全局图优化把各关键帧的姿态优化。

而双目LSD-Slam可以直接利用双目算出关键帧的深度。这比只用时间维度或者空间维度的方法有一些优势，如可以直接获取场景中的尺度信息且与相机运动无关。不过也有一些弱点，比如双目的基线通常为固定长度，这样就会使得三维重构精度只在一定深度范围得到保证。而时间序列上的图片构成的双目由于基线不固定，在对于场景的规模上可以自动适应，不过，时间序列上的图片组成的双目三维重构不能得到尺度信息，也不能有退化的运动形式。时间序列双目和实际双目组合在一起会得到非常之好处。

A、符号说明

黑体大写字母表示矩阵，黑体小写表示向量，表示矩阵的第n行，d表示深度z的倒数。

在双目LSD中，地图由一系列的关键帧组成，第i个关键帧用记，大括号中前两项分别代表双目相机某时刻的左图像和右图像，Di是深度倒数图，Vi是方差图。这些图片都是双目矫正后的图片，以左相机为参考系。

像素坐标记为，场景中3D点记为；若相机参数矩阵为K，则有；由此可以得到3D点p为。

B、深度估计

使用若干关键帧估计出场景的几何信息，每个关键帧包括一系列特征丰富（高图像梯度值的角点）的像素及其含高斯噪声的深度倒数图。而其他纹理较少区域的像素不再考虑，因为其能提供的视差信息较少也不可靠。图3为半稠密深度图及其方差图的示例。根据深度传递假设，可以使用前一个关键帧的数据来给当前帧深度图初始化。再通过新测量数据来更新深度图，并且对深度图正则化和误匹配点剔除。

双目LSD-Slam与单目的不同，双目既使用空间维度信息也使用时间维度信息来估计场景深度。

a）空间维度的双目：使用双目极线约束来获取双目的视差图，如果双目图像经过了双目矫正，则可以非常迅速地在行对齐的特征点中找到匹配点对，而得到视差图。

在行搜索特征匹配中，使用5个像素宽度的SSD灰度匹配算法来匹配特征点对。经过亚像素精度优化后的匹配点对，用文献【16】中的方法来计算方差图。如果深度倒数图的误差假设为高斯模型，就可以把搜索误差限制在均值加减两倍方差范围内。实际中，除了新初始化的情况外，匹配收索范围很小，就在一两个像素的邻域内。这样既会大大加速匹配点对的搜索速度也会使得误匹配的情况减少。根据误差的两个来源，竖直方向附近的梯度大的像素和水平方向附近的梯度较小的像素所能获取的视差数据不太可靠，可以把它们直接忽略掉。

当一个新的关键帧初始化完成后，紧接着用空间维度双目来更新深度倒数图和方差图，并修整深度数据，剔除被遮挡和填充空洞的深度数据。并把跟踪到的非关键帧视差数据整合到其所属的关键帧上：首先，关键帧像素u的深度倒数根据估计到的姿态映射到当前帧上，并据此来双目匹配特征点，算出深度倒数和方差（1-3式）。

最后，测量到的深度倒数与方差根据（5-6式）映射到关键帧并融合到深度图上。对于非关键帧的测量数据只能产生自已存在的角点像素，见图2。

b）时间维度双目：跟踪后，用当前帧和参考关键帧估计出视差图，并把视差图融合到此关键帧上，而且只使用深度倒数误差较小的像素来融合。误差判断标准是：梯度应该足够大，梯度方向不要平行于极线方向，且远离极点，文献【16】有详细说明。由于简单地使用5个像素的SSD来判断匹配点误差，所有使用跟踪过程得到的仿射关系来矫正仿射光照变化，C小节将详细说明。由于相对姿态直接从透视关系对齐估计得到，时间维度的双目误差常常大于空间维度双目的误差。

C、利用仿射光照矫正的直接透视对齐

直接从成像透视关系对齐得到相机运动，据此获取相机基于参考关键帧的运动信息和关键帧之间相互运动信息的图优化。于此，提出一种鲁棒方法来补偿仿射光照变化。

1）直接图像透视对齐：两张图片之间的运动关系，通过最小化成像误差来估计（6式）。

其中，为的运动参数。

同时还要估计出方差，见文献【11】。把当前帧跟踪到一个关键帧的优化目标函数为：

其中是鲁棒加权函数，可以将其选为Huber范数。

与文献【12】不同的是，在这只是估计到的运动信息，若再使用右图片会降低估计精度。因为基线长度不一样，右图片基线更长，使得遮挡反光等引起的误匹配点增多。

再考虑上各关键帧之间的残差（8式）：

联合目标函数为：

9式充分利用了所有的有用信息，包括初始化传递来的深度数据和其他非关键帧融合过来的数据。这有别于文献【12】中的复杂的利用四视图的方法。

用迭代的加权列为马夸特算法优化（9式），使其达到最小值：首先给定一个迭代初值，在每次迭代时左乘迭代增量由求解E的二阶近似的最小值得到，即（10式）。

（11式）是残差的自变量左乘后的偏导；是E的海塞矩阵的高斯牛顿近似；W是一个对角矩阵，包含着权值。

由（12式）得到新的姿态估计。这是一种从大概到精确的迭代过程，为了得到较好的效率和收敛至最优值。

假设误差相互独立，从上一次迭代的得到的海塞矩阵的逆是左乘子的方差矩阵，即式13.

而实际中，各误差是不独立的，相关系数比较大，使得只是一个较小的界限。这还是很有用的，其包含了不同只自由度噪声之间的相关信息。

2）仿射光照矫正：图像透视关系对齐是基于图片灰度一致性进行的。而这常常不能得到满足，例如，根据灰度调整的自动曝光。一个比较著名的应对方法是使用不受仿射光照变化影响的代价函数，如使用NCC来双目匹配而不用SSD。在这提出一种新的方法，该方法通过修改（6式）使其不受仿射光照变化的影响，改后为（14式）。

在使用LM算法迭代求解时，不直接把所有参数一块同时优化。而是，（1）固定a，b，优化；（2）固定，优化a，b，同时使用不同的加权方案。这是因为和a、b对于误匹配点的效果不一样。

*遮挡和过曝光的像素会严重影响a、b所获取的最小值，通常是被“拉”向同一个方向。很少及几个匹配点就可以影响到最小值，所有使用最点单冒险的方法，直接去掉SSD误差，例如。图5显示了两个场景下的情况和剔除与不剔除误匹配点的仿射映射关系。

*误匹配点却很少影响下的最小值，因为误匹配点随机“拉”向各方向，没有确定趋势。结果是，的某些分量只受稍多匹配点影响，而初始的时候误差较大，此时剔除误匹配点会使得优化结果为局部最优值。因此使用文献【11】中的方法，不是直接移除误匹配点而是减小其权重。

迭代a、b最小化（15式），

其中，由匹配点集：

，

可得其闭合解为：

由此得到的a、b，接下来可以用在时间维度的双目和深度传递的一致性检测。

D、基于关键帧的SLAM

一个关键帧生成完成后（即当其变为跟踪的参考关键帧并不再进行深度更新时），就将其加入到姿态图里面，再进行优化。