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。