ElasticFusion: Dense SLAM Without A Pose Graph
ElasticFusion: Dense SLAM Without A Pose Graph
一、主要贡献
-
surfel-based稠密建图。
-
将surfel分为激活区域和非激活区域，一段时间内未观测到的归入非激活区域。
-
每一帧，尝试把激活模型中当前相机估测位姿部分，登记至非激活模型在当前帧的部分，若登记成功，则检测到闭环，进行非刚体变换。发生闭环的非激活部分再次激活，放入登记的区域。（每一帧把同视角下的激活部分和未观测到的非激活部分匹配，来检测闭环）。
-
对于全局闭环，向随机encoding database里添加预测视角，每一帧匹配这个视角，如果匹配到，就登记这个视角并检测是否全局一致。如果是，对地图进行非刚体变换，消除闭环。
二、算法流程
-
ElasticFusion通过RGB-D图像配准计算位姿，根据输入点云利用ICP算法配准计算位姿。
-
计算相机位姿如果误差大于设定阈值，表示跟踪失败，启动重定位算法；如果误差小于设定阈值，则进入下一部分。
-
利用Random Ferns进行全局的回环检测算法，检测是否存在全局的回环，如果存在全局的回环，假设当前帧为Icur 检测到和第i帧存在回环；再利用第一部中的跟踪算法计算当前帧和第i帧之间的位姿，计算得到位姿变换后，在图像中均匀抽取一些点，建立约束，优化node参数。
-
在3中如果不存在全局的回环，则检测是否存在局部的回环，如果存在局部的回环，则同第3步，进行位姿估计，并且建立约束，优化node参数。
-
计算得到相机位姿后，将当前帧的点云和重建好的做融合，融合使用openGL 的shading language，如果在存在局部的或者全局的回环，在使用openGL进行点的融合时候，将优化之后的节点变量，作用于全部的点。
-
融合到全局模型中后，再用openGL投影得到当前视角下可以看到的点，用来对下一帧图像配准。
三、Surfel表示模型
对于每个点，存储了:
点的位置信息：(x, y, z)
面片的半径： r
法向量： n
颜色信息：(R, G, B )
点的获取时间信息：t
在进行点的融合更新时：点的位置信息，法向量，和颜色信息的更新方法类似于 KinectFusion采用加权融合的方式，面片的半径通过场景表面离相机光心的距离的求得，距离越大，面片的半径越大。
四、fused predicted tracking
4.1. Geometric Pose Estimation
几何误差针对深度图，error是在surfel表示中的顶点vertex中计算。
使用point-to-plane error:
4.2. Photometric Pose Estimation
光度误差针对彩色图（强度图），误差是在像素中计算。
4.3. Joint Optimisation
最小化联合cost：
求其jacobian，构造最小二乘形式，使用高斯牛顿法优化最小二乘问题，使用了三层的金字塔结构来迭代求解。
五、Deformation Graph
使用surfel级的表面闭环检测来实现局部和全局的一致性。模型表面闭环检测通过对所有surfel（激活和非激活的）进行非刚体形变来进行，非刚体形变的依据是表面约束（surface constraints）。具体方法是，基于surfel构造一个形变图(deformation graph)，通过优化形变图来进行surfel的调整，从而进行模型表面的形变。
由于每次重新初始化一个新的deformation graph要比保持更新一个已有的计算量小，并且简单可行，对于每次新获取的点云融合后，初始化一个新的 deformation graph。
Deformation Graph结构:
node是在重建好的点均匀抽样得到，如上图所示，红色的表示抽取的node，黑色的表示重建好的其它的点，node的数量和重建好点的数量成正相关。
5.1. Deformation Graph构建
新获取的点云融合到全局模型中后，每次初始化新的Deformation Graph，每次初始化新的Deformation Graph 要比保持更新同一个graph计算量小而且还要简单可行。
均匀抽取重建好的模型中的点来初始化Deformation Graph，抽取点的个数和重建好的模型点的个数成正相关，由于每次新的点添加到模型时是按照时间的先后顺序进行的，均与抽取Deformation Graph 的点也是按照时间先后顺序排列的。
抽取的Deformation Graph点之间建立连接，根据Deformation Graph node的时间来建立连接，而非根据Deformation Graph node 空间上的关系建立连接，按照空间关系建立连接，会造成ACTIVE和INACTIVE交叠的点相互干扰，并且 Deformation Graph是将ACTIVE点向INACTIVE点对齐，需要按照时间关系抽取。
5.2. 建立模型点和Graph nodes的图连接
建立每个模型点和 Deformation Graph nodes 图连接：
对于模型中点每个点，在Graph nodes中时间最近的节点c，找到时间上最相近的点后，和Graph nodes c前后α个节点计算距离，并且选取距离最近的k个节点相连接，建立连接后根据和每个node之间的距离关系计算更新模型点的权值。
5.3. 根据模型点和Graph nodes的图连接优化模型点
位置坐标更新：
法向量更新:
Node权值更新：
六、局部回环检测
局部的回环检测算法是把重建好的点，按照时间节点划分成ACTIVE和 INACTIVE，ACTIVE是最近时间内重建好的点，INACTIVE是以前重建好的点，根据第一步计算得到的位姿，将点云向相平面做投影，可以计算得到两帧点云，将这两帧点云配准。
如果可以配准上，存在回环，则抽取一些点建立约束，建立的约束如下表达式所示：
七、全局回环检测
全局的回环检测建立约束：
Randomized Ferns：
八、通过局部和全局回环检测建立的约束来优化node参数
九、Surfel融合
十、评估
1. 轨迹估计
数据集：RGB-D benchmark of Sturm
对比工作：DVO SLAM，RGB-D SLAM, MRSMap and Kintinuous.
指标：absolute trajectory (ATE) root-mean-square error
metric (RMSE)
2. 面元重建精度
数据集：ICL-NUIM dataset of Handa http://www.doc.ic.ac.uk/∼ahanda/VaFRIC/iclnuim.html
评估标准: mean distances from each point to the nearest
surface in the ground truth 3D model
轨迹精度（ATE RMSE）：
面元评估结果：
其中轨迹Kt1没有回环，所以和fame-to-model的结果相同。
具体效果：
3. 计算复杂度
Intel Core i7-4930K CPU at 3.4GHz, 32GB of RAM and an nVidia GeForce GTX 780 Ti GPU with 3GB of memory.
平均帧率22~31Hz。
十一、文章总结
-
基于RGB-D稠密的三维重建，一般使用网格模型融合点云，ElasticFusion 是为数不多使用surfel模型表示的。
-
传统的SLAM算法一般通过不断优化相机轨迹或者特征点的方式，来提高轨迹估计或者重建的精度，这篇文章采用不断优化重建的map的方式，提高重建和位姿估计的精度。
-
优化采用deformation graph的形式，和DynamicFusion中优化的方式类似。
-
融合了重定位算法（当相机跟丢时，重新计算相机的位姿）。
-
算法使用OpenGL对点云进行更新、融合、显示和投影。
-
算法融合RGB-D图像进行位姿估计，对于RGB图像通过颜色一致性约束计算位姿；对于点云通过ICP算法计算位姿，ElasticFusion融合了二者。
-
适合重建房间大小的场景，代码没有做优化，重建较大场景时，代码不能适用。