随着移动机器人、无人驾驶的发展，行业对 SLAM 技术的需求逐渐提高；另一方面，传感器技术的发展以及算法的进步使得 SLAM 技术也进入了从算法到产品转化的阶段。因为经常关注科技新闻，我还记得前几年扫地机器人盛行的时候，大量的产品采用的还是随机覆盖法，即检测到障碍就改变前进方向，可想而知其清扫效率往往不尽如人意，而这时某些厂商拿出了 SLAM 建图导航等功能作为一大卖点，在我看来这也是实现其智能化的关键一步。

扫地机器人路径规划

SLAM，即同步定位与地图构建（SLAM 或 Simultaneous localization and mapping），引用一段维基上的解释如下：

SLAM 是一种概念：希望机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的地图特征（比如，墙角，柱子等）定位自身位置和姿态，再根据自身位置增量式的构建地图，从而达到同时定位和地图构建的目的。

智能移动机器人的三个关键问题是“我是谁？”、“我要去哪？”、“我该怎么去？”。要实现导航，就需要采用合适的模型对周围的环境进行描述，根据环境中可观测的特征对自身进行定位。定位本质上可以定义为一个估计问题，通过传感器观测的存在不同程度的噪声的数据来估计位置；也可以定义为一个优化问题，通过多种观测数据之间的约束关系对位置进行优化。当然，噪声越低的传感器能够获得更好的定位精度，但需要在价格和精度上寻求平衡，目前常见的传感器有：激光雷达、深度摄像头、双目摄像头、IMU（惯导单元）等等。 建图实际上是在定位的基础上，将观测数据进行融合、把新的特征加入已有的世界模型中的过程。其难点在于需要实时跟踪地图的变化，回环并减小不确定性。

移动机器人控制模型

ORB-SLAM 是一个基于特征点的实时单目摄像头 SLAM 系统，用我自己的一句话概括其原理就是，**利用前后两帧图像中多组不变的特征点，计算相机坐标在两帧之间的相对运动，同时实现定位和建图。**支持宽基线的闭环检测和重定位，包括全自动初始化。该系统主要包含以下模块：跟踪（Tracking）、建图（Mapping）、重定位（Relocalization）、闭环检测（Loop closing）。项目主页，GitHub 地址，论文地址。

在本学期的《机器人技术基础》研讨课中，我和队友选择了这一种算法作为我们的入门教材，希望借此对 SLAM 系统有一个基本的认识。

前序知识

  1. ORB 特征

ORB_SLAM2 中使用 2011 年首次发布的 ORB 算子进行特征点的提取与描述，ORB 算法原理主要来自于文章 ORB: an efficient alternative to SIFT or SURF。

在现实生活中，我们从不同的距离、方向、角度或者不同的光照条件下观察一个物体时，物体的大小、形状、明暗都会有所不同，但我们的大脑依然可以判断它是同一件物体。为了让计算机也掌握这种特征，理想的特征描述子应该具备这些性质，即在大小、方向、明暗不同的图像中，同一特征点应具有足够相似的描述子，称之为描述子的可复现性（鲁棒性）。使用 BRIEF 算法得到的描述子并不具备以上这些性质，而 ORB 算法将基于 FAST 关键点的技术和基于 BRIEF 描述符的技术相结合，因此称为 ORB（Oriented FAST and Rotated BRIEF），主要解决 BRIEF 描述子不具备旋转不变性的问题。虽然没有解决尺度一致性问题，但在 OpenCV 的 ORB 实现中采用了图像金字塔来改善这方面的性能。

这里对 ORB 算法的具体原理不做详细的探究，只了解其是一种速度超快、且在一定程度上不受图像变换影响特征提取算法。学习具体原理可以参考 ORB 特征提取算法（理论篇） 等博客资料。

  1. 词袋模型 Bag of Words

BOW 就是 Bag of Words，翻译过来是指词袋，字面上非常直观：装着很多词汇的袋子。那么在这里该如何理解呢？

在一帧图像中可以提取到很多特征点，而每一个特征点可以理解为图像的某一个局部特征，那么相似场景的特征点描述应该是比较接近的。为了判断视频序列中存在的相似场景，我们可以将所有特征的描述统一化，形成一个唯一的描述。显然上文中的“词”就是指这里的特征子，这里采用的方法是一种树状的数据结构，对词汇进行分层聚类，依赖一种基于概率的方法形成一个统一描述，具体原理这里不再赘述。

  1. 3D-2D：PnP

假设从两张图像中得到了好几对配对好的特征点，那么就可以通过这些二维图像点之间的对应关系，恢复出在两帧之间摄像机的运动。这里涉及到的理论是对极几何方法，经典的有八点法等。

如果两张图像中的特征点的 3D 位置都可以获得，那么最少需要 3 个点对（包括了一个额外验证点）就可以估计相机运动，这里引用了 PnP（Perspective-n-Point）方法。因此，如果使用双目或 RGB-D 进行 SLAM，就可以直接使用 PnP 估计运动，而如果是单目视觉里程计，就需要先进行初始化。

边学边写，发现还有太多理论知识没有涉及到，以后有机会一定，读者有兴趣可以翻阅《视觉 SLAM 十四讲》。

ORB-SLAM 基本流程

ORB-SLAM 基本流程

由上图可以看到，ORB-SLAM 系统主要包括三个部分：Tracking、LocalMapping、LoopClosing。参考了一些博客，有以下简要分析：

  1. 跟踪（Tracking）

这一部分主要工作是从图像中提取 ORB 特征，根据上一帧进行姿态估计，或者进行通过全局重定位初始化位姿，然后跟踪已经重建的局部地图，优化位姿，再根据一些规则确定新的关键帧。

  1. 建图（LocalMapping）

这一部分主要完成局部地图构建。包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，使用局部捆集调整（Local BA），最后再对插入的关键帧进行筛选，去除多余的关键帧。

  1. 闭环检测（LoopClosing）

这一部分主要分为两个过程，分别是闭环探测和闭环校正。闭环检测先使用 WOB 进行探测，然后通过 Sim3 算法计算相似变换。闭环校正，主要是闭环融合和 Essential Graph 的图优化。

代码复现

为了对算法有个更直观的认识，我们尝试复现了一下其代码。传感器使用的是单个 RGB 摄像头（偷懒未经标定），直接运行在笔记本上，无 CUDA 加速。基本环境：

  • XPS15 9560 Ubuntu 18.4.03 i7-7700HQ 16.0 GB RAM *USB 免驱摄像头 720P 30 帧 焦距 2.15mm/视角 120 度

主要参考的教程为 使用 ROS 跑通 ORB_SLAM2；Ubuntu16.04+Ros+Usb 摄像头跑 ORB SLAM2；ORB_SLAM2 安装 血的教训，现在将我自己遇到过的问题整理如下。

编译过程中出现了关于 Eigen 的 deprecated 错误，提示‘Eigen::AlignedBit’ is deprecated，出现这个问题是因为安装的 eigen 版本太高，需要降低到 3.2.1 版本，然后重新编译安装所有依赖项。此外还要注意有些包搜索依赖的时候和默认路径不一样，例如 pangolin 会在/usr/include目录下进行搜索，导致 pangolin 安装失败。因此手动编译安装 Eigen 之后还需要sudo cp -r /usr/local/include/eigen3 /usr/include拷贝一下，修改路径。

编译完成后，ROS 调用摄像头的时候可能会出现报错，提示Unable to open camera calibration file [/****/****.yaml]，其实主要是没有安装 image_view，直接执行sudo apt-get install ros-indigo-image-view即可。另一种可能原因是。launch 文件中的设备号错了，笔记本电脑的 video0 一般是网络摄像头，插入的 USB 摄像头一般是 video1，如果想看 USB 摄像头是哪个端口号，可 cd 到根目录中的 dev 文件夹下查看。

在 ROS 执行。launch 文件的过程中可能会出现报错，提示[****.launch] is neither a launch file in package [****] nor is [****] a launch file name，根本原因就是系统没有把对应的。launch 文件添加到路径里。解决办法是回到上层，重新对。bash 文件执行 source。

最终运行的时候需要开三个终端，执行三个命令。为了方便起见，可以新建一个脚本文件。特别地，我新建了如下的。sh 文件，使用 chmod 命令修改权限之后可以直接执行，一键启动～

#启动 ORB-SLAM2 demo
gnome-terminal -x bash -c "roscore;"
sleep 2
gnome-terminal -x bash -c "source /home/henry/SLAM/usb_cam/build/devel/setup.bash;roslaunch usb_cam usb_cam-test.launch;"
gnome-terminal -x bash -c "rosrun ORB_SLAM2 Mono /home/henry/SLAM/ORB_SLAM2/Vocabulary/ORBvoc.txt  /home/henry/SLAM/ORB_SLAM2/Examples/ROS/ORB_SLAM2/my.yaml"
ORB-SLAM 界面

最终效果可以查看 演示视频，视频为直接录屏，右上角为原始图像，右下角为 ORB 特征点，左侧为关键帧与建图结果。后期经过调速处理，分为 2P，分别是教学楼走廊以及环教学楼的两个场景，均体现了回环检测的功能。由于均为手持拍摄，原始素材可能有一定的抖动。


本文参考资料： 一步步复现 ORBSLAM2 SLAM 简介 丁有爽