ElasticFusion

转载: https://blog.csdn.net/fuxingyin/article/details/51433793

泡泡机器人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 帧存在回环；再利用第一部中的跟踪算法计算当前帧和第 i

帧之间的位姿，计算得到位姿变换后，在图像中均匀抽取一些点，建立约束，优化 node 参数（关于如何优化 map 后续文章还会展开介绍）。

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

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

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

surfel 表示模型

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

对于每个点，存储了:
点的位置信息：(x,y,z)

面片得半径: r
法向量： n
颜色信息： (R,G,B)
点的获取时间信息： t

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

Deformation Graph

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

Deformation Graph 结构

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

每个 node 包含了本身的位置信息 Gng

，待优化的 仿射变换参数 GnR 和 Gnt，利用局部回环检测和全局回环检测建立的约束优化参数 GnR 和 Gnt

。

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

，

找到时间上最相近的点后，和 Graph nodes c

前后 α 个节点计算距离，并且选取距离最近的 k

个节点相连接。

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

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

模型中的点按照和 node 之间的距离关系，按照加权平均的方式，用优化之后的 GnR

和 Gnt

更新位置和法向量。

上式中 Msp

是模型中的点坐标（非 node）
上式中 M^sp 表示 node 作用后的坐标
GnR 和 Gnt 指建立连接 node 优化后的参数
wn(Ms) 是node n

的权值

法向量更新如下所示：

权值计算公式如下：

在 Msp

周围选取 n+1 个 nodes，每个 node 的权值和 node 到 Msp 的距离成负相关， dmax 指和第 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)

其中 ：
Tcur

：当前帧的 pose。
p(u, Dat) ：通过上一帧 pose 从模型投影得到的点。
Tcurp(u, Dat) ：active 点坐标。
Tinc ： 将投影得到的相机系下的 active 点 align 到世界系下的 inactive 点的 pose，pose 通过将世界系下的 inactive 点设为配准的模型，将相机系下的 active 点设为待配准的帧获得。 TinaP(u, Dat) 为世界系下 inactive 点坐标。
tcur：当前帧的时间
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)

其中：
Tcur

：当前帧 pose。
p(u, Dat) ：通过上一帧 pose 从模型投影得到的点。
TFern

：检测到的回环帧的 pose。

tcur

： 当前帧时间戳。
tFern

：回环帧时间戳。

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

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

用全局和局部回函检测建立的约束优化节点的参数 GnR

和 Gnt

，优化的目标函数如下：

优化过程中，保持仿射变换参数 R

是单位正交的

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

和模型点的更新策略一样，用公式 (8) (9) (10) 更新建立的约束点的坐标 ϕ(Qps)

，下式的含义是，将 source 点向 destination 点做优化，将两次重建好的点对齐。

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

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

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