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 c u r 检测到和第 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 前后 α α 个节点计算距离，并且选取距离最近的 k k 个节点相连接。

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

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

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

上式中 Msp M p s 是模型中的点坐标（非 node）
上式中 M^sp 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 m a x 指和第 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 c u r P ( u ,   D t a ) ;   T i n a P ( u ,   D t a ) ;   t c u r ;   t i n a ) = ( Q s P ;   Q d P ;   t s ;   t d )

其中 ：
Tcur T c u r ：当前帧的 pose。
p(u, Dat) p ( u ,   D t a ) ：通过上一帧 pose 从模型投影得到的点。
Tcurp(u, Dat) T c u r p ( u ,   D t a ) ：active 点坐标。
Tinc T i n c ： 将投影得到的相机系下的 active 点 align 到世界系下的 inactive 点的 pose，pose 通过将世界系下的 inactive 点设为配准的模型，将相机系下的 active 点设为待配准的帧获得。 TinaP(u, Dat) T i n a P ( u ,   D t a ) 为世界系下 inactive 点坐标。
tcur t c u r ：当前帧的时间
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 c u r P ( u ,   D t a ) ;   T F e r n P ( u ,   D t a ) ;   t c u r ;   t F e r n ) = ( Q s P ;   Q d P ;   t s ;   t d )

其中：
Tcur T c u r ：当前帧 pose。
p(u, Dat) p ( u ,   D t a ) ：通过上一帧 pose 从模型投影得到的点。
TFern T F e r n ：检测到的回环帧的 pose。

tcur t c u r ： 当前帧时间戳。
tFern t F e r n ：回环帧时间戳。

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

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

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

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

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

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

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

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

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

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