ElasticFusion 解析

泡泡机器人ElasticFusion讲解PPT：
http://pan.baidu.com/s/1eSDuhZs

文章评价

1、基于 RGB-D 稠密的三维重建，一般使用网格模型融合点云，ElasticFusion 是为数不多使用 surfel 模型表示的。
2、传统的 SLAM 算法一般通过不断优化相机轨迹或者特征点的方式，来提高轨迹估计或者重建的精度，这篇文章采用不断优化重建的 map 的方式，提高重建和位姿估计的精度。
3、优化采用 deformation graph 的形式，和 DynamicFusion 中优化的方式如出一辙。
4、融合了重定位算法（当相机跟丢时，重新计算相机的位姿）。
5、算法使用 OpenGL 对点云进行更新、融合、显示和投影。
6、算法融合 RGB-D 图像进行位姿估计，对于 RGB 图像通过颜色一致性约束计算位姿，对于点云通过 ICP 算法计算位姿，ElasticFusion 融合了二者。
7、适合重建房间大小的场景，代码没有做优化，重建较大场景时，代码不能适用。

后续博客给出代码结构。

文章和 2009 年 ICCV 上发表的文章 “In-hand Scanning with Online Closure” 思路很类似

算法流程

1、ElasticFusion 通过 RGB-D 图像配准计算位姿，根据输入点云利用 ICP 算法配准计算位姿的方法参见博客：
http://blog.csdn.net/fuxingyin/article/details/51425721

2、计算相机位姿如果误差大于设定阈值，表示跟踪失败，启动重定位算法；如果误差小于设定阈值，则进入下一部分。

3、利用 Random Ferns 进行全局的回环检测算法（后续博客单独介绍），检测是否存在全局的回环，如果存在全局的回环，假设当前帧为 Icur$I_{cur}$ 检测到和第 i$i$ 帧存在回环；再利用第一部中的跟踪算法计算当前帧和第 i$i$ 帧之间的位姿，计算得到位姿变换后，在图像中均匀抽取一些点，建立约束，优化 node 参数（关于如何优化 map 后续文章还会展开介绍）。

4、在第 3 部中如果不存在全局的回环，则检测是否存在局部的回环（局部回环检测算法后续会展开介绍），如果存在局部的回环，则同第 3 步，进行位姿估计，并且建立约束，优化 node 参数。

5、计算得到相机位姿后，将当前帧的点云和重建好的做融合，融合使用 openGL 的 shading language，如果在存在局部的或者全局的回环，在使用 openGL 进行点的融合时候，将优化之后的节点变量，作用于全部的点。

6、融合到全局模型中后，再用 openGL 投影得到当前视角下可以看到的点，用来对下一帧图像配准。

surfel 表示模型

实时的 RGB-D 重建一般用 KinectFusion 算法中提到的 TSDF 模型，这篇文章用 surfel 模型（点表示模型），示意图如下：

对于每个点，存储了:
点的位置信息：(x,y,z)$(x,y,z)$
面片得半径: r$r$
法向量： n$n$
颜色信息：(R,G,B)$(R,G,B)$
点的获取时间信息：t$t$

在进行点的融合更新时：点的位置信息，法向量，和颜色信息的更新方法类似于 KinectFusion 采用加权融合的方式，面片的半径通过场景表面离相机光心的距离的求得，距离越大，面片的半径越大。

Deformation Graph

由于每次重新初始化一个新的 deformation graph 要比保持更新一个已有的计算量小，并且简单可行，对于每次新获取的点云融合后，初始化一个新的 deformation graph。

Deformation Graph 结构

deformation graph 由一些 nodes 组成，node 是在重建好的点均匀抽样得到，如上图所示，红色的表示抽取的 node，黑色的表示重建好的其它的点，node 的数量和重建好点的数量成正相关。

每个 node 包含了本身的位置信息 Gng$G_g^n$ ，待优化的仿射变换参数 GnR$G_R^n$ 和 Gnt$G_t^n$ ，利用局部回环检测和全局回环检测建立的约束优化参数 GnR$G_R^n$ 和 Gnt$G_t^n$ 。

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 点对齐，需要按照时间关系抽取。关于 ACTIVE 点和 INACTIVE 点定义见下文。

建立模型点和 Graph nodes 的图连接

建立每个模型点和 Deformation Graph nodes 图连接：

对于模型中点每个点，在 Graph nodes 中时间最近的节点 c$c$ ，

找到时间上最相近的点后，和 Graph nodes c$c$ 前后 α$\alpha$ 个节点计算距离，并且选取距离最近的 k$k$ 个节点相连接。

建立连接后根据和每个 node 之间的距离关系计算更新模型点的权值。

根据模型点和 Graph nodes 的图连接优化模型点

模型中的点按照和 node 之间的距离关系，按照加权平均的方式，用优化之后的 GnR$G_R^n$ 和 Gnt$G_t^n$ 更新位置和法向量。

上式中 Msp$M_p^s$ 是模型中的点座标（非 node）
上式中 M^sp${\hat{M}}_p^s$ 表示 node 作用后的座标
GnR$G_R^n$ 和 Gnt$G_t^n$ 指建立连接 node 优化后的参数
wn(Ms)$w^n(M^s)$ 是node n$n$ 的权值

法向量更新如下所示：

权值计算公式如下：

在 Msp$M_p^s$ 周围选取 n+1$n+1$ 个 nodes，每个 node 的权值和 node 到 Msp$M_p^s$ 的距离成负相关，dmax$d_{max}$ 指和第 n+1$n+1$ node 的距离。

对于模型点和 Graph nodes 建立连接，并且对模型点做位姿优化，在 openGL 中的 shader 中进行。

局部回环检测算法

当相机移动的时候，存在回环，如果检测不出来，重建出来的结果会有重影。

如上图，相机开始不断地向左移动，移动一段距离后，返回到以前经过的位置，如第2幅图所示。

局部的回环检测算法是把重建好的点，按照时间节点划分成 ACTIVE 和 INACTIVE，ACTIVE 是最近时间内重建好的点，INACTIVE 是以前重建好的点，根据第一步计算得到的位姿，将点云向相平面做投影，可以计算得到两帧点云，将这两帧点云配准。

如图 3 所示，如果两幅点云有交叠，存在回环，那么投影得到的两帧点云可以配准上（第一步计算的误差小于一定阈值，Hessian 矩阵特征值大于一定阈值，详见论文）。

如果可以配准上，存在回环，则抽取一些点建立约束
建立的约束如下表达式所示：

Qp=(TcurP(u, Dat); TinaP(u, Dat); tcur; tina)=(QPs; QPd; ts; td)$Q_p=(T_{cur}P(u,D_t^a);T_{ina}P(u,D_t^a);t_{cur};t_{ina})=(Q_s^P;Q_d^P;t_s;t_d)$

其中 ：
Tcur$T_{cur}$ ：当前帧的 pose。
p(u, Dat)$p(u,D_t^a)$ ：通过上一帧 pose 从模型投影得到的点。
Tcurp(u, Dat)$T_{cur}p(u,D_t^a)$ ：active 点座标。
Tinc$T_{inc}$ ： 将投影得到的相机系下的 active 点 align 到世界系下的 inactive 点的 pose，pose 通过将世界系下的 inactive 点设为配准的模型，将相机系下的 active 点设为待配准的帧获得。 TinaP(u, Dat)$T_{ina}P(u,D_t^a)$ 为世界系下 inactive 点座标。
tcur$t_{cur}$ ：当前帧的时间
t$t$ ：对应的 INACTIVE 点的时间（模型投影点）

约束的含义是将 ACTIVE 点对齐到 INACTIVE 点。

全局回环检测

后续文章给出全局回环检测 Random Ferns 的论文和代码解析，ElasticFusion 重定位算法也是用的 Random Ferns 算法。

这里说下为什么有了局部的回环检测，为什么还要用全局的回环检测。

当相机移动距离或者角度较大时，因为累积误差的作用，当存在回环时，ACTIVE 和 INACTIVE 对应点不能交叠，从 ACTIVE 和 INACTIVE 投影出的点也解算不出来位姿变换来（两帧配准误差大于阈值），所以，这个时候需要全局的回环检测。

同样全局的回环检测建立约束：
Qp=(TcurP(u, Dat); TFernP(u, Dat); tcur; tFern)=(QPs; QPd; ts; td)$Q_p=(T_{cur}P(u,D_t^a);T_{Fern}P(u,D_t^a);t_{cur};t_{Fern})=(Q_s^P;Q_d^P;t_s;t_d)$

其中：
Tcur$T_{cur}$ ：当前帧 pose。
p(u, Dat)$p(u,D_t^a)$ ：通过上一帧 pose 从模型投影得到的点。
TFern$T_{Fern}$ ：检测到的回环帧的 pose。

tcur$t_{cur}$ ： 当前帧时间戳。
tFern$t_{Fern}$ ：回环帧时间戳。

含义：将当前帧点云对齐到 Fern 的点云

通过局部和全局回环检测建立的约束优化 node 参数

用全局和局部回函检测建立的约束优化节点的参数 GnR$G_R^n$ 和 Gnt$G_t^n$ ，优化的目标函数如下：

优化过程中，保持仿射变换参数 R$R$ 是单位正交的

优化过程中，保持相邻的 node 间参数的连续性

和模型点的更新策略一样，用公式 (8) (9) (10) 更新建立的约束点的座标 ϕ(Qps)$\varphi (Q_s^p)$ ，下式的含义是，将 source 点向 destination 点做优化，将两次重建好的点对齐。

固定 destination 点，优化前后 destination 点不能变化太大，改变 source 点座标，向 destination 点对齐。

综合以上优化以下目标函数

优化后的 deformation graph 参数，再用 openGL shader 作用于模型中其它点。

版权声明：本文为博主原创文章，未经博主允许不得转载。