[SLAM] a bite of SLAM

SLAM的定义及用途：

如它的名字所告诉我们的：即同时定位（Localization）与建图（Mapping）。应用场景一般多见于机器人导航，场景识别等任务。

SLAM的主要过程：

跟踪运动中的相机，估算出其在每个时刻的位置和姿态（用一个包含旋转和平移信息的变量来表示：矩阵或者向量），并将相机在不同时刻获取的图像帧融合重建成完整的三维地图。

传统的SLAM可分为视觉前端和优化后端两大模块。

视觉前端：

视觉前端主要完成的任务为：利用运动中的相机在不同时刻获取到的图像帧，通过特征匹配，求解出相邻帧之间的相机位姿变换，并完成图像帧之间的融合，重建出地图。视觉前端是一个局部优化过程，由于进行优化的约束条件仅限于相邻图像帧，因此在相机运动的过程中，容易产生误差漂移。

视觉前端依赖于机器人搭载的传感器，常用的传感器有相机（单目相机、双目相机、TOF相机），IMU（惯性测量单元），激光雷达等。单纯通过视觉传感器（即相机）来完成的SLAM又称为视觉SLAM。同时，根据使用的视觉传感器的不同，视觉SLAM也具有不同的算法复杂度和优缺点。

使用单目相机作为视觉传感器，优点是成本低，硬件简单，缺点是算法复杂，对相机的运动具有一定的条件要求，因为其是通过SFM（Structure From Motion）算法完成深度信息的计算，因此如果相机在运动时仅仅发生了旋转而没有平移的话，则无法通过Triangulation（三角测量）来计算目标的深度信息。同时，单目相机需要通过相机在不同时刻获取到的图像帧作为类似双目信息的数据来进行深度重建，因此在初始时刻是无法得到深度信息的。此外，由于SFM算法计算出的深度信息并不是真实的深度信息，因为它毕竟不是真正的双目深度重建。在双目相机中，在双目标定的时候已经通过标定板的信息完成了从像素尺度到真实尺度之间的映射。而SFM虽然也是通过相似三角形的关系计算出一个深度信息，但是该深度信息是一个相对值，并没有与真实尺度建立起联系。它的做法是：对于相机的初始平移，视为单位1。后续的平移，都是以这个单位进行换算的。而SFM重建出的深度，也是以这个单位来表示的。至于这个单位1对应的真实尺度，其实我们并不知道，只是人为地给定一个值而已（因为仅有一个单目视觉传感器是无法测量到真实距离的）。所以一般而言，对于单目SLAM，如果具有测量要求，一般都会与一个IMU配合使用，以获取真实尺度。

使用双目相机作为视觉传感器，优点是获取点云数据时，受物体表面材质的反光性质的影响比使用辅助光源的深度传感器方案（如TOF，结构光等）要小，同时其测量距离也相较其他方案要更远，深度图的分辨率更高。缺点是计算复杂度大（计算图像特征及完成特征匹配），同时对于特征的设计具有高鲁棒性的要求。这将导致双目视觉SLAM在建立稠密地图（dense map）和算法实时性上具有先天的劣势。

使用结构光方案的深度相机作为视觉传感器，与双目相机方案大同小异。原理都是利用了双目视差和三角测量，不同之处在于，结构光方案对图像的特征进行了编码，该编码特征在计算和匹配时相比双目相机方案的手工设计特征（如ORB，SIFT等）鲁棒性更好，计算更快，缺点是结构光对物体表面的材质敏感，如果是具有反射、透射性质的表面，或者是漫反射过强的表面，都会出现结构光编码信息的丢失，从而无法正确地匹配特征，导致深度信息的缺失。

使用TOF方案的深度相机作为视觉传感器，优点是帧率高，无需通过特征计算及匹配来获取深度信息，因此计算复杂度低，用于SLAM上具有更优的实时性。缺点同结构光方案，对物体表面的材质比较敏感。此外，TOF相机在图像分辨率和测量距离上，也同结构光方案，不如双目相机的方案。

在视觉前端中，主要的任务是完成对相机位姿的估计和建图，这时得到的还是一个局部优化的粗略结果，不可避免地包含噪声和误差，后续还需要通过后端优化，利用更多约束条件对其进行微调，以获得精确的位姿和地图。

估计相机位姿的常用算法是ICP（iterative closest point），简而言之就是寻找源点集中的每个点在目标点集中的最近邻点（用于判断最近点的距离可以在欧氏空间或特征空间中，可以是点到点的距离，也可以是点到面的距离）作为其对应点，建立起多组对应点。对于每组对应点，都用同一个仿射变换矩阵来描述这组对应点的坐标之间的关系（即旋转和平移关系），建立起一个超定方程组，求解出该仿射变换矩阵（之所以每组对应点之间都是同一个仿射变换矩阵，是因为这里描述的运动是一个刚体变换，源点集和目标点集都各自代表一个刚体对象）。计算出该仿射变换矩阵后，将其应用于源点集上，使源点集整体旋转平移到目标点集上，再计算变换后的各组对应点的坐标的总误差，如果小于给定阈值则停止迭代，如果大于给定阈值，则重复上述过程，直到两个点集的对齐（alignment）收敛。

在SLAM中的做法是，通过ICP来完成相邻帧之间的图像配准（前一帧作为目标点集，当前帧作为源点集），进而得到相邻帧之间的相机坐标系之间的旋转平移关系。通常以初始位置的相机坐标系作为全局坐标系，每一帧对应的相机坐标系相对于全局坐标系的旋转平移用于描述该帧对应的时刻的相机的位姿。由于所有相邻帧对之间的旋转平移都可以通过对该相邻帧对进行配准来得到，因此每一帧与第一帧之间的旋转平移也就都可以通过这些中间过程的旋转平移来计算得到，从而得到每一帧的相机位姿。

除了通过ICP算法直接对相邻帧的点云进行三维配准，图像配准还可以通过重投影算法来完成： 本质上是最小化重投影误差，即先将当前帧的点云投影到前一帧的相机坐标系中（投影使用的变换矩阵为需要优化的参数），再从前一帧的相机坐标系投影到前一帧的图像坐标系中；同时，前一帧的点云也重投影到其图像坐标系中。通过最小化该图像坐标系上这两个点集里所有对应点对的总误差，来得到当前帧和前一帧的相机坐标系之间的变换矩阵（也即相对位姿）。相邻帧的点云可以使用重投影算法来完成配准，相邻帧的RGB图像也可以使用重投影算法来完成配准，前者称之为几何位姿估计（geometric pose estimation），后者称之为光度位姿估计（photometric pose estimation）。

从配准对象不同的角度进行划分，位姿估计又可以分为frame to frame和frame to model两种方式。前者仅仅是利用相邻帧本身的信息完成图像配准，而后者则是利用当前帧与当前已重建的全局地图在前一帧中的投影来进行图像配准，由于用于配准的对象所包含的信息更多，因此配准效果相较于前者更优。

在经典的视觉SLAM算法中，特征计算使用的是人工设计的特征子和描述子，而近年来出现的一些视觉SLAM算法，则是利用了深度神经网络来提取图像帧的特征，在鲁棒性上相较于经典方法更优。

相机的位姿由一个旋转变换和一个平移变换来描述。由于相机位姿的求解是一个非线性优化问题，通常的一种求解思路是利用目标函数的梯度下降方向去寻找极值点，即通过不断地调整下降方向和搜索步长，以迭代的方式去逼近极值点。高斯牛顿法和LM算法（Levenberg-Marquardt method）就是其中的两种经典优化算法。而又因为这种迭代更新参数的策略要求参数对加法运算是闭合的，因此需要将不符合该性质的变换矩阵从李群形式转化为符合该性质的李代数形式，也即是使用一个无约束的向量来描述相机的旋转和平移运动。

在完成了位姿估计后，就可以通过每一帧的全局位姿，不断地将新的图像帧中的点云变换到全局坐标系中，与已重建的全局地图/模型完成融合（常见的融合方式是对在空间中非常接近的几个点进行加权融合，与相机的入射光线径向距离近的，与相机的光心距离近的，分配较大权重），逐帧地更新重建地图。对于稀疏SLAM（sparse SLAM）而言，在视觉前端中的建图部分至此就结束了。而对于稠密SLAM（dense SLAM）而言，此时得到的地图不仅是不够精确的，而且在可视化上式不够直观的，就是一群离散的点。所以对于稠密SLAM而言，还多了一个步骤，就是对地图/模型进行渲染。常用的两种表征模型是网格模型和面片模型，前者的代表为TSDF（Truncated Signed Distance Function），后者的代表为surfel（SURFace ELement）。

TSDF是将全局三维空间划分为一个三维网格，每个三维小格子储存其代表的三维空间位置距离相机光心的距离指示值。正数代表在物体表面的后面，负数代表在物体表面的前面，零值代表在物体的表面上，绝对值的大小表征距离物体表面的远近，超过-1和1的部分被截断为截断值。基于TSDF的稠密建图，是不断地利用位于不同帧的相机从不同视角获得的信息，从该视角将该帧的信息重投影到TSDF网格上，进而不断更新TSDF网格中各个三维小格子的指示值的过程。对于一个三维小格子而言，其指示值是从不同视角投影过来的信息共同作用的结果（加权求和）。得到了最终的TSDF网格后，就可以利用计算机图形程序完成地图的渲染，得到一个表面完整的三维模型。此外，利用光线追踪技术（ray casting），还可以得到在不同的位姿下，从该相机视角所看到的全局模型的轮廓。

surfel是一种类似于点云的表征方式。对于每一个三维点，除了三维空间坐标、RGB信息和表面法线信息之外，它还包含了其表面半径、初始时间戳（timestamp）、最后更新的时间戳、以及权重值（和跟相机光心的距离有关，距离越远，权重越小）。每个surfel都描述了一个单位表面的信息，该单位表面是一个圆形表面，且对于一个surfel集合而言，其所有的单位表面刚好可以完整地覆盖重建物体的表面。基于surfel的稠密建图，其实就是将稀疏建图的点云表征方式换成surfel表征方式，在得到最终的全局地图的surfel信息后，再利用计算机图形程序完成地图的渲染，进而得到一个表面完整的三维模型。

稠密建图由于计算量较大且易于并行化，多搭配GPU使用。

优化后端：

SLAM的优化后端完成的工作主要是对视觉前端得到的不够准确的相机位姿和重建地图进行优化微调。它可与视觉前端分开进行，作为一个离线操作，在近年来的一些SLAM方法中，也可以与视觉前端融为一体进行。

在视觉前端中，不管是进行位姿估计还是建图，都是利用相邻帧之间的关系来完成的，这种依赖局部约束且不停地链式进行的算法，必将导致优化误差逐帧累积，最终产生一个较大的误差漂移。因此，后端优化的思路就是从全局（整个相机运动过程）中选取一些关键帧，利用这些关键帧之间的关系建立起时间和空间跨度更大的、需要同时满足的全局约束，以优化之前得到的不够准确的各帧的相机位姿，实际上就是完成一个Bundle Adjustment。该全局优化问题可以通过建立和优化位姿图（pose graph）来求解。位姿图是以关键帧的全局位姿（被优化的参数）作为图的节点，以关键帧之间的相对位姿误差（对关键帧进行配准得到的相对位姿，与通过关键帧的全局位姿计算出的相对位姿，两者之间的误差）作为图的边的权重，通过令整个图的所有边的权重值总和最小，来优化得到每个图节点的值。这样可以使得本来单方向漂移的误差被摊到整个过程中，类似于完成对一个运动过程的估计的平滑（本质上的目的与卡尔曼滤波算法一致）。

图优化也是一个非线性优化问题，同样地，可以通过高斯牛顿法和LM算法来求解。

在完成了图优化后，关键帧的位姿得到了校正，因此就可以利用这些关键帧去对其相邻的数帧也进行位姿的校正。而在完成了所有帧的位姿校正后，也就可以利用这些校正后的位姿去重建更精确的地图。

在相机的运动过程中，有时候会重复地观测到以前观测过的区域（revisited areas），而由于视觉前端带来的误差漂移，会导致在再次对这些区域进行建图时，会与先前在同一区域建图的结果不重合，即出现了重影现象。因此在后端优化中，另一个重要的工作是进行闭环（loop closure）检测。所谓闭环检测，其实就是判断在相机的运动轨迹上，有没有观测到先前观测过的区域，而这些区域，就是闭环点。对闭环点的检测，也是通过对每一个到来的当前帧，都与各个历史关键帧进行比对来完成的：如果存在有一个历史关键帧与当前帧相似，即说明当前帧是一个闭环点。

用于闭环检测的常用算法主要有：词袋模型（bag-of-words model）和随机蕨算法（randomized fern method）。

词袋模型首先对一个图像帧上的描述子进行聚类：一个关于描述子的聚类称之为一个词，所有不同的词的集合称之为字典。对于在字典中和在单帧图像中出现频率较高的词，给予其较高的权重。当对比两个图像帧之间的相似度时，只要在这两个图像帧上，逐个词地进行权重对比即可，常用的用于衡量权重差异的尺度为L1范数。在逐词对比的过程中，首先需要从字典中按顺序检索出每一个用于帧间对比的词，这是通过K-D树来完成搜索的。

随机蕨算法是对一个图像帧进行稀疏编码（sparse encoding），即在图像帧的采样像素点上进行信息编码：一个像素点被称为一个蕨叶（fern），该像素点上的RGBD通道被称为该蕨叶上的一组节点（node），节点值都是binary的，即在每个节点上都有一个给定的阈值，该阈值将确定该节点值是0还是1。于是一个图像帧就可以用一个蕨丛来表征其信息。在随机蕨算法中，对于每个蕨叶都会建立起一个查找表，且查找表的每一行将对应这个蕨叶的一种编码结果。当每获取到一个新的图像帧，就将该图像帧的ID登记到描述其信息的蕨丛的各个蕨叶所对应的查找表的特定行上。如果有两个图像帧，其ID在超过某个给定阈值的数量的查找表里，都同时出现在同一行，则说明这两个图像帧相似，新获取到的这个图像帧是一个闭环点。反之，如果一个新获取到的图像帧，与所有的历史帧的相似度都足够小，则将该图像帧作为一个关键帧添加到关键帧的数据库中用于后续的闭环检测。随机蕨算法还可以在后端优化中完成位姿追踪失败后的重定位（re-localization）：在关键帧数据库中检索与当前帧相似度最高的几个关键帧，并利用它们与当前帧的相似度作为权重，对这些关键帧对应的位姿进行加权求和，作为对当前帧的位姿的一个估计。

在检测出闭环点之后，就可以利用闭环点建立起一个跨度较大的全局约束，其实就是将视觉前端中的单向链式约束的头尾相连，并根据头尾相连处的位姿必须相等作为一个自洽条件，进而将这个约束条件反馈到闭合后的整个环式结构的优化中，使得每个历史帧的位姿优化不仅依赖于其过去的图像帧的约束，也依赖于其未来的图像帧的约束。这样的优化过程鲁棒性更好，因为对不同的优化方向作出贡献的噪声将借由多约束条件在一定程度上相互抵消，误差漂移现象也因此得到改善。

SLAM的发展：

SLAM近年来发展衍生出的一些其他类型的变种有：非刚性SLAM，多目标模型SLAM，动态SLAM，语义SLAM等。

SLAM的一些开源项目：

在项目代码方面，当前比较具有代表性的一些开源SLAM方案有：

Dense SLAM: KinectFusion， ElasticFusion，BundleFusion，InfiniTAM，MaskFusion

Semi-Dense SLAM: LSD-SLAM

Sparse SLAM: ORB-SLAM