LSD-slam算法原理

LSD-slam算法原理

论文：LSD-SLAM: Large-Scale Direct Monocular SLAM

一、 预先知识：

1、三维刚体与相似变换

1.1 三维刚体变换

仅包括6个自由度，即三轴旋转和三轴偏移，变换矩阵定义如下：

李代数与变换矩阵的转换关系：
定义李代数：$\xi \in se\left(3\right)$
$$G={exp}_{se(3)}(\xi ),\xi =lo{g}_{SE(3)}(G)$$
变换矩阵叠加相乘，对应的李代数的运算：

1.2 三维相似变换

包括7个自由度，除三轴旋转及三轴偏移之外，还有一个尺度因子s，用于对深度测量的尺度进行评测。相似变换矩阵定义如下：

2、基于李群的带权重的高斯牛顿优化

直接法的误差公式：

其中，$\omega \left(p,d,\xi \right)$函数利用两帧间的位姿变换𝜉及像素深度d，将图像点p转换到对应相机坐标系下的图像坐标，即将图像坐标转换为另一帧的的图像坐标系，定义如下：

使用高斯牛顿法进行迭代，步长如下：

更新李代数：

考虑鲁棒性，减小一些错误约束对整体优化求解的影响，在误差方程及迭代过程中加入权重因素，如下所示：

对应的迭代步长变为：同样是高斯牛顿，在误差方程中加入权重，则对应的雅可比也相应变化。

最终的解算结果$\xi$与真实值之间的误差满足以下分布：

其中，解算误差方差与叠加的顺序有关，本文使用的是左乘。
$${\Sigma }_{\xi }={\left(J^{T}WJ\right)}^{-1}$$

3、不确定性评估

已知输入变量$X$的方差为${\mathrm{\Sigma }}_X$，则对应函数$f\left(X\right)$的方差为：

二、 LSD算法思路

LSD是一个单目大尺度半稠密定位及建图的方法，之所以是版稠密是因为深度地图只选用图像中具有较大梯度的图像点。

LSD的三个核心部分：

• 图像位姿跟踪
  跟踪部分主要任务是估计连续的图像对应的相机的位姿，它根据当前关键帧估计当前图像对应的相机位姿。相机位姿的初始值使用上一帧图像对应的相机位姿。
• 深度估计
  首先根据估计得到的相机位姿的变化，判断是否使用该帧图像创建关键帧，当相机距离当前使用的关键帧对应的时刻相机的距离大于一定阈值，则使用该帧图像创建新的关键帧，并得到新的深度地图。如果距离过短，则将该帧图像的深度估计结果进行卡尔曼融合到当前使用的关键帧上。无论是否新建关键帧，都需要进行深度估计，并进行深度正则化，筛除离群点。
• 地图优化
  这个部分完成两个任务：位姿优化及点云地图优化功能。此处的位姿优化与第一部分?(跟踪)不同的是，跟踪部分是每帧图像的位姿估计，使用6自由度的变换矩阵se(3)来评估，用于评估深度，而第三(本)部分的位姿是基于关键帧之间的位姿估计，使用的是相似矩阵sim(3)来评估。
  当关键帧成为参考帧，其对应的深度图不再被优化，则将该帧的深度图融合到全局地图。回环检测和尺度估计，对相近的两个关键帧进行相似变换矩阵评估，仍使用直接图像匹配法。

初始化：
LSD的初始化关键帧使用随机生成的深度及较大的方差值。需要使相机进行较大的平移运动，才能使算法的深度值收敛。

地图显示：
地图显示的是基于关键帧的位姿图，每个关键帧Κi包括：图像$I_i$，反深度地图$D_i$，以及深度方差$V_i$三部分。只有具有较大灰度的图像区域才有深度及深度方差估计。这也是“半深度“的含义。位姿图包含，关键帧相机的位姿及关键帧之间相机的运动变化，即位姿图中的边，用李代数${\xi }_{ji}$表示。同时，使用${\Sigma }_{ji}$表示“边”的方差。

三、算法各部分介绍

1、使用直接法估计图像帧的位姿

设当前最近的关键帧$Ki=(I_i,Di,Vi)$，李代数${\xi }_{ji}$表示新的图像帧$I_j$与图像帧$I_i$对应的相机的位姿变化，$I_i$被选为关键帧。${\xi }_{ji}$的求解通过最小化以下光度误差来获得：

其中，$||\ast |{|}_{\delta }$是Huber范函数，如下表述：

此外，$r_p^2$为光度残差， ${\sigma }_{r_p}^2$则对应表示光度误差的方差。
另外，求解加入权重因子，上文(第一大节，2小节) 所述。
该算法加入了对不确定的评估，不同像素的深度方差不同。

2、深度评估

深度评估更详细的介绍见lsd-slam深度滤波器

2.1 关键帧选择

当相机的运动大于一定的阈值之后，会创建新的关键帧。帧间运动距离的评估如下公式：

其中$W$是一个包含权重的对角阵，注意，缩放每个关键帧，使其平均反向深度为1。距离的阈值与关键帧的缩放因子相关来保证深度立体计算使用的基线较小。

2.2 深度地图创建

  关键帧的深度图在继承之前关键帧深度假设，并后期使用新的观测值进行滤波优化，再通过正则化去除离群点后被加入全局地图。然后，将深度图缩放为平均反向深度1-此缩放因子直接合并到sim(3)相机位姿中。最后，该帧将替换先前的关键帧，并用于跟踪后续的新帧。

3 基于相似矩阵sim(3)的直接法

算法的第三部分分为两部分：关键帧位姿估计及地图优化
本节主要说关键帧位姿估计，地图优化在下一节。

3.1 基于相似矩阵sim(3)的直接图像对齐

主要思路：将关键帧反深度归一，利用相似变换sim(3)去计算尺度以及相机位姿，并进行可靠性评估。

  单目slam，不同于双目以及RGBD相机能够获取环境点云深度，因此，单目只有图像坐标，不能直接对像素对应的深度进行测量，就是所谓的尺度，尺度(scale)指的就是图像归一化坐标到三维点的缩放因子，就是深度值。
通常在运行一段时间之后，单目VO就是出现尺度漂移。从而关于尺度的约束公式都是失效。
  该论文提出深度以及位姿估计的不确定性传播的方法。每个关键帧的反深度平均值，都通过缩放因子缩放为1，同时，计算相似变换矩阵sim(3)得到两帧之间的运动估计即位姿图中的边(edge)。其中sim(3)也包含了对尺度因子的评估。设该基于相似变换矩阵的直接法约束方程：

其中，同第一部分，同样使用Huber范数。 $r_p^2$为光度残差， ${\sigma }_{r_p}^2$则对应表示光度误差的方差。与上文定义相同。
而$r_d^2$为深度残差， ${\sigma }_{r_d}^2$则对应表示深度误差的方差。表述公式如下：

其中， $p^{{}^{\prime }}=\omega (p,D_i(p),{\xi }_{ji})$表示将i关键帧的图像点p映射到j关键帧的图像坐标。

Tips:
  通常一个离群点的两项误差均比较大。
  使用相似变换矩阵sim(3)来描述尺度和位姿时，需要加入深度约束。因为光度  误差不能完全约束深度，所以不能完全对尺度进行评估。
  同样使用，带权重的高斯牛顿进行求解。
  Sim(3)的解算相比se(3)稍微大一点点。

3.2 回环检测

  在关键帧的附近保留10个相近的关键帧，用于检测回环。使用FABMAO的方法检测回环，并用以下马氏距离进行双向评估，检测回环的正确性：

  当上式足够小，则认为回环达到。其中${Adj}_{jk}i$为伴随矩阵，保证${\Sigma }_{ijk}$处于一正确的正切空间。【这里的原因不知】。
  关于马氏距离的介绍，参见马氏距离
  马氏距离能够评估不均匀分布的变量的距离。

3.3三维变换矩阵sim(3)的收敛半径

  直接图像对齐的一个重要限制在于问题固有的非凸性，因此需要足够精确的初始化。虽然对于图像帧的位姿跟踪，有足够好的初始化可用（由前一帧的位姿提供），但在查找循环闭合约束时，尤其是对于大型循环闭合，情况并非如此。
  因为误差曲线是波浪不平的，因此除了使用较为精确的初始化，保证全局最优的唯一性外，也需要增大收敛半径，相当于平缓误差曲线，增大最优解周围的过度区间。防止最优解被跳过。
  解决上述问题的办法之一是使用少量的关键点计算较为精确的初始值，可以使用两帧之间已知深度的3D点云进行计算。
  另外，以下两种方法也可以增大sim(3)的收敛半径

• 1） 二阶最小化(ESM)
  直接法是使用两帧图像进行图像对齐，得到估计位姿，设这两帧为i帧和j帧，ESM的思路是使用双向的步长的均值作为迭代的步长，就是分别计算i帧作为参考帧，j帧作为被匹配帧的迭代步长，以及j帧作为参考帧，i帧作为被匹配帧的迭代步长求平均得到最后的迭代步长。
  就是求下降方向的时候，一个对i求导，一个对j求导，从而得到两个步长，将两个步长进行平均，当然，需要对其中一个取负向。详细参见另一篇文章直接法FC&IC*ESM求解方法介绍。
• 2） 从粗到精的多层金字塔方法
  多层金字塔多用在直接法中，作者经过试验发现，低分辨率的图像约束能够提高收敛半径。

4 地图更新

此为lsd算法第三部分中除位姿优化的另外一个环节，地图优化是指：全局的位姿图优化
约束方程如下：