[论文笔记]ElasticFusion中的Deformation Graph 优化

ElasticFusion: Dense SLAM Without A Pose Graph 没有姿态图稠密SLAM

[TOC]

摘要

提出了一种实时密集视觉SLAM的新方法。我们的系统能够捕捉全面稠密的全局一致的基于surfelbased的房间尺度环境的地图，使用RGB-D相机以增量的在线方式探索，没有任何姿态图优化或任何后处理步骤。这是通过使用密集的frame-to-model方法进行摄像机跟踪和基于窗口的表面融合以及通过非刚性表面变形的频繁模型细化来实现的。我们的方法尽可能多地应用局部model-to-model的表面闭环优化，以保持接近地图分布的模式，同时利用全局环路闭合从任意漂移中恢复并保持全局一致性。

构建surfel-based的房间尺度的稠密地图。

没有任何姿态图优化或任何后处理步骤。（传统的 SLAM 算法一般通过不断优化相机轨迹或者特征点的方式，来提高轨迹估计或者重建的精度，这篇文章采用不断优化重建的 map 的方式，提高重建和位姿估计的精度。）
这是通过使用密集的帧 - 模型摄像机跟踪和基于窗口的基于表面的融合以及通过非刚性表面变形的频繁模型细化来实现的。

II.方法概述

我们采用的架构通常在实时密集的可视SLAM系统中找到，它们在跟踪和地图之间交替

基于融合surfel的环境模型估计。我们方法的这一组成部分受到Keller[9]等人基于surfel的融合系统的启发，在第III节中概述了一些值得注意的差异。
对模型最近观测区域(模型活动区域)的数据进行跟踪和融合。将一段时间内没有观察到的地图较老的部分分割到模型的非活动区域(不用于跟踪或数据融合)
每帧，尝试将当前估计的相机帧内的活动模型部分与同一帧内的非活动模型部分标注(register)。如果标注成功，则对旧的非活动模型关闭了一个循环，并且整个模型没有严格变形，以反映该标注。然后重新激活地图中导致该循环关闭的非活动部分，以允许在地图的标注区域之间进行跟踪和表面融合(包括surfel剔除).
对于全局闭环检测，将场景的预测视图添加到随机fern编码数据库[6]。每个帧，尝试通过此数据库找到匹配的预测视图。如果检测到匹配，则将视图一起标记（register）并检查标记是否与模型的几何形状全局一致。如果是这样，请在地图中使用非刚性变形反映此标记，使surface进入全局对齐。

图2提供了方法概述的主要步骤的可视化。表面法线叠加在非活动模型上，用灰度着色;(i)当相机向左移动时，所有数据最初都在活动模型中;(ii)随着时间的推移，最近没有看到的地图区域被设置为非活动区域。注意突出显示的区域;(ii)相机重新检查地图的非活动区域，关闭一个局部循环并一起记录地表。先前突出显示的非活动区域随后变为活动区域;摄像机继续向右探测，更多的回路闭合;继续勘探新的地区;摄像机重新进入非活动区域，但漂移太远，无法关闭局部回路;(vii)此处的失调很明显，红色箭头表示从活动点到不活动点的等值点;(vili)触发一个全局循环闭包，使活动和非活动模型对齐;随着更多的局部环路关闭和非活跃地区重新活跃，继续向右探索;(x)最终完整地图彩色表面法线显示潜在的变形图和采样相机姿势在全局循环关闭数据库中。

图2

算法流程

ElasticFusion 通过 RGB-D 图像配准计算位姿，根据输入点云利用 ICP 算法和直接法求解位姿
计算相机位姿如果误差大于设定阈值，表示跟踪失败，启动重定位算法；如果误差小于设定阈值，则进入下一部分。
利用 Random Ferns 进行全局的回环检测算法（后续博客单独介绍），检测是否存在全局的回环，如果存在全局的回环，假设当前帧为 $I_{cur}$ 检测到和第 ii 帧存在回环；再利用第一部中的跟踪算法计算当前帧和第 ii 帧之间的位姿，计算得到位姿变换后，在图像中均匀抽取一些点，建立约束，优化 node 参数（关于如何优化 map 后续文章还会展开介绍）。
在第 3 部中如果不存在全局的回环，则检测是否存在局部的回环（局部回环检测算法后续会展开介绍），如果存在局部的回环，则同第 3 步，进行位姿估计，并且建立约束，优化 node 参数。
计算得到相机位姿后，将当前帧的点云和重建好的做融合，融合使用 openGL 的 shading language，如果在存在局部的或者全局的回环，在使用 openGL 进行点的融合时候，将优化之后的节点变量，作用于全部的点。
融合到全局模型中后，再用 openGL 投影得到当前视角下可以看到的点，用来对下一帧图像配准。

III.融合预测的跟踪

场景用一个无序的surfel列表 $M$ 表示(同文献[9]),其中每个表面 $M^{\hat s}$ 具有下面这些属性：
位置 $p\in R^3$
法线 $n\in R^3$
颜色 $c\in N^3$
权重 $w\in R$ (这里权重是什么？？？)
半径 $r\in R$ 给定点周围的局部表面区域，同时最小化可见孔，计算如文献[17]/半径通过场景表面离相机光心的距离的求得，距离越大，面片的半径越大
初始化时间戳 $t_0$
最后更新时间戳 $t$
表面处理初始化和深度图融合和文献[9]中一样，使用地图进行位姿估计时有两处不同
- 不仅通过几何frame-to-model预测深度图，还通过frame-to-model预测了颜色
- 我们定义了一个时间窗阈值 $\delta_t$ ，它将M划分为活动和非活动的surfel。(仅标记为活动模型表面的部分被用于相机姿势估计和深度图融合。当自上次更新表面以来（即，具有与其相关联的原始深度测量用于融合）的时间大于时 $\delta_t$ ，M中的表面被宣布为非活动状态。

A.几何位姿估计

当前帧深度图 $D_t$ (原文这里用的depth map)和来自上一帧的预测active模型深度图 $\hat D_{t-1}^a$ 之间的运动参数 $\xi$ ，通过最小化3D back-projected(反投影) 顶点之间的点到平面的成本误差函数得到：(2)
$E_{icp}=\sum_k((v^k-exp(\hat\xi)Tv^k_t)\cdot n^k)^2$

$v_t^k$ 是时间t这一帧深度图中第k个顶点在相机坐标系下的空间位置， $v^k$ 和 $n^k$ 是建立的地图中相关联的顶点和向量（第t-1步优化完毕得到）。T是从先前相机姿势到当前相机姿势的当前估计，并且 $exp(\xi)$ 是将李代数se3映射到对应李群SE3中，代表的应该是前一帧在世界坐标系的位置。顶点之间用投影数据关联互相关联.

该公式表达的含义是用深度图像估计相机姿态变换。而优化的误差函数则是非常正常地将新一帧中对应的点（应该是表达在空间坐标系下），按照姿态变换投影到上一帧位置中去，然后计算它们之间在空间的距离。法向量的左右是计算距离（法向量方向的投影）
其中所谓上一帧的位置，实际上应该叫做当前model的位置。即 $v^k$ 和 $n^k$ 都是与地图相关的量，因而不带下标t。

B.光度位姿估计

在当前的实时彩色图像 $c_t^l$ 和最后一帧的预测活动模型颜色 $\hat c_{t-1}^a$ 之间，目标是找到最小化像素之间光度误差（强度差异）成本的运动参数 $ξ$ ：(3)
$E_{rgb}=\sum_{u\in \Omega}(I(u,c_t^l)-I(\pi(Kexp(\hat\xi)Tp(u,D^l_t)),\hat c_{t-1}^a))^2$

式中，T为从前一个相机姿态到当前相机姿态转换的当前估计值。注意，方程2和3省略了3维向量和对应的齐次4维向量之间的转换(与T相乘时需要)，以简化符号。

$\hat c^a_{t-1}$ 这个值是从估计的active model部分的，而不是单纯的上一帧，所以这是个frame-model模型。 $c^l_t$ 则是新来的一帧彩色图像。T和李群计算，则将其从通过深度取到的深度图坐标系下的空间点转化成，世界坐标系下。

C.联合优化

最小化联合成本函数：
$E_{track}=E_{icp}+w_{rgb}E_{rgb}$

这里设置 $w_{rgb}=0.01$

IV.Deformation Graph变形图

为了确保地图中的局部和全局表面一致性，我们在一组surfel M中显示（反映reflect）出成功的表面闭环，这是通过根据后面第V和VI节中描述的任何一种闭环方法提供的表面约束使所有surfel（包括有效和无效）非刚性变形来实现的。我们采用基于Sumner等人的嵌入变形技术的空间变形方法。 [23]。
变形图由一组节点和变组成，每个节点具有:
- 时间戳 $G_{t0}^n$ ,
- 位置 $G_g^n\in R^3$ ,
- 相邻节点集合 $N(G^n)$ ,和相邻节点之间由（有向）边相连。
- 相邻节点个数k， $\forall n,|N(G^n)|$ (这里说他们的实验中使用K=4?这个都是一样的？)
- 每个节点还以 $3\times3$ 矩阵 $G^n_R$ 和 $3\times1$ 向量 $G_t^n$ 的形式存储放射变换，默认情况下分别初始化为标识和，当使表面变形时，每个节点的 $G_R^n$ 和参数 $G_t^n$ 将根据表面约束进行优化，见IV-C
为了将变形图应用于曲面，每个surfel $M^s$ 在图 $L(M^s,G)$ 中确定一组影响节点，表面的变形位置由下式给出：
$\hat M_p^s=\phi(M^s)=\sum_{n\in L(M^s,G)}\omega^n(M^s)[G_R^n(M_p^s-G_g^n)+G_g^n+G_t^n]$
表面的变形法线由下式给出：
$\hat M^s_n=\sum_{n\in L(M^s,G)}\omega^n(M^s)G_R^{{n-1}^T}M^s_n$

$\omega^n(M^s)$ 是一个标量，表示影响节点 $G^n$ 在surfel $M^s$ 上，当n=k时总和为1。

$\omega^n(M^s)=(1-||M^s_p-G^n_g||_2/d_{max})^2$
这里 $d_{max}$ 是 $M^s$ 中最近的K+1个节点的欧几里得距离。

Deformation Graph 结构图

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

建设每一帧都构造了一个新的变形图，因为它计算量小，比增量修改现有的变形图更简单

Deformation Graph 优化

建立连接

Graph Nodes从重建好的Surfel中均匀抽样初始化，如结构图中的红色的点。
Graph Nodes按照时间关系(而不是空间关系)建立连接(这里时间关系是指surfel上的时间戳 $\tau$ )
建设每一帧都构造了一个新的变形图，因为它计算量小，比增量修改现有的变形图更简单。初始化一个新的变形图G，按照时间关系(而不是空间关系)建立连接

利用时间顺序建立连接的原因：按时间比按空间关系计算效率上更高，但更重要的是防止表面的时间上不相关的区域相互影响（即活动和非活动区域）

Map 中其它点(灰色的点)和 Nodes 先按照时间关系搜索最近的点，再在最近点周围按照空间关系找最近的 Nodes 相连接。

Deformation Graph 结构和优化：

红色的点是抽取的 Nodes，每个 Node 有待优化参数 $G^n_R$ 和 $G^n_t$ (待优化的参数是仿射变换)
根据 Local Loop Closure 和 Global Loop Closure 建立的约束，优化 Nodes 中参数优化。
再将优化的参数作用于全部的点(Map中全部的点)：
位置坐标更新： $\hat M_p^s=\phi(M^s)=\sum_{n\in L(M^s,G)}\omega^n(M^s)[G_R^n(M_p^s-G_g^n)+G_g^n+G_t^n]$
法向量更新： $\hat M^s_n=\sum_{n\in L(M^s,G)}\omega^n(M^s)G_R^{{n-1}^T}M^s_n$
Node权值： $\omega^n(M^s)=(1-||M^s_p-G^n_g||_2/d_{max})^2$

这里大概地思路是：在Map中均匀抽取一些Node(红色的点)，每个Node都有待优化参数，根据局部闭环和全局闭环建立约束，优化参数，再将优化后的参数作用于Map中全部的点，对Map中所有点进行优化。

局部闭环

跟踪算法求解位姿，根据位姿将点云融合
按照时间将重建好的点划分成 ACTIVE 和 INACTIVE
根据求解得到的位姿，从 ACTIVE 和 INACTIVE 分别投影得到两幅点云
配准两幅点云，如果可以配准上，则说明存在 Local Loop Closure

大概思路：ACTIVE点是最近被相机拍到的，INACTICE则是很久没出现过的，如果两部分能有部分配准上，则说明回到原来到过的地方了。

建立约束：

$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)$

$T_{cur}$ ：当前帧的 pose。
$p(u, D^a_t)$ ：通过上一帧 pose 从模型投影得到的点。
$T_{cur}p(u, D^a_t)$ ：active 点坐标。
$T_{inc}$ ：将投影得到的相机系下的 active 点 align 到世界系下的 inactive 点的 pose，pose 通过将世界系下的 inactive 点设为配准的模型，将相机系下的 active 点设为待配准的帧获得。 $T_{ina}p(u, D^a_t)$ 为世界系下 inactive 点坐标。
$t_{cur}$ ：当前帧的时间
$t$ ：对应的 INACTIVE 点的时间（模型投影点）

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

全局闭环

为什么有了局部闭环还要全局闭环?
相机移动距离长时轨迹 drifts 太大，用 ACTIVE 和 INACTIVE 点投影得到的点云配准不上。

回环检测算法 Randomized Ferns

算法流程：

输入新图像，计算Block码值
根据Block码值计算和list表中帧之间的相似度
相似度判别是否加入关键字或者坐重定位

图像编码

每幅图像有m个block编码，每个block有n个fern

我理解为这里fern个数为图形的通道数，这里是rgbd四个通道，所以n=4

fern计算如下:
$f(I,\theta,\tau)=\left\{ \begin{aligned} 1 & &if & &I(\theta) \geq\tau \\ 0 & &if & &I(\theta) <\tau \end{aligned} \right. \ \ \theta=\{c,x\}$

其中 $\tau$ 是阈值，I为输入的图像， $\theta$ 包含通道c和像素点坐标x

block计算：
$F=\{f_i\}_{i=1}^n$

$f_i$ 为fern值

图像码值：
$C=\{F_k\}_{k=1}^m$

判断帧间相似度

对于一幅新获取的图像 $I$ ，首先计算每个 block 的编码 $b_I^k$ ，根据block的编码，在上图所示的列表中，索引到在 block $k$ 处具备相同编码的图像 $J$ ，图像 $I$ 和图像 $J$ 的相似度 $q_{IJ}$ 加1。

对于图像 $I$ 所有的block做上述运算，可以计算得到对于存储的所有的关键帧图像的相似度。

通过相似度判别当前帧是否加入做关键帧（对于所有存储的关键帧相似度小于一定阈值），或者存在回环（和某一帧图像相似度大于一定阈值），还可以通过和相似度大的图像配准，进行重定位。

建立约束

$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)$
$T_{cur}$ ：当前帧 pose。
$p(u, D^a_t)$ ：通过上一帧 pose 从模型投影得到的点。
$T_{Fern}$ ：检测到的回环帧的 pose。

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

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

优化

前面用全局和局部回函检测建立的约束，通过下面目标函数对节点的参数 $G^n_R$ 和 $G^n_t$ 进行优化，目标函数由四部分组成：

Loop Closure 建立的约束优化 nodes 参数

Deformation Graph算法流程

评估

从轨迹估计、曲面重建精度和计算性能等方面对系统性能进行了定性和定量评估。

A.轨迹跟踪

与其他四种最先进的基于RGB-D的SLAM系统DVO SLAM、RGB-D SLAM 、 MRSMap 和Kintinuous进行了对比，使用绝对轨迹(ATE)均方根误差度量(RMSE)，结果表明，我们的轨迹估计性能与现有的最先进的系统持平或更好

B.表面估计

在Handa等人的ICL-NUIM数据集上评估了表面重建结果。和a中列出的SLAM系统相比，

表III

我们还展示了一些以手持方式捕获的数据集的定性结果，展示了系统的通用性。每个数据集的统计数据列在表IV中。

表IV

C.计算性能

为了分析该系统的计算性能，我们提供了一个平均帧处理时间在整个酒店序列的图表。测试平台是一台台式机，CPU为Intel Core i7-4930K，主频为3.4GHz。32GB内存和nVidia GeForce GTX 780 Ti GPU与3GB内存。系统的执行时间随着map中surfels的数量而增加。总体平均每帧31ms扩展到峰值平均45ms，意味着最坏情况处理频率为22Hz。这是在广泛接受的最小频率范围内的融合密集SLAM算法.，并根据我们的定性结果显示，足以进行实时操作。

最后编辑于：2019.07.17 10:04:06

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 194,242评论 5赞 459
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 81,769评论 2赞 371
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 141,484评论 0赞 319
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 52,133评论 1赞 263
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 61,007评论 4赞 355
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 46,080评论 1赞 272
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 36,496评论 3赞 381
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 35,190评论 0赞 253
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 39,464评论 1赞 290
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 34,549评论 2赞 309
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 36,330评论 1赞 326
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 32,205评论 3赞 312
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 37,567评论 3赞 298
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 28,889评论 0赞 17
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 30,160评论 1赞 250
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 41,475评论 2赞 341
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 40,650评论 2赞 335