一、前言
视觉同步定位和映射 （vSLAM） 是指计算摄像机相对于周围环境的位置和方向，同时映射环境的过程。
您可以使用单眼摄像头执行 vSLAM。但是，深度无法准确计算，估计的轨迹未知，并且随着时间的推移而漂移。要生成无法从第一帧开始三角测量的初始地图，必须使用单眼相机的多个视图。更好、更可靠的解决方案是使用 RGB-D 相机，它由一个 RGB 彩色图像和一个深度图像组成。
此示例演示如何处理 RGB-D 图像数据以构建室内环境地图并估计相机的轨迹。
二、处理管道概述
RGB-D vSLAM 的管道与单目视觉同时定位和映射示例中的单目 vSLAM 管道非常相似。主要区别在于，在地图初始化阶段，3-D 地图点是从一对由一个彩色图像和一个深度图像组成的图像创建的，而不是由两帧彩色图像组成的。
-
地图初始化：初始三维世界点可以通过从彩色图像中提取ORB特征点，然后从深度图像中计算其三维世界位置来构建。彩色图像存储为第一个关键帧。
-
跟踪：初始化地图后，通过将彩色图像中的特征与最后一个关键帧中的特征进行匹配，估计每个新 RGB-D 图像的相机姿势。
-
局部映射：如果将当前彩色图像标识为关键帧，则会从深度图像计算新的三维地图点。在此阶段，使用束调整通过调整相机姿势和 3D 点来最小化重投影错误。
-
闭环：通过使用功能袋方法将每个关键帧与所有先前的关键帧进行比较来检测每个关键帧的循环。一旦检测到闭环，就会优化姿势图以优化所有关键帧的相机姿势。
三、下载并浏览输入图像序列
此示例中使用的数据来自 TUM RGB-D 基准。可以使用 Web 浏览器或运行以下代码将数据下载到临时文件夹。创建两个图像数据存储对象以分别存储颜色和深度图像。请注意，颜色和深度图像在数据集中以不同步的方式生成。因此，我们需要根据时间戳将彩色图像与深度图像相关联。
四、地图初始化
管道首先初始化包含三维世界点的地图。此步骤至关重要，对最终 SLAM 结果的准确性有重大影响。初始 ORB 特征点是使用辅助程序检测和提取特征从第一个彩色图像中提取的。它们对应的 3-D 世界位置可以使用从特征点的像素坐标和深度值计算出来.
五、初始化地点识别数据库
循环检测是使用词袋方法执行的。表示为对象的视觉词汇表是离线创建的，其 ORB 描述符是从数据集中的大量图像中提取的，闭环过程以增量方式构建一个数据库，表示为倒置对象，该数据库存储基于 ORB 特征包的可视单词到图像映射。
六、数据管理和可视化
使用第一对颜色和深度图像初始化地图后，可以存储第一个关键帧和相应的地图点，跟踪过程使用每个 RGB-D 图像执行，并确定何时插入新关键帧。
每个帧的处理方式如下：
-
为每个新的彩色图像提取 ORB 特征，然后匹配，与最后一个关键帧中具有已知相应 3-D 地图点的特征进行匹配。
-
使用透视n点算法估计相机姿势，该算法估计给定一组3D点及其相应的2D投影的校准相机的姿势。
-
给定照相机姿势，将最后一个关键帧观测到的地图点投影到当前帧中，并搜索要素对应关系
. -
使用当前帧中的 3D 到 2-D 对应关系，通过执行仅运动束调整来优化相机姿势。
-
将本地地图点投影到当前帧中，以搜索更多要素对应关系，并再次优化相机姿势。
-
跟踪的最后一步是确定当前帧是否应为新的关键帧。如果同时满足以下两个条件，则帧是关键帧：
-
自最后一个关键帧或当前帧跟踪的地图点少于 20 个或参考关键帧追踪的点的 100% 以来，至少已经过去了 25 个帧。
-
当前帧追踪的地图点小于参考关键帧追踪点的 90%。
如果当前帧要成为关键帧，请继续执行本地映射过程。否则，请启动下一帧的跟踪。
最后，对基本图应用姿态图优化以校正漂移。基本图是通过删除共可见性图中少于匹配项的连接在内部创建的。姿势图优化后，使用优化的姿势更新地图点的三维位置。
七、与地面真相比较
您可以将优化的相机轨迹与地面实况进行比较，以评估精度。下载的数据包含一个文件，该文件存储了每帧相机姿势的基本事实。数据已以MAT文件的形式保存。您还可以计算轨迹估计值的均方根误差 （RMSE）。
八、深度图像的密集重建
给定改进的相机姿势，您可以将相关深度图像中的所有有效图像点重新投影回 3-D 空间以执行密集重建。
九、程序
使用Matlab R2022b版本，点击打开。（版本过低，运行该程序可能会报错）
程序有偿获取：评论区下留言，博主看到会私信你。
