基于稀疏直接法的SLAM算法

张超，曹雄，徐春凤，韩成，白烨

（1.长春理工大学 计算机科学技术学院，长春 130022；2.长春理工大学光电信息学院，长春 130022）

SLAM（Simultaneous Localization and Mapping，同时定位与建图）的主要功能是指搭载特定传感器的主体，在没有环境先验信息的情况下，在运动过程中估计自己的位置和状态信息，同时建立环境模型[1]。2007年，Andrew Davison首次提出了基于视觉的MonoSLAM[2]，但是MonoSLAM只适合小场景。Klein和Murray在MonoSLAM基础上提出PTAM[3]，将视觉SLAM分为跟踪和建图线程，但PTAM具有跟踪易丢失，适合小场景等缺点。2014年Jakob Engel等人提出基于直接法的LSD-SLAM方法[4]。LSD-SLAM使用直接法进行半稠密地图的构建，并且算法的鲁棒性也会增加不少，而缺点是计算量大，精度相对较低。2016年Mur-Artal提出了ORB-SLAM2[5]，这种方法具有优秀的特征点和回环算法，但同时也存在着依赖特征点、计算量大等缺点。为此文献[6]使用平方根滤波方法进行SLAM的改进。

为了提高视觉SLAM算法构建地图的精度，通过分析对比目前的直接法和特征点法SLAM算法中的优缺点，提出了基于稀疏直接法的SLAM算法（简称SD-SLAM，sparse direct SLAM），最后通过实验验证了算法能够有效提高相机轨迹的精度。

1 基于稀疏直接法的SLAM

SD-SLAM流程如图1所示，在前端中使用改进的Shi-Tomasi特征点检测算法进行稀疏特征点的提取，然后基于稀疏特征点采用直接法进行相机位姿估计，同时利用构建的地图进行位姿优化，以提高相机轨迹精度。在后端中加入文献[5]中的回环检测模块以保证算法的完整性。

图1 算法流程图

1.1 改进的Shi-Tomasi特征提取算法

文献[7，8]使用SURF和SIFT特征提取算法进行目标跟踪，但SURF和SIFT特征提取算法并不能满足SLAM的实时性。Shi-Tomasi角点检测法[9]具有计算量大和不具备尺度不变性的缺点，而FAST角点检测算法[10]仅仅通过检测亮度差来确定角点，具有较快的速度。因此结合Shi-Tomasi和FAST角点检测算法，并对融合后的算法进行改进。使用改进后的算法提取特征点，然后基于稀疏特征点进行后续的相机位姿估计。特征点提取的步骤如下：

（1）对图像构建高斯金字塔；

（2）在图像金字塔每层上检测FAST特征点作为候选特征点；

（3）对每个候选特征点进行Shi-Tomasi角点检测并排序，选取前N个作为特征点；

（4）使用灰度质心法[11]为选择出来的特征点添加方向信息。

1.2 稀疏直接法位姿估计

如图2所示，在追踪线程中，相机运动到当前帧Im时，已知上一帧Im-1的位姿Tm-1、帧上提取的特征点u以及u对应深度du。需要求得当前帧Im和上一帧Im-1的位姿变换Tm,m-1。

图2 帧间灰度误差

因为单个特征点的灰度没有区分性，所以使用特征块（特征点的4*4领域）的灰度值。将Im-1帧上特征块u根据其深度du逆投影（π为投影方程）到Im-1帧相机坐标系中的三维空间点P，P经过帧间位姿Tm,m-1变换到Im帧相机坐标系中，再投影到Im帧上形成u′。

由于噪声导致位姿估计存在误差，所以u′与P的真实投影（图2中虚线框所示）存在误差e。

采用非线性优化方法，将误差e定义为误差项，将相机位姿Tm,m-1定义为优化变量，不断调整位姿使误差最小，构建最小二乘问题求解位姿Tm,m-1。

公式（3）为帧与帧之间的相机位姿估计，但不可避免的累计误差会导致相机轨迹精度下降，所以需要对相机位姿Tm,m-1进行优化。

文献[12]中提出，当前帧上的部分特征点对应的空间点可以被局部地图中的某些关键帧观测到，利用这个数据关联可以优化当前帧特征u′i的位置。如公式（4）所示，其中Ai为仿射变换函数[13]，Im为当前帧，In为ui所在关键帧。

由于关键帧和当前帧的环境光源和相机曝光可能发生变化，造成P点投影在帧Im和In中的点的灰度发生变化，不符合直接法的灰度不变性假设。所以对公式（4）进行改进以修正环境造成的偏差，得到精确的特征位置u′i。

其中，b为灰度补偿参数（下标m，n分别代表当前帧和关键帧）。在求得优化后的特征位置u′i后，以当前帧相对世界坐标系的位姿Tm,w为优化变量，将世界坐标系下空间点Pi的投影误差作为误差项，通过最小化误差项，优化相机位姿Tm,w。

帧序列相对于世界坐标系的相机位姿Tm,w构成相机轨迹。因此在前端中使用稀疏特征点和直接法进行相机位姿估计，同时使用构建的地图优化位姿，从而得到精度更高的相机位姿估计。

1.3 后端

前端的作用为追踪相机运动，后端需要对数据进行优化。首先在图像帧序列中生成关键帧，关键帧的选取关系到算法的运行效率。算法中关键帧的判断标准为：

（1）与上一关键帧间隔最少20帧；

（2）跟踪到的地图点的数量不少于50；

（3）与上一关键帧观测到地图点的重合率小于10%。

同时满足以上三个条件则创建关键帧，并对关键帧上的特征点进行Brief描述子[13]的计算，以便进行回环检测。关键帧中存储着帧位姿、特征点以及对应的地图点信息。

在局部优化中删除冗余的地图点。如果观测到该地图点的关键帧数量小于3，则认为该地图点冗余并删除该点。

最后添加了文献[5]的回环检测模块，使算法拥有回环的能力，提高算法的鲁棒性。

2 算法实现与结果分析

算法运行平台采用Intel-E3处理器，NVIDIA Quadra k620显卡，内存8Gb，操作系统为Ubuntu14.04。

采用TUM标准数据集作为实验使用的图像序列。TUM数据集是一个用于估计摄像头定位精度的优秀数据库，提供了许多图像序列，包括对应的轨迹真值。为了比较ORB-SLAM2和SD-SLAM算法的轨迹精度，使用TUM提供的检测工具检测轨迹的绝对误差。表1记录了ORB-SLAM2算法和SD-SLAM算法运行不同图像序列的估计轨迹和真实轨迹的均方根误差对比。图3展示了SD-SLAM算法运行不同图像序列的估计轨迹和真实轨迹的对比。

表1中可以看出在fr1/room、fr2/desk和fr2/360h图像序列中，SD-SLAM算法的轨迹精度相对ORB-SLAM2算法有所提高。图3展示了SD-SLAM算法运行4个图像序列的相机估计轨迹在xy轴上与真实轨迹的对比图，图中实线为真实轨迹，虚线轨迹为估计轨迹。fr2/360h图像序列相比其它图像序列场景中的纹理较少，而SD-SLAM算法需要提取特征进行位姿估计，所以SD-SLAM算法在运行fr2/360h图像序列时误差相对较大。

总体来说SD-SLAM能完整运行4个图像序列，在部分图像序列中相对ORB-SLAM2算法提高了轨迹精度。

表1 4个图像序列下两种算法的位姿轨迹均方根误差（m）对比

图3 SD-SLAM在4个图像序列中的估计轨迹与真实轨迹对比

3 结论

通过在前端中使用改进Shi-Tomasi特征点检测算法进行特征点提取，然后基于提取的稀疏特征点采用直接法进行相机位姿估计，同时利用构建的地图进行位姿优化。在后端中加入回环检测模块，保证算法的完整性。实验验证了SD-SLAM算法在适合特征提取的场景下相对ORB-SLAM2算法能有效地提高相机轨迹精度。

SD-SLAM算法的改进之处还有很多，例如算法的效率还需要进一步提高；在环境缺少纹理、相机强旋转运动时，跟踪容易丢失的问题依旧存在。