算法特点
- 基于RGB-D稠密的三维重建，ElasticFusion使用surfel模型表示的。
- 传统的SLAM算法通过不断优化相机轨迹或者特征点的方式，来提高轨迹和重建的精度，而ElasticFusion则采用不断重建map的方式，来提高重建和姿态估计的精度。
- 优化使用了deformation graph。
- 重建过程中融入了重定位算法，会在误差超出阈值的时候启用。
- 算法融合了RGB-D图像进行位姿估计，对于RGB图像通过颜色一致性约束计算位姿，对于点云通过ICP算法计算位姿，ElasticFusion将二者融合。
- 适合重建房间大小的场景，重建较大的场景时不太适用。
- 原作者使用的摄像头为
Microsoft Kinect / ASUS Xtion Pro Live，以此进行实时重建，并且运行机器必须有GPU，因为算法利用CUDA提高了tracking的速度 - 系统结构图
算法流程
- ElasticFusion通过RGB-D的图形配准，计算位置和姿态，根据输入的点云，使用ICP算法进行配准与计算。
- 计算相机位置姿态的时候设置了阈值，如果误差超出范围，则启用重定位算法，如果小于阈值，则继续进行下一步。
- 利用Random Ferns进行全局闭环检测。如存在全局闭环，
- 如果不存在全局闭环，则检测是否存在局部闭环，如果存在局部闭环，则
- 计算得出相机的位置姿态后，将当前帧与重建好的模型做融合，如果存在闭环，则使用openGL融合时，将优化之后的节点变量作用与所有点。
- 当前帧融入到全局模型中，再用openGL投影得到当前可见点，用作下一帧的配准。
Fused Predicted Tracking
场景的表现是通过一系列的无序表面图元，与Maik Keller的表示方法相似。
Keller也是做了一个基于点的Fusion，pipeline主要分四步
- Depth Map Preprocessing：使用相机固有的参数，深度摄像头采集的数据被转化成3D的点，存储在2D矩阵中。
- Depth Map Fusion：已知相机的姿态，输入的point被融入到已有的点中。加入新的点时要判断能否找到投影后合适的位置，如果有则通过权重的方式选择最合适的点。如果没有仍然会把新的point融入，但是会设置成unstable point。
- Camera Pose Estimation：所有已经建立的可信任的点都进入到了可视化阶段。重建稠密用了
surface splatting technique，使用iterative closest point (ICP)计算位姿。 - Dynamics Estimation：动态检测变化。
融合预测的跟踪
场景是通过一个无序的surfel列表$M$表示的。其中每个$M^{\hat{s}}$具有下面的属性。
- 位置$p \in R^{3}$
- 法线$n \in R^{3}$
- 颜色$c \in R^{3}$
- 权重$w \in R$
- 半径$r \in R$
参数通过如下的规则进行更新
$\begin{aligned} \hat{\mathbf{p}} &=\frac{w \mathbf{p}+w^{\prime} \mathbf{p}^{\prime}}{w+w^{\prime}} \\ \hat{\mathbf{n}} &=\frac{w \mathbf{n}+w^{\prime} \mathbf{n}^{\prime}}{w+w^{\prime}} \\ \hat{r} &=\frac{w r+w^{\prime} r^{\prime}}{w+w^{\prime}} \\ \hat{w} &=w+w^{\prime} \end{aligned}$
几何位姿估计
通过当前帧深度图$D_t$与上一帧预测的模型深度图$\hat{\mathcal{D}}_{t-1}^{a}$，来找到运动参数$\xi$来最小化3D反投影顶点之间的店到平面的成本误差函数得到
$E_{i c p}=\sum_{k}\left(\left(\mathbf{v}^{k}-\exp (\hat{\boldsymbol{\xi}}) \mathbf{T} \mathbf{v}_{t}^{k}\right) \cdot \mathbf{n}^{k}\right)^{2}$
其中$\mathbf{v}_{t}^{k}$代编的是时间t这一帧深度图中第k个顶点在相机坐标系下的空间位置，$v^{k}、n^{k}$是建立的map中相关的顶点与向量。T是由之前的相机姿态估计得来的当前姿态。
光度位姿估计
在当前的实时RGB图像$\mathcal{C}_{t}^{l}$和上一帧预测的活动模型$\mathcal{C}_{t-1}^{l}$之间找到最小化像素间的光度误差。
$E_{r g b}=\sum_{\mathbf{u} \in \Omega}\left(I\left(\mathbf{u}, \mathcal{C}_{t}^{l}\right)-I\left(\boldsymbol{\pi}\left(\mathbf{K} \exp (\hat{\boldsymbol{\xi}}) \mathbf{T} \mathbf{p}\left(\mathbf{u}, \mathcal{D}_{t}^{l}\right)\right), \hat{\mathcal{C}}_{t-1}^{a}\right)\right)^{2}$
联合优化
使用如下的函数最小化联合成本
$E_{t r a c k}=E_{i c p}+w_{r g b} E_{r g b}$
通过下面的方式声明一个2D的像素$u=(x, y)^{\top} \in \mathbb{R}^{2} . \mathcal{D}_{i} \in \mathbb{R}$是每帧的raw depth。给出摄像机标定矩阵$K_i$，把$\mathcal{D}_{i}$转化成对应的向量图$V_i$。然后使用一个双边滤波器处理。六个自由度包括，三个旋转的自由度，三个平面向量。
