一、前言

视觉同步定位和映射 （vSLAM） 是指计算摄像机相对于周围环境的位置和方向，同时映射环境的过程。该过程仅使用来自相机的视觉输入。vSLAM 的应用包括增强现实、机器人和自动驾驶。vSLAM 只需使用单眼摄像头即可执行。但是，由于使用单个相机无法准确计算深度，因此地图的比例和估计的轨迹是未知的，并且会随时间漂移。

此外，要引导系统，需要多个视图来生成初始地图，因为它不能从第一帧开始三角测量。使用立体摄像头可以解决这些问题，并提供更可靠的 vSLAM 解决方案。此示例演示如何处理来自立体相机的图像数据，以构建室外环境的地图并估计相机的轨迹。该示例使用 ORB-SLAM2算法的版本，该算法基于功能并支持立体摄像机。

二、处理管道概述

立体 vSLAM 的管道与单目视觉同时定位和映射示例中的单目 vSLAM 管道非常相似。主要区别在于，在地图初始化阶段，三维地图点是从同一立体对的一对立体图像创建的，而不是从不同帧的两个图像创建的。

• 地图初始化：管道首先使用视差图从一对立体图像初始化 3-D 点的地图。左侧图像存储为第一个关键帧。

• 跟踪：初始化地图后，对于每个新的立体对，通过将左图中的特征与最后一个关键帧中的特征进行匹配来估计相机的姿势。通过跟踪本地地图来优化估计的相机姿势。

• 局部映射：如果将当前左侧图像标识为关键帧，则会根据立体对的视差计算新的 3-D 地图点。在此阶段，使用束调整通过调整相机姿势和 3D 点来最小化重投影错误。

• 闭环：通过使用功能袋方法将每个关键帧与所有先前的关键帧进行比较来检测每个关键帧的循环。一旦检测到闭环，就会优化姿势图以优化所有关键帧的相机姿势。

三、下载并浏览输入立体图像序列

此示例中使用的数据来自多伦多大学航空航天研究所提供的 UTIAS 长期定位和映射数据集。您可以使用 Web 浏览器或运行以下代码将数据下载到目录。

使用两个图像数据存储对象来存储立体图像。

四、地图初始化

ORB-SLAM 管道首先初始化包含三维世界点的地图。此步骤至关重要，对最终 SLAM 结果的准确性有重大影响。初始 ORB 特征点对应关系是使用立体对的两个图像之间的匹配特征找到的。匹配的对应满足以下约束：

• 整流立体对图像中两个对应特征点之间的水平偏移小于最大视差。您可以从立体对图像的立体立体浮雕中确定近似的最大视差值。

• 整流立体对图像中两个对应特征点之间的垂直偏移小于阈值。

• 匹配要素的比例几乎相同。

与匹配特征点对应的 3-D 世界位置确定如下：

• 使用选择视差范围，通过半全局匹配 （SGM） 方法计算每对立体图像的视差图。

• 使用函数根据视差图计算三维世界点坐标。

• 在视差地图中查找与要素点及其 3-D 世界位置相对应的位置。

五、数据管理和可视化

使用第一个立体对初始化地图后，可以使用 函数来存储第一个关键帧和相应的地图点。

六、初始化地点识别数据库

循环检测是使用词袋方法执行的。表示为对象的视觉词汇表是离线创建的，其 ORB 描述符是从数据集中的大量图像中提取的。

七、跟踪

跟踪过程使用每一对执行，并确定何时插入新关键帧。

每个帧的处理方式如下：

1. 为每个新的立体图像对提取 ORB 特征，然后匹配，与最后一个关键帧中具有已知相应 3-D 地图点的特征进行匹配。

2. 使用透视n点算法估计相机姿势。

给定相机姿势，将最后一个关键帧观察到的地图点投影到当前帧中，并使用函数搜索要素对应关系。

1. 使用当前帧中的 3D 到 2-D 对应关系，通过使用 执行仅运动束调整来优化相机姿势。

2. 将本地地图点投影到当前帧中，以使用 搜索更多要素对应关系，并使用 再次优化相机姿势。

3. 跟踪的最后一步是确定当前帧是否应为新的关键帧。如果同时满足以下两个条件，则帧是关键帧：

• 自最后一个关键帧以来至少经过了 5 个帧，或者当前帧跟踪的地图点少于 80 个。

• 当前帧追踪的地图点小于参考关键帧追踪点的 90%。

如果当前帧要成为关键帧，请继续执行本地映射过程。否则，请启动下一帧的跟踪。

八、本地映射

对每个关键帧执行本地映射。确定新关键帧后，将其添加到关键帧并更新新关键帧观察到的地图点的属性。为确保包含尽可能少的异常值，必须在至少 3 个关键帧中观察到有效的地图点。

新地图点是通过对当前关键帧及其连接的关键帧中的 ORB 要素点进行三角测量来创建的。对于当前关键帧中的每个不匹配特征点，使用 搜索与连接的关键帧中其他不匹配点的匹配项。局部束平差可细化当前关键帧的姿势、连接的关键帧的姿势以及在这些关键帧中观察到的所有地图点。

九、闭环

闭环步骤采用本地映射进程处理的当前关键帧，并尝试检测并闭合循环。通过使用查询数据库中与当前关键帧在视觉上相似的图像来标识循环候选项。如果候选关键帧未连接到最后一个关键帧，并且其相邻关键帧的三个是循环候选项，则该候选关键帧有效。

最后，对基本图应用姿态图优化以校正漂移。基本图是通过删除共可见性图中少于匹配项的连接在内部创建的。姿势图优化后，使用优化的姿势更新地图点的三维位置。

十、与地面真相比较

您可以将优化的相机轨迹与实际数据进行比较，以评估解决方案的准确性。下载的数据包含一个文件，用于存储每帧的 GPS 位置。您可以使用自动驾驶工具箱中的 自动驾驶工具箱或地图工具箱中测绘工具箱将 GPS 位置从地理坐标转换为本地笛卡尔坐标。在此示例中，您只需从 M 文件加载转换后的 GPS 数据即可。

十一、立体图像的密集重建

给定精细的相机姿势，您可以从与关键帧对应的立体图像中执行密集重建。

十二、程序

使用Matlab R2022b版本，点击打开。（版本过低，运行该程序可能会报错）

程序有偿获取：评论区下留言，博主看到会私信你。