基于点云相关性的动态环境下RGB-D SLAM

作者：Weichen Dai, Yu Zhang, Ping Li, Zheng Fang, and Sebastian Scherer

来源：TPAMI 2020

编译：廖邦彦

审核：王志勇

转载自泡泡机器人SLAM，文章仅用于学术分享。

在这篇文章中，我们提出了一个消除动态环境下的运动物体影响的SLAM方法。该方法利用地图点之间的相关性，将属于静态场景的点和属于不同运动对象的点分离到不同的类别中。使用Delaunay三角化方法将整个地图点三角化得到一个稀疏图结构。在这个图结构中，顶点代表着地图点，每个边代表着相邻点的相关性。如果两个点的相对距离随着时间推移保持不变，它们之间就存在相关性，它们就可以视作刚性的一起运动。如果不是，则认为它们之间不存在相关性并且将其划归为不同组别。当在点云相关性优化过程中，不存在相关性的点之间的边被去除后，剩下的稀疏图结构将从静态场景中点将运动的物体分割出来。最大的组别被设定为可靠的静态地面点。最后，只使用这些点来实现运动估计。我们提出的方法针对于RGB-D传感器，使用公开的RBG-D基准来验证，在多种不同的极端环境中测试。实验结果显示，使用我们的方法可以实现鲁棒且精确的性能在不同动态环境下。与其他的SOTA方法相比，我们的方法能够提供有竞争性的精度以及良好的实时性能。

• 提出了一种基于点云相关性的分割方法来将静态和动态点分割开来。它能够从多帧中发掘时序信息来扩大捕捉的视野，同时该方法不局限于RGB-D传感器，只要能提供点云相关性的传感器皆可。

• 我们提出了一个基于RGB-D的SLAM方法来提高在动态环境下的运动估计的鲁棒性与精确性。

PROBLEM STATEMENT

所有状态定义如下

其中，T_k 表示的是在k时刻的转移矩阵 p_i表示的是第i个地图点。观测方程定义如下

其中，y_ik对应的是对于点p_i在位姿T_k下的观测信息，n_ik是零均值高斯噪声，定义如下

我们通过最大似然方法求解x，目标方程如下

误差定义

海塞矩阵定义

以上的方程假定为静态场景，对于一个动态场景，动态点不满足方程2的观测方程。对于一个动态点的观测模型定义如下。

其中的v_dk是运动物体的在时间戳k-1到k之间间隔T的平均速度。

SEGMENTATION USING POINT CORRELATIONS

分割方法流程图

主要分为三步，第一步我们建立了一个图结构来表示相邻地图点的相关性。第二步优化所有的地图点位置并且移除非一致性的边来划分整个图结构。第三步，一个深度优先搜索方法被使用来确定连接的部分。相同部分中的点被认为有一致性的运动，不同部分的点有相互独立的运动。

Step 1: Graph Initialization 

使用Delaunay三角剖分来降低三维点图构造的复杂性。在Delaunay三角剖分中，对于给定的离散点集进行三角测量，使任何三角形的内部没有点。它能够使三角形的所有角度的最小角度最大化。因此，在图中只连接了相邻的特征点。生成的图结构类似于稀疏面元。换句话说，由于它的稀疏性，在下一步中只验证相邻点之间的相关性。 

Step 2: Inconsistent Edge Culling

得到三角剖分后的图结构后，我们定义地图点的相关性观测，对于边I_ijk的z_ijk观测

其中的I_ijk代表着在第k个时间戳下的点p_i和p_j点之间的边，n_ijk代表着高斯噪声，h（）代表着对于边的观测模型。

与上文类似，采用最大似然估计状态向量。点云相关性优化的目标方程定义如下。

其中，x_g代表的是状态向量x的子集，误差定义如下

融合方程3的BA目标方程，最后的目标方程结果如下

在优化x_p过程中，我们使用平方的马氏距离作为决定观测是否是外点，根据P值来设定一个卡方分布值阈值。在迭代过程中，不一致的观测将被认为是外点被去除。因此如果一个边的所有观测都被认为是外点经过优化后，这个边就会被视为不一致边并且从图结构中去除。在优化过程中，海塞矩阵也会发生变化。

为了保证利用海塞矩阵特殊的稀疏特性使得算法实时运行，我们将方程12单独进行优化，并且使用Huber鲁棒核函数来提高鲁棒性。

Step 3: Determination of Static Points

使用深度优先算法来检查图G是否被划分为几个孤立的子图（连接的部分）。如果有不同的连接部分，则这些部分代表具有不同运动模式的不同点集。为了判断哪些点是可靠的静止点，拥有最大体积的连接部分将被假定为可靠的静态点。有以下两点原因。在一段时间内构建的地图中，静态点的空间体积通常最大，其体积随时间增长最大。静态点一般均匀分布在三维空间中，而动态点通常只分布在一个表面上，因为摄像头只能观察到一个运动物体的表面的一侧。因此，动态点的体积比静态点的体积要小得多。

DSLAM IN DYNAMIC ENVIRONMENTS USING RGB-D SENSORS

我们提出了一个集成了分割方法的叫做dynamic SLAM(DSLAM)为动态环境搭建的SLAM方法。这个方法使用RGB-D传感器来实现，它能够提供一个带有和彩色画面同步的深度图像。我们系统基于ORB-SLAM2搭建，整个系统流水线如下所示。

其中地图管理模块中，地图点被分为标记的和未标记的。标记的点表示一个地图点是可靠的静态点。未标记点表示一个地图点无法被可靠的定义为一个静态点。注意的是，所有的新地图点被初始化为标记点，并且将会进一步通过静态点检验模块来验证。

前端系统负责估计连续帧间的运动以及决定是否插入一个新帧。因此，前端包含运动估计，能够决定当前帧中的静态点以及能够决定是否插入新的一个关键帧。因为RGB-D传感器能够捕捉3D信息，当第一帧获得了足够数量的可用地图点后，系统初始化被认为完成。

当初始化完成后，以下步骤按序实施。

对新的每一帧，对于成功跟踪前一帧标记点的初步特征匹配。然后，在初步位姿估计部分，使用所有匹配进行单一位姿集束调整。这一步能够排除错误的特征匹配结果。在上一步中，值得注意的是，快速移动的物体上的特征点很难匹配到现存的地图点上。同时，这个方法也能排除掉有相当大的重投影误差的在快速运动物体上的对应点。因此，即使在进入静态点确认模块之前获得了初始化位姿，只有少数与运动目标相关的特征点可能干扰单一运动的集束调整。如果跟踪没有丢失，则对跟踪的地图点确定是否为静态点。然后，通过重投影搜索未跟踪的特征与局部地图点之间的匹配，并在局部地图跟踪步骤中再次优化姿态。最后，跟踪线程确定是否需要插入新的关键帧。

后端任务包括最优静态场景重建、使用滑动窗口确定静态点和位姿图优化。这些任务由在三个独立线程中运行的三个模块执行：局部建图、静态点确定和回环检测模块。局部建图模块处理每个新的关键帧，并使用局部集束调整来优化所有标记点。静态点确定模块采用点相关性优化来确定可靠的静态点，并从其他点上删除标记的状态，以消除移动物体的影响。回环检测模块使用词袋方法搜索回环检测。如果找到一个新的回环检测，则进行位姿图优化，以消除累积的全局漂移，实现全局一致性。

1. Determination of Static Points for RGB-D Sensors

1.1 Formulation of the Point-Correlation Measurement Model for RGB-D Sensors

Map-point measurement:

对于RGB-D传感器，y_ik能够通过下式获得

其中的c_u c_v是相机光心点，f_u f_v是焦距，u v是点p_i在RGB图像中的对应点，d是其对应深度。同时，我们可以得到RGB-D的传感器模型。

为了方便，我们使用a b c来代表y_ik

同时通过下式得到对应深度点的不确定度即标准差

这个标准差随着深度增加而增加。但是这个模型并没有考虑到更加显著的目标边的不确定性。我们使用了混合高斯模型来建立一个更精确的不确定性模型。在这个模型中，u和v被设定为独立变量。随机变量c被定义为在局部窗口

中的d变量。最后得到的混合高斯模型的均值和方差如下定义

其中

是一个以u v为中心窗口位置集。d_q是位置q的的一个深度测量，权重w_q根据核W来选择，W定义如下

图七展示了一个示例

因此，根据公式19，在第k时间点第i地图点的观测的3D协方差矩阵Σ_ik定义如下

Point-correlation measurement model:

根据公式8的点观测模型，点相关性观测模型定义如下

如果假设y_ik和y_jk观测是独立的则由下式可以得到z_ijk观测的协方差矩阵

1.2 Determination of Static Points by the Front End

只有当前帧和前一帧的测量值被用于优化。然后，计算每条边的每次测量误差的平方马氏距离。如果测量中的误差大于给定的阈值，则去除该测量值。如果删除一个边缘的所有测量值，则确定该边缘为不一致的边缘，并从图中删除。

1.3 Static Point Determination in the Back End

此模块在添加新关键帧后验证标记点。计算过程见算法1。与前端的静态确定模块相比，该模块尝试使用滑动窗口中的所有信息来确定静态点。

该模块搜索所有和当前关键帧有共视关系的关键帧，并且根据公式12构建BA方程，在迭代中去除外点，并在优化完成剩余的图结构中，分割成多个联通部分，将体积最大的部分中点设置为标记点，较小的部分设置为未标记点。

2.Map-Point Matching

地图点与特征点的匹配能够根据标记点确定FOV中的可靠特征。未标记点的匹配能够使得不可靠特征不用于运动估计。另外识别出不可靠特征点能够有效的降低识别静态点失败的风险以及能够降低误匹配的概率。

3 Map Management

3.1 Point Management

地图点管理分为三个部分，创建，更新，删除。Creation：

有两种方法可以创建地图点。第一种方法中，地图点是由两个共视的关键帧通过三角化创建的。第二种方法从具有深度测量的新特性点直接创建地图点。对于第一种方法，由于存在巨大的三角化误差，很难在移动目标上创建一个点。对于第二种方法，创建移动目标上的一个点，因为图像中被识别为移动目标上的特征点不能作为一个新特征顶啊。如前文所讲，第二种方法无法创建动态点，因为这些特征点与未标记的点相匹配。Update：

创建新的地图点后，更新标记点的位置可以提高前端运动估计的准确性，可以避免误匹配。因此，标记点和未标记点分别使用新信息进行更新。标记的点通过集束调整进行更新。未标记的点通过使用新一帧中新测量的静态点确定模块来更新。

Culling： 

除了创建之外，地图点还必须满足一个标准，来确保没有被错误三角化。每种类型的地图点都有它自己的标准。标记点必须有一定数量的成功观测结果，以确保它们是可跟踪的。否则，它们应该被删除。当未标记的点在滑动窗口的FOV中没有观察到时，它们将被移除。3.2 Keyframe Managemen

t当该帧中的大量观测被另外一帧观测到后，该帧被丢弃。

Abstract

In this paper, a simultaneous localization and mapping (SLAM) method that eliminates the influence of moving objects in dynamic environments is proposed. This method utilizes the correlation between map points to separate points that are part of the static scene and points that are part of different moving objects into different groups. A sparse graph is first created using Delaunay triangulation from all map points. In this graph, the vertices represent map points, and each edge represents the correlation between adjacent points. If the relative position between two points remains consistent over time, there is correlation between them, and they are considered to be moving together rigidly. If not, they are considered to have no correlation and to be in separate groups. After the edges between the uncorrelated points are removed during point-correlation optimization, the remaining graph separates the map points of the moving objects from the map points of the static scene. The largest group is assumed to be the group of reliable static map points. Finally, motion estimation is performed using only these points. The proposed method was implemented for RGB-D sensors, evaluated with a public RGB-D benchmark, and tested in several additional challenging environments. The experimental results demonstrate that robust and accurate performance can be achieved by the proposed SLAM method in both slightly and highly dynamic environments. Compared with other state-of-the-art methods, the proposed method can provide competitive accuracy with good real-time performance.

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~