Dense Visual SLAM with Probabilistic Surfel Map(PSM)

Dense Visual SLAM with Probabilistic Surfel Map(PSM)

    本文提出了一种全新的稠密视觉SLAM的概率Surfel图表示方法（PSM），其主要思想是通过对光度和几何不确定性分析建模，来维护一个全局一致的地图。算法的关键在于对传感器的测量不确定性进行合理建模和更新，以及应用这些不确定性改进前端姿态估计和后端优化。

    文章的主要贡献为：

    （1）提出了一种能够对测量不确定性进行适当建模的概率Surfel图表示方法（PSM）。

    （2 ）提出了一种机制（主要通过关键帧PSM）来将PSM应用于稠密视觉里程计中的鲁棒位姿优化框架，成功地减少了相机的漂移。

    （3）对于后端优化，设计了一种全局一致的地图优化方案，该方案既利用了位姿图中的位姿约束，又利用了基于PSM的位姿点约束，以提高相机轨迹的精度。此外，还提出了一种通过变形关键帧来匹配全局PSM的稠密网格生成方法。

1. 视觉SLAM中的两种方法

1.1. Frame-to-frame Visual SLAM

    帧到帧视觉SLAM是一种预测相邻帧间相对相机位姿的方法，只保留局部3D地图而不进行全局地图融合。

1.2. Frame-to-model Visual SLAM

    帧到模型视觉SLAM通常建立一个全局环境地图，并使用该地图辅助前端位姿估计。全局地图的常用表示包括稀疏/稠密的三维点云、surfel地图和3D volumes。

    本文对这两种方法进行了结合，与帧到帧方法相比，保持了全局一致的地图表示，即PSM，不受体素表示方法的限制；与建立需要大量计算的稠密地图的帧到模型方法相比，文中的方法在地图融合过程中只保留具有高置信度的点，从而构建了一个可靠的基于稀疏点云的地图。当需要密集网格时，变形并融合关键帧中的稠密点云。通过这样做，文中方法需要更少的内存，并且在视觉SLAM期间计算效率更高，并且能够在需要时提供高质量的密集网格。

2. 概率surfel图

2.1. PSM

    PSM的表示：

    位置：，位置的协方差矩阵：
    强度值：，强度的协方差：
    法向量：
    高斯分布：，
    新来的测量值表示：

    法向量n近似为两个向量的叉积，一个向量是从这个点到它右边的邻居点，另一个是到它下方的点。

2.2. PSM的更新

2.3. 算法流程图

3. Dense Visual Odometry With Probabilistic Surfel Map

3.1. Robust Camera Pose Estimation

    给定RGB-D输入，最小化光度（强度）和几何（深度）误差，以产生鲁棒性和准确性更高的位姿估计

    位姿估计：

    残差项：

    使用Lucas-Kanade算法进行优化。

    Kerl等提出，光度残差近似满足具有5个自由度的零均值的t分布。

    紧接着在DVO中扩展为光度残差和几何残差均近似满足t分布。

    Babu等在σ-DVO中提出混合加权的方法，将t分布应用于光度残差，概率传感器噪声模型应用于几何残差。

3.2. PSM-Based Visual Odometry

    主要思想是将全局PSM中的现有部分与当前关键帧的新观测结合起来，构造一个关键帧PSM，用于与该关键帧相关联的帧的相对位姿估计。

    用得到的关键帧PSM来修正残差项：

    在得到一帧的相对位姿估计后，更新全局PSM：

4. Back-End Optimization With Probabilistic Surfel Map

4.1. Keyframe Insertion & Loop Closure

    主要使用两种策略来筛选关键帧：

    （1）和基于共视图的方法相似，当当前帧中的活动点数小于阈值时，生成新的关键帧。

    （2）考虑不确定性，当强度的平均不确定性和深度的平均不确定性

    超过一定阈值时，表示当前帧和地图之间存在严重的误匹配，这种情况下之前的帧会转换得到新的关键帧。

    在插入新的关键帧后，通过在kd树中搜索最近邻的关键帧位姿来检测回环并建立位姿约束。

4.2. Map Optimization

    待优化的变量包括相机位姿和PSM点，使用两种约束：pose-pose constraint和pose-pointconstraint。

    算法流程如下：

4.3. Dense Mesh Deformation

    采用ElasticFusion中的非刚性变形的思想，利用PSM和关键帧位姿生成稠密的环境网格。文中并没有在世界坐标系下变形多个稠密点云，而是将每个关键帧的深度图向PSM变形，最后将变形后的深度图融合在一起。

5. 实验结果

    采用多线程的方式，在DVO SLAM的基础上实现。

    实验环境：Intel Xeon E5 @ 2.4GHz

    数据集：TUM、ICLNUM synthetic dataset from Handa

    评估：use the root mean squared of tracking errors (ATE,[m] and relative pose errors (RPE,[m/s]) to measure the camera drift in visualodometry；and the root mean squared absolute tracking errors(ATE, [m]) to measure trajectory error for the complete SLAM system。

5.1. Quantitative Comparison on the TUM RGB-D Dataset

5.2. Sample Rate

    在算法中，另一个直接关系到精度和计算性能的重要因素是面元数。为了避免计算时间过长，不必注册所有像素，而是统一地从每个新关键帧采样点，只将其中的一小部分注册到PSM中。

5.3. Dense Surface Estimation

6. 展望

    处理动态环境，结合视觉惯性SLAM。