面向动态环境基于面元的RGB-D SLAM系统
点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
作者：robot L
https://zhuanlan.zhihu.com/p/142175916
本文仅做学术分享，如有侵权，请联系删除。
主要内容
面向动态环境基于面元的RGB-D SLAM系统，主要内容如下：
同时估计RGB-D相机位姿并分割当前帧图片中的静态像素。将当前帧像素的静态概率当做一个取值范围为
的连续变量，和位姿一起联合优化。
构建静态场景的面元地图。为此，地图中的每个面元添加额外的生存能力项，由对应观测像素的静态概率决定。如果生存能力过小，则表明该面元属于动态面元，从地图中移除该面元。
代码地址：raluca-scona/staticfusion。
缺点
初始若干帧内不能有大量动态物体，这样才能保证初始静态场景面元地图的准确性。
摘要
动态环境对于视觉SLAM是有挑战性的因为运动物体能够损害相机位姿跟踪并且导致地图的损坏。在本文中，我们提出一种动态环境中的鲁棒RGB-D SLAM方法，检测运动物体同时重建背景结构。大多数方法采用隐式的鲁棒惩罚项或者外点滤除技术来处理运动物体，我们的方法是同时估计相机运动和当前RGB-D相机对的静态概率/动态分割。该分割被用于加权稠密RGB-D融合来估计环境纯静态部分的3D模型。通过利用3D模型用于帧-模型对齐，同时静态/动态分割，相机运动估计降低了漂移——同时对场景中的动态元素更鲁棒。证实被提供，比较了提出的方法和相关的SOTA方法，使用静态和动态序列。提出的方法达到了相似的表现在静态环境中，和提高的精度和鲁棒性在动态序列中。
R. Scona, M. Jaimez, Y. R. Petillot, M. Fallon, and D. Cremers. StaticFusion: Background Reconstruction for dense RGB-D SLAM in Dynamic Environments. IEEE International Conference on Robotics and Automation, 3849-3856, 2018.
1 介绍
SOTA稠密视觉SLAM方法能够产生大型室内场景中令人印象深刻的重建。这些方法，然而，经常依赖环境是静态的假设，相机运动平滑并且场景中有充分的几何或纹理用于可靠的跟踪。我们专注于静态场景的假设。动态环境中的鲁棒操作是一个开放性问题。例如，大多数里程计方法执行当前图片和之前参考之间的配准，同时假设图片间被估计的变换是由相机运动导致的。动态元素然乱了该假设，并且可能导致位姿跟踪中的失败。此外，如果没有主动监测和分割，动态物体可能被融合到地图中，其可能导致不可逆转的损害。
提高SLAM在动态环境中的表现是特别重要的问题对于移动机器人来说。机器人很少在纯静态环境中操作，这种需求将会严重限制部署的范围。在示例应用中，协作机器人例如Rethink Baxter执行任务在运动的设备和装置之间。移动服务机器人面对类似的挑战，因为它们的环境被人类占据。
我们解决上述问题通过联合估计RGB-D相机的运动同时分割场景为静态和动态部分。相机运动被计算通过对齐即将到来的帧和环境的稠密面元模型（类似ElasticFusion[1]）。分解静态和运动部分导致了一个仅包含静态元素的静态背景。
运动物体的有效的检测和分割一般需要时序反馈或者多帧构架。我们证实背景3D重建是一个高效的方式来传播这种时序信息不需要引入巨大的时间消耗。同时，场景中的结果地图更有意义，因为它仅包含场景中的结构化元素和静态物体。
本文贡献如下：
一个新构架，同时估计相机运动并分割当前帧中的静态物体。
一个稠密建图系统，仅融合时序一致的数据（即，它存储过去静态的信息）。
大量的评估，证实了提出的算法在动态环境中超过了SOTA解决方案，并达到了具有竞争力的运行时间（30ms/帧）。
2 相关工作
室内环境中的稠密视觉SLAM已经取得了巨大进步通过消费级3D相机的出现例如微软Kinect或者Asus Xtion。该领域的一个开创性工作是KinectFusion[2]，首个使用RGB-D数据执行实时稠密跟踪和数据融合的系统。该方法使用一个固定大小的体素表示组合在GPU上。后续研究提供了令人印象深刻的表现，扩展范围[3][4]，回环能力[5][6]为了保证3D扫描和建图更大的室内场景变得更鲁棒和容易使用。
虽然大多数现存方法专注于扫描静态环境，还有为了增加动态场景的鲁棒性做出的特殊努力。
动态元素的隐式处理：使用鲁棒代价函数很常见在视觉里程计前端中，其惩罚高残差点的贡献，隐式增加了位姿估计的鲁棒性。Gutierrez等人[9]比较了不同的鲁棒函数专注于位姿估计的质量，同时Kerl等人[10][8]证实了小运动物体的鲁棒性。这些解决方案是不足够的或者会失败当运动部分占据图片的很大一部分时。
专注重建的方法，例如ElasticFusion[1]和Keller等人的方法[4]，需要点在连续帧中被重复观测到，在整合到3D模型中之前。类似地，在这些方法中，动态元素不会显式的被检测和处理，导致鲁棒性限制在小运动场景。
外点拒绝策略：一个常见处理动态物体的策略是检测和滤除噪声点。
对于Keller等人[4]，没有较近模型点对应的输入点被用于区域生长过程的种子点来分割当前图片为静态和动态部分。接着，与动态输入点匹配的模型点从重建中被移除。这种方法只有在对场景进行了有信心的重建之后才是有效的。
在DTAM中[11]（单目稠密建图系统），作者抛弃了光度误差大于某个特定阈值的像素。对于ORB-SLAM[12][13]，作者强制一个有效的最适生存策略，判断关键帧和用于位姿跟踪的点的合法性。通过宽进严出的策略，他们证实了令人印象深刻的鲁棒性和多功能性。
然而，在这些方法中，没有时间或空间一致性被强制在连续帧中被检测的动态点之间。
强制空间和时间一致性：Jaimez等人[14]介绍了一而终联合视觉里程计和场景流估计方法。类似的，BaMVO[15]是一个里程计方法，重建前4帧的环境通过时序传播。二者都是帧-帧方法，它们引入了随着时间的无界漂移，并对于图片中大部分被动态占据的情形中不有效。Runz等人[16]提出了一个方法，重建和跟踪每个运动物体通过单独的3D模型，是首个实时执行稠密重建和多物体融合的方法，显式处理动态元素，并强制空间和时间一致性。
最终，可能通过使用运动先验例如IMU[17]或者机器人里程计[18]来增加视觉SLAM在动态环境中的鲁棒性，但是对于许多用例来说，只使用单个相机来提高位姿跟踪和地图构建的准确性更吸引人。
3 框架和记号
本文提出一种致力于背景重建和前景动态物体分割的SLAM系统，流程图如图1所示。
系统输入是RGB-D图片流。一个RGB-D图片由一个彩色图片 ，一个深度图片
组成，其中
是图片平面。同时，密度图片
由
计算得到。
图1. 系统流程图
首先，输入 被分割成
个几何簇
通过使用K-Means算法，类似在[14]中使用。聚类的目的是为了降低后续场景分割的计算复杂度。
接着，将到目前为止构建的静态场景地图投影到上一帧中，得到 。有了
和
，我们创新地联合估计相机运动
和基于运动的场景分割。
中的每个簇
被分配一个静态概率
。
在联合估计问题被解决后，使用聚类和静态概率结果计算逐像素分割图片 ，然后与
一起被用于加权3D融合。
4 相机运动和场景分割的联合估计
为了同时估计两种性质，我们提出一种新的构架基于最小化如下两种能量项：
(1)
其中 表示静态概率集合，
编码静态簇的光度和几何一致性。
补偿
通过强制残差很大的簇为动态簇，以及两个正则项：一个平滑先验和一个几何先验。
A 相机运动
几何和光度残差定义如下：
(2)
(3)
其中 表示当前帧图片的像素坐标。扭曲函数定义如下：
(4)
本文创新点是使用静态概率加权这些残差
(5)
其中 是像素个数，
是像素
所属簇
的静态概率。
用于平衡几何和光度误差。
是柯西鲁棒函数：
(6)
其中 是参数。
和
加权几何和光度残差根据测量噪声和不连续性，具体形式为：
(7)
(8)
B 静态/动态分割
(9)
其中 表示簇
中的像素个数，(9)和(5)的组合会鼓励
尽可能小当簇
的像素平均残差大于
。
此外，还包含一个平滑先验鼓励连续的簇具有相似得分
(10)
其中 当簇
在空间中连续。
最后，一个几何先验，惩罚较大的平均深度差：
(11)
其中
(9)-(11)的组合构成 ：
(13)
C 求解器
虽然(1)对 是非线性且非凸的，但是它对于
是凸的，已知
的前提下，可以求出
的解析解。因此，我们使用迭代重加权最小二乘（Iteratively Re-weighted Least Squares, IRLS）求解
，然后求
的解析解。重复多次，可以得到好的收敛结果。
5 基于面元的3D重建
使用[4]和ElasticFusion[1]中的面元表示静态场景的3D模型。一个面元是一个3D圆盘，具有位置和法向量 ，颜色
，半径
，生存能力
（其中
表示能够生存），初始时间戳
，最后一次更新时间戳
和更新次数
。
我们使用[1][4]中的预处理和数据关联方法。不同的是我们为每个面元新定义生存能力，用来移除与动态输入点匹配的面元。融合过程如算法1所示。
算法1中的第8行 即为当前帧像素/面元
的生存能力。生存能力的取值范围是
。使用如下的对数几率后，可以将取值范围转化为
。对数几率函数是单调递增函数，在
时取值为0。
(14)
由静态概率
和两个新定义变量的
和
决定。后两者的定义如下
(15)
(16)
分别用来倾向选择离图片中心 近的像素/面元
，以及运动较小的帧。
最终累加当前帧生存能力的对数几率到模型中对应的面元中，如果累加值是正数，则说明模型中的该面元属于静态的概率较大；是负数则说明该面元可能是动态的。具体地，我们将移除连续10帧生存能力的对数几率的累加值为负数的面元。同时，仿照[4]，我们也移除违反空闲空间条件的面元。
7 评估
我们执行我们方法的完整评估在静态和动态环境中。首先，结果被展示对于几个Freiburg数据集序列[21]。该数据集具有相机轨迹的真值，允许我们同时测量相对和绝对漂移。Freiburg数据集仅包含少量高动态序列，因此，我们包含了额外的手持相机记录的序列来提供更一般的多样化场景的验证。
我们比较我们方法的准确性，StaticFusion（SF）和相关的SOTA方法：
1）Jaimez等人的联合视觉里程计和光流估计[14]（VO-SF）。一个面向动态场景的里程计方法，比较是有用的来探究重建静态场景3D模型的好处。
2）ElasticFusion[1]（EF），为了探究面向静态环境SOTA方法在动态序列中的表现。
3）Co-Fusion[16]（CF），SOTA方法用于跟踪和重建多运动物体。
4）Kim等人的基于背景模型的视觉里程计[15]（BaMVO），其与我们的方法相关，但是使用多帧策略而不是帧-模型对齐。由于我们不能复现发表的结果使用该方法的公开代码，我们只包括了在原始发表文章中展示的数值结果。
实验被执行在一个Interl(R) Core(TM) i7-3770 CPU at 3.4GHz和GeForce GTX 1070 GPU使用Ubuntu 16.04操作系统。我们使用对齐的RGB-D图片和维持的默认参数对于比较的方法。考虑到相机分辨率，我们使用QVGA（320x240）并保持其他方法的默认分辨率。
运行时间：大约30ms/帧。
A. 定量评估
考虑到位姿估计的准确性，评估被执行通过使用Sturm等人提出的度量[21]：
平移和旋转相对位姿误差（RPE）来测量每秒的平均局部漂移。
平移绝对轨迹误差（ATE）来测量全局轨迹质量。
为了完整性，序列在静态，低动态和高动态场景中被包括在评测中。结果记录在表1和表2中。可以看到StaticFusion在静态环境中的表现和ElasticFusion在ATE和RPE标准中具有相似结果。表2证实了帧-模型对齐策略在里程计方法中的优势，减少了整体漂移。在高动态环境中，我们的系统超过了其他方法由于以下原因：
表1. 相对位姿误差（RPE）
表2. 绝对轨迹误差（ATE）
EF不是为处理场景中的动态而设计的。因此运动物体损害了3D重建，因此也损害位姿估计。
VO-SFheBaMVO是基于里程计的方法，不能处理多余50%的图片包含动态的序列。
CF跟踪和重建刚体物体，并且它的表现恶化当非刚体元素例如行人出现在场景中时。
如第6节提到的那样，StaticFusion需要有限动态的序列在模型初始化时，因此，它产生高误差在fr3/walking_halfsphere中。我们包括额外的序列fr3/walking_halfsphere*，其跳过了初始具有高动态的5秒，为了展示方法在初始帧不具有挑战性时的能力（注意其他方法仍然失败在该例中）。
为了可视化我们的背景模型如何处理运动物体，图3展示了在序列fri3/walking_static中地图构建的时序进化过程。可以看到地图进化反映了场景变化。它高效地移除了运动部分，仅保持有价值的先验用于准确的动态分割。
图3. 顶部：背景重建进化反映了场景变化。底部：对应的RGB图片（左）和计算的分割（右）对于每个实例，其中红色意味着动态，蓝色意味着静态。从（A）到（C），模型适应地移除动态物体基于正确的分割。（D）的实例中的序列包含大量的动态，模型提供了一个场景的有价值先验，允许正确的分割和位姿估计。
B. 定性评估使用手持相机
在本节我们评估方法在具有大量动态的场景中。为此，我们记录两端手持RGB-D相机的序列。在第一个序列中，相机观测一个行人和物体交互，并执行不同速度的运动。偶尔该行人保持静止，并靠近相机，这对于跟踪系统具有挑战，因为必须尽快分割出行人在他们移动时为了不丢失传感器的跟踪。对于第二个测试，我们记录了一个“selfie序列”当一个行人拿着相机自拍时。这是一个复杂的测试因为行人经常会占据图片超过50%的空间，并且看上去相对相机是伪静态的（其表示了一个强大的局部最小对于任何不把运动部分显示分离方法的运动估计）。
对于第一个序列的结果展示在图6中。可以看到行人初始被插入到地图中，因为他没有运动，但是被移除当他运动时。StaticFusion估计的分割几乎是完美的在整个序列中：行人一致被正确分割，同时球和门也被分割当它们被移动时。
图6. 顶部：第一个序列StaticFusion构建的地图的进化。
第二个序列开始和结束当相机在同一个位置时（见图4），其允许我们测量整体误差对于整条轨迹。可以观测到我们的算法能够提供较好的预测（ ）甚至当人覆盖相机的大部分视野时。此外，它能够准确地跟踪相机，其看起来非常复杂对于一个基于直接相机对齐的系统来说。
图4. 左：StaticFusion估计的轨迹用于在第7节-B中的第二个实验。右：一些序列的输入图片（第一列），和相同视角下的预测（第二列）和估计的分割（第三列）。
ElasticFusion[1]和Co-Fusion[16]也被测试在这些序列中，并且它们都不能提供好的位姿估计。图5展示了一些失败样例。图5(A)展示了ElasticFusion构建的第一个序列的地图（注意两个椅子被构建和没有对齐的墙）。图5(B)展示了Co-Fusion如何发现过多运动物体在第二个序列中，破坏了整体地图。对于那个序列，估计的轨迹长度为9.5米，我们方法的最终漂移是1.5cm，而ElasticFusion和Co-Fusion的漂移分别是1.03cm和0.88cm。
图5. (A)展示了ElasticFusion如何多次增加行人到地图中，并最终不能估计相机运动。在(B)中Co-Fusion发现并跟踪多个独立的不存在物体（注意到蓝色圈里面的周围区域是行人的部分点云）。