【论文阅读】Dynamic SLAM Semantic monocular visual localization and mapping

Dynamic-SLAM：动态环境下基于深度学习的语义单目视觉定位与映射

开源项目（Github上有代码但不知是原作者的还是复现的）

该论文基于ORB-SLAM2（基于特征跟踪的SLAM）构建，是在ORB-SLAM2上构建的一个基于深度学习的动态环境下的语义单目视觉SLAM系统。Dynamic-SLAM主要包括视觉里程计前端，前端包含两个线程和一个模块，分别是跟踪线程、物体检测线程和语义校正模块；SLAM后端，主要包括局部建图线程和回环闭合线程，框架如下图所示。个人感觉，该论文和DXSLAM的区别在于，DXSLAM在ORB-SLAM2的基础上提高SLAM系统在各种环境变化下的稳定性和准确性，而Dynamic-SLAM在ORB2-SLAM上专注于处理动态环境中的挑战，并且Dynamic-SLAM是单目。

面向的问题

传统SLAM框架在动态环境下工作时，容易受到动态物体干扰，导致性能下降。

方法概述

针对动态环境下SLAM的难题，利用深度学习在物体检测方面的优势，提出一种语义级同步定位与地图构建框架Dynamic-SLAM。首先，基于卷积神经网络，在新的检测线程中构建结合先验知识的SSD物体检测器，对动态物体进行语义级检测。然后，针对现有SSD（Single Shot MultiBox Detector,基于深度学习的目标检测算法）物体检测网络召回率较低的问题，提出一种基于相邻帧速度不变性的漏检补偿算法，大大提高检测召回率。最后，构建一种基于特征的视觉SLAM系统，在跟踪线程中通过选择性跟踪算法对动态物体的特征点进行处理，大大降低误匹配导致的位姿估计误差。与原SSD网络相比，系统的召回率由82.3%提高到99.8%。实验表明，Dynamic-SLAM的定位精度高于目前最先进的系统。该系统利用移动机器人成功定位并构建了现实世界动态环境中的精确环境地图。总之，我们的实验演示验证了 Dynamic-SLAM 在机器人定位和地图绘制方面比动态环境中最先进的 SLAM 系统具有更高的准确性和鲁棒性。

主要贡献

•针对现有SSD物体检测网络召回率较低的问题，针对SLAM系统提出了一种基于相邻帧速度不变性的漏检补偿算法，大大提高了检测的召回率，为后 续模块提供了良好的基础。
•提出了一种选择跟踪算法，以简单有效的方式剔除动态物体，提高了系统的鲁棒性和准确性。
•构建了基于特征的视觉Dynamic-SLAM系统，在SSD卷积神经网络的基础上，结合深度学习技术构建了新的物体检测线程，结合先验知识实现了机 器人定位与建图时语义层面的动态物体检测。

系统介绍

本研究在ORB-SLAM2的基础上，增加了融入语义的动态物体决策模块，利用特征点优化视觉里程计算法，构建了Dynamic-SLAM。Dynamic-SLAM的鲁棒性通过对图像中的静动态特征进行分割，将动态部分视为离群值来实现。利用选择性跟踪算法计算出的静态特征点进行姿态估计和非线性优化，避免动态环境物体的干扰。针对现有SSD物体检测网络召回率较低的问题，提出了一种基于基本运动模型的漏检补偿算法，大大提高了物体检测模块的准确率。如图为Dynamic-SLAM的简要框架图。

1）漏检补偿算法

传统的物体检测任务中，由于图片之间没有明显的相关性，所以无法通过上下文信息来提高检测精度。 但在SLAM中，视频帧是按时间序列到达的，可以利用前几帧的检测结果来预测下一帧的检测结果，从而避免下一帧关键帧的漏检或误检。
在目标检测任务的准确率评估中，通常有两个方面受到研究者的关注：Precision（精确度）和Recall（召回率）。对于SLAMIDE问题中的动态目标检测任务，后者更为重要，也就是召回率更为重要。在发生漏检或误检时，相邻两幅图像之间的差异会导致特征点正确匹配的数量发生剧烈变化，从而导致系统不稳定。（这里我没有太理解，按照这个论文的解释，这里不应该是准确度更重要吗，为啥要说更关注召回率呢？视觉SLAM十四讲中也提到，SLAM过程更要求精确度，对召回率相对宽松）。

啊后来查了资料并且仔细看了论文才发现，这里说的更关注召回率和回环检测更关注的准确率指的不是一个东西。这里的召回率指的是检测动态物体的时候的召回率，SLAM更关注准确率指的是回环检测的准确率。

接下来介绍论文提出的相邻帧的漏检补偿模型：

该模型基于一个合理的假设：动态物体的运动速度在短时间内趋于恒定（即加速度趋于0）。动态物体在像素平面上的速度记为v（上箭头），amax表示动态物体在像素平面上的速度变化率阈值。它们之间应满足如下关系：

运动速度用于描述动态物体在某几帧之间的运动位移，实际视频序列中相邻两帧之间的时间差很短，动态物体在运动位移上的变化量变化不大。该模型可以用来判断是否发生了漏检，同时也提供了漏检时的补偿策略。通过∆v（上箭头）可以确定当前帧K在短时间内前几帧与当前帧K之间对应的边界框，如果大于amax，则认为是不匹配，即漏检。当前关键帧K进入SSD网络，输出检测到的物体列表，列表中的每一项都是边界框的定位，也就是检测到的物体位置坐标KDi(0<i<NK，其中NK为帧K中的边界框编号)。对于上一帧K−1Di的每一个检测结果，如果在当前帧amax确定的区域K Ai(K ai,u,K ai,v,K−1 ˆw,K−1 hˆ)内没有检测到，则认为是漏检，需要将KDˆi(K ˆci,u,K ˆci,u,K−1 ˆw,K−1 hˆ)加入到K帧的检测列表中，其中：

其中k为对当前帧进行漏检补偿的前几帧的个数。算法处理如下：

这个算法可能比较绕，我按照我的理解给大家解释一下，不正确的地方欢迎大家指正：

1. 速度不变性假设：为了解决漏检问题，论文提出了一个基于速度不变性的漏检补偿算法。这个算法的核心假设是：在短期内，动态物体的速度（即在像素平面上的运动速度）倾向于保持不变（即加速度接近0）。

2. 补偿策略：算法通过比较连续帧中检测到的动态物体的位置变化，来预测和补偿漏检的物体。如果当前帧中某个物体的检测结果与前一帧中的位置变化超出了预设的阈值，那么认为发生了漏检，系统会根据前一帧的信息和物体的速度变化来预测当前帧中该物体的位置，并将其添加到检测列表中。

3. 算法实现：具体来说，算法会计算前一帧中检测到的物体在当前帧中可能的位置（基于速度不变性假设），如果在当前帧的相应位置没有检测到物体，则将该物体的位置添加到当前帧的检测结果中，从而补偿漏检。

4. 简而言之，漏检补偿算法通过利用物体在短时间内速度变化不大的特性，预测和补偿那些在当前帧中可能被漏检的动态物体，以此来提高SLAM系统在动态环境中对动态物体检测的准确性。

   漏检补偿算法的效果如下图所示，它在动态物体的选择和跟踪中起着至关重要的作用。
   上式中k和amax的选取影响漏检补偿的灵敏度，k过小会导致过灵敏，产生误差补偿。一般选取3~5帧后再确定K帧，同时如果连续两帧以上检测不到动态物体，则放弃补偿。
   这样虽然会引入少量的误差补偿，但可以明显减少漏检的发生。另外，amax过小，正确的检测结果可能会被当做漏检，取值过大，系统不灵敏，多个动态物体检测区域可能会重叠。

这里给出图注的翻译，可以帮助大家更好的理解：图3. 漏检补偿算法示例图。红色框表示SSD Network的检测结果。根据阈值amax（蓝色虚线框）可判断漏检，然后根据速度不变假设（绿色虚线框）计算补偿结果。

2）基于先验知识的动态对象确定

环境物体的语义是人基于经验对环境的解读，在记忆陌生环境的过程中，人会借助先验知识自动忽略车辆、行人等动态物体，而记住建筑、植被等静态物体。如果SLAM系统没有从语义层面理解周围环境，就无法真正区分动态和静态，只能在短时间内发现运动物体，无法保证长期的一致性。因此，结合先验知识的物体检测结果可以在语义层面进行动态物体决策模型。对于Dynamic-SLAM，前端SSD物体检测模块的输出信息是冗余的，只需要知道检测到的特征点是静态的还是动态的即可。根据人的先验知识，物体的动态特性可以从0分（静态）到10分（动态）打分，将分数与预先定义的阈值进行比较，从而对特征点进行静态和动态的区分。该区间常见物体的大致分数如图4所示。

事实上，这是一个最大化后验问题。当前一帧的边界框被检测为动态物体时，当前帧的边界框也会被判断为同一个点并重新检测，然后进入漏检补偿循环。设 C={K ci : K = 1, 2, …, K, …, N } 和 Z={K di : K = 1, 2, …, K, …, N } 作为边界框的真实情况和测量值，根据贝叶斯规则：

进而：

在Dynamic-SLAM框架中，第K帧的判定结果会受到过去多帧的干扰，按照第K-1帧进行条件概率展开，有：

基于先验知识的动态目标确定解决了神经网络获取的冗余语义信息，在特征点处理中更加实用。

以上叙述具体是什么意思呢？个人的简单理解如下：

1. 应用到动态物体识别：在动态物体识别的上下文中，我们想要计算的是在检测到某个特征点的情况下，该特征点属于动态物体的概率。具体来说，我们有：

   • C：特征点实际上是动态物体的真实状态（是我们想要推断的）。
   • Z：特征点的检测结果（是我们观测到的）。

2. 先验概率：P© 表示在没有任何额外信息的情况下，一个特征点是动态物体的概率。这可以基于先验知识来设定，例如，我们知道行人和车辆通常是动态的，因此这些类别的先验概率会更高。

3. 似然概率：P(Z∣C) 表示在已知特征点是动态或静态的情况下，检测到当前特征点状态的概率。例如，如果一个物体是动态的，那么在连续帧中检测到它的位置发生变化的概率会更高。

4. 后验概率：通过贝叶斯定理，我们可以计算出后验概率 P(C∣Z)，即在观测到检测结果 Z 的情况下，特征点是动态物体的概率。这个后验概率结合了先验知识和检测结果，为我们提供了一个更新后的信念，即根据当前的证据，特征点属于动态物体的可能性。

5. 决策阈值：最后，通过与一个预设的阈值比较后验概率，可以决定特征点的状态。如果后验概率高于阈值，则认为特征点是动态的；否则，认为是静态的。

3）对象检测模块和选择性跟踪

特征跟踪并不麻烦，但并不是所有特征都可以用于跟踪，动态物体的特征也并非都不能用于跟踪。这里有两种特殊情况：

1. 一个物体被归类为动态物体，但它在场景中是静态的（例如，停车场里的汽车）
2. 动态物体占据了摄像头的大部分视野（例如，在非常拥挤的购物中心）。区分不同场景中的前景动态特征点和背景静态特征点至关重要。

Dynamic-SLAM的具体系统结构如图2（在前面）所示。物体检测线程利用SSD物体检测网络计算物体的类别和位置。之后通过语义矫正模块进一步将物体划分为动态物体或静态物体，然后将动态物体的位置提供给跟踪线程。跟踪线程对每一帧关键帧图像进行ORB特征提取，然后将结果与参考帧进行匹配，得到两幅图像特征点的对应关系，用于估计相机位姿。
初始化完成后，位姿估计为PnP（Perspective-n-Point）问题，采用以Bundle Adjustment为代表的非线性优化方法求解该问题，该方法可以充分利用所有匹配结果，得到最优的位姿估计，非线性优化的代价函数定义如下：

最小重投影误差是观测到的像素坐标 ui 与当前姿态 ξ 下 3D 点 Pi 的重投影坐标之间的差值。优化的目标是找到一个最小化重投影误差的姿态。在动态环境下，特征点在动态物体上的重投影误差会过大，导致相机位姿无法收敛到最优值。Dynamic-SLAM语义矫正模块的决策结果表示为一张掩膜图像mask(u,v)={0,1}，是一个与原图宽高相同的二维矩阵，元素值为0的点表示静态像素点，元素值为1的点表示为动态像素点。

同时，前端视觉里程计会随时读取遮罩图像，并根据遮罩图像值对特征点进行选择性跟踪操作。

图5示出了选择性跟踪算法的流程。
由语义校正模块获取动态特征点的像素坐标Kpdi。然后，计算在边界框KDi周围距离为l的像素区域L内静态特征点的平均像素位移¯SL(u,v)。最后，计算动态特征点的像素位移并进行判断：

其中，ρ为决策系数，¯ SL为决策阈值，计算如下：

如果动态特征点与静态特征点的相对位移在可以接受的范围内，则允许用于跟踪，否则进行剔除。

选择性跟踪算法可以充分利用深度学习的检测结果，在动态物体处于静止状态或动态物体占据大部分视野时，能够很好的解决特征点问题，保证系统的鲁棒性，在一定程度上牺牲了维度信息，但该策略在机器人低速时是有效的。
为了保证DynamicSLAM系统的实时性，将物体检测和跟踪分为两个线程，同时设计了一个新的数据结构类Detection，保证其安全高效，支持并发操作来传递检测结果。此外，还使用了unique_lock互斥锁来保证两个线程不会发生访问冲突。由于物体检测线程和跟踪线程的处理速度并不同步，因此采用共享变量的异步读写来实现线程间通信，最大限度地利用CPU时间。

上述算法大概是啥意思呢，个人理解是：

1. 选择性跟踪算法：在SLAM系统中，跟踪算法用于在连续的图像帧之间跟踪特征点，以估计相机的运动。然而，在动态环境中，如果跟踪算法错误地跟踪了动态物体上的特征点，就会导致位姿估计的误差。为了避免这种情况，Dynamic-SLAM系统采用了选择性跟踪算法。

2. 动态物体的排除：选择性跟踪算法的核心思想是排除那些属于动态物体的特征点。这是通过将语义校正模块的输出（即动态物体的掩码图像）与特征点跟踪模块结合起来实现的。掩码图像中的每个像素点标记为静态（0）或动态（1），算法只选择静态像素点上的特征点进行跟踪。

3. 处理特殊情况：文中提到了两种特殊情况：一是被错误分类为动态物体但实际上是静态的物体（例如停车场中的车辆）；二是动态物体占据了视野的大部分（例如拥挤的购物中心）。选择性跟踪算法需要能够区分这些情况下的前景动态特征点和背景静态特征点。

4. 算法实现：文中描述了选择性跟踪算法的具体实现。算法首先获取动态特征点的像素坐标，然后计算静态特征点的平均像素位移，最后通过比较动态特征点与静态特征点的相对位移来决定是否使用该特征点进行跟踪。

处理特殊情况

1. 错误分类的静态物体：

   • 问题：在停车场中静止的车辆可能被错误地分类为动态物体。
   • 解决方案：系统通过分析物体在连续帧中的运动模式来调整其分类。如果一个物体在多帧中的位置变化很小或没有变化，即使它最初被分类为动态，系统也会将其重新分类为静态。

2. 动态物体占据视野大部分：

   • 问题：在拥挤的购物中心，动态物体（如人群）可能占据相机视野的大部分，使得难以找到足够的静态特征点进行跟踪。
   • 解决方案：系统通过设置动态物体的动态特性评分阈值来优化检测。如果检测到的动态物体过多，系统会调整其检测敏感度，以便更好地区分动态和静态物体。

算法实现

1. 获取动态特征点的像素坐标：

   • 通过SSD对象检测网络，系统识别出图像中的动态物体，并为每个检测到的物体提供一个边界框（bounding box）。
   • 边界框的坐标被用来确定动态特征点的位置。

2. 计算静态特征点的平均像素位移：

   • 系统在每个帧中跟踪静态特征点，计算它们在连续帧之间的位移。
   • 通过统计一定区域内所有静态特征点的位移，计算平均位移。这个平均值用于评估动态特征点的位移是否在正常范围内。

3. 比较动态特征点与静态特征点的相对位移：

   • 对于每个动态特征点，计算其在连续帧中的位移，并与静态特征点的平均位移进行比较。
   • 如果动态特征点的位移接近静态特征点的平均位移，这可能表明该特征点实际上是静态的（例如，停车场中静止的车辆）。
   • 通过设定一个阈值（例如，动态特征点的位移不超过静态特征点平均位移的一定比例），系统决定是否将该特征点用于跟踪。

4. 决定是否使用特征点进行跟踪：

   • 如果动态特征点的位移在可接受的范围内，它将被用于跟踪，以帮助估计相机的运动。
   • 如果位移超出正常范围，该特征点将被视为动态物体的一部分，不会用于跟踪，从而避免因跟踪动态物体而导致的定位误差。

论文中个人认为比较好的介绍

一些SLAM方面的介绍

1、为了使视觉SLAM系统在动态环境下正常工作，通常的做法是避免使用动态物体上的特征点，因此需要提前计算出动态物体的位置。而基于深度学习的物体检测与语义分割在这方面取得了显著的成果，将深度学习技术运用到SLAM中对于解决现实动态环境下机器人定位与建图问题至关重要。

2、ORB-SLAM2框架继承了经典的SLAM线程模型，采用ORB特征点和三个主要的并行线程来完成SLAM系统：跟踪线程用于实时跟踪特征点，局部建图线程用于构建局部Bundle Adjustments（BA）地图，闭环线程用于校正累积的漂移并执行位姿图优化。它可以让系统在大场景和大循环下长时间运行，从而保证轨迹和地图的全局一致性。ORB-SLAM2是定位和建图性能最好的框架之一，但在处理动态环境问题方面仍存在很多不足，需要进一步探索。

为什么选择SSD

3、相比于图像识别，物体检测更加困难，因为它不仅需要对图片中的不同物体进行分类，还需要给出每个物体的位置。以 R-CNN 为首的神经网络方法，如 SPP-Net、Fast R-CNN、Faster R-CNN 等，利用卷积神经网络自动学习特征，避免了人工设计特征的限制，经过候选框提取、特征提取、分类等操作得到物体检测结果。这些方法虽然精度较高，但由于候选框提取和物体识别分两步进行，非常耗时。相比之下，YOLO（You Only Look Once）算法摒弃了生成候选区域的中间步骤，直接通过单个卷积神经网络处理每个边界框的回归问题并预测对应类别的概率，因此速度很快，准确率也很好。

4、SSD（Single Shot MultiBox Detector）由刘等人提出，借鉴了Faster-RCNN中的Anchor boxes来适应不同形状的物体，是Dynamic-SLAM中的特征点识别器，其采用了VGG16的基本网络结构，前五层不变，而将fc6、fc7层利用àtrous算法转化为两个卷积层，并在后面添加三个卷积层和一个平均池化层，最后通过非最大抑制（NMS）得到最终的检测结果。通过省略初始候选框的生成步骤，使得整个物体检测过程可以在单个网络中完成，从而实现了较高的检测效率（46 fps，Titan X）和检测准确率（77.2%），在保证速度的前提下，SSD已经获得了足够的准确率超越基于候选框的方法。

动态环境SLAM介绍

早在 2003 年，就有一些关于动态环境中的 SLAM（SLAMIDE）问题的研究。传统的动态环境中的 SLAM 方法可以分为两类：一类是动态物体的检测与跟踪；另一类是动态物体的检测与滤波。前者被概括为运动物体检测与跟踪（DATMO）方法。一些研究者提出，在获得一组合理的检测特征后，至少有四种相关的数据关联技术可用于处理 SLAMIDE 问题，包括 GNN，JPDA ，MHT和 RANSAC。Charles 等人提出了一种将最小二乘与滑动窗口优化和广义期望最大化相结合的框架。陈等人提出了一种仅依赖于地标、无需先验信息的动态环境中的 SLAM 系统。 Walcott-Bryant 等人提出了一种用于动态环境的动态姿态图 (DPG) 模型。2018 年，Bahraini 等人提出了一种使用 MultiLevel-RANSAC 分割和跟踪多个运动物体的方法。总体而言，大多数视觉研究人员仍然专注于如何使用特征检测方法从相邻帧中提取动态物体。特征检测方法的一个潜在缺点是当动态物体移动太慢或太快时它会失败。Muhamad 等人对动态环境中的视觉 SLAM 和运动恢复结构 (SfM) 问题进行了调查。按照分类，动态 SLAM 是一种鲁棒的视觉 SLAM。
SLAMIDE一直是SLAM领域难以克服的难题，原因在于传统SLAM理论完全基于静态环境的假设，其挑战主要来自两个方面：第一，很难从平面像素定义一个动态物体；第二，动态物体不易被检测和跟踪。

关于和本论文相似方法的介绍

大多数 SLAMIDE 研究人员专注于激光雷达 SLAM 和 RGB-D SLAM，因为这些传感器可以直接获取深度信息来估计动态物体的位置。相比之下，单目视觉系统中的 SLAMIDE 研究非常有限。Barnes 等人提出了一种自监督方法来忽略单目摄像机图像中的干扰物，从而稳健地估计城市动态环境中的车辆运动。Bescos 等人使用了与我们类似的方法（B. Bescós, J.M. Fácil, J. Civera, J.J.I.R. Neira, A. Letters, DynSLAM: Tracking, mapping and inpainting in dynamic scenes, IEEE Robot. Autom. Lett. (2018) 1-1.）。他们将ORB-SLAM2与Mask RCNN结合起来，实现了动态环境下的单目和立体系统，并结合多视角几何模型和深度学习算法实现了RGB-D系统。